Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Stop re-publishing joint states #1

Closed
wants to merge 9 commits into from
Closed

Stop re-publishing joint states #1

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
0fc443e
Simplify use of arguments
rhaschke Oct 25, 2021
ef9d26e
Place XML comments in front of xacro commands
rhaschke Oct 25, 2021
07c9c36
Fix typo
rhaschke Oct 25, 2021
aebcdc6
Simplify use of argument `paused`
rhaschke Oct 27, 2021
d78f0e5
Move `remap from=equilibrium_pose` to the `interactive_marker.py` node
rhaschke Oct 25, 2021
f6ac65f
Add 'ns' argument to allow specifying the robotNamespace for Gazebo
rhaschke Oct 25, 2021
affc1f8
Allow starting the Gazebo robot w/ and w/o a namespace.
rhaschke Oct 27, 2021
09af9c7
Drop re-publishing joint_states_desired
rhaschke Oct 27, 2021
afc75a5
Replace re-publishing joint_states with remapping
rhaschke Oct 27, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
15 changes: 4 additions & 11 deletions franka_control/launch/franka_control.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,10 @@

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

<!-- Remap both, arm and gripper joint states to default ROS topic joint_states -->
<remap from="franka_state_controller/joint_states" to="joint_states" />
<remap from="franka_gripper/joint_states" to="joint_states" />

<include file="$(find franka_gripper)/launch/franka_gripper.launch" if="$(arg load_gripper)">
<arg name="robot_ip" value="$(arg robot_ip)" />
</include>
Expand All @@ -17,15 +21,4 @@
<rosparam command="load" file="$(find franka_control)/config/default_controllers.yaml" />
<node name="state_controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="franka_state_controller"/>
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" output="screen"/>
<node name="joint_state_publisher" type="joint_state_publisher" pkg="joint_state_publisher" output="screen">
<rosparam if="$(arg load_gripper)" param="source_list">[franka_state_controller/joint_states, franka_gripper/joint_states] </rosparam>
<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
View file
Open in desktop
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
View file
Open in desktop
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
2 changes: 1 addition & 1 deletion franka_description/robots/panda_gazebo.xacro
View file
Open in desktop
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
76 changes: 8 additions & 68 deletions franka_gazebo/launch/panda.launch
View file
Open in desktop
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,80 +32,19 @@

<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"/>
<!-- Remap both, arm and gripper joint states to default ROS topic joint_states -->
<remap from="franka_state_controller/joint_states" to="joint_states" />
<remap from="franka_gripper/joint_states" to="joint_states" />
</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" />

<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
57 changes: 57 additions & 0 deletions franka_gazebo/launch/panda_ns.launch.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,57 @@
<?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" />

<arg name="unpause" value="$(eval '' if arg('paused') else '-unpause')" />
<node name="$(arg arm_id)_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 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)"
/>

<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />

<!-- 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" />
<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
View file
Open in desktop
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