servo_node republishes current joint positions instead of commanding motion #3657
Description
Description
Hello!
When running MoveIt Servo, the servo node publishes joint trajectory commands that exactly match the current joint state instead of producing incremental motion commands. No errors or warnings are reported by the Servo node while this is happening.
I am using a node that subscribes to the /joy topic and upon detecting a button press publishes to the servo_node twist input topic. The twist that it publishes is fixed for now. I've tried publishing to the servo_node twist input topic in cli but the same behavior happens when I do that too.
Here are the relevant files:
servo node config:
###############################################
# Modify all parameters related to servoing here
###############################################
# Optionally override Servo's internal velocity scaling when near singularity or collision (0.0 = use internal velocity scaling)
# override_velocity_scaling_factor = 0.0 # valid range [0.0:1.0]
## Properties of outgoing commands
publish_period: 0.02 # 1/Nominal publish rate [seconds]
max_expected_latency: 0.1 # delay between sending a command and the robot executing it [seconds]
command_in_type: "speed_units" # "unitless"> in the range [-1:1], as if from joystick. "speed_units"> cmds are in m/s and rad/s
scale:
# Scale parameters are only used if command_in_type=="unitless"
linear: 0.2 # Max linear velocity. Unit is [m/s]. Only used for Cartesian commands.
rotational: 0.8 # Max angular velocity. Unit is [rad/s]. Only used for Cartesian commands.
# Max joint angular/linear velocity. Only used for joint commands on joint_command_in_topic.
joint: 0.5
incoming_command_timeout: 0.1 # [seconds] If no incoming commands are received in this time, Servo will stop the robot.
# What type of topic does your robot driver expect?
# Currently supported are std_msgs/Float64MultiArray or trajectory_msgs/JointTrajectory
command_out_type: trajectory_msgs/JointTrajectory
# What to publish? Can save some bandwidth as most robots only require positions or velocities
publish_joint_positions: true
publish_joint_velocities: false
publish_joint_accelerations: false
## Plugins for smoothing outgoing commands
use_smoothing: true
smoothing_filter_plugin_name: "online_signal_smoothing::AccelerationLimitedPlugin"
# If is_primary_planning_scene_monitor is set to true, the Servo server's PlanningScene advertises the /get_planning_scene service,
# which other nodes can use as a source for information about the planning environment.
# NOTE: If a different node in your system is responsible for the "primary" planning scene instance (e.g. the MoveGroup node),
# then is_primary_planning_scene_monitor needs to be set to false.
is_primary_planning_scene_monitor: false
check_octomap_collisions: false # Check collision against the octomap (if a 3D sensor plugin is available)
## MoveIt properties
move_group_name: arm # Often 'manipulator' or 'arm'
## Configure handling of singularities and joint limits
lower_singularity_threshold: 10.0 # Start decelerating when the condition number hits this (close to singularity)
hard_stop_singularity_threshold: 30.0 # Stop when the condition number hits this
leaving_singularity_threshold_multiplier: 2.0 # Multiply the hard stop limit by this when leaving singularity (see https://github.com/moveit/moveit2/pull/620)
# Added as a buffer to joint variable position bounds [in that joint variable's respective units].
# Can be of size 1, which applies the margin to all joints, or the same size as the number of degrees of freedom of the active joint group.
# If moving quickly, make these values larger.
joint_limit_margins: [0.12]
## Topic names
cartesian_command_in_topic: ~/delta_twist_cmds # Topic for incoming Cartesian twist commands
joint_command_in_topic: ~/delta_joint_cmds # Topic for incoming joint angle commands
joint_topic: /joint_states # Get joint states from this topic
status_topic: ~/status # Publish status to this topic
command_out_topic: /arm_controller/joint_trajectory # Publish outgoing commands here
## Collision checking for the entire robot body
check_collisions: true # Check collisions?
collision_check_rate: 10.0 # [Hz] Collision-checking can easily bog down a CPU if done too often.
self_collision_proximity_threshold: 0.01 # Start decelerating when a self-collision is this far [m]
scene_collision_proximity_threshold: 0.02 # Start decelerating when a scene collision is this far [m]SRDF:
<?xml version="1.0" encoding="UTF-8"?>
<robot name="arm">
<group name="arm">
<chain base_link="arm_link0" tip_link="arm_hand_tcp"/>
</group>
<group name="hand">
<joint name="arm_finger_joint1"/>
</group>
<group name="arm_and_hand">
<group name="arm"/>
<group name="hand"/>
</group>
<group_state name="default" group="arm_and_hand">
<joint name="arm_finger_joint1" value="0.001"/>
<joint name="arm_joint1" value="0"/>
<joint name="arm_joint2" value="-0.785398"/>
<joint name="arm_joint3" value="0"/>
<joint name="arm_joint4" value="-2.35619"/>
<joint name="arm_joint5" value="0"/>
<joint name="arm_joint6" value="1.5707"/>
<joint name="arm_joint7" value="0.785398"/>
</group_state>
<end_effector name="end_effector" group="hand" parent_group="arm" parent_link="arm_hand_tcp"/>
<virtual_joint name="world_joint" type="fixed" parent_frame="world" child_link="arm_link0"/>
<disable_collisions link1="arm_hand" link2="arm_leftfinger" reason="Adjacent"/>
<disable_collisions link1="arm_hand" link2="arm_link3" reason="Never"/>
<disable_collisions link1="arm_hand" link2="arm_link4" reason="Never"/>
<disable_collisions link1="arm_hand" link2="arm_link5" reason="Default"/>
<disable_collisions link1="arm_hand" link2="arm_link6" reason="Never"/>
<disable_collisions link1="arm_hand" link2="arm_link7" reason="Adjacent"/>
<disable_collisions link1="arm_hand" link2="arm_rightfinger" reason="Adjacent"/>
<disable_collisions link1="arm_leftfinger" link2="arm_link3" reason="Never"/>
<disable_collisions link1="arm_leftfinger" link2="arm_link4" reason="Never"/>
<disable_collisions link1="arm_leftfinger" link2="arm_link6" reason="Never"/>
<disable_collisions link1="arm_leftfinger" link2="arm_link7" reason="Never"/>
<disable_collisions link1="arm_leftfinger" link2="arm_rightfinger" reason="Default"/>
<disable_collisions link1="arm_link0" link2="arm_link1" reason="Adjacent"/>
<disable_collisions link1="arm_link0" link2="arm_link2" reason="Never"/>
<disable_collisions link1="arm_link0" link2="arm_link3" reason="Never"/>
<disable_collisions link1="arm_link0" link2="arm_link4" reason="Never"/>
<disable_collisions link1="arm_link1" link2="arm_link2" reason="Adjacent"/>
<disable_collisions link1="arm_link1" link2="arm_link3" reason="Never"/>
<disable_collisions link1="arm_link1" link2="arm_link4" reason="Never"/>
<disable_collisions link1="arm_link2" link2="arm_link3" reason="Adjacent"/>
<disable_collisions link1="arm_link2" link2="arm_link4" reason="Never"/>
<disable_collisions link1="arm_link3" link2="arm_link4" reason="Adjacent"/>
<disable_collisions link1="arm_link3" link2="arm_link5" reason="Never"/>
<disable_collisions link1="arm_link3" link2="arm_link6" reason="Never"/>
<disable_collisions link1="arm_link3" link2="arm_link7" reason="Never"/>
<disable_collisions link1="arm_link3" link2="arm_rightfinger" reason="Never"/>
<disable_collisions link1="arm_link4" link2="arm_link5" reason="Adjacent"/>
<disable_collisions link1="arm_link4" link2="arm_link6" reason="Never"/>
<disable_collisions link1="arm_link4" link2="arm_link7" reason="Never"/>
<disable_collisions link1="arm_link4" link2="arm_rightfinger" reason="Never"/>
<disable_collisions link1="arm_link5" link2="arm_link6" reason="Adjacent"/>
<disable_collisions link1="arm_link5" link2="arm_link7" reason="Default"/>
<disable_collisions link1="arm_link6" link2="arm_link7" reason="Adjacent"/>
<disable_collisions link1="arm_link6" link2="arm_rightfinger" reason="Never"/>
<disable_collisions link1="arm_link7" link2="arm_rightfinger" reason="Never"/>
</robot>Launch file:
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.substitutions import Command
from launch_ros.substitutions import FindPackageShare
from launch.substitutions import PathJoinSubstitution
from launch.actions import IncludeLaunchDescription, GroupAction
from launch.launch_description_sources import FrontendLaunchDescriptionSource, PythonLaunchDescriptionSource
from moveit_configs_utils import MoveItConfigsBuilder
from launch_param_builder import ParameterBuilder
from ament_index_python.packages import get_package_share_directory
from moveit_configs_utils.launches import generate_move_group_launch
from moveit_configs_utils.launches import generate_static_virtual_joint_tfs_launch
import os
def generate_launch_description():
arm_description_package = get_package_share_directory("arm_description")
urdf_file = os.path.join(arm_description_package, 'urdf', 'arm.urdf.xacro')
robot_description = Command(['xacro ', urdf_file])
moveit_config = (
MoveItConfigsBuilder("arm", package_name="arm_moveit_config")
.robot_description(file_path=os.path.join(arm_description_package, "urdf", "arm.urdf.xacro"))
.robot_description_semantic(file_path=os.path.join(arm_description_package, "srdf", "arm.srdf"))
.planning_pipelines(
pipelines=["ompl", "pilz_industrial_motion_planner", "stomp"],
default_planning_pipeline="ompl"
)
.to_moveit_configs()
)
moveit_servo_params = {
"moveit_servo": ParameterBuilder("arm_moveit_config")
.yaml("config/servo.yaml")
.to_dict()
}
# Foxglove launch is wrapped in a GroupAction to isolate its global parameters.
# Foxglove sets a `capabilities` parameter globally, which conflicts with
# MoveIt's `move_group` capabilities if not scoped. GroupAction prevents
# parameter leakage while still allowing Foxglove to start correctly.
foxglove_bridge_launch = GroupAction([
IncludeLaunchDescription(
FrontendLaunchDescriptionSource(
os.path.join(
get_package_share_directory('foxglove_bridge'),
'launch',
'foxglove_bridge_launch.xml'
)
)
)
])
robot_state_publisher_node = Node(
package='robot_state_publisher',
executable='robot_state_publisher',
output='screen',
parameters=[{'robot_description': robot_description}]
)
arm_control = IncludeLaunchDescription(
PythonLaunchDescriptionSource([
FindPackageShare('arm_control'),
'/launch',
'/arm_control.launch.py'
])
)
move_group_launch = generate_move_group_launch(moveit_config)
static_virtual_joint_tfs_launch = generate_static_virtual_joint_tfs_launch(moveit_config)
acceleration_filter_update_period = {"update_period": 0.02}
planning_group_name = {"planning_group_name": "arm"}
moveit_servo_node = Node(
package="moveit_servo",
executable="servo_node",
parameters=[
moveit_servo_params,
acceleration_filter_update_period,
planning_group_name,
moveit_config.robot_description,
moveit_config.robot_description_semantic,
moveit_config.robot_description_kinematics,
moveit_config.joint_limits,
],
output="screen",
)
joy_node = Node(
package='joy',
executable='joy_node',
output='screen'
)
operator_node = Node(
package='arm_operator',
executable='operator',
output='screen'
)
return LaunchDescription([
foxglove_bridge_launch,
robot_state_publisher_node,
arm_control,
move_group_launch,
static_virtual_joint_tfs_launch,
moveit_servo_node,
joy_node,
operator_node
])Python code for the node that subscribes to the /joy topic and publishes to the servo_node input topic:
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Joy
from control_msgs.msg import JointJog
from geometry_msgs.msg import TwistStamped
from geometry_msgs.msg import PoseStamped
from moveit_msgs.srv import ServoCommandType
class Operator(Node):
def __init__(self):
super().__init__('operator_servo')
self.joy_subscriber = self.create_subscription(Joy, '/joy', self.joy_callback, 10)
self.switch_command_type_client = self.create_client(ServoCommandType, '/servo_node/switch_command_type')
self.switch_command_type_client.wait_for_service()
self.joint_jog_publisher = self.create_publisher(JointJog, 'servo_node/delta_joint_cmds', 10)
self.twist_command_publisher = self.create_publisher(TwistStamped, 'servo_node/delta_twist_cmds', 10)
self.pose_command_publisher = self.create_publisher(PoseStamped, 'servo_node/pose_target_cmds', 10)
request = ServoCommandType.Request()
request.command_type = ServoCommandType.Request.TWIST
future = self.switch_command_type_client.call_async(request)
rclpy.spin_until_future_complete(self, future)
self.get_logger().info('OperatorServo node started')
def joy_callback(self, msg: Joy):
twist_cmd = TwistStamped()
twist_cmd.header.stamp = self.get_clock().now().to_msg()
twist_cmd.header.frame_id = 'world'
twist_cmd.twist.linear.x = 0.1
twist_cmd.twist.linear.y = 0.0
twist_cmd.twist.linear.z = 0.0
twist_cmd.twist.angular.x = 0.0
twist_cmd.twist.angular.y = 0.0
twist_cmd.twist.angular.z = 0.0
# y button
if (msg.buttons[3] == 1):
self.twist_command_publisher.publish(twist_cmd)
self.get_logger().info('Published Twist Command')
def main(args=None):
rclpy.init(args=args)
node = Operator()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()URDF:
<?xml version="1.0" ?>
<robot name="arm" xmlns:xacro="http://www.ros.org/wiki/xacro">
<link name="arm_link0">
<visual>
<geometry>
<mesh filename="package://arm_description/meshes/visual/link0.dae" />
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="package://arm_description/meshes/collision/link0.stl" />
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="-0.041018 -0.00014 0.049974"/>
<mass value="0.629769"/>
<inertia ixx="0.00315" ixy="8.2904e-07" ixz="0.00015" iyy="0.00388" iyz="8.2299e-06" izz="0.004285"/>
</inertial>
</link>
<link name="arm_link1">
<visual>
<geometry>
<mesh filename="package://arm_description/meshes/visual/link1.dae" />
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="package://arm_description/meshes/collision/link1.stl" />
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0.003875 0.002081 -0.04762"/>
<mass value="4.970684"/>
<inertia ixx="0.70337" ixy="-0.000139" ixz="0.006772" iyy="0.70661" iyz="0.019169" izz="0.009117"/>
</inertial>
</link>
<joint name="arm_joint1" type="revolute">
<origin rpy="0 0 0" xyz="0 0 0.333"/>
<parent link="arm_link0"/>
<child link="arm_link1"/>
<axis xyz="0 0 1"/>
<limit effort="87.0" lower="-2.8973" upper="2.8973" velocity="2.175"/>
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
<dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
</joint>
<link name="arm_link2">
<visual>
<geometry>
<mesh filename="package://arm_description/meshes/visual/link2.dae" />
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="package://arm_description/meshes/collision/link2.stl" />
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="-0.003141 -0.02872 0.003495"/>
<mass value="0.646926"/>
<inertia ixx="0.007962" ixy="-0.003925" ixz="0.010254" iyy="0.02811" iyz="0.000704" izz="0.025995"/>
</inertial>
</link>
<joint name="arm_joint2" type="revolute">
<origin rpy="-1.5707963267948966 0 0" xyz="0 0 0"/>
<parent link="arm_link1"/>
<child link="arm_link2"/>
<axis xyz="0 0 1"/>
<limit effort="87.0" lower="-1.7628" upper="1.7628" velocity="2.175"/>
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-1.7628" soft_upper_limit="1.7628"/>
<dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
</joint>
<link name="arm_link3">
<visual>
<geometry>
<mesh filename="package://arm_description/meshes/visual/link3.dae" />
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="package://arm_description/meshes/collision/link3.stl" />
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="2.7518e-02 3.9252e-02 -6.6502e-02"/>
<mass value="3.228604"/>
<inertia ixx="0.037242" ixy="-0.004761" ixz="-0.011396" iyy="0.036155" iyz="-0.012805" izz="0.01083"/>
</inertial>
</link>
<joint name="arm_joint3" type="revolute">
<origin rpy="1.5707963267948966 0 0" xyz="0 -0.316 0"/>
<parent link="arm_link2"/>
<child link="arm_link3"/>
<axis xyz="0 0 1"/>
<limit effort="87.0" lower="-2.8973" upper="2.8973" velocity="2.175"/>
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
<dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
</joint>
<link name="arm_link4">
<visual>
<geometry>
<mesh filename="package://arm_description/meshes/visual/link4.dae"/>
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="package://arm_description/meshes/collision/link4.stl"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="-5.317e-02 1.04419e-01 2.7454e-02"/>
<mass value="3.587895"/>
<inertia ixx="0.025853" ixy="0.007796" ixz="-0.001332" iyy="0.019552" iyz="0.008641" izz="0.028323"/>
</inertial>
</link>
<joint name="arm_joint4" type="revolute">
<origin rpy="1.5707963267948966 0 0" xyz="0.0825 0 0"/>
<parent link="arm_link3"/>
<child link="arm_link4"/>
<axis xyz="0 0 1"/>
<limit effort="87.0" lower="-3.0718" upper="-0.0698" velocity="2.175"/>
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-3.0718" soft_upper_limit="-0.0698"/>
<dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
</joint>
<link name="arm_link5">
<visual>
<geometry>
<mesh filename="package://arm_description/meshes/visual/link5.dae" />
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="package://arm_description/meshes/collision/link5.stl" />
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="-1.1953e-02 4.1065e-02 -3.8437e-02"/>
<mass value="1.225946"/>
<inertia ixx="0.035549" ixy="-0.002117" ixz="-0.004037" iyy="0.029474" iyz="0.000229" izz="0.008627"/>
</inertial>
</link>
<joint name="arm_joint5" type="revolute">
<origin rpy="-1.5707963267948966 0 0" xyz="-0.0825 0.384 0"/>
<parent link="arm_link4"/>
<child link="arm_link5"/>
<axis xyz="0 0 1"/>
<limit effort="12.0" lower="-2.8973" upper="2.8973" velocity="2.61"/>
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
<dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
</joint>
<link name="arm_link6">
<visual>
<geometry>
<mesh filename="package://arm_description/meshes/visual/link6.dae" />
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="package://arm_description/meshes/collision/link6.stl" />
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="6.0149e-02 -1.4117e-02 -1.0517e-02"/>
<mass value="1.666555"/>
<inertia ixx="0.001964" ixy="0.000109" ixz="-0.001158" iyy="0.004354" iyz="0.000341" izz="0.005433"/>
</inertial>
</link>
<joint name="arm_joint6" type="revolute">
<origin rpy="1.5707963267948966 0 0" xyz="0 0 0"/>
<parent link="arm_link5"/>
<child link="arm_link6"/>
<axis xyz="0 0 1"/>
<limit effort="12.0" lower="-0.0175" upper="3.7525" velocity="2.61"/>
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-0.0175" soft_upper_limit="3.7525"/>
<dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
</joint>
<link name="arm_link7">
<visual>
<geometry>
<mesh filename="package://arm_description/meshes/visual/link7.dae" />
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="package://arm_description/meshes/collision/link7.stl" />
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="1.0517e-02 -4.252e-03 6.1597e-02"/>
<mass value="0.735522"/>
<inertia ixx="0.012516" ixy="-0.000428" ixz="-0.001196" iyy="0.010027" iyz="-0.000741" izz="0.004815"/>
</inertial>
</link>
<joint name="arm_joint7" type="revolute">
<origin rpy="1.5707963267948966 0 0" xyz="0.088 0 0"/>
<parent link="arm_link6"/>
<child link="arm_link7"/>
<axis xyz="0 0 1"/>
<limit effort="12.0" lower="-2.8973" upper="2.8973" velocity="2.61"/>
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
<dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
</joint>
<link name="arm_link8">
<inertial>
<mass value="0"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
</link>
<joint name="arm_joint8" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0.107"/>
<parent link="arm_link7"/>
<child link="arm_link8"/>
</joint>
<joint name="arm_hand_joint" type="fixed">
<parent link="arm_link8"/>
<child link="arm_hand"/>
<origin rpy="0 0 -0.7853981633974483" xyz="0 0 0"/>
</joint>
<link name="arm_hand">
<visual>
<geometry>
<mesh filename="package://arm_description/meshes/visual/hand.dae" />
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="package://arm_description/meshes/collision/hand.stl" />
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="-0.01 0 0.03"/>
<mass value="0.73"/>
<inertia ixx="0.001" ixy="0" ixz="0" iyy="0.0025" iyz="0" izz="0.0017"/>
</inertial>
</link>
<!-- Define the hand_tcp frame -->
<link name="arm_hand_tcp">
<inertial>
<mass value="0"/>
<origin rpy="0 0 0" xyz="0 0 0"/>
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
</inertial>
</link>
<joint name="arm_hand_tcp_joint" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0.1034"/>
<parent link="arm_hand"/>
<child link="arm_hand_tcp"/>
</joint>
<link name="arm_leftfinger">
<visual>
<geometry>
<mesh filename="package://arm_description/meshes/visual/finger.dae" />
</geometry>
</visual>
<!-- screw mount -->
<collision>
<origin rpy="0 0 0" xyz="0 18.5e-3 11e-3"/>
<geometry>
<box size="22e-3 15e-3 20e-3"/>
</geometry>
</collision>
<!-- cartriage sledge -->
<collision>
<origin rpy="0 0 0" xyz="0 6.8e-3 2.2e-3"/>
<geometry>
<box size="22e-3 8.8e-3 3.8e-3"/>
</geometry>
</collision>
<!-- diagonal finger -->
<collision>
<origin rpy="0.5235987755982988 0 0" xyz="0 15.9e-3 28.35e-3"/>
<geometry>
<box size="17.5e-3 7e-3 23.5e-3"/>
</geometry>
</collision>
<!-- rubber tip with which to grasp -->
<collision>
<origin rpy="0 0 0" xyz="0 7.58e-3 45.25e-3"/>
<geometry>
<box size="17.5e-3 15.2e-3 18.5e-3"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0.015"/>
<inertia ixx="2.3749999999999997e-06" ixy="0" ixz="0" iyy="2.3749999999999997e-06" iyz="0" izz="7.5e-07"/>
</inertial>
</link>
<link name="arm_rightfinger">
<visual>
<origin rpy="0 0 3.141592653589793" xyz="0 0 0"/>
<geometry>
<mesh filename="package://arm_description/meshes/visual/finger.dae" />
</geometry>
</visual>
<!-- screw mount -->
<collision>
<origin rpy="0 0 0" xyz="0 -18.5e-3 11e-3"/>
<geometry>
<box size="22e-3 15e-3 20e-3"/>
</geometry>
</collision>
<!-- cartriage sledge -->
<collision>
<origin rpy="0 0 0" xyz="0 -6.8e-3 2.2e-3"/>
<geometry>
<box size="22e-3 8.8e-3 3.8e-3"/>
</geometry>
</collision>
<!-- diagonal finger -->
<collision>
<origin rpy="-0.5235987755982988 0 0" xyz="0 -15.9e-3 28.35e-3"/>
<geometry>
<box size="17.5e-3 7e-3 23.5e-3"/>
</geometry>
</collision>
<!-- rubber tip with which to grasp -->
<collision>
<origin rpy="0 0 0" xyz="0 -7.58e-3 45.25e-3"/>
<geometry>
<box size="17.5e-3 15.2e-3 18.5e-3"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 0"/>
<mass value="0.015"/>
<inertia ixx="2.3749999999999997e-06" ixy="0" ixz="0" iyy="2.3749999999999997e-06" iyz="0" izz="7.5e-07"/>
</inertial>
</link>
<joint name="arm_finger_joint1" type="prismatic">
<parent link="arm_hand"/>
<child link="arm_leftfinger"/>
<origin rpy="0 0 0" xyz="0 0 0.0584"/>
<axis xyz="0 1 0"/>
<limit effort="100" lower="0.0" upper="0.04" velocity="0.2"/>
<dynamics damping="0.3"/>
</joint>
<joint name="arm_finger_joint2" type="prismatic">
<parent link="arm_hand"/>
<child link="arm_rightfinger"/>
<origin rpy="0 0 0" xyz="0 0 0.0584"/>
<axis xyz="0 -1 0"/>
<limit effort="100" lower="0.0" upper="0.04" velocity="0.2"/>
<mimic joint="arm_finger_joint1"/>
<dynamics damping="0.3"/>
</joint>
</robot>ROS Distro
Jazzy
OS and version
Ubuntu 24.04
Source or binary build?
Binary
If binary, which release version?
ros-jazzy-moveit-servo/noble,now 2.12.3-1noble.20251110.183003 amd64
If source, which branch?
No response
Which RMW are you using?
CycloneDDS
Steps to Reproduce
Setup moveit servo with the above configs and robot description.
Expected behavior
servo_node should publish joint trajectories that move the robot.
Actual behavior
servo_node publishes joint trajectory commands that exactly match the current joint state instead of producing incremental motion commands, so the robot doesnt move.
Backtrace or Console output
No response