Merge pull request #22 in OSHC/syropod_highlevel_controller from feature/joint_offset_param to master

* commit '467f7a806a6f08bf73e3aa613e74bade0112c072':
  Added param to add joint position offset to input and output joint state/command to/from SHC

Author: benjamin-tam

include/syropod_highlevel_controller/model.h

@@ -627,6 +627,7 @@ class Joint
 
   const double min_position_ = 0.0;            ///< The minimum position allowed for this joint
   const double max_position_ = 0.0;            ///< The maximum position allowed for this joint
+  const double offset_ = 0.0;                  ///< The position offset applied at output of SHC
   std::vector<double> packed_positions_ ;      ///< The defined position of this joint in a 'packed' state
   const double unpacked_position_ = 0.0;       ///< The defined position of this joint in an 'unpacked' state
   const double max_angular_speed_ = 0.0;       ///< The maximum angular speed of this joint

src/model.cpp

@@ -1031,6 +1031,7 @@ Joint::Joint(std::shared_ptr<Leg> leg, std::shared_ptr<Link> reference_link,
     , id_name_(leg->getIDName() + "_" + params.joint_id.data[id_number_ - 1] + "_joint")
     , min_position_(params.joint_parameters[leg->getIDNumber()][id_number_ - 1].data.at("min"))
     , max_position_(params.joint_parameters[leg->getIDNumber()][id_number_ - 1].data.at("max"))
+    , offset_(params.joint_parameters[leg->getIDNumber()][id_number_ - 1].data.at("offset"))
     , unpacked_position_(params.joint_parameters[leg->getIDNumber()][id_number_ - 1].data.at("unpacked"))
     , max_angular_speed_(params.joint_parameters[leg->getIDNumber()][id_number_ - 1].data.at("max_vel"))
 {
@@ -1080,6 +1081,7 @@ Joint::Joint(std::shared_ptr<Joint> joint)
     , id_name_(joint->id_name_)
     , min_position_(joint->min_position_)
     , max_position_(joint->max_position_)
+    , offset_(joint->offset_)
     , packed_positions_(joint->packed_positions_)
     , unpacked_position_(joint->unpacked_position_)
     , max_angular_speed_(joint->max_angular_speed_)

src/state_controller.cpp

@@ -792,7 +792,7 @@ void StateController::publishDesiredJointState(void)
       {
         std::shared_ptr<Joint> joint = joint_it->second;
         std_msgs::Float64 position_command_msg;
-        position_command_msg.data = joint->desired_position_;
+        position_command_msg.data = joint->desired_position_ + joint->offset_;
         joint->desired_position_publisher_.publish(position_command_msg);
       }
     }
@@ -1579,7 +1579,7 @@ void StateController::jointStatesCallback(const sensor_msgs::JointState &joint_s
     ROS_ASSERT(leg != NULL);
     std::shared_ptr<Joint> joint = leg->getJointByIDName(joint_name);
     ROS_ASSERT(joint != NULL);
-    joint->current_position_ = joint_states.position[i];
+    joint->current_position_ = joint_states.position[i] - joint->offset_;
     if (get_velocity_values)
     {
       joint->current_velocity_ = joint_states.velocity[i];