ROS Iron, Ubuntu 22.04 MoveIt2 from current source code.

Things are generally working, however, I tried testing some of the RobotState API calls for joints and get all zeros. Using the planning_scene_monitor returns the correct joint positions. Am I calling the API incorrectly?

Here is an example script (with MoveIt and a generic sim launched separately):

!/usr/bin/env python3

import rclpy from rclpy.node import Node from moveit.planning import MoveItPy from moveit.core.robot_state import RobotState

class MyMoveIt(Node):

def __init__(self):
    super().__init__('my_move_it')
    self._robots = MoveItPy(node_name="Robots")
    self._robots_state = RobotState(self._robots.get_robot_model())
    self._planning_scene_monitor = self._robots.get_planning_scene_monitor()

def get_joints(self, group):
    self._robots_state.update(force=True)
    return self._robots_state.get_joint_group_positions(group)

def get_joints_dict(self):
    self._robots_state.update(force=True)
    return self._robots_state.joint_positions

def get_planning_joints_dict(self):
    self._robots_state.update(force=True)
    with self._planning_scene_monitor.read_write() as scene:
        return scene.current_state.joint_positions

def main(args=None):

rclpy.init(args=args)
m = MyMoveIt()
joint_group = m.get_joints("floor_robot")
joint_dict = m.get_joints_dict()
joint_monitor = m.get_planning_joints_dict()

print(joint_group)
print(joint_dict)
print(joint_monitor)
rclpy.spin(m)
rclpy.shutdown()

if name == 'main': main()

The output shows all zeros for the joints from the API call RobotState.get_joint_group_positions(group) [test_moveit.py-1] [0. 0. 0. 0. 0. 0. 0. 0.]

And also all zeros from the API call RobotState.joint_positions()

[test_moveit.py-1] {'agv1_joint': 0.0, 'agv1_tray_joint': 0.0, 'agv2_joint': 0.0, 'agv2_tray_joint': 0.0, 'agv3_joint': 0.0, 'agv3_tray_joint': 0.0, 'agv4_joint': 0.0, 'agv4_tray_joint': 0.0, 'ceiling_elbow_joint': 0.0, 'ceiling_gripper_joint': 0.0, 'ceiling_shoulder_lift_joint': 0.0, 'ceiling_shoulder_pan_joint': 0.0, 'ceiling_wrist_1_joint': 0.0, 'ceiling_wrist_2_joint': 0.0, 'ceiling_wrist_3_joint': 0.0, 'floor_elbow_joint': 0.0, 'floor_gripper_joint': 0.0, 'floor_shoulder_lift_joint': 0.0, 'floor_shoulder_pan_joint': 0.0, 'floor_wrist_1_joint': 0.0, 'floor_wrist_2_joint': 0.0, 'floor_wrist_3_joint': 0.0, 'gantry_rotation_joint': 0.0, 'gantry_tray_joint': 0.0, 'gantry_x_axis_joint': 0.0, 'gantry_y_axis_joint': 0.0, 'linear_actuator_joint': 0.0}

But correct joints from the call with planning_scene_monitor.read_write() as scene:

return scene.current_state.joint_positions

[test_moveit.py-1] {'agv1_joint': -1.7048140679586195e-11, 'agv1_tray_joint': -5.552037940503851e-08, 'agv2_joint': -1.7048140679586195e-11, 'agv2_tray_joint': -5.552037940503851e-08, 'agv3_joint': -1.7048140679586195e-11, 'agv3_tray_joint': -5.552037940503851e-08, 'agv4_joint': -1.7048140679586195e-11, 'agv4_tray_joint': -5.552037940503851e-08, 'ceiling_elbow_joint': 1.5651503528892992, 'ceiling_gripper_joint': 1.630038185318483e-06, 'ceiling_shoulder_lift_joint': -1.5652087758128068, 'ceiling_shoulder_pan_joint': -0.0017447112508088836, 'ceiling_wrist_1_joint': 3.1367932678009036, 'ceiling_wrist_2_joint': -1.5673497252117758, 'ceiling_wrist_3_joint': 0.000510919071003535, 'floor_elbow_joint': 1.574333861266786, 'floor_gripper_joint': -7.900014820094725e-07, 'floor_shoulder_lift_joint': -1.5700042778017689, 'floor_shoulder_pan_joint': -1.02951697193987e-05, 'floor_wrist_1_joint': -1.5773201526882323, 'floor_wrist_2_joint': -1.570989247348104, 'floor_wrist_3_joint': -1.0382574576972559e-05, 'gantry_rotation_joint': -1.5705067419525387, 'gantry_tray_joint': -1.7064930890597907e-07, 'gantry_x_axis_joint': 1.9999758196996476, 'gantry_y_axis_joint': 0.0002563403403361481, 'linear_actuator_joint': -1.328206018428893e-06}

moveit / moveit2_tutorials

RobotState api calls for joints return all zeros #853

!/usr/bin/env python3