Skip to content

Commit

Permalink
Merge PRs frankaemika#184, frankaemika#157, frankaemika#173, frankaem…
Browse files Browse the repository at this point in the history 
…ika#183, frankaemika#186, frankaemika#187 into develop
  • Loading branch information
@rhaschke
rhaschke committed Oct 28, 2021
5 parents d6ba2fe + e584a10 + 22abcf3 + a706b12 + 39dfdd1 commit 767e613
Show file tree
Hide file tree
Showing 8 changed files with 135 additions and 141 deletions.
36 changes: 0 additions & 36 deletions .travis.yml
View file

This file was deleted.

4 changes: 2 additions & 2 deletions franka_description/robots/dual_panda_example.urdf.xacro
View file
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>
60 changes: 36 additions & 24 deletions franka_description/robots/panda_arm.urdf.xacro
View file
Original file line number Diff line number Diff line change
@@ -1,20 +1,25 @@
<?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="" />
<!-- The transmission type that is used to control arm joints in gazebo: position, velocity, or effort -->
<xacro:arg name="transmission" default="effort" />

<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 +32,47 @@
<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:property name="transmission" value="$(arg transmission)" />
<!-- sanity check -->
<xacro:if value="${transmission.lower() not in ['position', 'velocity', 'effort']}" >
${xacro.fatal("transmission:=" + transmission + " is not one of position, velocity, or effort")}
</xacro:if>
<!-- Unify transmission argument into title case and expand to full name -->
<xacro:property name="transmission" value="hardware_interface/${transmission.title()}JointInterface" lazy_eval="false" />
<xacro:gazebo-joint joint="$(arg arm_id)_joint1" transmission="${transmission}" />
<xacro:gazebo-joint joint="$(arg arm_id)_joint2" transmission="${transmission}" />
<xacro:gazebo-joint joint="$(arg arm_id)_joint3" transmission="${transmission}" />
<xacro:gazebo-joint joint="$(arg arm_id)_joint4" transmission="${transmission}" />
<xacro:gazebo-joint joint="$(arg arm_id)_joint5" transmission="${transmission}" />
<xacro:gazebo-joint joint="$(arg arm_id)_joint6" transmission="${transmission}" />
<xacro:gazebo-joint joint="$(arg arm_id)_joint7" transmission="${transmission}" />

<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
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
75 changes: 6 additions & 69 deletions franka_gazebo/launch/panda.launch
View file
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 All @@ -11,6 +12,7 @@
<!-- Robot Customization -->
<arg name="arm_id" default="panda" doc="Name of the panda robot to spawn" />
<arg name="use_gripper" default="true" doc="Should a franka hand be mounted on the flange?" />
<arg name="transmission" default="effort" doc="Transmission type used for arm joints. One of position, velocity, effort." />
<arg name="controller" default=" " doc="Which example controller should be started? (One of {cartesian_impedance,model,force}_example_controller)" />
<arg name="x" default="0" doc="How far forward to place the base of the robot in [m]?" />
<arg name="y" default="0" doc="How far leftwards to place the base of the robot in [m]?" />
Expand All @@ -31,80 +33,15 @@

<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" />

<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
70 changes: 70 additions & 0 deletions franka_gazebo/launch/panda_ns.launch.xml
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,70 @@
<?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)
transmission:=$(arg transmission)
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" />

<!-- 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>
<node name="$(arg node_prefix)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="$(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>
27 changes: 19 additions & 8 deletions franka_gazebo/src/franka_gripper_sim.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -421,22 +421,35 @@ void FrankaGripperSim::onGraspGoal(const franka_gripper::GraspGoalConstPtr& goal
}

void FrankaGripperSim::onGripperActionGoal(const control_msgs::GripperCommandGoalConstPtr& goal) {
control_msgs::GripperCommandResult result;
double width_d = goal->command.position * 2.0;

ROS_INFO_STREAM_NAMED("FrankaGripperSim", "New Gripper Command Action Goal received: "
<< goal->command.max_effort << "N");
<< width_d << "m, " << goal->command.max_effort
<< "N");

// HACK: As one gripper finger is <mimic>, MoveIt!'s trajectory execution manager
// only sends us the width of one finger. Multiply by 2 to get the intended width.
double width = this->finger1_.getPosition() + this->finger2_.getPosition();

if (width_d > kMaxFingerWidth || width_d < 0.0) {
std::string error =
"Commanding out of range width! max_width = " + std::to_string(kMaxFingerWidth) +
" command = " + std::to_string(width_d);
ROS_ERROR_STREAM_NAMED("FrankaGripperSim", error);
result.reached_goal = static_cast<decltype(result.reached_goal)>(false);
action_gc_->setAborted(result, error);
return;
}

franka_gripper::GraspEpsilon eps;
eps.inner = this->tolerance_gripper_action_;
eps.outer = this->tolerance_gripper_action_;

transition(State::GRASPING,
Config{.width_desired = goal->command.position * 2.0 < width ? 0 : kMaxFingerWidth,
.speed_desired = this->speed_default_,
.force_desired = goal->command.max_effort,
.tolerance = eps});
transition(State::GRASPING, Config{.width_desired = width_d,
.speed_desired = this->speed_default_,
.force_desired = goal->command.max_effort,
.tolerance = eps});

waitUntilStateChange();

Expand All @@ -446,15 +459,13 @@ void FrankaGripperSim::onGripperActionGoal(const control_msgs::GripperCommandGoa
return;
}

control_msgs::GripperCommandResult result;
if (this->state_ != State::HOLDING) {
result.reached_goal = static_cast<decltype(result.reached_goal)>(false);
std::string error = "Unexpected state transistion: The gripper not in HOLDING as expected";
action_gc_->setAborted(result, error);
return;
}

double width_d = goal->command.position * 2.0;
width = this->finger1_.getPosition() + this->finger2_.getPosition(); // recalculate
bool inside_tolerance = width_d - this->tolerance_gripper_action_ < width and
width < width_d + this->tolerance_gripper_action_;
Expand Down
2 changes: 1 addition & 1 deletion franka_gazebo/test/launch/panda-gazebo.urdf
View file
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 767e613

Please sign in to comment.