Temp fix for twist frame conversion

moveit · Jun 21, 2023 · 30142bd · 30142bd
1 parent 563dd0d
commit 30142bd
Show file tree

Hide file tree

Showing 2 changed files with 37 additions and 23 deletions.
diff --git a/moveit_ros/moveit_servo/src/command_processor.cpp b/moveit_ros/moveit_servo/src/command_processor.cpp
@@ -87,7 +87,8 @@ Eigen::VectorXd CommandProcessor::jointDeltaFromCommand(const Twist& command)
   Eigen::VectorXd cartesian_position_delta;
 
   const bool valid_command = isValidCommand(command);
-  if (valid_command)
+  const bool is_planning_frame = command.frame_id == servo_params_.planning_frame;
+  if (is_planning_frame && valid_command)
   {
     // Compute the Cartesian position delta based on incoming command, assumed to be in m/s
     cartesian_position_delta = command.velocities * servo_params_.publish_period;
@@ -111,6 +112,9 @@ Eigen::VectorXd CommandProcessor::jointDeltaFromCommand(const Twist& command)
     servo_status_ = StatusCode::INVALID;
     if (!valid_command)
       RCLCPP_WARN_STREAM(LOGGER, "Invalid twist command.");
+    if (!is_planning_frame)
+      RCLCPP_WARN_STREAM(LOGGER,
+                         "Command frame is: " << command.frame_id << " expected: " << servo_params_.planning_frame);
   }
   return joint_position_delta;
 }
@@ -128,30 +132,38 @@ Eigen::VectorXd CommandProcessor::jointDeltaFromCommand(const Pose& command)
   const bool reached = satisfies_angular_tolerance && satisfies_linear_tolerance;
 
   const bool valid_command = isValidCommand(command);
-  if (reached)
-  {
-    servo_status_ = StatusCode::POSE_ACHIEVED;
-  }
-  else if (valid_command && !reached)
+  const bool is_planning_frame = command.frame_id == servo_params_.planning_frame;
+
+  if (is_planning_frame)
   {
-    Eigen::Vector<double, 6> cartesian_position_delta;
+    if (reached)
+    {
+      servo_status_ = StatusCode::POSE_ACHIEVED;
+    }
+    else if (valid_command && !reached)
+    {
+      Eigen::Vector<double, 6> cartesian_position_delta;
 
-    // Compute linear and angular change needed.
-    cartesian_position_delta.head<3>() = command.pose.translation() - ee_pose.translation();
+      // Compute linear and angular change needed.
+      cartesian_position_delta.head<3>() = command.pose.translation() - ee_pose.translation();
 
-    Eigen::Quaterniond q_current(ee_pose.rotation()), q_target(command.pose.rotation());
-    Eigen::Quaterniond q_error = q_target * q_current.inverse();
-    Eigen::AngleAxisd angle_axis_error(q_error);
-    cartesian_position_delta.tail<3>() = angle_axis_error.axis() * angle_axis_error.angle();
+      Eigen::Quaterniond q_current(ee_pose.rotation()), q_target(command.pose.rotation());
+      Eigen::Quaterniond q_error = q_target * q_current.inverse();
+      Eigen::AngleAxisd angle_axis_error(q_error);
+      cartesian_position_delta.tail<3>() = angle_axis_error.axis() * angle_axis_error.angle();
 
-    // Compute the required change in joint angles.
-    joint_position_delta = jointDeltaFromIK(cartesian_position_delta);
+      // Compute the required change in joint angles.
+      joint_position_delta = jointDeltaFromIK(cartesian_position_delta);
+    }
   }
   else
   {
     servo_status_ = StatusCode::INVALID;
     if (!valid_command)
       RCLCPP_WARN_STREAM(LOGGER, "Invalid pose command.");
+    if (!is_planning_frame)
+      RCLCPP_WARN_STREAM(LOGGER,
+                         "Command frame is: " << command.frame_id << " expected: " << servo_params_.planning_frame);
   }
   return joint_position_delta;
 }

diff --git a/moveit_ros/moveit_servo/src/servo.cpp b/moveit_ros/moveit_servo/src/servo.cpp
@@ -411,18 +411,20 @@ Twist Servo::toPlanningFrame(const Twist& command)
 
   if (command.frame_id != servo_params_.planning_frame)
   {
-    const bool can_transform =
-        transform_buffer_.canTransform(servo_params_.planning_frame, command.frame_id, rclcpp::Time(0));
+    const bool can_transform = robot_state_->knowsFrameTransform(servo_params_.planning_frame);
     if (can_transform)
     {
-      geometry_msgs::msg::TransformStamped command_to_planning_frame =
-          transform_buffer_.lookupTransform(servo_params_.planning_frame, command.frame_id, rclcpp::Time(0));
-      const Eigen::Isometry3d planning_frame_transfrom = tf2::transformToEigen(command_to_planning_frame);
-
+      // TODO : Find out why transform obtained from tf2 does not work properly.
+      // RobotState::GetGlobalLinkTransform does not work for all frames in the environment.
+      // The behaviour is same if robot_state_ = planning_scene_monitor_->getStateMonitor()->getCurrentState()
+      // is called before computing the transform.
+      const Eigen::Isometry3d planning_frame_transform =
+          robot_state_->getGlobalLinkTransform(servo_params_.planning_frame).inverse() *
+          robot_state_->getGlobalLinkTransform(command.frame_id);
       // Apply the transformation to the command vector
       transformed_twist.frame_id = servo_params_.planning_frame;
-      transformed_twist.velocities.head<3>() = planning_frame_transfrom.linear() * command.velocities.head<3>();
-      transformed_twist.velocities.tail<3>() = planning_frame_transfrom.linear() * command.velocities.tail<3>();
+      transformed_twist.velocities.head<3>() = planning_frame_transform.linear() * command.velocities.head<3>();
+      transformed_twist.velocities.tail<3>() = planning_frame_transform.linear() * command.velocities.tail<3>();
     }
     else
     {