From 9408396fa8456bcd9a5004e48863939233d28071 Mon Sep 17 00:00:00 2001 From: Thore Goll Date: Wed, 1 Feb 2023 15:03:53 +0100 Subject: [PATCH 1/4] FIX: FrankaHWSim only controls joints beloning to robot --- .../include/franka_gazebo/franka_hw_sim.h | 5 +++++ franka_gazebo/src/franka_hw_sim.cpp | 17 ++++++++++++++++- 2 files changed, 21 insertions(+), 1 deletion(-) diff --git a/franka_gazebo/include/franka_gazebo/franka_hw_sim.h b/franka_gazebo/include/franka_gazebo/franka_hw_sim.h index 0d9dae5f4..d94b9019e 100644 --- a/franka_gazebo/include/franka_gazebo/franka_hw_sim.h +++ b/franka_gazebo/include/franka_gazebo/franka_hw_sim.h @@ -16,6 +16,7 @@ #include #include #include +#include #include #include #include @@ -24,10 +25,14 @@ #include #include #include +#include namespace franka_gazebo { const double kDefaultTauExtLowpassFilter = 1.0; // no filtering per default of tau_ext_hat_filtered +const std::array 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, diff --git a/franka_gazebo/src/franka_hw_sim.cpp b/franka_gazebo/src/franka_hw_sim.cpp index 58d1de35e..fb80907b2 100644 --- a/franka_gazebo/src/franka_hw_sim.cpp +++ b/franka_gazebo/src/franka_hw_sim.cpp @@ -16,6 +16,7 @@ #include #include #include +#include #include #include #include @@ -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(); 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?"); @@ -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); From 15d3c01cd4a9913c4ad555f525550883093b69f6 Mon Sep 17 00:00:00 2001 From: Thore Goll Date: Wed, 1 Feb 2023 15:47:46 +0100 Subject: [PATCH 2/4] ADD: `parent` + `{xyz,rpy}` params to franka_robot.xacro These can be used if you want to simulate the robot on a mobile base. In these cases the `$(arm_id)_link0` must not be connected to `world` but instead fixed to a link on the mobile robot instead. You could generate a custom combined platform + arm URDF like so: ```xml ``` --- .../robots/common/franka_robot.xacro | 48 ++++++++----------- franka_description/robots/fr3/fr3.urdf.xacro | 26 +++++++++- .../robots/panda/panda.urdf.xacro | 26 +++++++++- franka_description/test/franka_robot_urdf.py | 25 ++++++++++ 4 files changed, 95 insertions(+), 30 deletions(-) diff --git a/franka_description/robots/common/franka_robot.xacro b/franka_description/robots/common/franka_robot.xacro index 23e5c87af..fc37d6c1e 100644 --- a/franka_description/robots/common/franka_robot.xacro +++ b/franka_description/robots/common/franka_robot.xacro @@ -1,47 +1,39 @@ - - - - - - - - - - + - + - + - + - - - - - - - - - - + + + + + + + + + + + diff --git a/franka_description/robots/fr3/fr3.urdf.xacro b/franka_description/robots/fr3/fr3.urdf.xacro index 0a235891b..8035a8ead 100644 --- a/franka_description/robots/fr3/fr3.urdf.xacro +++ b/franka_description/robots/fr3/fr3.urdf.xacro @@ -3,10 +3,34 @@ + + + + + + + + + + + + + + + + + + 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)"> diff --git a/franka_description/robots/panda/panda.urdf.xacro b/franka_description/robots/panda/panda.urdf.xacro index e5dc4b745..04939171c 100644 --- a/franka_description/robots/panda/panda.urdf.xacro +++ b/franka_description/robots/panda/panda.urdf.xacro @@ -3,10 +3,34 @@ + + + + + + + + + + + + + + + + + + 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)"> diff --git a/franka_description/test/franka_robot_urdf.py b/franka_description/test/franka_robot_urdf.py index af60a0fcf..4aa7e4562 100644 --- a/franka_description/test/franka_robot_urdf.py +++ b/franka_description/test/franka_robot_urdf.py @@ -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') From e30b0d455ad24c5baa347f3a9ada6c82b759ae52 Mon Sep 17 00:00:00 2001 From: Thore Goll Date: Wed, 1 Feb 2023 16:25:31 +0100 Subject: [PATCH 3/4] ADD: CHANGELOG entry --- CHANGELOG.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 982640ee7..830d6e2b4 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -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`: `` macro now supports to customize the `parent` frame and its `xyz` + `rpy` offset ## 0.10.1 - 2022-09-15 From 121176db0bab847b247c1a3e8e9640d98f2eb5c1 Mon Sep 17 00:00:00 2001 From: Thore Goll Date: Mon, 27 Feb 2023 08:40:58 +0100 Subject: [PATCH 4/4] FIX: Unknown substitution "hand" ...is now a macro argument and not a xacro arg anymore --- franka_description/robots/common/franka_robot.xacro | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/franka_description/robots/common/franka_robot.xacro b/franka_description/robots/common/franka_robot.xacro index fc37d6c1e..b78eb0776 100644 --- a/franka_description/robots/common/franka_robot.xacro +++ b/franka_description/robots/common/franka_robot.xacro @@ -65,7 +65,7 @@ tip="${arm_id}_joint8" /> - +