How to control the xmate ros controller and robotiq controller simulataneously by separate threads?

[Charlie0257]

Hi, @xspkwy Thanks for your great work!

I noticed you wrote "In the end, we modify the xmate ros controller and robotiq controller code: xmate3Robotiq.py => xmate3Robotiq_new.py, which throw separate threads to control them simulataneously" in the README.

I don't found xmate3Robotiq_new.py. Could you tell me how to control them simulataneously?

Best, Charlie

[xspkwy]

Hello, I added the code, hope useful for you!

[Charlie0257]

Thanks for your answer!

Could you give a demo as you do in xmate3Robotiq.py——

if __name__ == "__main__":
    np.set_printoptions(suppress=True, precision=4)
    control_mode = "pd_ee_pose"  # "pd_joint_delta_pos",

    if control_mode == "pd_joint_delta_pos":
        kp = [200.0, 200.0, 200.0, 200.0, 50.0, 50.0, 50.0]
        kd = [20.0, 20.0, 20.0, 20.0, 10.0, 10.0, 10.0]
    elif control_mode == "pd_ee_pose":
        kp = [300.0, 300.0, 300.0, 50.0, 50.0, 50.0]
        kd = [20.0, 20.0, 20.0, 10.0, 10.0, 10.0]
    else:
        raise NotImplementedError
    print("current kp: ", kp)
    print("current kd: ", kd)

    print("+++++ Setup robotiq controller +++++++")
    gripper_listener = Robotiq2FGripper_Listener()
    print("+++++ Setup robot arm controller +++++++")
    imp_opendoor = ROS_ImpOpendoor(
        gripper_listener=gripper_listener, control_mode=control_mode
    )

    imp_opendoor.get_realstate()
    ## robot qpos
    robot_arm_state = imp_opendoor.real_state.q_m
    gripper_state = np.repeat(gripper_listener.gPO / 255 * 0.068, 2)  # 0-255 => 0-0.068
    real_robot_qpos = np.concatenate((robot_arm_state, gripper_state))
    print("=== real robot current qpos: ", real_robot_qpos)
    ## robot ee pose (robot frame)
    real_ee_pose_mat = imp_opendoor.real_state.toolTobase_pos_m
    real_ee_pose_base = mat2pose(np.reshape(real_ee_pose_mat, (4, 4)))
    print("=== real robot ee pose: ", real_ee_pose_base)
    ## target ee pose
    target_link7_pose = sapien.Pose(
        np.array([0.6, 0, 0.4]), np.array([0, 0.707, 0.707, 0])
    )
    ### link7 pose: Pose([0, 0, 0.431991], [0, -0.707107, 0.707107, 0])
    ### handcolorframe pose: Pose([-0.0773266, -0.0402823, 0.419322], [-0.00315852, 0.704288, 0.00583035, -0.709884])
    target_ee_mat = np.reshape(target_link7_pose.to_transformation_matrix(), 16)
    print("=== target robot ee pose: ", target_link7_pose)
    print("=== target robot ee mat: ", target_ee_mat)
    for k in range(10):
        imp_opendoor.exec_trajs(target_ee_mat, stiffness=kp, damping=kd)
    print("Done!")