Added actual tip pose to legState message which is generated using FK from motor joint position outputs

Author: Talbot, Fletcher (Data61, Pullenvale)

include/syropod_highlevel_controller/model.h

@@ -545,8 +545,9 @@ class Leg : public enable_shared_from_this<Leg>
     * Updates joint transforms and applies forward kinematics to calculate a new tip pose. 
     * Sets leg current tip pose to new pose if requested.
     * @param[in] set_current Flag denoting of the calculated tip pose should be set as the current tip pose.
+    * @param[in] use_actual Flag denoting if the joint position values used for FK come from actual motor outputs
     */
-  Pose applyFK(const bool& set_current = true);
+  Pose applyFK(const bool& set_current = true, const bool& use_actual = false);
 
 private:
   shared_ptr<Model> model_;        ///< A pointer to the parent robot model object.

msg/LegState.msg

@@ -5,6 +5,7 @@ geometry_msgs/PoseStamped walker_tip_pose
 geometry_msgs/PoseStamped target_tip_pose
 geometry_msgs/PoseStamped poser_tip_pose
 geometry_msgs/PoseStamped model_tip_pose
+geometry_msgs/PoseStamped actual_tip_pose
 
 geometry_msgs/TwistStamped model_tip_velocity
 

src/model.cpp

@@ -1016,9 +1016,10 @@ double Leg::applyIK(const bool& simulation)
 /*******************************************************************************************************************//**
  * Updates joint transforms and applies forward kinematics to calculate a new tip pose. 
  * Sets leg current tip pose to new pose if requested.
- * @param[in] set_current Flag denoting of the calculated tip pose should be set as the current tip pose.
+ * @param[in] set_current Flag denoting if the calculated tip pose should be set as the current tip pose.
+ * @param[in] use_actual Flag denoting if the joint position values used for FK come from actual motor outputs
 ***********************************************************************************************************************/
-Pose Leg::applyFK(const bool& set_current)
+Pose Leg::applyFK(const bool& set_current, const bool& use_actual)
 {
   //Update joint transforms - skip first joint since it's transform is constant
   JointContainer::iterator joint_it;
@@ -1027,21 +1028,29 @@ Pose Leg::applyFK(const bool& set_current)
     shared_ptr<Joint> joint = joint_it->second;
     const shared_ptr<Link> reference_link = joint->reference_link_;
     double joint_angle = reference_link->actuating_joint_->desired_position_;
+    if (use_actual)
+    {
+      joint_angle = reference_link->actuating_joint_->current_position_;
+    }
     joint->current_transform_ = createDHMatrix(reference_link->dh_parameter_d_,
                                                reference_link->dh_parameter_theta_ + joint_angle,
                                                reference_link->dh_parameter_r_,
                                                reference_link->dh_parameter_alpha_);
   }
   const shared_ptr<Link> reference_link = tip_->reference_link_;
   double joint_angle = reference_link->actuating_joint_->desired_position_;
+  if (use_actual)
+  {
+    joint_angle = reference_link->actuating_joint_->current_position_;
+  }
   tip_->current_transform_ = createDHMatrix(reference_link->dh_parameter_d_,
                                             reference_link->dh_parameter_theta_ + joint_angle,
                                             reference_link->dh_parameter_r_,
                                             reference_link->dh_parameter_alpha_);
 
   //Get world frame position of tip
   Pose tip_pose = tip_->getPoseRobotFrame();
-  if (set_current)
+  if (set_current && !use_actual)
   {
     if (current_tip_pose_ != Pose::Undefined())
     {

src/state_controller.cpp

@@ -853,6 +853,10 @@ void StateController::publishLegState(void)
     msg.model_tip_pose.header.stamp = ros::Time::now();
     msg.model_tip_pose.header.frame_id = "base_link";
     msg.model_tip_pose.pose = leg->getCurrentTipPose().toPoseMessage();
+    
+    msg.actual_tip_pose.header.stamp = ros::Time::now();
+    msg.actual_tip_pose.header.frame_id = "base_link";
+    msg.actual_tip_pose.pose = leg->applyFK(false, true).toPoseMessage();
 
     // Tip velocities
     msg.model_tip_velocity.header.stamp = ros::Time::now();