Skip to content

Commit

Permalink
Browse files Browse the repository at this point in the history
  • Loading branch information
rhaschke committed Nov 7, 2021
5 parents 6bc0977 + dcaf6d7 + 4ead405 + 3f06e43 + f376283 commit 7cc7c26
Show file tree
Hide file tree
Showing 9 changed files with 129 additions and 105 deletions.
5 changes: 4 additions & 1 deletion franka_control/config/default_controllers.yaml
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
position_joint_trajectory_controller:
position_joint_trajectory_controller: &default_trajectory_controller
type: position_controllers/JointTrajectoryController
joints:
- panda_joint1
Expand All @@ -25,6 +25,9 @@ position_joint_trajectory_controller:
panda_joint7:
goal: 0.05

# define default controller for panda_moveit_config:
joint_trajectory_controller: *default_trajectory_controller

franka_state_controller:
type: franka_control/FrankaStateController
publish_rate: 30 # [Hz]
Expand Down
6 changes: 0 additions & 6 deletions franka_control/launch/franka_control.launch
Original file line number Diff line number Diff line change
Expand Up @@ -22,10 +22,4 @@
<rosparam unless="$(arg load_gripper)" param="source_list">[franka_state_controller/joint_states] </rosparam>
<param name="rate" value="30"/>
</node>
<node name="joint_state_desired_publisher" type="joint_state_publisher" pkg="joint_state_publisher" output="screen">
<rosparam if="$(arg load_gripper)" param="source_list">[franka_state_controller/joint_states_desired, franka_gripper/joint_states] </rosparam>
<rosparam unless="$(arg load_gripper)" param="source_list">[franka_state_controller/joint_states_desired] </rosparam>
<param name="rate" value="30"/>
<remap from="/joint_states" to="/joint_states_desired" />
</node>
</launch>
4 changes: 2 additions & 2 deletions franka_description/robots/dual_panda_example.urdf.xacro
Original file line number Diff line number Diff line change
Expand Up @@ -26,11 +26,11 @@
</link>

<!-- right arm with gripper -->
<xacro:panda_arm arm_id="$(arg arm_id_1)" connected_to="base" xyz="0 -0.5 1" safety_distance="0.03"/>
<xacro:panda_arm arm_id="$(arg arm_id_1)" ns="$(arg arm_id_1)" connected_to="base" xyz="0 -0.5 1" safety_distance="0.03" />
<xacro:hand ns="$(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:panda_arm arm_id="$(arg arm_id_2)" connected_to="base" xyz="0 0.5 1" safety_distance="0.03"/>
<xacro:panda_arm arm_id="$(arg arm_id_2)" ns="$(arg arm_id_2)" connected_to="base" xyz="0 0.5 1" safety_distance="0.03" />
<xacro:hand ns="$(arg arm_id_2)" rpy="0 0 ${-pi/4}" connected_to="$(arg arm_id_2)_link8" safety_distance="0.03"/>

</robot>
51 changes: 27 additions & 24 deletions franka_description/robots/panda_arm.urdf.xacro
Original file line number Diff line number Diff line change
@@ -1,20 +1,23 @@
<?xml version='1.0' encoding='utf-8'?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="panda">

<xacro:arg name="arm_id" default="panda" /> <!-- Name of this panda -->
<xacro:arg name="hand" default="false" /> <!-- Should a franka_gripper be mounted at the flange?" -->
<xacro:arg name="gazebo" default="false" /> <!-- Is the robot being simulated in gazebo?" -->

<xacro:property name="arm_id" value="$(arg arm_id)" />
<!-- Name of this panda -->
<xacro:arg name="arm_id" default="panda" />
<!-- Should a franka_gripper be mounted at the flange?" -->
<xacro:arg name="hand" default="false" />
<!-- Is the robot being simulated in gazebo?" -->
<xacro:arg name="gazebo" default="false" />
<!-- Namespace of robot in Gazebo/ROS (default: empty) -->
<xacro:arg name="ns" default="" />

<xacro:unless value="$(arg gazebo)">
<!-- Create a URDF for a real hardware -->
<xacro:include filename="$(find franka_description)/robots/panda_arm.xacro" />
<xacro:panda_arm arm_id="${arm_id}" safety_distance="0.03"/>
<xacro:panda_arm arm_id="$(arg arm_id)" safety_distance="0.03"/>

