Skip to content

Commit

Permalink
Merge pull request #199 in SWDEV/franka_ros from refactor-franka-desc…
Browse files Browse the repository at this point in the history
…ription-to-support-fr3 to develop

* commit '7e8be0dd968f6c4b80aa54d4f6bcc06bd0e7284f':
  fix cartesian pose example
  generalize over arm_id in franka_example_controllers.yaml
  CHANGE: Parameterize tests for both panda + fr3
  FIX: Make <arg> in `franka_robot` macro property
  CHANGE: Extract <limits> & <safety_controller> into custom macro
  update CHANGELOG.md
  allow selecting fr3 urdf for launch files
  define joint limits for fr3
  load joint limits directly in top-level URDf
  use correct urdfs in launch files
  refactor franka_description to support for fr3
  • Loading branch information
kmohyeldine committed Sep 2, 2022
2 parents 28886ca + 7e8be0d commit bedad2f
Show file tree
Hide file tree
Showing 39 changed files with 462 additions and 331 deletions.
3 changes: 3 additions & 0 deletions .ci/Dockerfile.melodic
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@ RUN apt-get update -y && apt-get install -y \
clang-format-7 \
clang-tidy-7 \
pycodestyle \
python-pip \
python3-catkin-tools \
ros-melodic-libfranka \
ros-melodic-boost-sml \
Expand All @@ -15,3 +16,5 @@ RUN apt-get update -y && apt-get install -y \
ros-melodic-orocos-kdl \
ros-melodic-urdfdom-py \
&& rm -rf /var/lib/apt/lists/*

RUN pip install parameterized
3 changes: 3 additions & 0 deletions .ci/Dockerfile.noetic
Original file line number Diff line number Diff line change
Expand Up @@ -6,6 +6,7 @@ RUN apt-get update -y && apt-get install -y \
clang-tidy-7 \
pycodestyle \
liborocos-kdl-dev \
python3-pip \
python3-catkin-tools \
ros-noetic-libfranka \
ros-noetic-boost-sml \
Expand All @@ -15,3 +16,5 @@ RUN apt-get update -y && apt-get install -y \
ros-noetic-gazebo-ros-control \
ros-noetic-urdfdom-py \
&& rm -rf /var/lib/apt/lists/*

RUN pip install parameterized
2 changes: 2 additions & 0 deletions CHANGELOG.md
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,8 @@ Requires `libfranka` >= 0.8.0
* `franka_example_controllers`: Normalize rotations before usage
* `franka_example_controllers`: Extend the `teleop_joint_pd_example_controller` with markers indicating leader and follower roles + consistently use leader and follower as robot names in the example.
* `franka_example_controllers`: Don't require MoveIt for `move_to_start.launch`
* **BREAKING** `franka_description`: Refactor URDF files to also support FR3
* `franka_control`: Introduce optional `robot` argument in the `franka_control.launch` launch file that chooses either the URDF for panda or fr3. This argument can also be used in the launch files of `franka_examples`. The default is "panda".

## 0.9.1 - 2022-08-29

Expand Down
2 changes: 1 addition & 1 deletion franka_control/launch/franka_combined_control.launch
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@
<launch>

<!-- The path to a robot description (xacro) file to control -->
<arg name="robot" default="$(find franka_description)/robots/dual_panda_example.urdf.xacro" />
<arg name="robot" default="$(find franka_description)/robots/dual_panda/dual_panda_example.urdf.xacro" />

<!-- Additional XACRO args. Be sure to escape colons and equal signs
with backslashes, because XACRO use same syntax as roslaunch:
Expand Down
5 changes: 3 additions & 2 deletions franka_control/launch/franka_control.launch
Original file line number Diff line number Diff line change
@@ -1,11 +1,12 @@
<?xml version="1.0" ?>
<launch>
<arg name="robot_ip" />
<arg name="arm_id" default="panda" />
<arg name="robot" default="panda" doc="choose your robot. Possible values: [panda, fr3]"/>
<arg name="arm_id" default="$(arg robot)" />
<arg name="load_gripper" default="true" />
<arg name="xacro_args" default="" />

<param name="robot_description" command="$(find xacro)/xacro $(find franka_description)/robots/panda_arm.urdf.xacro hand:=$(arg load_gripper) arm_id:=$(arg arm_id) $(arg xacro_args)"/>
<param name="robot_description" command="$(find xacro)/xacro $(find franka_description)/robots/$(arg robot)/$(arg robot).urdf.xacro hand:=$(arg load_gripper) arm_id:=$(arg arm_id) $(arg xacro_args)"/>

<include file="$(find franka_gripper)/launch/franka_gripper.launch" if="$(arg load_gripper)">
<arg name="robot_ip" value="$(arg robot_ip)" />
Expand Down
2 changes: 1 addition & 1 deletion franka_description/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,6 @@ install(DIRECTORY robots
DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
)

catkin_add_nosetests(test/panda_arm_urdf.py)
catkin_add_nosetests(test/franka_robot_urdf.py)
catkin_add_nosetests(test/hand_urdf.py)
catkin_add_nosetests(test/dual_panda_example_urdf.py)
Original file line number Diff line number Diff line change
@@ -1,11 +1,12 @@
<?xml version='1.0' encoding='utf-8'?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="panda">

<xacro:include filename="utils.xacro" />
<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 panda arm. Serves to differentiate between arms in case of multiple instances. -->
<xacro:macro name="panda_arm" params="arm_id:='panda' description_pkg:='franka_description' connected_to:='' xyz:='0 0 0' rpy:='0 0 0' gazebo:=false safety_distance:=0">
<!-- 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}"/>
Expand All @@ -27,12 +28,11 @@
</xacro:link_with_sc>

<joint name="${arm_id}_joint1" type="revolute">
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973" />
<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" />
<limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750" />
<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>

Expand All @@ -43,12 +43,11 @@
</xacro:link_with_sc>

<joint name="${arm_id}_joint2" type="revolute">
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-1.7628" soft_upper_limit="1.7628" />
<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" />
<limit effort="87" lower="-1.7628" upper="1.7628" velocity="2.1750" />
<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>

Expand All @@ -59,12 +58,11 @@
</xacro:link_with_sc>

<joint name="${arm_id}_joint3" type="revolute">
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973" />
<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" />
<limit effort="87" lower="-2.8973" upper="2.8973" velocity="2.1750" />
<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>

Expand All @@ -75,12 +73,11 @@
</xacro:link_with_sc>

<joint name="${arm_id}_joint4" type="revolute">
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-3.0718" soft_upper_limit="-0.0698" />
<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" />
<limit effort="87" lower="-3.0718" upper="-0.0698" velocity="2.1750" />
<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>

Expand All @@ -92,12 +89,11 @@
</xacro:link_with_sc>

<joint name="${arm_id}_joint5" type="revolute">
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973" />
<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" />
<limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100" />
<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>

Expand All @@ -108,12 +104,11 @@
</xacro:link_with_sc>

<joint name="${arm_id}_joint6" type="revolute">
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-0.0175" soft_upper_limit="3.7525" />
<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" />
<limit effort="12" lower="-0.0175" upper="3.7525" velocity="2.6100" />
<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>

Expand All @@ -125,12 +120,11 @@
</xacro:link_with_sc>

<joint name="${arm_id}_joint7" type="revolute">
<safety_controller k_position="100.0" k_velocity="40.0" soft_lower_limit="-2.8973" soft_upper_limit="2.8973"/>
<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"/>
<limit effort="12" lower="-2.8973" upper="2.8973" velocity="2.6100"/>
<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>

Expand Down
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
<?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="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: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}" />
Expand Down
93 changes: 93 additions & 0 deletions franka_description/robots/common/franka_robot.xacro
Original file line number Diff line number Diff line change
@@ -0,0 +1,93 @@
<?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>
6 changes: 6 additions & 0 deletions franka_description/robots/common/hand.urdf.xacro
Original file line number Diff line number Diff line change
@@ -0,0 +1,6 @@
<?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>
File renamed without changes.
Original file line number Diff line number Diff line change
@@ -1,12 +1,12 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">

<!-- Load interial properties. Property is implicitly passed to macros. -->
<xacro:property name="inertial_config" value="$(find franka_description)/robots/inertial.yaml"/>
<!-- 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 <interial> tag based on yaml-load properties -->
<!-- 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) -->
Expand Down Expand Up @@ -219,4 +219,23 @@
</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>
36 changes: 36 additions & 0 deletions franka_description/robots/dual_panda/dual_panda_example.urdf.xacro
Original file line number Diff line number Diff line change
@@ -0,0 +1,36 @@
<?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>
Loading

0 comments on commit bedad2f

Please sign in to comment.