168 lines
8.4 KiB
XML
168 lines
8.4 KiB
XML
<?xml version='1.0' encoding='utf-8'?>
|
|
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
|
<xacro:macro name="franka_robot"
|
|
params="arm_id
|
|
joint_limits
|
|
kinematics
|
|
inertials
|
|
dynamics
|
|
parent:='world'
|
|
xyz:='0 0 0'
|
|
rpy:='0 0 0'
|
|
hand:='true'
|
|
ee_id:=none
|
|
gazebo:='false'
|
|
with_sc:='false'
|
|
ros2_control:=false
|
|
robot_ip:=''
|
|
use_fake_hardware:=false
|
|
fake_sensor_commands:=false
|
|
gazebo_effort:=false
|
|
no_prefix:='false'
|
|
arm_prefix:=''
|
|
connected_to:='base'
|
|
multi_arm:=false">
|
|
<xacro:include filename="$(find moonshot_franka_description)/robots/common/utils.xacro" />
|
|
|
|
<xacro:include filename="$(find moonshot_franka_description)/robots/common/franka_arm.xacro" />
|
|
<xacro:property name="arm_prefix_modified" value="${'' if arm_prefix == '' else arm_prefix + '_'}" />
|
|
<xacro:franka_arm
|
|
arm_id="${arm_id}"
|
|
arm_prefix="${arm_prefix_modified}"
|
|
no_prefix="${no_prefix}"
|
|
xyz="${xyz}"
|
|
rpy="${rpy}"
|
|
safety_distance="0.03"
|
|
gazebo="${gazebo}"
|
|
joint_limits="${joint_limits}"
|
|
kinematics="${kinematics}"
|
|
inertials="${inertials}"
|
|
dynamics="${dynamics}"
|
|
with_sc="${with_sc}"
|
|
connected_to="${connected_to}"
|
|
/>
|
|
|
|
<xacro:if value="${hand and ee_id == 'franka_hand'}">
|
|
<xacro:include filename="$(find moonshot_franka_description)/end_effectors/common/franka_hand.xacro"/>
|
|
<xacro:include filename="$(find moonshot_franka_description)/end_effectors/common/utils.xacro" />
|
|
<xacro:include filename="$(find moonshot_franka_description)/end_effectors/$(arg ee_id)/$(arg ee_id)_arguments.xacro" />
|
|
<xacro:if value="${arm_id == 'fp3'}">
|
|
<xacro:property name="ee_color" value="black" />
|
|
</xacro:if>
|
|
<xacro:if value="${arm_id == 'fr3' or arm_id == 'fer'}">
|
|
<xacro:property name="ee_color" value="white" />
|
|
</xacro:if>
|
|
<xacro:property name="special_connection" value="$(arg special_connection)" />
|
|
<xacro:property name="connection" value="${special_connection if special_connection != '' else arm_id + '_link8'}" />
|
|
<xacro:franka_hand
|
|
connected_to="${arm_prefix_modified}${connection}"
|
|
arm_id="${arm_id}"
|
|
arm_prefix="${arm_prefix_modified}"
|
|
ee_id="${ee_id}_${ee_color}"
|
|
ee_inertials="${xacro.load_yaml('$(find moonshot_franka_description)/end_effectors/$(arg ee_id)/inertials.yaml')}"
|
|
rpy_ee="$(arg rpy_ee)"
|
|
xyz_ee="$(arg xyz_ee)"
|
|
tcp_xyz="$(arg tcp_xyz)"
|
|
tcp_rpy="$(arg tcp_rpy)"
|
|
safety_distance="$(arg safety_distance)"
|
|
gazebo="${gazebo}"
|
|
description_pkg="$(arg description_pkg)"
|
|
with_sc="${with_sc}"
|
|
/>
|
|
</xacro:if>
|
|
|
|
<xacro:if value="${hand and ee_id != 'none' and ee_id != 'franka_hand'}">
|
|
<xacro:include filename="$(find moonshot_franka_description)/end_effectors/common/ee_with_one_link.xacro"/>
|
|
<xacro:include filename="$(find moonshot_franka_description)/end_effectors/common/utils.xacro" />
|
|
<xacro:include filename="$(find moonshot_franka_description)/end_effectors/$(arg ee_id)/$(arg ee_id)_arguments.xacro" />
|
|
<xacro:property name="special_connection" value="$(arg special_connection)" />
|
|
<xacro:property name="connection" value="${special_connection if special_connection == '' else arm_id + '_link8'}" />
|
|
<xacro:ee_with_one_link
|
|
connected_to="${arm_prefix_modified}${connection}"
|
|
arm_id="${arm_id}"
|
|
arm_prefix="${arm_prefix_modified}"
|
|
ee_id="${ee_id}"
|
|
ee_inertials="${xacro.load_yaml('$(find moonshot_franka_description)/end_effectors/$(arg ee_id)/inertials.yaml')}"
|
|
rpy_ee="$(arg rpy_ee)"
|
|
xyz_ee="$(arg xyz_ee)"
|
|
tcp_xyz="$(arg tcp_xyz)"
|
|
tcp_rpy="$(arg tcp_rpy)"
|
|
safety_distance="$(arg safety_distance)"
|
|
gazebo="${gazebo}"
|
|
description_pkg="$(arg description_pkg)"
|
|
with_sc="${with_sc}"
|
|
/>
|
|
</xacro:if>
|
|
|
|
<!-- Definition of additional Gazebo tags -->
|
|
<xacro:if value="${gazebo}">
|
|
|
|
<xacro:if value="${parent != ''}">
|
|
<!-- Gazebo requires a joint to a link called "world" for statically mounted robots -->
|
|
<xacro:if value="${parent == 'world'}">
|
|
<link name="${parent}" />
|
|
</xacro:if>
|
|
<joint name="${parent}_joint" type="fixed">
|
|
<origin xyz="${xyz}" rpy="${rpy}" />
|
|
<parent link="${parent}" />
|
|
<child link="${arm_id}_link0" />
|
|
</joint>
|
|
</xacro:if>
|
|
|
|
<xacro:gazebo-joint joint="${arm_id}_joint1" transmission="hardware_interface/PositionJointInterface" />
|
|
<xacro:gazebo-joint joint="${arm_id}_joint2" transmission="hardware_interface/PositionJointInterface" />
|
|
<xacro:gazebo-joint joint="${arm_id}_joint3" transmission="hardware_interface/PositionJointInterface" />
|
|
<xacro:gazebo-joint joint="${arm_id}_joint4" transmission="hardware_interface/PositionJointInterface" />
|
|
<xacro:gazebo-joint joint="${arm_id}_joint5" transmission="hardware_interface/PositionJointInterface" />
|
|
<xacro:gazebo-joint joint="${arm_id}_joint6" transmission="hardware_interface/PositionJointInterface" />
|
|
<xacro:gazebo-joint joint="${arm_id}_joint7" transmission="hardware_interface/PositionJointInterface" />
|
|
|
|
<xacro:gazebo-joint joint="${arm_id}_joint1" transmission="hardware_interface/VelocityJointInterface" />
|
|
<xacro:gazebo-joint joint="${arm_id}_joint2" transmission="hardware_interface/VelocityJointInterface" />
|
|
<xacro:gazebo-joint joint="${arm_id}_joint3" transmission="hardware_interface/VelocityJointInterface" />
|
|
<xacro:gazebo-joint joint="${arm_id}_joint4" transmission="hardware_interface/VelocityJointInterface" />
|
|
<xacro:gazebo-joint joint="${arm_id}_joint5" transmission="hardware_interface/VelocityJointInterface" />
|
|
<xacro:gazebo-joint joint="${arm_id}_joint6" transmission="hardware_interface/VelocityJointInterface" />
|
|
<xacro:gazebo-joint joint="${arm_id}_joint7" transmission="hardware_interface/VelocityJointInterface" />
|
|
|
|
<xacro:gazebo-joint joint="${arm_id}_joint1" transmission="hardware_interface/EffortJointInterface" />
|
|
<xacro:gazebo-joint joint="${arm_id}_joint2" transmission="hardware_interface/EffortJointInterface" />
|
|
<xacro:gazebo-joint joint="${arm_id}_joint3" transmission="hardware_interface/EffortJointInterface" />
|
|
<xacro:gazebo-joint joint="${arm_id}_joint4" transmission="hardware_interface/EffortJointInterface" />
|
|
<xacro:gazebo-joint joint="${arm_id}_joint5" transmission="hardware_interface/EffortJointInterface" />
|
|
<xacro:gazebo-joint joint="${arm_id}_joint6" transmission="hardware_interface/EffortJointInterface" />
|
|
<xacro:gazebo-joint joint="${arm_id}_joint7" transmission="hardware_interface/EffortJointInterface" />
|
|
|
|
<xacro:transmission-franka-state arm_id="${arm_id}" />
|
|
<xacro:transmission-franka-model arm_id="${arm_id}"
|
|
root="${arm_id}_joint1"
|
|
tip="${arm_id}_joint8"
|
|
/>
|
|
|
|
<xacro:if value="${ee_id == 'franka_hand'}">
|
|
<xacro:gazebo-joint joint="${arm_id}_finger_joint1" transmission="hardware_interface/EffortJointInterface" />
|
|
<xacro:gazebo-joint joint="${arm_id}_finger_joint2" transmission="hardware_interface/EffortJointInterface" />
|
|
<!-- Friction specific material for Rubber/Rubber contact -->
|
|
<xacro:gazebo-friction link="${arm_id}_leftfinger" mu="1.13" />
|
|
<xacro:gazebo-friction link="${arm_id}_rightfinger" mu="1.13" />
|
|
</xacro:if>
|
|
</xacro:if>
|
|
|
|
<xacro:if value="${ros2_control}">
|
|
<xacro:include filename="$(find moonshot_franka_description)/robots/common/franka_arm.ros2_control.xacro"/>
|
|
<xacro:franka_arm_ros2_control
|
|
arm_id="${arm_id}"
|
|
robot_ip="${robot_ip}"
|
|
use_fake_hardware="$(arg use_fake_hardware)"
|
|
fake_sensor_commands="$(arg fake_sensor_commands)"
|
|
gazebo="$(arg gazebo)"
|
|
hand="$(arg hand)"
|
|
gazebo_effort="$(arg gazebo_effort)"
|
|
arm_prefix="${arm_prefix}"
|
|
multi_arm="$(arg multi_arm)"
|
|
/>
|
|
</xacro:if>
|
|
|
|
</xacro:macro>
|
|
</robot>
|