<xacro:if value="$(arg hand)">
<xacro:include filename="$(find franka_description)/robots/hand.xacro"/>
<xacro:hand ns="${arm_id}" rpy="0 0 ${-pi/4}" connected_to="${arm_id}_link8" safety_distance="0.03"/>
<xacro:hand ns="$(arg arm_id)" rpy="0 0 ${-pi/4}" connected_to="$(arg arm_id)_link8" safety_distance="0.03"/>
</xacro:if>
</xacro:unless>

Expand All @@ -27,40 +30,40 @@
<xacro:include filename="$(find franka_description)/robots/utils.xacro" />
<xacro:include filename="$(find franka_description)/robots/panda_gazebo.xacro" />

<xacro:panda_arm arm_id="${arm_id}" />
<xacro:panda_arm arm_id="$(arg arm_id)" />

<xacro:if value="$(arg hand)">
<xacro:hand ns="${arm_id}" rpy="0 0 ${-pi/4}" connected_to="${arm_id}_link8" />
<xacro:gazebo-joint joint="${arm_id}_finger_joint1" transmission="hardware_interface/EffortJointInterface" />
<xacro:gazebo-joint joint="${arm_id}_finger_joint2" transmission="hardware_interface/EffortJointInterface" />
<xacro:hand ns="$(arg arm_id)" rpy="0 0 ${-pi/4}" connected_to="$(arg arm_id)_link8" />
<xacro:gazebo-joint joint="$(arg arm_id)_finger_joint1" transmission="hardware_interface/EffortJointInterface" />
<xacro:gazebo-joint joint="$(arg arm_id)_finger_joint2" transmission="hardware_interface/EffortJointInterface" />
</xacro:if>

<!-- 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="${arm_id}_link0" />
<child link="$(arg arm_id)_link0" />
</joint>


<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:gazebo-joint joint="$(arg arm_id)_joint1" transmission="hardware_interface/EffortJointInterface" />
<xacro:gazebo-joint joint="$(arg arm_id)_joint2" transmission="hardware_interface/EffortJointInterface" />
<xacro:gazebo-joint joint="$(arg arm_id)_joint3" transmission="hardware_interface/EffortJointInterface" />
<xacro:gazebo-joint joint="$(arg arm_id)_joint4" transmission="hardware_interface/EffortJointInterface" />
<xacro:gazebo-joint joint="$(arg arm_id)_joint5" transmission="hardware_interface/EffortJointInterface" />
<xacro:gazebo-joint joint="$(arg arm_id)_joint6" transmission="hardware_interface/EffortJointInterface" />
<xacro:gazebo-joint joint="$(arg 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:transmission-franka-state arm_id="$(arg arm_id)" />
<xacro:transmission-franka-model arm_id="$(arg arm_id)"
root="$(arg arm_id)_joint1"
tip="$(arg arm_id)_joint8"
/>

<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<robotNamespace>${arm_id}</robotNamespace>
<robotNamespace>$(arg ns)</robotNamespace>
<controlPeriod>0.001</controlPeriod>
<robotSimType>franka_gazebo/FrankaHWSim</robotSimType>
</plugin>
Expand Down
3 changes: 2 additions & 1 deletion franka_description/robots/panda_gazebo.xacro
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,7 @@
<!-- || PandaRobot With Retrieval of Feasible Parameters Using || -->
<!-- || Penalty-Based Optimization || -->
<!-- || by: Claudio Gaz, Marco Cognetti, Alexander Oliva, || -->
<!-- || Paolo Robuffo Giordano, Alessandro de Lucaa || -->
<!-- || Paolo Robuffo Giordano, Alessandro de Luca || -->
<!-- =============================================================== -->

<xacro:macro name="panda_arm" params="arm_id:='panda'">
Expand Down Expand Up @@ -356,6 +356,7 @@
<origin xyz="0 0 0.0584" rpy="0 0 0"/>
<axis xyz="0 -1 0"/>
<limit effort="20" lower="0.0" upper="0.04" velocity="0.2"/>
<mimic joint="${ns}_finger_joint1" />
<dynamics damping="0.3"/>
</joint>
</xacro:macro>
Expand Down
22 changes: 22 additions & 0 deletions franka_gazebo/config/sim_controllers.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,28 @@ franka_state_controller:
- $(arg arm_id)_joint6
- $(arg arm_id)_joint7

