Skip to content

Commit

Permalink
Merge pull request #321 from frankaemika/github-#313
Browse files Browse the repository at this point in the history 
GitHub #313
  • Loading branch information
@gollth
gollth authored Apr 24, 2023
2 parents cf53956 + 121176d commit 3584069
Show file tree
Hide file tree
Showing 7 changed files with 119 additions and 32 deletions.
2 changes: 2 additions & 0 deletions CHANGELOG.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,8 @@ Requires `libfranka` >= 0.8.0
* `franka_control`: Fix unexpected long delay when calling error recovery ([#317](https://github.com/frankaemika/franka_ros/issues/317))
* Fix a possible compilation error by sorting include directories by topological order ([#319](https://github.com/frankaemika/franka_ros/issues/319)).
* `franka_control`: Fix a bug where `error_recovery` actions recover future errors ([#316](https://github.com/frankaemika/franka_ros/issues/316)).
* `franka_gazebo`: `FrankaHWSim` only acts on joints belonging to a Franka robot. This allows to combine a Franka robot and others (like mobile platforms) in same URDF ([#313](https://github.com/frankaemika/franka_ros/issues/313))
* `franka_description`: `<xacro:franka_robot/>` macro now supports to customize the `parent` frame and its `xyz` + `rpy` offset

## 0.10.1 - 2022-09-15

Expand Down
50 changes: 21 additions & 29 deletions franka_description/robots/common/franka_robot.xacro
View file
Original file line number Diff line number Diff line change
@@ -1,47 +1,39 @@
<?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:macro name="franka_robot" params="arm_id joint_limits parent:='world' xyz:='0 0 0' rpy:='0 0 0' hand:=false gazebo:=false tcp_xyz:='0 0 0.1034' tcp_rpy='0 0 0'">

<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:franka_arm arm_id="${arm_id}" safety_distance="0.03" gazebo="${gazebo}" joint_limits="${joint_limits}"/>

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

<!-- Define additional Gazebo tags -->
<xacro:if value="$(arg gazebo)">
<xacro:if value="${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:if value="${parent != ''}">
<!-- Gazebo requires a joint to a link called "world" for statically mounted robots -->
<xacro:if value="${parent == 'world'}">
<link name="${parent}" />
</xacro:if>
<joint name="${parent}_joint" type="fixed">
<origin xyz="${xyz}" rpy="${rpy}" />
<parent link="${parent}" />
<child link="${arm_id}_link0" />
</joint>
</xacro:if>

<xacro:gazebo-joint joint="${arm_id}_joint1" transmission="hardware_interface/PositionJointInterface" />
<xacro:gazebo-joint joint="${arm_id}_joint2" transmission="hardware_interface/PositionJointInterface" />
Expand Down Expand Up @@ -73,7 +65,7 @@
tip="${arm_id}_joint8"
/>

<xacro:if value="$(arg hand)">
<xacro:if value="${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 -->
Expand Down
26 changes: 25 additions & 1 deletion franka_description/robots/fr3/fr3.urdf.xacro
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,10 +3,34 @@

<xacro:include filename="$(find franka_description)/robots/common/franka_robot.xacro"/>

<!-- Name for this robot -->
<xacro:arg name="arm_id" default="fr3" />

<!-- 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" />
<!-- If `gazebo` arg is set, to which frame shall $(arm_id)_link0 be parented. Empty string for not fixing at all -->
<xacro:arg name="parent" default="world" />
<!-- If `gazebo` arg is set and `parent` not empty, what position offset between `parent` & $(arm_id)_link0 -->
<xacro:arg name="xyz" default="0 0 0" />
<!-- If `gazebo` arg is set and `parent` not empty, what rotation offset between `parent` & $(arm_id)_link0 -->
<xacro:arg name="rpy" default="0 0 0" />

<xacro:franka_robot arm_id="$(arg arm_id)"
joint_limits="${xacro.load_yaml('$(find franka_description)/robots/fr3/joint_limits.yaml')}">
joint_limits="${xacro.load_yaml('$(find franka_description)/robots/fr3/joint_limits.yaml')}"
hand="$(arg hand)"
tcp_xyz="$(arg tcp_xyz)"
tcp_rpy="$(arg tcp_rpy)"
gazebo="$(arg gazebo)"
parent="$(arg parent)"
xyz="$(arg xyz)"
rpy="$(arg rpy)">
</xacro:franka_robot>

</robot>
26 changes: 25 additions & 1 deletion franka_description/robots/panda/panda.urdf.xacro
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,10 +3,34 @@

<xacro:include filename="$(find franka_description)/robots/common/franka_robot.xacro"/>

<!-- Name for this robot -->
<xacro:arg name="arm_id" default="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" />
<!-- If `gazebo` arg is set, to which frame shall $(arm_id)_link0 be parented. Empty string for not fixing at all -->
<xacro:arg name="parent" default="world" />
<!-- If `gazebo` arg is set and `parent` not empty, what position offset between `parent` & $(arm_id)_link0 -->
<xacro:arg name="xyz" default="0 0 0" />
<!-- If `gazebo` arg is set and `parent` not empty, what rotation offset between `parent` & $(arm_id)_link0 -->
<xacro:arg name="rpy" default="0 0 0" />

<xacro:franka_robot arm_id="$(arg arm_id)"
joint_limits="${xacro.load_yaml('$(find franka_description)/robots/panda/joint_limits.yaml')}">
joint_limits="${xacro.load_yaml('$(find franka_description)/robots/panda/joint_limits.yaml')}"
hand="$(arg hand)"
tcp_xyz="$(arg tcp_xyz)"
tcp_rpy="$(arg tcp_rpy)"
gazebo="$(arg gazebo)"
parent="$(arg parent)"
xyz="$(arg xyz)"
rpy="$(arg rpy)">
</xacro:franka_robot>

</robot>
25 changes: 25 additions & 0 deletions franka_description/test/franka_robot_urdf.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -124,6 +124,31 @@ def test_gazebo_arg_will_add_top_level_world_link(self):
# Check if robot is directly connected to the world link
self.assertJointBetween(urdf, 'world', arm_id + '_link0', type="fixed")

def test_parent_arg_will_add_fixed_joint(self):
arm_id = self.robot
parent = "foo"
urdf = self.xacro(self.file, args="gazebo:=true parent:=%s" % parent)
self.assertJointBetween(urdf, parent, arm_id + "_link0", type="fixed")

def test_parent_arg_will_not_top_level_link(self):
arm_id = self.robot
parent = "foo"
urdf = self.xacro(self.file, args="gazebo:=true parent:=%s" % parent)
with self.assertRaises(AssertionError):
urdf.get_root()

def test_xyz_will_apply_fixed_offset_to_parent(self):
xyz = [0, 1, 2]
urdf = self.xacro(self.file, args="gazebo:=true xyz:='%s'" % " ".join(str(x) for x in xyz))
joint = urdf.joint_map['world_joint']
self.assertListEqual(joint.origin.xyz, xyz)

def test_rpy_will_apply_fixed_offset_to_parent(self):
rpy = [3, 4, 5]
urdf = self.xacro(self.file, args="gazebo:=true rpy:='%s'" % " ".join(str(x) for x in rpy))
joint = urdf.joint_map['world_joint']
self.assertListEqual(joint.origin.rpy, rpy)

def test_gazebo_arg_will_insert_gazebo_ros_control_plugin(self):
urdf = self.xacro(self.file, args='gazebo:=true')

Expand Down
5 changes: 5 additions & 0 deletions franka_gazebo/include/franka_gazebo/franka_hw_sim.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@
#include <pluginlib/class_list_macros.h>
#include <ros/ros.h>
#include <urdf/model.h>
#include <array>
#include <boost/optional.hpp>
#include <boost_sml/sml.hpp>
#include <cmath>
Expand All @@ -24,10 +25,14 @@
#include <map>
#include <memory>
#include <mutex>
#include <string>

namespace franka_gazebo {

const double kDefaultTauExtLowpassFilter = 1.0; // no filtering per default of tau_ext_hat_filtered
const std::array<std::string, 9> kRobotJointSuffixes = {
"_joint1", "_joint2", "_joint3", "_joint4", "_joint5",
"_joint6", "_joint7", "_finger_joint1", "_finger_joint2"};

/**
* A custom implementation of a [gazebo_ros_control](http://wiki.ros.org/gazebo_ros_control) plugin,
Expand Down
17 changes: 16 additions & 1 deletion franka_gazebo/src/franka_hw_sim.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@
#include <std_msgs/Bool.h>
#include <std_srvs/SetBool.h>
#include <Eigen/Dense>
#include <algorithm>
#include <boost/algorithm/clamp.hpp>
#include <boost/optional.hpp>
#include <iostream>
Expand Down Expand Up @@ -112,6 +113,16 @@ bool FrankaHWSim::initSim(const std::string& robot_namespace,
// Fill a 'Joint' struct which holds all necessary data
auto joint = std::make_shared<franka_gazebo::Joint>();
joint->name = transmission.joints_[0].name_;

if (std::none_of(kRobotJointSuffixes.begin(), kRobotJointSuffixes.end(),
[&](auto suffix) { return joint->name == arm_id_ + suffix; })) {
ROS_WARN_STREAM_NAMED("franka_hw_sim",
"Joint '" << joint->name << "' contains a '" << transmission.type_
<< "' transmission, but it's not part of the Franka robot. "
"Ignoring this joint in FrankaHWSim");
continue;
}

if (urdf == nullptr) {
ROS_ERROR_STREAM_NAMED(
"franka_hw_sim", "Could not find any URDF model. Was it loaded on the parameter server?");
Expand Down Expand Up @@ -153,7 +164,11 @@ bool FrankaHWSim::initSim(const std::string& robot_namespace,
// Register all supported command interfaces
for (const auto& transmission : transmissions) {
for (const auto& k_interface : transmission.joints_[0].hardware_interfaces_) {
auto joint = this->joints_[transmission.joints_[0].name_];
auto name = transmission.joints_[0].name_;
if (this->joints_.count(name) == 0) {
continue;
}
auto joint = this->joints_[name];
if (transmission.type_ == "transmission_interface/SimpleTransmission") {
ROS_INFO_STREAM_NAMED("franka_hw_sim", "Found transmission interface of joint '"
<< joint->name << "': " << k_interface);
Expand Down

0 comments on commit 3584069

Please sign in to comment.