From 1f2ca85f592f916ae93f0da5d38c839001ed138a Mon Sep 17 00:00:00 2001
From: Wilson Jallet <ManifoldFR@users.noreply.github.com>
Date: Thu, 17 Feb 2022 15:27:20 +0100
Subject: [PATCH] Add continuous (SO2) variant of double_pendulum (#111)
* Make double_pendulum's joints unbounded revolute (continuous)
* Add continuous (SO(2)) variant of double_pendulum
* Remove update to deprecated function; inherit from DoublePendulumLoader
* Add unit test
---
python/example_robot_data/robots_loader.py | 5 +
.../urdf/double_pendulum_continuous.urdf | 167 ++++++++++++++++++
unittest/test_load.py | 3 +
3 files changed, 175 insertions(+)
create mode 100644 robots/double_pendulum_description/urdf/double_pendulum_continuous.urdf
diff --git a/python/example_robot_data/robots_loader.py b/python/example_robot_data/robots_loader.py
index 018e317..d26b04f 100644
--- a/python/example_robot_data/robots_loader.py
+++ b/python/example_robot_data/robots_loader.py
@@ -483,6 +483,10 @@ class DoublePendulumLoader(RobotLoader):
urdf_subpath = "urdf"
+class DoublePendulumContinuousLoader(DoublePendulumLoader):
+ urdf_filename = "double_pendulum_continuous.urdf"
+
+
def loadDoublePendulum():
warnings.warn(_depr_msg('loadDoublePendulum()', 'double_pendulum'), FutureWarning, 2)
return DoublePendulumLoader().robot
@@ -531,6 +535,7 @@ ROBOTS = {
'baxter': BaxterLoader,
'cassie': CassieLoader,
'double_pendulum': DoublePendulumLoader,
+ 'double_pendulum_continuous': DoublePendulumContinuousLoader,
'hector': HectorLoader,
'hyq': HyQLoader,
'icub': ICubLoader,
diff --git a/robots/double_pendulum_description/urdf/double_pendulum_continuous.urdf b/robots/double_pendulum_description/urdf/double_pendulum_continuous.urdf
new file mode 100644
index 0000000..97dc683
--- /dev/null
+++ b/robots/double_pendulum_description/urdf/double_pendulum_continuous.urdf
@@ -0,0 +1,167 @@
+<?xml version="1.0" encoding="utf-8"?>
+<!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com)
+ Commit Version: 1.5.1-0-g916b5db Build Version: 1.5.7152.31018
+ For more information, please see http://wiki.ros.org/sw_urdf_exporter -->
+<robot
+ name="2dof_planar">
+ <link
+ name="base_link">
+ <inertial>
+ <origin
+ xyz="0.0030299 2.6279E-13 0.02912"
+ rpy="0 0 0" />
+ <mass
+ value="0.10159" />
+ <inertia
+ ixx="2.636E-05"
+ ixy="-1.007E-16"
+ ixz="-2.3835E-06"
+ iyy="1.8042E-05"
+ iyz="-4.6617E-09"
+ izz="1.8795E-05" />
+ </inertial>
+ <visual>
+ <origin
+ xyz="0 0 0"
+ rpy="0 0 0" />
+ <geometry>
+ <mesh
+ filename="package://example-robot-data/robots/double_pendulum_description/meshes/base_link.stl" />
+ </geometry>
+ <material
+ name="">
+ <color
+ rgba="0.96078 1 0 1" />
+ </material>
+ </visual>
+ <collision>
+ <origin
+ xyz="0 0 0"
+ rpy="0 0 0" />
+ <geometry>
+ <mesh
+ filename="package://example-robot-data/robots/double_pendulum_description/meshes/base_link.stl" />
+ </geometry>
+ </collision>
+ </link>
+ <link
+ name="link1">
+ <inertial>
+ <origin
+ xyz="0.0086107 2.1727E-06 0.036012"
+ rpy="0 0 0" />
+ <mass
+ value="0.26703" />
+ <inertia
+ ixx="0.00040827"
+ ixy="1.2675E-09"
+ ixz="1.8738E-05"
+ iyy="0.00038791"
+ iyz="3.5443E-08"
+ izz="3.6421E-05" />
+ </inertial>
+ <visual>
+ <origin
+ xyz="0 0 0"
+ rpy="0 0 0" />
+ <geometry>
+ <mesh
+ filename="package://example-robot-data/robots/double_pendulum_description/meshes/link1.stl" />
+ </geometry>
+ <material
+ name="">
+ <color
+ rgba="0.64706 0.61961 0.58824 1" />
+ </material>
+ </visual>
+ <collision>
+ <origin
+ xyz="0 0 0"
+ rpy="0 0 0" />
+ <geometry>
+ <mesh
+ filename="package://example-robot-data/robots/double_pendulum_description/meshes/link1.stl" />
+ </geometry>
+ </collision>
+ </link>
+ <joint
+ name="joint1"
+ type="continuous">
+ <origin
+ xyz="0.0060872 0 0.035"
+ rpy="0 0 0" />
+ <parent
+ link="base_link" />
+ <child
+ link="link1" />
+ <axis
+ xyz="1 0 0" />
+ <limit
+ lower="0"
+ upper="0"
+ effort="0"
+ velocity="0" />
+ <dynamics
+ damping="0.05" />
+ </joint>
+ <link
+ name="link2">
+ <inertial>
+ <origin
+ xyz="-0.0050107 1.9371E-10 0.10088"
+ rpy="0 0 0" />
+ <mass
+ value="0.33238" />
+ <inertia
+ ixx="0.0011753"
+ ixy="-3.854E-13"
+ ixz="-2.9304E-08"
+ iyy="0.0011666"
+ iyz="-5.2365E-12"
+ izz="1.4553E-05" />
+ </inertial>
+ <visual>
+ <origin
+ xyz="0 0 0"
+ rpy="0 0 0" />
+ <geometry>
+ <mesh
+ filename="package://example-robot-data/robots/double_pendulum_description/meshes/link2.stl" />
+ </geometry>
+ <material
+ name="">
+ <color
+ rgba="0.64706 0.61961 0.58824 1" />
+ </material>
+ </visual>
+ <collision>
+ <origin
+ xyz="0 0 0"
+ rpy="0 0 0" />
+ <geometry>
+ <mesh
+ filename="package://example-robot-data/robots/double_pendulum_description/meshes/link2.stl" />
+ </geometry>
+ </collision>
+ </link>
+ <joint
+ name="joint2"
+ type="continuous">
+ <origin
+ xyz="0.023 0 0.1"
+ rpy="0 0 0" />
+ <parent
+ link="link1" />
+ <child
+ link="link2" />
+ <axis
+ xyz="1 0 0" />
+ <limit
+ lower="0"
+ upper="0"
+ effort="0"
+ velocity="0" />
+ <dynamics
+ damping="0.05" />
+ </joint>
+</robot>
diff --git a/unittest/test_load.py b/unittest/test_load.py
index 233d5bf..7069931 100755
--- a/unittest/test_load.py
+++ b/unittest/test_load.py
@@ -53,6 +53,9 @@ class RobotTestCase(unittest.TestCase):
def test_double_pendulum(self):
self.check('double_pendulum', 2, 2)
+ def test_double_pendulum_continuous(self):
+ self.check('double_pendulum_continuous', 4, 2)
+
def test_hector(self):
self.check('hector', 7, 6)
--
GitLab