effort_joint_trajectory_controller: &default_trajectory_controller
type: effort_controllers/JointTrajectoryController
joints:
- $(arg arm_id)_joint1
- $(arg arm_id)_joint2
- $(arg arm_id)_joint3
- $(arg arm_id)_joint4
- $(arg arm_id)_joint5
- $(arg arm_id)_joint6
- $(arg arm_id)_joint7
gains:
$(arg arm_id)_joint1: { p: 600, d: 30, i: 0 }
$(arg arm_id)_joint2: { p: 600, d: 30, i: 0 }
$(arg arm_id)_joint3: { p: 600, d: 30, i: 0 }
$(arg arm_id)_joint4: { p: 600, d: 30, i: 0 }
$(arg arm_id)_joint5: { p: 250, d: 10, i: 0 }
$(arg arm_id)_joint6: { p: 150, d: 10, i: 0 }
$(arg arm_id)_joint7: { p: 50, d: 5, i: 0 }

# define default controller for panda_moveit_config:
joint_trajectory_controller: *default_trajectory_controller

model_example_controller:
type: franka_example_controllers/ModelExampleController
arm_id: $(arg arm_id)
Expand Down
75 changes: 5 additions & 70 deletions franka_gazebo/launch/panda.launch
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
<?xml version="1.0"?>
<launch>
<arg name="ns" default="" doc="Namespace to launch the Gazebo robot in" />

<!-- Gazebo & GUI Configuration -->
<arg name="gazebo" default="true" doc="Should the gazebo simulation be launched? Use false in case if you want to include this file and launch gazebo yourself" />
Expand Down Expand Up @@ -31,82 +32,16 @@

<include file="$(find gazebo_ros)/launch/empty_world.launch" if="$(arg gazebo)">
<arg name="world_name" value="$(arg world)"/>
<!-- Always start in paused mode, and only unpause when spawning the model -->
<arg name="paused" value="true"/>
<arg name="gui" value="$(eval not arg('headless'))"/>
<arg name="use_sim_time" value="true"/>
</include>

<group ns="$(arg arm_id)">
<param name="robot_description"
command="xacro $(find franka_description)/robots/panda_arm.urdf.xacro
gazebo:=true
hand:=$(arg use_gripper)
arm_id:=$(arg arm_id)
xyz:='$(arg x) $(arg y) $(arg z)'
rpy:='$(arg roll) $(arg pitch) $(arg yaw)'">
</param>

<rosparam file="$(find franka_gazebo)/config/franka_hw_sim.yaml" subst_value="true" />
<rosparam file="$(find franka_gazebo)/config/sim_controllers.yaml" subst_value="true" />

<param name="m_ee" value="0.76" if="$(arg use_gripper)" />

<node name="$(arg arm_id)_model_spawner"
pkg="gazebo_ros"
type="spawn_model"
if="$(arg paused)"
args="-param robot_description -urdf -model $(arg arm_id)
$(arg initial_joint_positions)
">
</node>
<node name="$(arg arm_id)_model_spawner"
pkg="gazebo_ros"
type="spawn_model"
unless="$(arg paused)"
args="-param robot_description -urdf -model $(arg arm_id) -unpause
$(arg initial_joint_positions)
">
</node>


<!-- Spawn required ROS controllers -->
<node pkg="controller_manager"
type="spawner"
name="$(arg arm_id)_gripper_spawner"
if="$(arg use_gripper)"
args="franka_gripper"
respawn="false"
/>

<node pkg="controller_manager"
type="spawner"
name="$(arg arm_id)_controller_spawner"
respawn="false" output="screen"
args="franka_state_controller $(arg controller)"
/>

<remap to="cartesian_impedance_example_controller/equilibrium_pose" from="equilibrium_pose" />

<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
<node name="joint_state_publisher" type="joint_state_publisher" pkg="joint_state_publisher">
<rosparam param="source_list">[franka_state_controller/joint_states, franka_gripper/joint_states] </rosparam>
<param name="rate" value="30"/>
</node>
<node name="joint_state_desired_publisher" type="joint_state_publisher" pkg="joint_state_publisher">
<rosparam param="source_list">[franka_state_controller/joint_states_desired, franka_gripper/joint_states] </rosparam>
<param name="rate" value="30"/>
<remap from="joint_states" to="joint_states_desired" />
</node>

