Moveit_Servo Joint position Goal?

[DariusPietsch]

Is your feature request related to a problem? Please describe.

I used the moveit servo C++ API to create a Pose following setup for my robot. I want my robot to be adaptive and change tasks and goal positions mid trajectory. Moveit_Servo plays nicely with this idea as it is reactive and I can change goal type mid trajectory. I setup a test program, that switches between JOG and Pose commands on the fly.

To me it would be interesting to also be able to provide a joint position goal values. To allow servoing the robot to a specific joint pose.

Or am I missing something obvious and dynamic goal type changing should not be done with servo? Is there a better way to implement adaptive behavior with moveit?

Describe the solution you'd like

I propose adding a new CommandType::JOINT_POSE. This command type allows to set a joint position goal. Updating the trajectory should then steer to this position.

Describe alternatives you've considered

I took a look at the Jog msg api, which also allows to define a position delta. But moveit servo only allows for velocity commands as of now. (You will get the warning Joint jog command displacements field is not yet supported, ignoring. when providing delta positions). Maybe both features can be intruduced together?

Additional context

I also noticed, that Jog commands are not limiting to velocity/acceleration limits. Is this intended behavior or a separate issue?

If this is an interesting and useful change, I can invest some time and create a pull request with an implementation of the new CommandType and its behavior.

[github-actions[bot]]

This issue is being labeled as stale because it has been open 45 days with no activity. It will be automatically closed after another 45 days without follow-ups.

[foreignlands]

I also need this interface. Humanoid robots are booming nowadays, and teleoperation to control the movements of robotic arms has become increasingly common. If we build a robotic arm with basically the same configuration and then send joint angle values to it via a teleoperated arm to ultimately control its motion, the process might be safer if we can implement it through MoveIt Servo.

[AndyZe]

Can't you fill the displacements field in the JointJog message? https://github.com/ros-controls/control_msgs/blob/master/control_msgs/msg/JointJog.msg

The tutorial mentions this, except the tutorial fills the velocities field rather than displacements.

https://moveit.picknik.ai/main/doc/examples/realtime_servo/realtime_servo_tutorial.html

[IrvingF7]

Can't you fill the displacements field in the JointJog message? https://github.com/ros-controls/control_msgs/blob/master/control_msgs/msg/JointJog.msg

The tutorial mentions this, except the tutorial fills the velocities field rather than displacements.

https://moveit.picknik.ai/main/doc/examples/realtime_servo/realtime_servo_tutorial.html

Hi Andy!

I was under the impression that joint position's API was there, but it's not actually implemented. I could be misunderstanding something tho. See this comment I made a while ago.

https://github.com/xArm-Developer/xarm_ros2/issues/140#issuecomment-3253854093

[DariusPietsch]

Just for clarification: The warning arrises from line 228 of the servo_node.cpp file:

std::optional<KinematicState> ServoNode::processJointJogCommand(const moveit::core::RobotStatePtr& robot_state)
{
  std::optional<KinematicState> next_joint_state = std::nullopt;
  // Reject any other command types that had arrived simultaneously.
  new_twist_msg_ = new_pose_msg_ = false;

  if (!latest_joint_jog_.displacements.empty())
  {
    RCLCPP_WARN(node_->get_logger(), "Joint jog command displacements field is not yet supported, ignoring.");
    latest_joint_jog_.displacements.clear();  // Only warn once per message.
  }

displacements are not used for the JOG command.