How can I get the positions of two arm simultaneously in a dual ur5 robot system?

92 Views Asked by At

I build a dual ur5 robot system and generate a dua_ur5_moveit_config. When I bring up the two ur5 robots and connect them to the ros, I cannot get the positions of the left arm and right arm. The error is Failed to fetch current robot state. Then I remap the topic rosrun topic_tools relay right/joint_states joint_states. But this method only allows me to get the right arm position correctly. How can I get the correct positions of the two arms simultaneously ?

enter image description here

rostopic list

/attached_collision_object
/clicked_point
/collision_object
/execute_trajectory/cancel
/execute_trajectory/feedback
/execute_trajectory/goal
/execute_trajectory/result
/execute_trajectory/status
/initialpose
/joint_states
/left/joint_group_vel_controller/command
/left/joint_states
/left/pos_joint_traj_controller/command
/left/pos_joint_traj_controller/follow_joint_trajectory/cancel
/left/pos_joint_traj_controller/follow_joint_trajectory/feedback
/left/pos_joint_traj_controller/follow_joint_trajectory/goal
/left/pos_joint_traj_controller/follow_joint_trajectory/result
/left/pos_joint_traj_controller/follow_joint_trajectory/status
/left/pos_joint_traj_controller/state
/left/scaled_pos_joint_traj_controller/command
/left/scaled_pos_joint_traj_controller/follow_joint_trajectory/cancel
/left/scaled_pos_joint_traj_controller/follow_joint_trajectory/feedback
/left/scaled_pos_joint_traj_controller/follow_joint_trajectory/goal
/left/scaled_pos_joint_traj_controller/follow_joint_trajectory/result
/left/scaled_pos_joint_traj_controller/follow_joint_trajectory/status
/left/scaled_pos_joint_traj_controller/state
/left/speed_scaling_factor
/left/ur_hardware_interface/io_states
/left/ur_hardware_interface/robot_mode
/left/ur_hardware_interface/robot_program_running
/left/ur_hardware_interface/safety_mode
/left/ur_hardware_interface/script_command
/left/ur_hardware_interface/set_mode/cancel
/left/ur_hardware_interface/set_mode/feedback
/left/ur_hardware_interface/set_mode/goal
/left/ur_hardware_interface/set_mode/result
/left/ur_hardware_interface/set_mode/status
/left/ur_hardware_interface/tool_data
/left/wrench
/move_base_simple/goal
/move_group/cancel
/move_group/display_contacts
/move_group/display_planned_path
/move_group/feedback
/move_group/goal
/move_group/monitored_planning_scene
/move_group/ompl/parameter_descriptions
/move_group/ompl/parameter_updates
/move_group/plan_execution/parameter_descriptions
/move_group/plan_execution/parameter_updates
/move_group/planning_scene_monitor/parameter_descriptions
/move_group/planning_scene_monitor/parameter_updates
/move_group/result
/move_group/sense_for_plan/parameter_descriptions
/move_group/sense_for_plan/parameter_updates
/move_group/status
/move_group/trajectory_execution/parameter_descriptions
/move_group/trajectory_execution/parameter_updates
/pickup/cancel
/pickup/feedback
/pickup/goal
/pickup/result
/pickup/status
/place/cancel
/place/feedback
/place/goal
/place/result
/place/status
/planning_scene
/planning_scene_world
/right/joint_group_vel_controller/command
/right/joint_states
/right/pos_joint_traj_controller/command
/right/pos_joint_traj_controller/follow_joint_trajectory/cancel
/right/pos_joint_traj_controller/follow_joint_trajectory/feedback
/right/pos_joint_traj_controller/follow_joint_trajectory/goal
/right/pos_joint_traj_controller/follow_joint_trajectory/result
/right/pos_joint_traj_controller/follow_joint_trajectory/status
/right/pos_joint_traj_controller/state
/right/scaled_pos_joint_traj_controller/command
/right/scaled_pos_joint_traj_controller/follow_joint_trajectory/cancel
/right/scaled_pos_joint_traj_controller/follow_joint_trajectory/feedback
/right/scaled_pos_joint_traj_controller/follow_joint_trajectory/goal
/right/scaled_pos_joint_traj_controller/follow_joint_trajectory/result
/right/scaled_pos_joint_traj_controller/follow_joint_trajectory/status
/right/scaled_pos_joint_traj_controller/state
/right/speed_scaling_factor
/right/ur_hardware_interface/io_states
/right/ur_hardware_interface/robot_mode
/right/ur_hardware_interface/robot_program_running
/right/ur_hardware_interface/safety_mode
/right/ur_hardware_interface/script_command
/right/ur_hardware_interface/set_mode/cancel
/right/ur_hardware_interface/set_mode/feedback
/right/ur_hardware_interface/set_mode/goal
/right/ur_hardware_interface/set_mode/result
/right/ur_hardware_interface/set_mode/status
/right/ur_hardware_interface/tool_data
/right/wrench
/rosout
/rosout_agg
/tf
/tf_static
/trajectory_execution_event

get_dual_positions.py

import rospy, sys
import moveit_commander
from moveit_commander import MoveGroupCommander
from geometry_msgs.msg import Pose
from copy import deepcopy

class MoveIt_cloth:
    def __init__(self):
        moveit_commander.roscpp_initialize(sys.argv)
        rospy.init_node('get_dual_arms_positions', anonymous=True)
        self.left_arm = MoveGroupCommander('left_arm')
        self.right_arm = MoveGroupCommander('right_arm')

        self.left_arm.set_pose_reference_frame('desk')
        self.right_arm.set_pose_reference_frame('desk')

        left_end_effector_link = self.left_arm.get_end_effector_link()
        right_end_effector_link = self.right_arm.get_end_effector_link()

        init_left_arm_pose=self.left_arm.get_current_pose().pose
        init_right_arm_pose=self.right_arm.get_current_pose().pose

        left_start_pose = self.left_arm.get_current_pose(left_end_effector_link).pose
        right_start_pose = self.right_arm.get_current_pose(right_end_effector_link).pose

        print("#"*30)
        print('the left_arm reference frame is',self.left_arm.get_pose_reference_frame())
        print('the left_arm of the frame where all planning is performed is',self.left_arm.get_planning_frame())
        # print("init_left_arm_pose is",init_left_arm_pose)
        print('the left_arm end link is',left_end_effector_link)
        print('the left_arm start position is ',left_start_pose)

        print("#"*30)
        print('the right_arm reference frame is',self.right_arm.get_pose_reference_frame())
        print('the right_arm of the frame where all planning is performed is',self.right_arm.get_planning_frame())
        # print("init_right_arm_pose is",init_right_arm_pose)
        print('the right_arm end link is',right_end_effector_link)
        print('the right_arm start position is ',right_start_pose)
if __name__ == "__main__":

    try:
        MoveIt_cloth()
    except rospy.ROSInterruptException:
        pass

I remaped the topic rosrun topic_tools relay right/joint_states joint_states. and using get_current_pose(). But this method only allows me to get the right arm position correctly.

0

There are 0 best solutions below