<!-- Start only if cartesian_impedance_example_controller -->
<node name="interactive_marker"
pkg="franka_example_controllers"
type="interactive_marker.py"
if="$(eval arg('controller') == 'cartesian_impedance_example_controller')">
<param name="link_name" value="$(arg arm_id)_link0" />
</node>

</group>
<!-- Depending on whether the namespace is empty or not, don't start in a namespace (or do) -->
<include file="$(dirname)/panda_ns.launch.xml" pass_all_args="true" if="$(eval arg('ns') == '')" />
<include file="$(dirname)/panda_ns.launch.xml" pass_all_args="true" if="$(eval arg('ns') != '')" ns="$(arg ns)" />

<node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_gazebo)/config/franka_sim_description_with_marker.rviz" if="$(arg rviz)"/>

Expand Down
66 changes: 66 additions & 0 deletions franka_gazebo/launch/panda_ns.launch.xml
Original file line number Diff line number Diff line change
@@ -0,0 +1,66 @@
<?xml version="1.0"?>
<launch>

<!-- This is a helper launch file for panda.launch
As roslaunch doesn't allow for empty ns arguments and to avoid code duplication,
we factored out code here that should run either in or out a namespace.
All arguments are simply inherited from the parent launch file.
-->
<param name="robot_description"
command="xacro $(find franka_description)/robots/panda_arm.urdf.xacro
gazebo:=true
hand:=$(arg use_gripper)
arm_id:=$(arg arm_id)
ns:=$(arg ns)
xyz:='$(arg x) $(arg y) $(arg z)'
rpy:='$(arg roll) $(arg pitch) $(arg yaw)'">
</param>

<rosparam file="$(find franka_gazebo)/config/franka_hw_sim.yaml" subst_value="true" />
<rosparam file="$(find franka_gazebo)/config/sim_controllers.yaml" subst_value="true" />

<param name="m_ee" value="0.76" if="$(arg use_gripper)" />

<!-- Avoid node names of the form arm_id/arm_id_xxx. Use arm_id/xxx instead. -->
<arg name="node_prefix" value="$(eval '' if arg('ns') == arg('arm_id') else arg('arm_id') + '_')" />

<arg name="unpause" value="$(eval '' if arg('paused') else '-unpause')" />
<node name="$(arg node_prefix)model_spawner"
pkg="gazebo_ros"
type="spawn_model"
args="-param robot_description -urdf -model $(arg arm_id) $(arg unpause)
$(arg initial_joint_positions)
"/>

<!-- Spawn required ROS controllers -->
<node pkg="controller_manager"
type="spawner"
name="$(arg node_prefix)gripper_spawner"
if="$(arg use_gripper)"
args="franka_gripper"
respawn="false"
/>

<node pkg="controller_manager"
type="spawner"
name="$(arg node_prefix)controller_spawner"
respawn="false" output="screen"
args="franka_state_controller $(arg controller)"
/>

<node name="$(arg node_prefix)robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
<node name="$(arg node_prefix)joint_state_publisher" type="joint_state_publisher" pkg="joint_state_publisher">
<rosparam param="source_list">[franka_state_controller/joint_states, franka_gripper/joint_states] </rosparam>
<param name="rate" value="30"/>
</node>

<!-- Start only if cartesian_impedance_example_controller -->
<node name="$(arg node_prefix)cartesian_goal_marker"
pkg="franka_example_controllers"
type="interactive_marker.py"
if="$(eval arg('controller') == 'cartesian_impedance_example_controller')">
<param name="link_name" value="$(arg arm_id)_link0" />
<remap to="cartesian_impedance_example_controller/equilibrium_pose" from="equilibrium_pose" />
</node>

</launch>
2 changes: 1 addition & 1 deletion franka_gazebo/test/launch/panda-gazebo.urdf
Original file line number Diff line number Diff line change
Expand Up @@ -7,7 +7,7 @@
<!-- || PandaRobot With Retrieval of Feasible Parameters Using || -->
<!-- || Penalty-Based Optimization || -->
<!-- || by: Claudio Gaz, Marco Cognetti, Alexander Oliva, || -->
<!-- || Paolo Robuffo Giordano,Alessandro de Lucaa third-party || -->
<!-- || Paolo Robuffo Giordano, Alessandro de Luca third-party || -->
<!-- =============================================================== -->
<link name="world"/>
<joint name="panda_joint_panda_mount" type="fixed">
Expand Down

0 comments on commit 7cc7c26

Please sign in to comment.