Skip to content
Snippets Groups Projects

Compare revisions

Changes are shown as if the source revision was being merged into the target revision. Learn more about comparing revisions.

Source

Select target project
No results found
Select Git revision
  • ccnc-demo
  • feature/detailed-hardware-collision
  • feature/unified_franka_desriptions
  • master
  • noetic/main
  • v0.0
  • v1.0
7 results

Target

Select target project
  • ceti/ros/packages/forked-franka-packages/franka_description
1 result
Select Git revision
  • ccnc-demo
  • feature/detailed-hardware-collision
  • feature/unified_franka_desriptions
  • master
  • noetic/main
  • v0.0
  • v1.0
7 results
Show changes
Commits on Source (2)
Show whitespace changes
Inline Side-by-side
Showing
with 1304 additions and 1 deletion
CMakeLists.txt
View file @ 658a336d
......@@ -10,3 +10,7 @@ install(DIRECTORY meshes
install(DIRECTORY robots
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)
catkin_add_nosetests(test/franka_robot_urdf.py)
catkin_add_nosetests(test/hand_urdf.py)
catkin_add_nosetests(test/dual_panda_example_urdf.py)
package.xml
View file @ 658a336d
<?xml version="1.0"?>
<package format="2">
<name>franka_description</name>
<version>0.8.2</version>
<version>0.10.0</version>
<description>franka_description contains URDF files and meshes of Franka Emika robots</description>
<maintainer email="support@franka.de">Franka Emika GmbH</maintainer>
<license>Apache 2.0</license>
......@@ -14,4 +14,5 @@
<buildtool_depend>catkin</buildtool_depend>
<exec_depend>xacro</exec_depend>
<test_depend>rosunit</test_depend>
</package>
robots/common/franka_arm.xacro 0 → 100644
View file @ 658a336d
<?xml version='1.0' encoding='utf-8'?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="panda">
<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')} -->
<xacro:macro name="franka_arm" params="arm_id:='panda' description_pkg:='franka_description' connected_to:='' xyz:='0 0 0' rpy:='0 0 0' gazebo:=false safety_distance:=0 joint_limits" >
<xacro:unless value="${not connected_to}">
<joint name="${arm_id}_joint_${connected_to}" type="fixed">
<parent link="${connected_to}"/>
<child link="${arm_id}_link0"/>
<origin rpy="${rpy}" xyz="${xyz}"/>
</joint>
</xacro:unless>
<xacro:link_with_sc name="link0" gazebo="${gazebo}">
<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 name="link1" gazebo="${gazebo}">
<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="${arm_id}_joint1" type="revolute">
<origin rpy="0 0 0" xyz="0 0 0.333" />
<parent link="${arm_id}_link0" />
<child link="${arm_id}_link1" />
<axis xyz="0 0 1" />
<xacro:franka-limits name="joint1" config="${joint_limits}" />
<dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16" />
</joint>
<xacro:link_with_sc name="link2" gazebo="${gazebo}">
<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="${arm_id}_joint2" type="revolute">
<origin rpy="${-pi/2} 0 0" xyz="0 0 0" />
<parent link="${arm_id}_link1" />
<child link="${arm_id}_link2" />
<axis xyz="0 0 1" />
<xacro:franka-limits name="joint2" config="${joint_limits}" />
<dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16" />
</joint>
<xacro:link_with_sc name="link3" gazebo="${gazebo}">
<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="${arm_id}_joint3" type="revolute">
<origin rpy="${pi/2} 0 0" xyz="0 -0.316 0" />
<parent link="${arm_id}_link2" />
<child link="${arm_id}_link3" />
<axis xyz="0 0 1" />
<xacro:franka-limits name="joint3" config="${joint_limits}" />
<dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16" />
</joint>
<xacro:link_with_sc name="link4" gazebo="${gazebo}">
<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="${arm_id}_joint4" type="revolute">
<origin rpy="${pi/2} 0 0" xyz="0.0825 0 0" />
<parent link="${arm_id}_link3" />
<child link="${arm_id}_link4" />
<axis xyz="0 0 1" />
<xacro:franka-limits name="joint4" config="${joint_limits}" />
<dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16" />
</joint>
<xacro:link_with_sc name="link5" gazebo="${gazebo}">
<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="${arm_id}_joint5" type="revolute">
<origin rpy="${-pi/2} 0 0" xyz="-0.0825 0.384 0" />
<parent link="${arm_id}_link4" />
<child link="${arm_id}_link5" />
<axis xyz="0 0 1" />
<xacro:franka-limits name="joint5" config="${joint_limits}" />
<dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16" />
</joint>
<xacro:link_with_sc name="link6" gazebo="${gazebo}">
<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="${arm_id}_joint6" type="revolute">
<origin rpy="${pi/2} 0 0" xyz="0 0 0" />
<parent link="${arm_id}_link5" />
<child link="${arm_id}_link6" />
<axis xyz="0 0 1" />
<xacro:franka-limits name="joint6" config="${joint_limits}" />
<dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16" />
</joint>
<xacro:link_with_sc name="link7" gazebo="${gazebo}" rpy="0 0 ${pi/4}">
<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="${arm_id}_joint7" type="revolute">
<origin rpy="${pi/2} 0 0" xyz="0.088 0 0"/>
<parent link="${arm_id}_link6"/>
<child link="${arm_id}_link7"/>
<axis xyz="0 0 1"/>
<xacro:franka-limits name="joint7" config="${joint_limits}" />
<dynamics D="1" K="7000" damping="0.003" friction="0.0" mu_coulomb="0" mu_viscous="16"/>
</joint>
<link name="${arm_id}_link8"/>
<joint name="${arm_id}_joint8" type="fixed">
<origin rpy="0 0 0" xyz="0 0 0.107"/>
<parent link="${arm_id}_link7"/>
<child link="${arm_id}_link8"/>
</joint>
</xacro:macro>
</robot>
robots/common/franka_hand.xacro 0 → 100644
View file @ 658a336d
<?xml version="1.0" encoding="utf-8"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="hand">
<!-- safety_distance: Minimum safety distance in [m] by which the collision volumes are expanded and which is enforced during robot motions -->
<xacro:macro name="franka_hand" params="connected_to:='' arm_id:='panda' rpy:='0 0 0' xyz:='0 0 0' tcp_xyz:='0 0 0.1034' tcp_rpy:='0 0 0' safety_distance:=0 gazebo:=false description_pkg:=franka_description">
<xacro:unless value="${connected_to == ''}">
<joint name="${arm_id}_hand_joint" type="fixed">
<parent link="${connected_to}" />
<child link="${arm_id}_hand" />
<origin xyz="${xyz}" rpy="${rpy}" />
</joint>
</xacro:unless>
<xacro:link_with_sc name="hand" gazebo="${gazebo}">
<self_collision_geometries>
<xacro:collision_capsule xyz="0 0 0.04" direction="y" radius="${0.04+safety_distance}" length="0.1" />
<xacro:collision_capsule xyz="0 0 0.10" direction="y" radius="${0.02+safety_distance}" length="0.1" />
</self_collision_geometries>
</xacro:link_with_sc>
<!-- Define the hand_tcp frame -->
<link name="${arm_id}_hand_tcp" />
<joint name="${arm_id}_hand_tcp_joint" type="fixed">
<origin xyz="${tcp_xyz}" rpy="${tcp_rpy}" />
<parent link="${arm_id}_hand" />
<child link="${arm_id}_hand_tcp" />
</joint>
<link name="${arm_id}_leftfinger">
<visual>
<geometry>
<mesh filename="package://${description_pkg}/meshes/visual/finger.dae" />
</geometry>
</visual>
<!-- screw mount -->
<collision>
<origin xyz="0 18.5e-3 11e-3" rpy="0 0 0" />
<geometry>
<box size="22e-3 15e-3 20e-3" />
</geometry>
</collision>
<!-- cartriage sledge -->
<collision>
<origin xyz="0 6.8e-3 2.2e-3" rpy="0 0 0" />
<geometry>
<box size="22e-3 8.8e-3 3.8e-3" />
</geometry>
</collision>
<!-- diagonal finger -->
<collision>
<origin xyz="0 15.9e-3 28.35e-3" rpy="${pi/6} 0 0" />
<geometry>
<box size="17.5e-3 7e-3 23.5e-3" />
</geometry>
</collision>
<!-- rubber tip with which to grasp -->
<collision>
<origin xyz="0 7.58e-3 45.25e-3" rpy="0 0 0" />
<geometry>
<box size="17.5e-3 15.2e-3 18.5e-3" />
</geometry>
</collision>
<xacro:if value="${gazebo}">
<xacro:inertial_props name="leftfinger" />
</xacro:if>
</link>
<link name="${arm_id}_rightfinger">
<visual>
<origin xyz="0 0 0" rpy="0 0 ${pi}" />
<geometry>
<mesh filename="package://${description_pkg}/meshes/visual/finger.dae" />
</geometry>
</visual>
<!-- screw mount -->
<collision>
<origin xyz="0 -18.5e-3 11e-3" rpy="0 0 0" />
<geometry>
<box size="22e-3 15e-3 20e-3" />
</geometry>
</collision>
<!-- cartriage sledge -->
<collision>
<origin xyz="0 -6.8e-3 2.2e-3" rpy="0 0 0" />
<geometry>
<box size="22e-3 8.8e-3 3.8e-3" />
</geometry>
</collision>
<!-- diagonal finger -->
<collision>
<origin xyz="0 -15.9e-3 28.35e-3" rpy="${-pi/6} 0 0" />
<geometry>
<box size="17.5e-3 7e-3 23.5e-3" />
</geometry>
</collision>
<!-- rubber tip with which to grasp -->
<collision>
<origin xyz="0 -7.58e-3 45.25e-3" rpy="0 0 0" />
<geometry>
<box size="17.5e-3 15.2e-3 18.5e-3" />
</geometry>
</collision>
<xacro:if value="${gazebo}">
<xacro:inertial_props name="rightfinger" />
</xacro:if>
</link>
<joint name="${arm_id}_finger_joint1" type="prismatic">
<parent link="${arm_id}_hand" />
<child link="${arm_id}_leftfinger" />
<origin xyz="0 0 0.0584" rpy="0 0 0" />
<axis xyz="0 1 0" />
<limit effort="100" lower="0.0" upper="0.04" velocity="0.2" />
<dynamics damping="0.3" />
</joint>
<joint name="${arm_id}_finger_joint2" type="prismatic">
<parent link="${arm_id}_hand" />
<child link="${arm_id}_rightfinger" />
<origin xyz="0 0 0.0584" rpy="0 0 0" />
<axis xyz="0 -1 0" />
<limit effort="100" lower="0.0" upper="0.04" velocity="0.2" />
<mimic joint="${arm_id}_finger_joint1" />
<dynamics damping="0.3" />
</joint>
</xacro:macro>
</robot>
robots/common/franka_robot.xacro 0 → 100644
View file @ 658a336d
<?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">
<!-- Name of this panda -->
<!-- Should a franka_gripper be mounted at the flange?" -->
<xacro:arg name="hand" default="false" />
<!-- Positional offset between $(arm_id)_hand -> $(arm_id)_hand_tcp [m]. Only used when hand:=true -->
<xacro:arg name="tcp_xyz" default="0 0 0.1034" />
<!-- Rotational offset between $(arm_id)_hand -> $(arm_id)_hand_tcp [rad]. Only used when hand:=true -->
<xacro:arg name="tcp_rpy" default="0 0 0" />
<!-- Is the robot being simulated in gazebo?" -->
<xacro:arg name="gazebo" default="false" />
<xacro:include filename="$(find franka_description)/robots/common/utils.xacro" />
<xacro:include filename="$(find franka_description)/robots/common/franka_arm.xacro" />
<xacro:franka_arm arm_id="${arm_id}" safety_distance="0.03" gazebo="$(arg gazebo)" joint_limits="${joint_limits}"/>
<xacro:if value="$(arg hand)">
<xacro:include filename="$(find franka_description)/robots/common/franka_hand.xacro"/>
<xacro:franka_hand
arm_id="$(arg arm_id)"
rpy="0 0 ${-pi/4}"
tcp_xyz="$(arg tcp_xyz)"
tcp_rpy="$(arg tcp_rpy)"
connected_to="$(arg arm_id)_link8"
safety_distance="0.03"
gazebo="$(arg gazebo)"
/>
</xacro:if>
<!-- Define additional Gazebo tags -->
<xacro:if value="$(arg gazebo)">
<xacro:arg name="xyz" default="0 0 0" />
<xacro:arg name="rpy" default="0 0 0" />
<!-- Gazebo requires a joint to a link called "world" for statically mounted robots -->
<link name="world" />
<joint name="world_joint" type="fixed">
<origin xyz="$(arg xyz)" rpy="$(arg rpy)" />
<parent link="world" />
<child link="$(arg arm_id)_link0" />
</joint>
<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="$(arg 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>
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<controlPeriod>0.001</controlPeriod>
<robotSimType>franka_gazebo/FrankaHWSim</robotSimType>
</plugin>
<self_collide>true</self_collide>
</gazebo>
</xacro:if>
</xacro:macro>
</robot>
robots/common/hand.urdf.xacro 0 → 100644
View file @ 658a336d
<?xml version="1.0" encoding="utf-8"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="hand">
<xacro:include filename="$(find franka_description)/robots/common/utils.xacro" />
<xacro:include filename="franka_hand.xacro"/>
<xacro:franka_hand arm_id="panda" safety_distance="0.03"/>
</robot>
robots/common/inertial.yaml 0 → 100644
View file @ 658a336d
# This file does not contain official inertial properties
# of panda robot. The values are from the identification
# results published in: Identification of the Franka Emika
# PandaRobot With Retrieval of Feasible Parameters Using
# Penalty-Based Optimization
# by: Claudio Gaz, Marco Cognetti, Alexander Oliva,
# Paolo Robuffo Giordano, Alessandro de Luca
link0:
origin:
xyz: -0.041018 -0.00014 0.049974
rpy: 0 0 0
mass: 0.629769
inertia:
xx: 0.00315
yy: 0.00388
zz: 0.004285
xy: 8.2904E-07
xz: 0.00015
yz: 8.2299E-06
link1:
origin:
xyz: 0.003875 0.002081 -0.04762
rpy: 0 0 0
mass: 4.970684
inertia:
xx: 0.70337
yy: 0.70661
zz: 0.0091170
xy: -0.00013900
xz: 0.0067720
yz: 0.019169
link2:
origin:
xyz: -0.003141 -0.02872 0.003495
rpy: 0 0 0
mass: 0.646926
inertia:
xx: 0.0079620
yy: 2.8110e-02
zz: 2.5995e-02
xy: -3.9250e-3
xz: 1.0254e-02
yz: 7.0400e-04
link3:
origin:
xyz: 2.7518e-02 3.9252e-02 -6.6502e-02
rpy: 0 0 0
mass: 3.228604
inertia:
xx: 3.7242e-02
yy: 3.6155e-02
zz: 1.0830e-02
xy: -4.7610e-03
xz: -1.1396e-02
yz: -1.2805e-02
link4:
origin:
xyz: -5.317e-02 1.04419e-01 2.7454e-02
rpy: 0 0 0
mass: 3.587895
inertia:
xx: 2.5853e-02
yy: 1.9552e-02
zz: 2.8323e-02
xy: 7.7960e-03
xz: -1.3320e-03
yz: 8.6410e-03
link5:
origin:
xyz: -1.1953e-02 4.1065e-02 -3.8437e-02
rpy: 0 0 0
mass: 1.225946
inertia:
xx: 3.5549e-02
yy: 2.9474e-02
zz: 8.6270e-03
xy: -2.1170e-03
xz: -4.0370e-03
yz: 2.2900e-04
link6:
origin:
xyz: 6.0149e-02 -1.4117e-02 -1.0517e-02
rpy: 0 0 0
mass: 1.666555
inertia:
xx: 1.9640e-03
yy: 4.3540e-03
zz: 5.4330e-03
xy: 1.0900e-04
xz: -1.1580e-03
yz: 3.4100e-04
link7:
origin:
xyz: 1.0517e-02 -4.252e-03 6.1597e-02
rpy: 0 0 0
mass: 7.35522e-01
inertia:
xx: 1.2516e-02
yy: 1.0027e-02
zz: 4.8150e-03
xy: -4.2800e-04
xz: -1.1960e-03
yz: -7.4100e-04
hand:
origin:
xyz: -0.01 0 0.03
rpy: 0 0 0
mass: 0.73
inertia:
xx: 0.001
yy: 0.0025
zz: 0.0017
xy: 0
xz: 0
yz: 0
leftfinger: &finger
origin:
xyz: 0 0 0
rpy: 0 0 0
mass: 0.015
inertia:
xx: 2.3749999999999997e-06
yy: 2.3749999999999997e-06
zz: 7.5e-07
xy: 0
xz: 0
yz: 0
rightfinger: *finger
robots/common/utils.xacro 0 → 100644
View file @ 658a336d
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- Load inertial properties. Property is implicitly passed to macros. -->
<xacro:property name="inertial_config" value="$(find franka_description)/robots/common/inertial.yaml"/>
<xacro:property name="inertial" value="${xacro.load_yaml(inertial_config)}"/>
<!-- ============================================================== -->
<!-- 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="inertial_props" params="name inertial:=^">
<xacro:unless value="${name in inertial}">${xacro.warning('No inertia properties defined for: ' + name)}</xacro:unless>
<xacro:if value="${name in inertial}">
<!-- Access inertia properties of link 'name' -->
<xacro:property name="inertial" value="${inertial[name]}" lazy_eval="false" />
<inertial>
<origin rpy="${inertial.origin.rpy}" xyz="${inertial.origin.xyz}" />
<mass value="${inertial.mass}" />
<xacro:property name="I" value="${inertial.inertia}" />
<inertia ixx="${I.xx}" ixy="${I.xy}" ixz="${I.xz}" iyy="${I.yy}" iyz="${I.yz}" izz="${I.zz}" />
</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 prefix=${arm_id}_ rpy:='0 0 0' **self_collision_geometries gazebo:=false">
<!-- sub-link defining detailed meshes for collision with environment -->
<link name="${prefix}${name}">
<visual>
<geometry>
<mesh filename="package://${description_pkg}/meshes/visual/${name}.dae" />
</geometry>
</visual>
<collision>
<geometry>
<mesh filename="package://franka_description/meshes/collision/${name}.stl" />
</geometry>
</collision>
<xacro:if value="${gazebo}">
<xacro:inertial_props name="${name}" />
</xacro:if>
</link>
<!-- sub-link defining coarse geometries of real robot's internal self-collision -->
<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: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 panda 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 panda to simulate (default 'panda') -->
<!-- ========================================================== -->
<xacro:macro name="transmission-franka-state" params="arm_id:=panda">
<transmission name="${arm_id}_franka_state">
<type>franka_hw/FrankaStateInterface</type>
<joint name="${arm_id}_joint1"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></joint>
<joint name="${arm_id}_joint2"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></joint>
<joint name="${arm_id}_joint3"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></joint>
<joint name="${arm_id}_joint4"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></joint>
<joint name="${arm_id}_joint5"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></joint>
<joint name="${arm_id}_joint6"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></joint>
<joint name="${arm_id}_joint7"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></joint>
<actuator name="${arm_id}_motor1"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></actuator>
<actuator name="${arm_id}_motor2"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></actuator>
<actuator name="${arm_id}_motor3"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></actuator>
<actuator name="${arm_id}_motor4"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></actuator>
<actuator name="${arm_id}_motor5"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></actuator>
<actuator name="${arm_id}_motor6"><hardwareInterface>franka_hw/FrankaStateInterface</hardwareInterface></actuator>
<actuator name="${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 panda to simulate (default 'panda') -->
<!-- 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:=panda root:=panda_joint1 tip:=panda_joint7">
<transmission name="${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. panda/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>
</robot>
robots/dual_panda/dual_panda_example.urdf.xacro 0 → 100644
View file @ 658a336d
<?xml version="1.0" encoding="utf-8"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="panda">
<xacro:arg name="arm_id_1" default="panda_1" />
<xacro:arg name="arm_id_2" default="panda_2" />
<xacro:include filename="$(find franka_description)/robots/common/franka_arm.xacro"/>
<xacro:include filename="$(find franka_description)/robots/common/franka_hand.xacro"/>
<!-- box shaped table as base for the 2 Pandas -->
<link name="base">
<visual>
<origin xyz="0 0 0.5" rpy="0 0 0"/>
<geometry>
<box size="1 2 1" />
</geometry>
<material name="White">
<color rgba="1.0 1.0 1.0 1.0"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0.5" rpy="0 0 0"/>
<geometry>
<box size="1 2 1" />
</geometry>
</collision>
</link>
<!-- right arm with gripper -->
<xacro:franka_arm arm_id="$(arg arm_id_1)" connected_to="base" xyz="0 -0.5 1" safety_distance="0.03" joint_limits="${xacro.load_yaml('$(find franka_description)/robots/panda/joint_limits.yaml')}"/>
<xacro:franka_hand arm_id="$(arg arm_id_1)" rpy="0 0 ${-pi/4}" connected_to="$(arg arm_id_1)_link8" safety_distance="0.03"/>
<!-- left arm with gripper -->
<xacro:franka_arm arm_id="$(arg arm_id_2)" connected_to="base" xyz="0 0.5 1" safety_distance="0.03" joint_limits="${xacro.load_yaml('$(find franka_description)/robots/panda/joint_limits.yaml')}"/>
<xacro:franka_hand arm_id="$(arg arm_id_2)" rpy="0 0 ${-pi/4}" connected_to="$(arg arm_id_2)_link8" safety_distance="0.03"/>
</robot>
robots/fr3/fr3.urdf.xacro 0 → 100644
View file @ 658a336d
<?xml version='1.0' encoding='utf-8'?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="fr3">
<xacro:include filename="$(find franka_description)/robots/common/franka_robot.xacro"/>
<xacro:arg name="arm_id" default="fr3" />
<xacro:franka_robot arm_id="$(arg arm_id)"
joint_limits="${xacro.load_yaml('$(find franka_description)/robots/fr3/joint_limits.yaml')}">
</xacro:franka_robot>
</robot>
robots/fr3/joint_limits.yaml 0 → 100644
View file @ 658a336d
joint1:
limit:
lower: -2.3093
upper: 2.3093
velocity: 2.0
effort: 87.0
joint2:
limit:
lower: -1.5133
upper: 1.5133
velocity: 1.0
effort: 87.0
joint3:
limit:
lower: -2.4937
upper: 2.4937
velocity: 1.50
effort: 87.0
joint4:
limit:
lower: -2.7478
upper: -0.4461
velocity: 1.25
effort: 87.0
joint5:
limit:
lower: -2.4800
upper: 2.4800
velocity: 3.00
effort: 12.0
joint6:
limit:
lower: 0.8521
upper: 4.2094
velocity: 1.50
effort: 12.0
joint7:
limit:
lower: -2.6895
upper: 2.6895
velocity: 3.00
effort: 12.0
robots/panda/joint_limits.yaml 0 → 100644
View file @ 658a336d
joint1:
limit:
lower: -2.7973
upper: 2.7973
velocity: 2.1750
effort: 87.0
joint2:
limit:
lower: -1.6628
upper: 1.6628
velocity: 2.1750
effort: 87.0
joint3:
limit:
lower: -2.7973
upper: 2.7973
velocity: 2.1750
effort: 87.0
joint4:
limit:
lower: -2.9718
upper: -0.1698
velocity: 2.1750
effort: 87.0
joint5:
limit:
lower: -2.7973
upper: 2.7973
velocity: 2.6100
effort: 12.0
joint6:
limit:
lower: 0.0825
upper: 3.6525
velocity: 2.6100
effort: 12.0
joint7:
limit:
lower: -2.7973
upper: 2.7973
velocity: 2.6100
effort: 12.0
robots/panda/panda.urdf.xacro 0 → 100644
View file @ 658a336d
<?xml version='1.0' encoding='utf-8'?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="panda">
<xacro:include filename="$(find franka_description)/robots/common/franka_robot.xacro"/>
<xacro:arg name="arm_id" default="panda" />
<xacro:franka_robot arm_id="$(arg arm_id)"
joint_limits="${xacro.load_yaml('$(find franka_description)/robots/panda/joint_limits.yaml')}">
</xacro:franka_robot>
</robot>
test/__init__.py 0 → 100644
View file @ 658a336d
test/dual_panda_example_urdf.py 0 → 100644
View file @ 658a336d
import sys
import subprocess
from .urdf_test_case import UrdfTestCase, PKG
from urdf_parser_py.urdf import URDF, Mesh, Cylinder, Sphere
file = 'dual_panda/dual_panda_example.urdf.xacro'
class TestPandaArmExampleURDF(UrdfTestCase):
def test_generate_urdf_without_args_is_possible(self):
urdf = self.xacro(file) # does not throw
def test_custom_arm_id_1_renames_links_0_up_to_8(self):
arm_id = 'foo'
urdf = self.xacro(file, args='arm_id_1:=%s' % arm_id)
for link in ['%s_link%s' % (arm_id, i) for i in range(0, 9)]:
self.assertContainsLink(urdf, link)
def test_custom_arm_id_1_renames_joints_1_up_to_8(self):
arm_id = 'foo'
urdf = self.xacro(file, args='arm_id_1:=%s' % arm_id)
for joint in ['%s_joint%s' % (arm_id, i) for i in range(1, 9)]:
self.assertContainsJoint(urdf, joint)
def test_custom_arm_id_2_renames_links_0_up_to_8(self):
arm_id = 'foo'
urdf = self.xacro(file, args='arm_id_2:=%s' % arm_id)
for link in ['%s_link%s' % (arm_id, i) for i in range(0, 9)]:
self.assertContainsLink(urdf, link)
def test_custom_arm_id_2_renames_joints(self):
arm_id = 'foo'
urdf = self.xacro(file, args='arm_id_2:=%s' % arm_id)
for joint in ['%s_joint%s' % (arm_id, i) for i in range(1, 9)]:
self.assertContainsJoint(urdf, joint)
if __name__ == '__main__':
import rosunit
rosunit.unitrun(PKG, 'dual_panda_example.urdf.xacro', TestPandaArmExampleURDF)
test/franka_robot_urdf.py 0 → 100644
View file @ 658a336d
import os
import sys
import subprocess
from unittest import TestCase
from parameterized import parameterized_class
from .urdf_test_case import UrdfTestCase, PKG
from urdf_parser_py.urdf import URDF, Mesh, Cylinder, Sphere
@parameterized_class(('robot',), [("panda",), ("fr3",)])
class FrankaRobotUrdfTest(UrdfTestCase):
@property
def file(self):
return os.path.join(self.robot, self.robot + '.urdf.xacro')
def test_generate_urdf_without_xacro_args_is_possible(self):
self.xacro(self.file) # does not throw
def test_generate_urdf_without_xacro_args_contains_link0_up_to_link8(self):
arm_id = self.robot
urdf = self.xacro(self.file)
for i in range(0, 9):
self.assertContainsLink(urdf, '%s_link%s' % (arm_id, i))
def test_generate_urdf_without_xacro_args_contains_joint1_up_to_joint8(self):
arm_id = self.robot
urdf = self.xacro(self.file)
for i in range(1, 9):
joint = 'panda_joint%s' % i
self.assertContainsJoint(urdf, '%s_joint%s' % (arm_id, i))
def test_generate_urdf_without_xacro_args_dont_use_gripper(self):
urdf = self.xacro(self.file)
self.assertFalse(
self.links_with(urdf, lambda link: 'hand' in link), # should be empty
'Found one or more links containing "hand", probably URDF contains a franka_gripper'
)
self.assertFalse(
self.joints_with(urdf, lambda joint: 'hand' in joint), # should be empty
'Found one or more joints containing "hand", probably URDF contains a franka_gripper'
)
def test_generate_urdf_without_xacro_args_uses_fine_collision_models(self):
arm_id = self.robot
urdf = self.xacro(self.file)
for i in range(0, 8):
link = '%s_link%s' % (arm_id, i)
collisions = self.collision_geometries(urdf, link)
self.assertGreaterEqual(len(collisions), 1, "Link '%s' does not have any collision meshes assigned to it" % link)
self.assertIsInstance(
collisions[0], Mesh,
"Link '%s' is expected to have mesh geometries in <collision> not '%s'" % (link, collisions[0].__class__.__name__)
)
def test_generate_urdf_without_xacro_args_uses_coarse_collision_models_for_sc_links(self):
urdf = self.xacro(self.file)
for name in urdf.link_map:
if not name.endswith('_sc'): continue
geometries = self.collision_geometries(urdf, name)
for geometry in geometries:
self.assertIsInstance(
geometry, (Cylinder, Sphere),
"Link '%s' is expected to define only a capsule <collision> geometry (made from Cylinders and Spheres, not '%s')" % (name, geometry.__class__.__name__)
)
def test_generate_urdf_without_xacro_args_doesnt_insert_inertial_tags_for_any_link(self):
urdf = self.xacro(self.file)
for name, link in urdf.link_map.items():
self.assertIsNone(
link.inertial,
"Link '%s' is expected to have no <inertial> defined but actually has one:\n%s" % (name, link.inertial)
)
def test_generate_urdf_with_hand_but_not_gazebo_doesnt_insert_inertial_tags_for_any_link(self):
urdf = self.xacro(self.file, args='hand:=true')
for name, link in urdf.link_map.items():
self.assertIsNone(
link.inertial,
"Link '%s' is expected to have no <inertial> defined but actually has one:\n%s" % (name, link.inertial)
)
def test_custom_arm_id_renames_links(self):
arm_id = 'foo'
urdf = self.xacro(self.file, args='arm_id:=%s' % arm_id)
for link in urdf.link_map.keys():
self.assertIn(arm_id, link)
def test_custom_arm_id_renames_joints(self):
arm_id = 'foo'
urdf = self.xacro(self.file, args='arm_id:=%s' % arm_id)
for joint in urdf.joint_map.keys():
self.assertIn(arm_id, joint)
def test_generate_urdf_with_hand_puts_franka_gripper_into_urdf(self):
arm_id = self.robot
urdf = self.xacro(self.file, args='hand:=true')
for name in ['hand', 'hand_tcp', 'leftfinger', 'rightfinger']:
link = '%s_%s' % (arm_id, name)
self.assertContainsLink(urdf, link)
for name in ['hand_joint', 'hand_tcp_joint', 'finger_joint1', 'finger_joint2']:
joint = '%s_%s' % (arm_id, name)
self.assertContainsJoint(urdf, joint)
def test_custom_arm_id_with_hand_renames_hand_joints_and_links(self):
arm_id = 'foo'
urdf = self.xacro(self.file, args='arm_id:=%s hand:=true' % arm_id)
for name in ['hand', 'hand_tcp', 'leftfinger', 'rightfinger']:
link = '%s_%s' % (arm_id, name)
self.assertContainsLink(urdf, link)
for name in ['hand_joint', 'hand_tcp_joint', 'finger_joint1', 'finger_joint2']:
joint = '%s_%s' % (arm_id, name)
self.assertContainsJoint(urdf, joint)
def test_gazebo_arg_will_add_top_level_world_link(self):
arm_id = self.robot
urdf = self.xacro(self.file, args='gazebo:=true')
# Check if the world link exists
self.assertEqual('world', urdf.get_root())
# Check if robot is directly connected to the world link
self.assertJointBetween(urdf, 'world', arm_id + '_link0', type="fixed")
def test_gazebo_arg_will_insert_gazebo_ros_control_plugin(self):
urdf = self.xacro(self.file, args='gazebo:=true')
for gazebo in urdf.gazebos:
for child in gazebo:
if child.tag == 'plugin':
self.assertIn('name', child.attrib, '<plugin> tag does not have a "name" attribute')
self.assertEqual('gazebo_ros_control', child.attrib['name'])
# Successfully found plugin, break out of all loops
break
else:
# Inner loop not broken, continue with next gazebo tag
continue
# Inner loop was broken, break also outer loop
break
else:
# Outer loop not broken -> no valid plugin found!
self.fail('No <plugin name="gazebo_ros_control"> found in URDF')
def test_gazebo_arg_will_insert_position_interfaces(self):
arm_id = self.robot
urdf = self.xacro(self.file, args='gazebo:=true')
for joint in ['%s_joint%s' % (arm_id, i) for i in range(1,8)]:
self.assertJointHasTransmission(urdf, joint, 'hardware_interface/PositionJointInterface')
def test_gazebo_arg_will_insert_velocity_interfaces(self):
arm_id = self.robot
urdf = self.xacro(self.file, args='gazebo:=true')
for joint in ['%s_joint%s' % (arm_id, i) for i in range(1,8)]:
self.assertJointHasTransmission(urdf, joint, 'hardware_interface/VelocityJointInterface')
def test_gazebo_arg_will_insert_effort_interfaces(self):
arm_id = self.robot
urdf = self.xacro(self.file, args='gazebo:=true')
for joint in ['%s_joint%s' % (arm_id, i) for i in range(1,8)]:
self.assertJointHasTransmission(urdf, joint, 'hardware_interface/EffortJointInterface')
def test_gazebo_arg_will_insert_franka_state_interface(self):
arm_id = self.robot
urdf = self.xacro(self.file, 'gazebo:=true')
for transmission in urdf.transmissions:
if transmission.type == 'franka_hw/FrankaStateInterface':
self.assertListEqual(
['%s_joint%s' % (arm_id, i) for i in range(1,8)],
[joint.name for joint in transmission.joints]
)
break
else:
# Didn't break out of loop -> no state interface found
self.fail('No franka_hw/FrankaStateInterface found in URDF')
def test_gazebo_arg_will_insert_franka_model_interface(self):
arm_id = self.robot
urdf = self.xacro(self.file, args='gazebo:=true')
for transmission in urdf.transmissions:
if transmission.type == 'franka_hw/FrankaModelInterface':
self.assertEqual(
arm_id + '_joint1', transmission.joints[0].name,
"First joint in the FrankaModelInterface transmission must be the root of the chain, i.e. '$(arm_id)_joint1' not '%s'" % transmission.joints[0].name
)
self.assertEqual(
arm_id + '_joint8', transmission.joints[1].name,
"Second joint in the FrankaModelInterface transmission must be the tip of the chain, i.e. '$(arm_id)_joint8' not '%s'" % transmission.joints[1].name
)
break
else:
# Didn't break out of loop -> no model interface found
self.fail('No franka_hw/FrankaModelInterface found in URDF')
def test_gazebo_arg_and_hand_will_insert_effort_interfaces_for_fingers(self):
arm_id = self.robot
urdf = self.xacro(self.file, args='gazebo:=true hand:=true')
for joint in [arm_id + '_finger_joint1', arm_id + '_finger_joint2']:
self.assertJointHasTransmission(urdf, joint, 'hardware_interface/EffortJointInterface')
def test_setting_gazebo_arg_forces_to_have_no_geometries_inside_sc_links(self):
urdf = self.xacro(self.file, args='gazebo:=true')
for name, link in urdf.link_map.items():
if not name.endswith('_sc'): continue
self.assertEqual(
len(link.collisions), 0,
"Link '%s' is expected to have no <collision> tags defined but has %s" % (name, len(link.collisions))
)
def test_setting_gazebo_arg_with_hand_forces_to_have_no_geometries_inside_sc_links(self):
urdf = self.xacro(self.file, args='gazebo:=true hand:=true')
for name, link in urdf.link_map.items():
if not name.endswith('_sc'): continue
self.assertEqual(
len(link.collisions), 0,
"Link '%s' is expected to have no <collision> tags defined but has %s" % (name, len(link.collisions))
)
def test_generate_urdf_without_tcp_args_uses_default_10_34_cm_hand_tcp_offset(self):
arm_id = self.robot
urdf = self.xacro(self.file, args='hand:=true')
joint = urdf.joint_map[arm_id + '_hand_tcp_joint']
self.assertListEqual(joint.origin.xyz, [0, 0, 0.1034])
self.assertListEqual(joint.origin.rpy, [0, 0, 0])
def test_setting_tcp_xyz_arg_moves_the_hand_tcp_link(self):
arm_id = self.robot
urdf = self.xacro(self.file, args='hand:=true tcp_xyz:="1 2 3"')
joint = urdf.joint_map[arm_id + '_hand_tcp_joint']
self.assertListEqual(joint.origin.xyz, [1, 2, 3])
def test_setting_tcp_rpy_arg_rotates_the_hand_tcp_link(self):
arm_id = self.robot
urdf = self.xacro(self.file, args='hand:=true tcp_rpy:="3.1415 0.123 42"')
joint = urdf.joint_map[arm_id + '_hand_tcp_joint']
self.assertListEqual(joint.origin.rpy, [3.1415, 0.123, 42])
if __name__ == '__main__':
import rosunit
rosunit.unitrun(PKG, 'URDF', FrankaRobotUrdfTest)
test/hand_urdf.py 0 → 100644
View file @ 658a336d
import sys
import subprocess
from .urdf_test_case import UrdfTestCase, PKG
from urdf_parser_py.urdf import URDF, Mesh, Cylinder, Sphere
class TestHandURDF(UrdfTestCase):
def test_generate_urdf_without_args_is_possible(self):
self.xacro('common/hand.urdf.xacro') # does not throw
if __name__ == '__main__':
import rosunit
rosunit.unitrun(PKG, 'common/hand.urdf.xacro', TestHandURDF)
test/urdf_test_case.py 0 → 100644
View file @ 658a336d
import subprocess
from urdf_parser_py.urdf import URDF
from unittest import TestCase
PKG = 'franka_description'
class UrdfTestCase(TestCase):
def xacro(self, file, args=''):
"""
Generates a URDF from a XACRO file. If the URDF is invalid the test is
automatically failed.
@param file: The name of the xacro input file within `franka_description/robots/`
@param args: A optional string of xacro args to append, e.g. ("foo:=1 bar:=true")
@return: The generated URDF, as urdf_parser_py.urdf.URDF type
"""
try:
return URDF.from_xml_string(subprocess.check_output(
'xacro $(rospack find %s)/robots/%s %s' % (PKG, file, args),
shell=True)
)
except subprocess.CalledProcessError as e:
self.fail('Could not generate URDF from "%s", probably syntax error: %s' % (file, e.output))
def assertContainsLink(self, urdf, link):
"""
Check that the URDF contains a certain link
@param urdf: The URDF to test
@param link: The name of the link to find
@return: True if 'link' is present in 'urdf', False otherwise
"""
self.assertIn(link, urdf.link_map, 'Link "%s" not found in URDF' % link)
def assertContainsJoint(self, urdf, joint):
"""
Check that the URDF contains a certain joint
@param urdf: The URDF to test
@param joint: The name of the joint to find
@return: True if 'joint' is present in 'urdf', False otherwise
"""
self.assertIn(joint, urdf.joint_map, 'Joint "%s" not found in URDF' % joint)
def links_with(self, urdf, predicate):
"""
Iterate all links in the URDF and filter with a predicate
@param urdf: The URDF with the links to filter
@param predicate: A function, which gets the link name as argument and which
should return true if the link should be in the list or not
@return: A list of link names matching the predicate
"""
return list(filter(predicate, urdf.link_map.keys()))
def joints_with(self, urdf, predicate):
"""
Iterate all joints in the URDF and filter with a predicate
@param urdf: The URDF with the joints to filter
@param predicate: A function, which gets the joint name as argument and which
should return true if the joint should be in the list or not
@return: A list of joint names matching the predicate
"""
return list(filter(predicate, urdf.joint_map.keys()))
def collision_geometries(self, urdf, link):
"""
Find all <collision> geometries of a certain link in a URDF.
Fail the test if the link does not exist.
@param urdf: The URDF to search for
@param link: The name of the link to get its collision geometries for
@return: A list of urdf_parser_py.urdf.Geometry
"""
self.assertContainsLink(urdf, link)
return [ collision.geometry for collision in urdf.link_map[link].collisions ]
def assertJointBetween(self, urdf, parent, child, type=None):
"""
Assert that there exists a <joint> between two links
@param urdf: The URDF in which to look for such a joint
@param parent: The name of the parent link of the joint to look for
@param child: The name of the child link of the joint to look for
@param type: None, to search for any joint, or one of ['unknown', 'revolute',
'continuous', 'prismatic', 'floating', 'planar', 'fixed'] to check
for a specific type only.
"""
candidates = list(filter(lambda j: j.parent == parent, urdf.joints))
self.assertTrue(candidates, "Could not find any joint in URDF which parent is '%s'" % parent)
candidates = list(filter(lambda j: j.child == child, candidates))
self.assertTrue(candidates, "Could not find any joint in URDF which child is '%s'" % child)
if type is not None:
candidates = list(filter(lambda j: j.joint_type == type, candidates))
self.assertTrue(candidates, "Could not find any joint in URDF from %s -> %s which is %s" % (parent, child, type))
def assertJointHasTransmission(self, urdf, joint, type):
"""
Assert that there exists a <transmission> to a joint with a certain type
@param urdf: The URDF in which to look for the transmission
@param joint: The name of the joint for which the transmission is for
@param type: The type of the hw interface, e.g. 'hardware_interface/PositionJointInterface'
"""
self.assertContainsJoint(urdf, joint)
for transmission in urdf.transmissions:
j = transmission.joints[0]
if j.name == joint and j.hardwareInterfaces[0] == type:
# Successfully found matching transmission, break out of loop
break
else:
# Transmission Loop not broken -> no suitable transmission for joint found
self.fail('No suitable "%s" transmission tag for "%s" found in URDF' % (type, joint))