Moblie-Aloha家务机器人-7.双臂操作一大一小（Dual为模板）

新建xsarm_two_arms.launch

<launch>

  <arg name="robot_model_1"                     default="vx300s"/>
  <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_two_arms.launch use_dual_rviz:=true  use_sim:=true

测试：

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

新建xsarm_two_arms.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="vx300s", 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=1.0)
    robot_1.arm.set_single_joint_position("waist", math.pi/4.0)
    robot_1.gripper.close(delay=1.0)
    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=1.0)
    robot_2.arm.set_single_joint_position("waist", -math.pi/4.0)
    robot_2.gripper.close(delay=1.0)
    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_two_arms.py