initial forking commit
This commit is contained in:
88
robots/common/franka_arm.ros2_control.xacro
Normal file
88
robots/common/franka_arm.ros2_control.xacro
Normal file
@@ -0,0 +1,88 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
|
||||
<xacro:macro name="franka_arm_ros2_control"
|
||||
params="arm_id
|
||||
robot_ip
|
||||
use_fake_hardware:=^|false
|
||||
fake_sensor_commands:=^|false
|
||||
gazebo:=^|false
|
||||
hand:=^|false
|
||||
gazebo_effort:=^|false
|
||||
arm_prefix:=''
|
||||
multi_arm:=false">
|
||||
|
||||
<xacro:property name="arm_prefix_modified" value="${'' if arm_prefix == '' else arm_prefix + '_'}" />
|
||||
<ros2_control name="${arm_prefix_modified}FrankaHardwareInterface" type="system">
|
||||
<hardware>
|
||||
<param name="arm_id">${arm_id}</param>
|
||||
<param name="prefix">${arm_prefix}</param>
|
||||
<xacro:if value="${use_fake_hardware}">
|
||||
<plugin>fake_components/GenericSystem</plugin>
|
||||
<param name="fake_sensor_commands">${fake_sensor_commands}</param>
|
||||
<param name="state_following_offset">0.0</param>
|
||||
</xacro:if>
|
||||
|
||||
<xacro:if value="${gazebo}">
|
||||
<plugin>franka_ign_ros2_control/IgnitionSystem</plugin>
|
||||
</xacro:if>
|
||||
|
||||
<xacro:if value="${use_fake_hardware == 0 and gazebo == 0 and multi_arm == 0}">
|
||||
<plugin>franka_hardware/FrankaHardwareInterface</plugin>
|
||||
<param name="robot_ip">${robot_ip}</param>
|
||||
<param name="arm_prefix">${arm_prefix}</param>
|
||||
</xacro:if>
|
||||
<xacro:if value="${use_fake_hardware == 0 and gazebo == 0 and multi_arm == 1}">
|
||||
<plugin>franka_hardware/MultiFrankaHardwareInterface</plugin>
|
||||
<param name="robot_ip">${robot_ip}</param>
|
||||
<param name="arm_prefix">${arm_prefix}</param>
|
||||
</xacro:if>
|
||||
</hardware>
|
||||
|
||||
<xacro:macro name="configure_joint" params="joint_name initial_position">
|
||||
<joint name="${joint_name}">
|
||||
<!--
|
||||
deactivated for gazebo velocity and position interface due to a bug
|
||||
https://github.com/ros-controls/gz_ros2_control/issues/343
|
||||
|
||||
Command Interfaces -->
|
||||
<xacro:if value="${multi_arm == 0}">
|
||||
<command_interface name="position"/>
|
||||
<command_interface name="velocity"/>
|
||||
</xacro:if>
|
||||
<xacro:if value="${gazebo == 0 or gazebo_effort == 1}">
|
||||
<command_interface name="effort"/>
|
||||
</xacro:if>
|
||||
|
||||
<!-- State Interfaces -->
|
||||
<state_interface name="position">
|
||||
<param name="initial_value">${initial_position}</param>
|
||||
</state_interface>
|
||||
<state_interface name="velocity"/>
|
||||
<param name="initial_value">0.0</param>
|
||||
<state_interface name="effort"/>
|
||||
</joint>
|
||||
</xacro:macro>
|
||||
|
||||
<xacro:configure_joint joint_name="${arm_prefix_modified}${arm_id}_joint1" initial_position="0.0"/>
|
||||
<xacro:configure_joint joint_name="${arm_prefix_modified}${arm_id}_joint2" initial_position="${-pi/4}"/>
|
||||
<xacro:configure_joint joint_name="${arm_prefix_modified}${arm_id}_joint3" initial_position="0.0"/>
|
||||
<xacro:configure_joint joint_name="${arm_prefix_modified}${arm_id}_joint4" initial_position="${-3*pi/4}"/>
|
||||
<xacro:configure_joint joint_name="${arm_prefix_modified}${arm_id}_joint5" initial_position="0.0"/>
|
||||
<xacro:configure_joint joint_name="${arm_prefix_modified}${arm_id}_joint6" initial_position="${pi/2}"/>
|
||||
<xacro:configure_joint joint_name="${arm_prefix_modified}${arm_id}_joint7" initial_position="${pi/4}"/>
|
||||
<xacro:if value="${gazebo and hand}">
|
||||
<xacro:configure_joint joint_name="${arm_id}_finger_joint1" initial_position="0.0" />
|
||||
</xacro:if>
|
||||
|
||||
</ros2_control>
|
||||
<xacro:if value="${gazebo}">
|
||||
<gazebo>
|
||||
<plugin filename="franka_ign_ros2_control-system" name="ign_ros2_control::IgnitionROS2ControlPlugin">
|
||||
<parameters>$(find franka_gazebo_bringup)/config/franka_gazebo_controllers.yaml</parameters>
|
||||
</plugin>
|
||||
</gazebo>
|
||||
</xacro:if>
|
||||
|
||||
</xacro:macro>
|
||||
</robot>
|
||||
153
robots/common/franka_arm.xacro
Normal file
153
robots/common/franka_arm.xacro
Normal file
@@ -0,0 +1,153 @@
|
||||
<?xml version='1.0' encoding='utf-8'?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="fer">
|
||||
|
||||
<xacro:include filename="$(find franka_description)/robots/common/utils.xacro" />
|
||||
|
||||
<!-- safety_distance: Minimum safety distance in [m] by which the collision volumes are expanded and which is enforced during robot motions -->
|
||||
<!-- arm_id: Namespace of the robot arm. Serves to differentiate between arms in case of multiple instances. -->
|
||||
<!-- joint_limits: description of the joint limits that comes from a YAML file. Example definition: ${xacro.load_yaml('$(find franka_description)/robots/fr3/joint_limits.yaml')} -->
|
||||
<!-- kinematics: description of the kinematics that comes from a YAML file. Example definition: ${xacro.load_yaml('$(find franka_description)/robots/fr3/kinematics.yaml')} -->
|
||||
<!-- inertials: description of the inertials that comes from a YAML file. Example definition: ${xacro.load_yaml('$(find franka_description)/robots/fr3/inertials.yaml')} -->
|
||||
<!-- dynamics: description of the dynamics that comes from a YAML file. Example definition: ${xacro.load_yaml('$(find franka_description)/robots/fr3/dynamics.yaml')} -->
|
||||
|
||||
<xacro:macro name="franka_arm" params="arm_id arm_prefix:='' no_prefix:=false description_pkg:='franka_description' connected_to:='base' xyz:='0 0 0' rpy:='0 0 0' gazebo:='false' safety_distance:=0 joint_limits inertials kinematics dynamics with_sc:=false" >
|
||||
|
||||
<!-- Define a property that defaults to 'arm_prefix_arm_id' concatenated with an underscore if 'no_prefix' is not set -->
|
||||
<xacro:property name="prefix" value="${'' if no_prefix else arm_prefix + arm_id + '_'}" />
|
||||
|
||||
<xacro:if value="${gazebo}">
|
||||
<xacro:property name="connected_to" value="" />
|
||||
</xacro:if>
|
||||
|
||||
<xacro:unless value="${not connected_to}">
|
||||
<link name="${connected_to}">
|
||||
</link>
|
||||
<joint name="${prefix}${connected_to}_joint" type="fixed">
|
||||
<parent link="${connected_to}"/>
|
||||
<child link="${prefix}link0"/>
|
||||
<origin rpy="${rpy}" xyz="${xyz}"/>
|
||||
</joint>
|
||||
</xacro:unless>
|
||||
|
||||
<xacro:link_with_sc no_prefix="${no_prefix}" name="link0" gazebo="${gazebo}" with_sc="${with_sc}">
|
||||
<self_collision_geometries>
|
||||
<xacro:collision_capsule xyz="-0.075 0 0.06" direction="x" radius="${0.06+safety_distance}" length="0.03" />
|
||||
</self_collision_geometries>
|
||||
</xacro:link_with_sc>
|
||||
|
||||
<xacro:link_with_sc no_prefix="${no_prefix}" name="link1" gazebo="${gazebo}" with_sc="${with_sc}">
|
||||
<self_collision_geometries>
|
||||
<xacro:collision_capsule xyz="0 0 -191.5e-3" radius="${0.06+safety_distance}" length="0.283" />
|
||||
</self_collision_geometries>
|
||||
</xacro:link_with_sc>
|
||||
|
||||
<joint name="${prefix}joint1" type="revolute">
|
||||
<xacro:franka-kinematics name="joint1" config="${kinematics}" />
|
||||
<parent link="${prefix}link0" />
|
||||
<child link="${prefix}link1" />
|
||||
<axis xyz="0 0 1" />
|
||||
<xacro:franka-limits name="joint1" config="${joint_limits}" />
|
||||
<xacro:franka-dynamics name="joint1" config="${dynamics}" />
|
||||
</joint>
|
||||
|
||||
<xacro:link_with_sc no_prefix="${no_prefix}" name="link2" gazebo="${gazebo}" with_sc="${with_sc}">
|
||||
<self_collision_geometries>
|
||||
<xacro:collision_capsule xyz="0 0 0" radius="${0.06+safety_distance}" length="0.12" />
|
||||
</self_collision_geometries>
|
||||
</xacro:link_with_sc>
|
||||
|
||||
<joint name="${prefix}joint2" type="revolute">
|
||||
<xacro:franka-kinematics name="joint2" config="${kinematics}" />
|
||||
<parent link="${prefix}link1" />
|
||||
<child link="${prefix}link2" />
|
||||
<axis xyz="0 0 1" />
|
||||
<xacro:franka-limits name="joint2" config="${joint_limits}" />
|
||||
<xacro:franka-dynamics name="joint2" config="${dynamics}" />
|
||||
</joint>
|
||||
|
||||
<xacro:link_with_sc no_prefix="${no_prefix}" name="link3" gazebo="${gazebo}" with_sc="${with_sc}">
|
||||
<self_collision_geometries>
|
||||
<xacro:collision_capsule xyz="0 0 -0.145" radius="${0.06+safety_distance}" length="0.15" />
|
||||
</self_collision_geometries>
|
||||
</xacro:link_with_sc>
|
||||
|
||||
<joint name="${prefix}joint3" type="revolute">
|
||||
<xacro:franka-kinematics name="joint3" config="${kinematics}" />
|
||||
<parent link="${prefix}link2" />
|
||||
<child link="${prefix}link3" />
|
||||
<axis xyz="0 0 1" />
|
||||
<xacro:franka-limits name="joint3" config="${joint_limits}" />
|
||||
<xacro:franka-dynamics name="joint3" config="${dynamics}" />
|
||||
</joint>
|
||||
|
||||
<xacro:link_with_sc no_prefix="${no_prefix}" name="link4" gazebo="${gazebo}" with_sc="${with_sc}">
|
||||
<self_collision_geometries>
|
||||
<xacro:collision_capsule xyz="0 0 0" radius="${0.06+safety_distance}" length="0.12" />
|
||||
</self_collision_geometries>
|
||||
</xacro:link_with_sc>
|
||||
|
||||
<joint name="${prefix}joint4" type="revolute">
|
||||
<xacro:franka-kinematics name="joint4" config="${kinematics}" />
|
||||
<parent link="${prefix}link3" />
|
||||
<child link="${prefix}link4" />
|
||||
<axis xyz="0 0 1" />
|
||||
<xacro:franka-limits name="joint4" config="${joint_limits}" />
|
||||
<xacro:franka-dynamics name="joint4" config="${dynamics}" />
|
||||
</joint>
|
||||
|
||||
<xacro:link_with_sc no_prefix="${no_prefix}" name="link5" gazebo="${gazebo}" with_sc="${with_sc}">
|
||||
<self_collision_geometries>
|
||||
<xacro:collision_capsule xyz="0 0 -0.26" radius="${0.060+safety_distance}" length="0.10" />
|
||||
<xacro:collision_capsule xyz="0 0.08 -0.13" radius="${0.025+safety_distance}" length="0.14" />
|
||||
</self_collision_geometries>
|
||||
</xacro:link_with_sc>
|
||||
|
||||
<joint name="${prefix}joint5" type="revolute">
|
||||
<xacro:franka-kinematics name="joint5" config="${kinematics}" />
|
||||
<parent link="${prefix}link4" />
|
||||
<child link="${prefix}link5" />
|
||||
<axis xyz="0 0 1" />
|
||||
<xacro:franka-limits name="joint5" config="${joint_limits}" />
|
||||
<xacro:franka-dynamics name="joint5" config="${dynamics}" />
|
||||
</joint>
|
||||
|
||||
<xacro:link_with_sc no_prefix="${no_prefix}" name="link6" gazebo="${gazebo}" with_sc="${with_sc}">
|
||||
<self_collision_geometries>
|
||||
<xacro:collision_capsule xyz="0 0 -0.03" radius="${0.05+safety_distance}" length="0.08" />
|
||||
</self_collision_geometries>
|
||||
</xacro:link_with_sc>
|
||||
|
||||
<joint name="${prefix}joint6" type="revolute">
|
||||
<xacro:franka-kinematics name="joint6" config="${kinematics}" />
|
||||
<parent link="${prefix}link5" />
|
||||
<child link="${prefix}link6" />
|
||||
<axis xyz="0 0 1" />
|
||||
<xacro:franka-limits name="joint6" config="${joint_limits}" />
|
||||
<xacro:franka-dynamics name="joint6" config="${dynamics}" />
|
||||
</joint>
|
||||
|
||||
<xacro:link_with_sc no_prefix="${no_prefix}" name="link7" gazebo="${gazebo}" rpy="0 0 ${pi/4}" with_sc="${with_sc}">
|
||||
<self_collision_geometries>
|
||||
<xacro:collision_capsule xyz="0 0 0.01" direction="z" radius="${0.04+safety_distance}" length="0.14" />
|
||||
<xacro:collision_capsule xyz="0.06 0 0.082" direction="x" radius="${0.03+safety_distance}" length="0.01" />
|
||||
</self_collision_geometries>
|
||||
</xacro:link_with_sc>
|
||||
|
||||
<joint name="${prefix}joint7" type="revolute">
|
||||
<xacro:franka-kinematics name="joint7" config="${kinematics}" />
|
||||
<parent link="${prefix}link6"/>
|
||||
<child link="${prefix}link7"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<xacro:franka-limits name="joint7" config="${joint_limits}" />
|
||||
<xacro:franka-dynamics name="joint7" config="${dynamics}" />
|
||||
</joint>
|
||||
|
||||
<link name="${prefix}link8"/>
|
||||
|
||||
<joint name="${prefix}joint8" type="fixed">
|
||||
<xacro:franka-kinematics name="joint8" config="${kinematics}" />
|
||||
<parent link="${prefix}link7" />
|
||||
<child link="${prefix}link8" />
|
||||
</joint>
|
||||
</xacro:macro>
|
||||
</robot>
|
||||
167
robots/common/franka_robot.xacro
Normal file
167
robots/common/franka_robot.xacro
Normal file
@@ -0,0 +1,167 @@
|
||||
<?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 franka_description)/robots/common/utils.xacro" />
|
||||
|
||||
<xacro:include filename="$(find 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 franka_description)/end_effectors/common/franka_hand.xacro"/>
|
||||
<xacro:include filename="$(find franka_description)/end_effectors/common/utils.xacro" />
|
||||
<xacro:include filename="$(find 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 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 franka_description)/end_effectors/common/ee_with_one_link.xacro"/>
|
||||
<xacro:include filename="$(find franka_description)/end_effectors/common/utils.xacro" />
|
||||
<xacro:include filename="$(find 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 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 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>
|
||||
269
robots/common/utils.xacro
Normal file
269
robots/common/utils.xacro
Normal file
@@ -0,0 +1,269 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
|
||||
|
||||
<!-- ============================================================== -->
|
||||
<!-- Macro to add an <inertial> tag based on yaml-load properties -->
|
||||
<!-- -->
|
||||
<!-- name: Name of the robot link (without prefix) -->
|
||||
<!-- inertial: Dictionary of inertia properties (see inertial.yaml) -->
|
||||
<!-- ============================================================== -->
|
||||
<xacro:macro name="franka-inertials" params="name inertials:=^">
|
||||
<xacro:unless value="${name in inertials}">${xacro.warning('No inertia properties defined for: ' + name)}</xacro:unless>
|
||||
<xacro:if value="${name in inertials}">
|
||||
<!-- Access inertia properties of link 'name' -->
|
||||
<xacro:property name="link_inertials" value="${inertials[name]}" lazy_eval="false" />
|
||||
<inertial>
|
||||
<origin rpy="${link_inertials.origin.rpy}" xyz="${link_inertials.origin.xyz}" />
|
||||
<mass value="${link_inertials.mass}" />
|
||||
<xacro:if value="${'inertia' in link_inertials}">
|
||||
<xacro:property name="I" value="${link_inertials.inertia}" />
|
||||
<inertia ixx="${I.xx}" ixy="${I.xy}" ixz="${I.xz}" iyy="${I.yy}" iyz="${I.yz}" izz="${I.zz}" />
|
||||
</xacro:if>
|
||||
</inertial>
|
||||
</xacro:if>
|
||||
</xacro:macro>
|
||||
|
||||
<!-- ========================================================== -->
|
||||
<!-- Macro to add a single link, both with -->
|
||||
<!-- * detailed meshes for environmental collision checking -->
|
||||
<!-- * and coarse geometry models for self-collision checking -->
|
||||
<!-- (only if 'gazebo' param is set) -->
|
||||
<!-- -->
|
||||
<!-- name: Name of the robot link (without prefix) -->
|
||||
<!-- rpy: Rotation of the *_sc link relative to parent [rad] -->
|
||||
<!-- self_collision_geometries: self <collision> models -->
|
||||
<!-- ========================================================== -->
|
||||
<xacro:macro name="link_with_sc" params="name no_prefix:='false' rpy:='0 0 0' **self_collision_geometries gazebo:=false with_sc:=false">
|
||||
<xacro:property name="prefix" value="${'' if no_prefix else arm_prefix + arm_id + '_'}" />
|
||||
<!-- sub-link defining detailed meshes for collision with environment -->
|
||||
<link name="${prefix}${name}">
|
||||
<visual name="${prefix}${name}_visual">
|
||||
<geometry>
|
||||
<mesh filename="package://${description_pkg}/meshes/robot_arms/${arm_id}/visual/${name}.dae" />
|
||||
</geometry>
|
||||
</visual>
|
||||
<collision name="${prefix}${name}_collision">
|
||||
<geometry>
|
||||
<mesh filename="package://franka_description/meshes/robot_arms/${arm_id}/collision/${name}.stl" />
|
||||
</geometry>
|
||||
</collision>
|
||||
<xacro:franka-inertials name="${name}" />
|
||||
</link>
|
||||
|
||||
<!-- sub-link defining coarse geometries of real robot's internal self-collision -->
|
||||
<xacro:if value="${with_sc}">
|
||||
<link name="${prefix}${name}_sc">
|
||||
<xacro:unless value="${gazebo}">
|
||||
<xacro:insert_block name="self_collision_geometries" />
|
||||
</xacro:unless>
|
||||
</link>
|
||||
<!-- fixed joint between both sub-links -->
|
||||
<joint name="${prefix}${name}_sc_joint" type="fixed">
|
||||
<origin rpy="${rpy}" />
|
||||
<parent link="${prefix}${name}" />
|
||||
<child link="${prefix}${name}_sc" />
|
||||
</joint>
|
||||
</xacro:if>
|
||||
</xacro:macro>
|
||||
|
||||
<!-- =========================================================== -->
|
||||
<!-- Add a <collision> tag with a capsule, made from a cylinder -->
|
||||
<!-- with two spheres at its caps. The capsule will always be -->
|
||||
<!-- aligned with the axis of 'direction' you pass along. -->
|
||||
<!-- -->
|
||||
<!-- radius: Radii of both the cylinder and both spheres [m] -->
|
||||
<!-- length: Length of the cylinder/distance between the centers -->
|
||||
<!-- of the spheres. NOT overall length of capsule! -->
|
||||
<!-- xyz: Position of the center of the capsule/cylinder -->
|
||||
<!-- direction: One of { x, y, z, -x, -y, -z } -->
|
||||
<!-- =========================================================== -->
|
||||
<xacro:macro name="collision_capsule" params="radius length xyz:='0 0 0' direction:='z'">
|
||||
<xacro:property name="r" value="${pi/2.0 if 'y' in direction else 0}" />
|
||||
<xacro:property name="p" value="${pi/2.0 if 'x' in direction else 0}" />
|
||||
<xacro:property name="y" value="0" />
|
||||
<xacro:property name="x" value="${xyz.split(' ')[0]}" />
|
||||
<xacro:property name="y" value="${xyz.split(' ')[1]}" />
|
||||
<xacro:property name="z" value="${xyz.split(' ')[2]}" />
|
||||
<!-- Sphere center offsets from center of cylinder -->
|
||||
<xacro:property name="sx" value="${length / 2.0 if 'x' in direction else 0}" />
|
||||
<xacro:property name="sy" value="${length / 2.0 if 'y' in direction else 0}" />
|
||||
<xacro:property name="sz" value="${length / 2.0 if 'z' in direction else 0}" />
|
||||
|
||||
<collision>
|
||||
<origin xyz="${x} ${y} ${z}" rpy="${r} ${p} ${y}"/>
|
||||
<geometry>
|
||||
<cylinder radius="${radius}" length="${length}" />
|
||||
</geometry>
|
||||
</collision>
|
||||
<collision>
|
||||
<origin xyz="${x+sx} ${y+sy} ${z+sz}" />
|
||||
<geometry>
|
||||
<sphere radius="${radius}" />
|
||||
</geometry>
|
||||
</collision>
|
||||
<collision>
|
||||
<origin xyz="${x-sx} ${y-sy} ${z-sz}" />
|
||||
<geometry>
|
||||
<sphere radius="${radius}" />
|
||||
</geometry>
|
||||
</collision>
|
||||
</xacro:macro>
|
||||
|
||||
<!-- ========================================================== -->
|
||||
<!-- Adds the required tags to simulate one joint in gazebo -->
|
||||
<!-- -->
|
||||
<!-- joint - Name of the fer joint to simulate -->
|
||||
<!-- transmission - type of the transmission of the joint -->
|
||||
<!-- ========================================================== -->
|
||||
<xacro:macro name="gazebo-joint" params="joint transmission:=hardware_interface/EffortJointInterface">
|
||||
<gazebo reference="${joint}">
|
||||
<!-- Needed for ODE to output external wrenches on joints -->
|
||||
<provideFeedback>true</provideFeedback>
|
||||
</gazebo>
|
||||
|
||||
<transmission name="${joint}_transmission">
|
||||
<type>transmission_interface/SimpleTransmission</type>
|
||||
<joint name="${joint}">
|
||||
<hardwareInterface>${transmission}</hardwareInterface>
|
||||
</joint>
|
||||
<actuator name="${joint}_motor">
|
||||
<hardwareInterface>${transmission}</hardwareInterface>
|
||||
</actuator>
|
||||
</transmission>
|
||||
</xacro:macro>
|
||||
|
||||
<xacro:macro name="gazebo-friction" params="link mu">
|
||||
<gazebo reference="${link}">
|
||||
<collision>
|
||||
<max_contacts>10</max_contacts>
|
||||
<surface>
|
||||
<contact>
|
||||
<ode>
|
||||
<!-- These two parameter need application specific tuning. -->
|
||||
<!-- Usually you want no "snap out" velocity and a generous -->
|
||||
<!-- penetration depth to keep the grasping stable -->
|
||||
<max_vel>0</max_vel>
|
||||
<min_depth>0.001</min_depth>
|
||||
</ode>
|
||||
</contact>
|
||||
<friction>
|
||||
<ode>
|
||||
<mu>${mu}</mu>
|
||||
<mu2>${mu}</mu2>
|
||||
</ode>
|
||||
</friction>
|
||||
<bounce/>
|
||||
</surface>
|
||||
</collision>
|
||||
</gazebo>
|
||||
</xacro:macro>
|
||||
|
||||
<!-- ========================================================== -->
|
||||
<!-- Adds the required tags to provide a `FrankaStateInterface` -->
|
||||
<!-- when simulating with franka_hw_sim plugin -->
|
||||
<!-- -->
|
||||
<!-- arm_id - Arm ID of the fer to simulate (default 'fer') -->
|
||||
<!-- ========================================================== -->
|
||||
<xacro:macro name="transmission-franka-state" params="arm_id:=fer">
|
||||
<transmission name="${arm_prefix}${arm_id}_franka_state">
|
||||
<type>franka_hw/FrankaStateInterface</type>
|
||||
<joint name="${arm_prefix}${arm_id}_joint1"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></joint>
|
||||
<joint name="${arm_prefix}${arm_id}_joint2"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></joint>
|
||||
<joint name="${arm_prefix}${arm_id}_joint3"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></joint>
|
||||
<joint name="${arm_prefix}${arm_id}_joint4"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></joint>
|
||||
<joint name="${arm_prefix}${arm_id}_joint5"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></joint>
|
||||
<joint name="${arm_prefix}${arm_id}_joint6"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></joint>
|
||||
<joint name="${arm_prefix}${arm_id}_joint7"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></joint>
|
||||
|
||||
<actuator name="${arm_prefix}${arm_id}_motor1"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></actuator>
|
||||
<actuator name="${arm_prefix}${arm_id}_motor2"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></actuator>
|
||||
<actuator name="${arm_prefix}${arm_id}_motor3"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></actuator>
|
||||
<actuator name="${arm_prefix}${arm_id}_motor4"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></actuator>
|
||||
<actuator name="${arm_prefix}${arm_id}_motor5"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></actuator>
|
||||
<actuator name="${arm_prefix}${arm_id}_motor6"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></actuator>
|
||||
<actuator name="${arm_prefix}${arm_id}_motor7"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></actuator>
|
||||
</transmission>
|
||||
</xacro:macro>
|
||||
|
||||
<!-- ============================================================ -->
|
||||
<!-- Adds the required tags to provide a `FrankaModelInterface` -->
|
||||
<!-- when simulating with franka_hw_sim plugin -->
|
||||
<!-- -->
|
||||
<!-- arm_id - Arm ID of the fer to simulate (default 'fer') -->
|
||||
<!-- root - Joint name of the base of the robot -->
|
||||
<!-- tip - Joint name of the tip of the robot (flange or hand) -->
|
||||
<!-- ============================================================ -->
|
||||
<xacro:macro name="transmission-franka-model" params="arm_id:=fer root:=fer_joint1 tip:=fer_joint7">
|
||||
<transmission name="${arm_prefix}${arm_id}_franka_model">
|
||||
<type>franka_hw/FrankaModelInterface</type>
|
||||
<joint name="${root}">
|
||||
<role>root</role>
|
||||
<hardwareInterface>franka_hw/FrankaModelInterface</hardwareInterface>
|
||||
</joint>
|
||||
<joint name="${tip}">
|
||||
<role>tip</role>
|
||||
<hardwareInterface>franka_hw/FrankaModelInterface</hardwareInterface>
|
||||
</joint>
|
||||
|
||||
<actuator name="${root}_motor_root"><hardwareInterface>franka_hw/FrankaModelInterface</hardwareInterface></actuator>
|
||||
<actuator name="${tip}_motor_tip" ><hardwareInterface>franka_hw/FrankaModelInterface</hardwareInterface></actuator>
|
||||
</transmission>
|
||||
</xacro:macro>
|
||||
|
||||
<xacro:macro name="inertia-cylinder" params="mass radius h">
|
||||
<inertial>
|
||||
<mass value="${mass}" />
|
||||
<inertia ixx="${1./12 * mass * (3 * radius**2 + h**2)}" ixy = "0" ixz = "0"
|
||||
iyy="${1./12 * mass * (3 * radius**2 + h**2)}" iyz = "0"
|
||||
izz="${1./2 * mass * radius**2}" />
|
||||
</inertial>
|
||||
</xacro:macro>
|
||||
|
||||
<!-- ========================================================================= -->
|
||||
<!-- Adds the <limit ... /> & <safety_controller/> tags given a config file -->
|
||||
<!-- of joint limits -->
|
||||
<!-- -->
|
||||
<!-- config - YAML struct defining joint limits (e.g. fer/joint_limits.yaml) -->
|
||||
<!-- name - Name of the joint for which to add limits to -->
|
||||
<!-- ========================================================================= -->
|
||||
<xacro:macro name="franka-limits" params="config name">
|
||||
<xacro:property name="limits" value="${config[name]['limit']}" lazy_eval="false" />
|
||||
<limit lower="${limits.lower}"
|
||||
upper="${limits.upper}"
|
||||
effort="${limits.effort}"
|
||||
velocity="${limits.velocity}" />
|
||||
<safety_controller k_position="100.0"
|
||||
k_velocity="40.0"
|
||||
soft_lower_limit="${limits.lower}"
|
||||
soft_upper_limit="${limits.upper}" />
|
||||
</xacro:macro>
|
||||
|
||||
<!-- ========================================================================= -->
|
||||
<!-- Adds the <dynamics ... /> tag given a config file of joint dynamics -->
|
||||
<!-- -->
|
||||
<!-- config - YAML struct defining joint dynamics (e.g. fer/dynamics.yaml) -->
|
||||
<!-- name - Name of the joint for which to add dynamics to -->
|
||||
<!-- ========================================================================= -->
|
||||
<xacro:macro name="franka-dynamics" params="config name">
|
||||
<xacro:property name="dynamics" value="${config[name]['dynamic']}" lazy_eval="false" />
|
||||
<dynamics D="${dynamics.D}"
|
||||
K="${dynamics.K}"
|
||||
damping="${dynamics.damping}"
|
||||
friction="${dynamics.friction}"
|
||||
mu_coulomb="${dynamics.mu_coulomb}"
|
||||
mu_viscous="${dynamics.mu_viscous}" />
|
||||
</xacro:macro>
|
||||
|
||||
<!-- ========================================================================= -->
|
||||
<!-- Adds the <kinematics ... /> tag given a config file of joint kinematics -->
|
||||
<!-- -->
|
||||
<!-- config - YAML struct defining joint kinematics (e.g. fer/kinematics.yaml) -->
|
||||
<!-- name - Name of the joint for which to add kinematics to -->
|
||||
<!-- ========================================================================= -->
|
||||
<xacro:macro name="franka-kinematics" params="config name">
|
||||
<xacro:property name="kinematics" value="${config[name]['kinematic']}" lazy_eval="false" />
|
||||
<origin rpy="${kinematics.roll} ${kinematics.pitch} ${kinematics.yaw}"
|
||||
xyz="${kinematics.x} ${kinematics.y} ${kinematics.z}" />
|
||||
</xacro:macro>
|
||||
|
||||
</robot>
|
||||
Reference in New Issue
Block a user