Moblie-Aloha家务机器人-5.双臂操作wx250s（ROS1）

仿真：
直接修改代码xsarm_dual.launch 或者新建xsarm_dual_wx250s.launch

<launch>

  <arg name="robot_model_1"                     default="wx250s"/>
  <arg name="robot_name_1"                      default="arm_1"/>
  <arg name="base_link_1"                       default="base_link"/>
  <arg name="modes_1"                           default="$(find interbotix_xsarm_dual)/config/modes_1.yaml"/>

  <arg name="robot_model_2"                     default="wx250s"/>
  <arg name="robot_name_2"                      default="arm_2"/>
  <arg name="base_link_2"                       default="base_link"/>
  <arg name="modes_2"                           default="$(find interbotix_xsarm_dual)/config/modes_2.yaml"/>

  <arg name="use_sim"                           default="false"/>
  <arg name="use_dual_rviz"                     default="false"/>
  <arg name="rvizconfig"                        default="$(find interbotix_xsarm_dual)/rviz/xsarm_dual.rviz" />

  <include file="$(find interbotix_xsarm_control)/launch/xsarm_control.launch">
    <arg name="robot_model"                       value="$(arg robot_model_1)"/>
    <arg name="robot_name"                        value="$(arg robot_name_1)"/>
    <arg name="base_link_frame"                   value="$(arg base_link_1)"/>
    <arg name="use_world_frame"                   value="false"/>
    <arg name="use_rviz"                          value="false"/>
    <arg name="mode_configs"                      value="$(arg modes_1)"/>
    <arg name="use_sim"                           value="$(arg use_sim)"/>
  </include>

  <include file="$(find interbotix_xsarm_control)/launch/xsarm_control.launch">
    <arg name="robot_model"                       value="$(arg robot_model_2)"/>
    <arg name="robot_name"                        value="$(arg robot_name_2)"/>
    <arg name="base_link_frame"                   value="$(arg base_link_2)"/>
    <arg name="use_world_frame"                   value="false"/>
    <arg name="use_rviz"                          value="false"/>
    <arg name="mode_configs"                      value="$(arg modes_2)"/>
    <arg name="use_sim"                           value="$(arg use_sim)"/>
  </include>

  <node
    name="robot_1_transform_broadcaster"
    pkg="tf2_ros"
    type="static_transform_publisher"
    args="0 -0.25 0 0 0 0 /world /$(arg robot_name_1)/$(arg base_link_1)"/>

  <node
    name="robot_2_transform_broadcaster"
    pkg="tf2_ros"
    type="static_transform_publisher"
    args="0 0.25 0 0 0 0 /world /$(arg robot_name_2)/$(arg base_link_2)"/>

  <node if="$(arg use_dual_rviz)"
    name="rviz"
    pkg="rviz"
    type="rviz"
    args="-d $(arg rvizconfig)"/>

</launch>

roslaunch interbotix_xsarm_dual xsarm_dual_wx250s.launch use_dual_rviz:=true  use_sim:=true

测试：

cd ~/interbotix_ws/src/interbotix_ros_manipulators/interbotix_ros_xsarms/examples/interbotix_xsarm_dual/scripts/

修改xsarm_dual.py中的机器人型号为wx250s，或者新建xsarm_dual_wx250s.py

import math
import rospy
from threading import Thread
from interbotix_xs_modules.arm import InterbotixManipulatorXS

# This script is used to make two WidowX200 arms work in tandem with one another
#
# To get started, open a terminal and type 'roslaunch interbotix_xsarm_dual xsarm_dual.launch'
# Then change to this directory and type 'python xsarm_dual.py'
# Note that the 'robot_name' argument used when instantiating an InterbotixManipulatorXS instance
# is the same name as the 'robot_name_X' launch file argument

def robot_1():
    robot_1 = InterbotixManipulatorXS(robot_model="wx250s", robot_name="arm_1", moving_time=1.0, gripper_pressure=1.0, init_node=False)
    robot_1.arm.set_ee_pose_components(x=0.3, z=0.2)
    #robot_1.gripper.open(delay=0.05)
    robot_1.arm.set_single_joint_position("waist", math.pi/4.0)
    #robot_1.gripper.close(delay=0.05)
    robot_1.arm.set_single_joint_position("waist", 0)
    robot_1.arm.go_to_sleep_pose()

def robot_2():
    robot_2 = InterbotixManipulatorXS(robot_model="wx250s", robot_name="arm_2", moving_time=1.0, gripper_pressure=1.0, init_node=False)
    robot_2.arm.set_ee_pose_components(x=0.3, z=0.2)
    #robot_2.gripper.open(delay=0.05)
    robot_2.arm.set_single_joint_position("waist", math.pi/4.0)
    #robot_2.gripper.close(delay=0.05)
    robot_2.arm.set_single_joint_position("waist", 0)
    robot_2.arm.go_to_sleep_pose()

def main():
    rospy.init_node("xsarm_dual")
    Thread(target=robot_1).start()
    Thread(target=robot_2).start()

if __name__=='__main__':
    main()

python3 xsarm_dual_wx250s.py

实际测试：

警告：此操作的前提是机械臂运动空间没有限制，可以自由运动，安装在静态ALOHA2的机械臂，由于外框限制以及手臂限制，操作可能会引发干涉，谨慎使用！

仿真：

roslaunch interbotix_xsarm_dual xsarm_dual_wx250s.launch use_dual_rviz:=true

测试：

cd ~/interbotix_ws/src/interbotix_ros_manipulators/interbotix_ros_xsarms/examples/interbotix_xsarm_dual/scripts/

python3 xsarm_dual_wx250s.py