From e3d5cfa75112f44c6e3f661fefa44f9e2dff664b Mon Sep 17 00:00:00 2001
From: Rohan Budhiraja <12423606+proyan@users.noreply.github.com>
Date: Fri, 1 Apr 2022 14:51:45 +0200
Subject: [PATCH] cassie/cassie_v2 sdf: remove first primatic joint, add color
information (#121)
* cassie/cassie_v2 sdf: remove first primatic joint, add color information
* robots_loader: use updated sdf parser for pin version > 2.9.1
* cassie srdf: update joint info, add missing joints
Co-authored-by: Rohan Budhiraja <proyan@users.noreply.github.com>
---
python/example_robot_data/robots_loader.py | 37 +++++++--
robots/cassie_description/robots/cassie.sdf | 82 ++++++++++++++++---
.../cassie_description/robots/cassie_v2.sdf | 12 ---
robots/cassie_description/srdf/cassie_v2.srdf | 80 ++++++++++++------
4 files changed, 156 insertions(+), 55 deletions(-)
diff --git a/python/example_robot_data/robots_loader.py b/python/example_robot_data/robots_loader.py
index ca04140..15eabd6 100644
--- a/python/example_robot_data/robots_loader.py
+++ b/python/example_robot_data/robots_loader.py
@@ -43,6 +43,7 @@ def readParamsFromSrdf(model, SRDF_PATH, verbose=False, has_rotor_parameters=Tru
q0 = pin.neutral(model)
if referencePose is not None:
q0 = model.referenceConfigurations[referencePose].copy()
+ q0 = pin.normalize(model, q0)
return q0
@@ -51,6 +52,8 @@ class RobotLoader(object):
urdf_filename = ''
srdf_filename = ''
sdf_filename = ''
+ sdf_root_link_name = ''
+ sdf_parent_guidance = []
urdf_subpath = 'robots'
srdf_subpath = 'srdf'
sdf_subpath = ''
@@ -66,17 +69,30 @@ class RobotLoader(object):
raise AttributeError("Please choose between URDF *or* SDF")
df_path = join(self.path, self.urdf_subpath, self.urdf_filename)
builder = RobotWrapper.BuildFromURDF
+ if self.model_path is None:
+ self.model_path = getModelPath(df_path, self.verbose)
+ self.df_path = join(self.model_path, df_path)
+ self.robot = builder(self.df_path, [join(self.model_path, '../..')],
+ pin.JointModelFreeFlyer() if self.free_flyer else None)
else:
df_path = join(self.path, self.sdf_subpath, self.sdf_filename)
try:
builder = RobotWrapper.BuildFromSDF
+ if self.model_path is None:
+ self.model_path = getModelPath(df_path, self.verbose)
+ self.df_path = join(self.model_path, df_path)
+ if tuple(int(i) for i in pin.__version__.split('.')) > (2, 9, 1):
+ self.robot = builder(self.df_path,
+ package_dirs=[join(self.model_path, '../..')],
+ root_joint=pin.JointModelFreeFlyer() if self.free_flyer else None,
+ root_link_name=self.sdf_root_link_name,
+ parent_guidance=self.sdf_parent_guidance)
+ else:
+ self.robot = builder(self.df_path,
+ package_dirs=[join(self.model_path, '../..')],
+ root_joint=pin.JointModelFreeFlyer() if self.free_flyer else None)
except AttributeError:
raise ImportError("Building SDF models require pinocchio >= 3.0.0")
- if self.model_path is None:
- self.model_path = getModelPath(df_path, self.verbose)
- self.df_path = join(self.model_path, df_path)
- self.robot = builder(self.df_path, [join(self.model_path, '../..')],
- pin.JointModelFreeFlyer() if self.free_flyer else None)
if self.srdf_filename:
self.srdf_path = join(self.model_path, self.path, self.srdf_subpath, self.srdf_filename)
@@ -161,11 +177,20 @@ def loadANYmal(withArm=None):
class CassieLoader(RobotLoader):
path = 'cassie_description'
- sdf_filename = "cassie_v2.sdf"
+ if tuple(int(i) for i in pin.__version__.split('.')) > (2, 9, 1):
+ sdf_filename = "cassie.sdf"
+ else:
+ sdf_filename = "cassie_v2.sdf"
sdf_subpath = 'robots'
srdf_filename = "cassie_v2.srdf"
ref_posture = "standing"
free_flyer = True
+ sdf_root_link_name = "pelvis"
+ sdf_parent_guidance = [
+ "left-roll-op", "left-yaw-op", "left-pitch-op", "left-knee-op", "left-tarsus-spring-joint", "left-foot-op",
+ "right-roll-op", "right-yaw-op", "right-pitch-op", "right-knee-op", "right-tarsus-spring-joint",
+ "right-foot-op"
+ ]
class TalosLoader(RobotLoader):
diff --git a/robots/cassie_description/robots/cassie.sdf b/robots/cassie_description/robots/cassie.sdf
index 68ec25b..8436b9f 100644
--- a/robots/cassie_description/robots/cassie.sdf
+++ b/robots/cassie_description/robots/cassie.sdf
@@ -38,6 +38,9 @@
<uri>model://example-robot-data/robots/cassie_description/meshes/pelvis.stl</uri>
</mesh>
</geometry>
+ <material>
+ <ambient>0.35 0.35 0.35 1</ambient>
+ </material>
</visual>
<collision name="pelvis_collision">
<pose frame="">0.02 0 0.04 0 0 0</pose>
@@ -69,6 +72,9 @@
<uri>model://example-robot-data/robots/cassie_description/meshes/hip-roll.stl</uri>
</mesh>
</geometry>
+ <material>
+ <ambient>0.75 0.75 0.75 1</ambient>
+ </material>
</visual>
</link>
@@ -156,6 +162,9 @@
<uri>model://example-robot-data/robots/cassie_description/meshes/hip-yaw.stl</uri>
</mesh>
</geometry>
+ <material>
+ <ambient>0.75 0.75 0.75 1</ambient>
+ </material>
</visual>
</link>
@@ -243,6 +252,9 @@
<uri>model://example-robot-data/robots/cassie_description/meshes/hip-pitch.stl</uri>
</mesh>
</geometry>
+ <material>
+ <ambient>0.75 0.75 0.75 1</ambient>
+ </material>
</visual>
<collision name="left-hip-pitch_collision">
<pose frame="">0.053 0.0 -0.0379 0 1.57 0</pose>
@@ -339,6 +351,9 @@
<uri>model://example-robot-data/robots/cassie_description/meshes/achilles-rod.stl</uri>
</mesh>
</geometry>
+ <material>
+ <ambient>0.75 0.75 0.75 1</ambient>
+ </material>
</visual>
</link>
@@ -374,6 +389,9 @@
<uri>model://example-robot-data/robots/cassie_description/meshes/knee.stl</uri>
</mesh>
</geometry>
+ <material>
+ <ambient>0.75 0.75 0.75 1</ambient>
+ </material>
</visual>
</link>
@@ -493,6 +511,9 @@
<uri>model://example-robot-data/robots/cassie_description/meshes/shin.stl</uri>
</mesh>
</geometry>
+ <material>
+ <ambient>0.35 0.35 0.35 1</ambient>
+ </material>
</visual>
<collision name="left-shin_collision">
<pose frame="">0.22 0.01 0.0 0 1.57 0</pose>
@@ -543,6 +564,9 @@
<uri>model://example-robot-data/robots/cassie_description/meshes/tarsus.stl</uri>
</mesh>
</geometry>
+ <material>
+ <ambient>0.35 0.35 0.35 1</ambient>
+ </material>
</visual>
<collision name="left-tarsus_collision">
<pose frame="">0.21 -0.03 0.0 0 1.57 0</pose>
@@ -591,6 +615,9 @@
<uri>model://example-robot-data/robots/cassie_description/meshes/heel-spring.stl</uri>
</mesh>
</geometry>
+ <material>
+ <ambient>0.35 0.35 0.35 1</ambient>
+ </material>
</visual>
</link>
@@ -628,6 +655,9 @@
<uri>model://example-robot-data/robots/cassie_description/meshes/foot-crank.stl</uri>
</mesh>
</geometry>
+ <material>
+ <ambient>0.35 0.35 0.35 1</ambient>
+ </material>
</visual>
</link>
@@ -667,6 +697,9 @@
<uri>model://example-robot-data/robots/cassie_description/meshes/plantar-rod.stl</uri>
</mesh>
</geometry>
+ <material>
+ <ambient>0.75 0.75 0.75 1</ambient>
+ </material>
</visual>
</link>
@@ -696,9 +729,13 @@
<visual name="left-foot_visual">
<geometry>
<mesh>
+ <scale>1 1 1</scale>
<uri>model://example-robot-data/robots/cassie_description/meshes/foot.stl</uri>
</mesh>
</geometry>
+ <material>
+ <ambient>0.75 0.75 0.75 1</ambient>
+ </material>
</visual>
<collision name="left-foot_collision">
<pose frame="">0.01 0.045 0 0 1.57 -0.695</pose>
@@ -803,6 +840,9 @@
<uri>model://example-robot-data/robots/cassie_description/meshes/hip-roll.stl</uri>
</mesh>
</geometry>
+ <material>
+ <ambient>0.75 0.75 0.75 1</ambient>
+ </material>
</visual>
</link>
@@ -891,6 +931,9 @@
<uri>model://example-robot-data/robots/cassie_description/meshes/hip-yaw.stl</uri>
</mesh>
</geometry>
+ <material>
+ <ambient>0.75 0.75 0.75 1</ambient>
+ </material>
</visual>
</link>
@@ -979,6 +1022,9 @@
<uri>model://example-robot-data/robots/cassie_description/meshes/hip-pitch.stl</uri>
</mesh>
</geometry>
+ <material>
+ <ambient>0.75 0.75 0.75 1</ambient>
+ </material>
</visual>
<collision name="right-hip-pitch_collision">
<pose frame="">0.053 0.0 0.0379 0 1.57 0</pose>
@@ -1076,6 +1122,9 @@
<uri>model://example-robot-data/robots/cassie_description/meshes/achilles-rod.stl</uri>
</mesh>
</geometry>
+ <material>
+ <ambient>0.75 0.75 0.75 1</ambient>
+ </material>
</visual>
</link>
@@ -1112,6 +1161,9 @@
<uri>model://example-robot-data/robots/cassie_description/meshes/knee.stl</uri>
</mesh>
</geometry>
+ <material>
+ <ambient>0.75 0.75 0.75 1</ambient>
+ </material>
</visual>
</link>
@@ -1234,6 +1286,9 @@
<uri>model://example-robot-data/robots/cassie_description/meshes/shin.stl</uri>
</mesh>
</geometry>
+ <material>
+ <ambient>0.35 0.35 0.35 1</ambient>
+ </material>
</visual>
<collision name="right-shin_collision">
<pose frame="">0.22 0.01 0.0 0 1.57 0</pose>
@@ -1285,6 +1340,9 @@
<uri>model://example-robot-data/robots/cassie_description/meshes/tarsus.stl</uri>
</mesh>
</geometry>
+ <material>
+ <ambient>0.35 0.35 0.35 1</ambient>
+ </material>
</visual>
<collision name="right-tarsus_collision">
<pose frame="">0.21 -0.03 0.0 0 1.57 0</pose>
@@ -1334,6 +1392,9 @@
<uri>model://example-robot-data/robots/cassie_description/meshes/heel-spring.stl</uri>
</mesh>
</geometry>
+ <material>
+ <ambient>0.35 0.35 0.35 1</ambient>
+ </material>
</visual>
</link>
@@ -1371,6 +1432,9 @@
<uri>model://example-robot-data/robots/cassie_description/meshes/foot-crank.stl</uri>
</mesh>
</geometry>
+ <material>
+ <ambient>0.35 0.35 0.35 1</ambient>
+ </material>
</visual>
</link>
@@ -1411,6 +1475,9 @@
<uri>model://example-robot-data/robots/cassie_description/meshes/plantar-rod.stl</uri>
</mesh>
</geometry>
+ <material>
+ <ambient>0.75 0.75 0.75 1</ambient>
+ </material>
</visual>
</link>
@@ -1444,6 +1511,9 @@
<uri>model://example-robot-data/robots/cassie_description/meshes/foot.stl</uri>
</mesh>
</geometry>
+ <material>
+ <ambient>0.75 0.75 0.75 1</ambient>
+ </material>
</visual>
<collision name="right-foot_collision">
<pose frame="">0.01 0.045 0 0 1.57 -0.695</pose>
@@ -1587,18 +1657,6 @@
</axis>
</joint>
- <joint name="static" type="prismatic">
- <parent>world</parent>
- <child>pelvis</child>
- <axis>
- <xyz>0 0 1</xyz>
- <limit>
- <lower>0</lower>
- <upper>0.2</upper>
- </limit>
- </axis>
- </joint>
-
<!-- Attach the plugin to this model -->
<plugin filename="libcassie_plugin.so" name="CassiePlugin">
</plugin>
diff --git a/robots/cassie_description/robots/cassie_v2.sdf b/robots/cassie_description/robots/cassie_v2.sdf
index 79d17b7..3c5c7f5 100644
--- a/robots/cassie_description/robots/cassie_v2.sdf
+++ b/robots/cassie_description/robots/cassie_v2.sdf
@@ -1067,18 +1067,6 @@
</axis>
</joint>
- <joint name="static" type="prismatic">
- <parent>world</parent>
- <child>pelvis</child>
- <axis>
- <xyz>0 0 1</xyz>
- <limit>
- <lower>0</lower>
- <upper>0.2</upper>
- </limit>
- </axis>
- </joint>
-
<!-- Attach the plugin to this model -->
<plugin filename="libcassie_plugin.so" name="CassiePlugin">
</plugin>
diff --git a/robots/cassie_description/srdf/cassie_v2.srdf b/robots/cassie_description/srdf/cassie_v2.srdf
index 22c20e5..53f843a 100644
--- a/robots/cassie_description/srdf/cassie_v2.srdf
+++ b/robots/cassie_description/srdf/cassie_v2.srdf
@@ -27,29 +27,59 @@
<joint name="right-foot-op" />
</group>
- <group_state name="standing" group="whole_body">
- <joint name="root_joint" value="-0.00992617 -0.00004808 1.02458068 0.0 0.0 0.0 1.0" />
- <joint name="left-roll-op" value="-0.00513955" />
- <joint name="left-yaw-op" value="0.00459357" />
- <joint name="left-pitch-op" value="0.59575303" />
- <joint name="left-knee-op" value="0.03162615" />
- <joint name="left-knee-shin-joint" value="-0.51390881" />
- <joint name="left-shin-tarsus-joint" value="0.25810122" />
- <joint name="left-tarsus-spring-joint" value="0.53679290" />
- <joint name="left-achilles-spring-joint" value="-0.79073525" />
- <joint name="left-tarsus-crank-joint" value="-1.44545535" />
- <joint name="left-crank-rod-joint" value="1.44544632" />
- <joint name="left-foot-op" value="-1.44548599" />
- <joint name="right-roll-op" value="0.00529991" />
- <joint name="right-yaw-op" value=" 0.00443722" />
- <joint name="right-pitch-op" value="0.59614815" />
- <joint name="right-knee-op" value="0.03053357" />
- <joint name="right-knee-shin-joint" value="-0.51316503" />
- <joint name="right-shin-tarsus-joint" value="0.25804492" />
- <joint name="right-tarsus-spring-joint" value="0.53716726" />
- <joint name="right-achilles-spring-joint"value="-0.79113522" />
- <joint name="right-tarsus-crank-joint" value="-1.46635249" />
- <joint name="right-crank-rod-joint" value="1.46950485" />
- <joint name="right-foot-op" value="-1.44610592" />
- </group_state>
+
+
+<group_state name="standing" group="whole_body">
+ <joint name="root_joint" value="-5.27093838e-01 -2.76381629e-04 9.39151616e-01 9.61380040e-06 9.38217754e-04 2.14890379e-05 9.99999560e-01"/>
+ <joint name="left-roll-ip" value="-0.01295769"/>
+ <joint name="left-roll-joint" value="0.01295123"/>
+ <joint name="left-yaw-ip" value="0.01484168"/>
+ <joint name="left-yaw-joint" value="-0.01488228"/>
+ <joint name="left-pitch-ip" value="1.5890490074366923"/>
+ <joint name="left-pitch-joint" value="-1.39665541"/>
+ <joint name="left-pitch-rod-joint" value=" 0.00604391 0.00460982 -0.0835031 0.99647853"/>
+ <joint name="left-achilles-spring-joint" value="3.4570118e-08"/>
+ <joint name="left-knee-ip" value="1.40316539"/>
+ <joint name="left-knee-joint" value="-1.6305735"/>
+ <joint name="left-knee-spring-joint" value="-5.11663898e-06"/>
+ <joint name="left-knee-shin-joint" value="-5.11663898e-06"/>
+ <joint name="left-shin-tarsus-joint" value="0.6929022"/>
+ <joint name="left-tarsus-spring-joint" value="-0.63338278"/>
+ <joint name="left-tarsus-crank-joint" value="-1.75141719"/>
+ <joint name="left-crank-rod-joint" value="1.75140759"/>
+ <joint name="left-plantar-foot-joint" value=" 1.11717507e-13 -2.82102963e-06 -7.68002342e-01 6.40447034e-01"/>
+ <joint name="left-foot-ip" value="1.42120011"/>
+ <joint name="left-foot-joint" value="-3.17264196"/>
+ <joint name="left-foot-op" value="-1.75144185"/>
+ <joint name="left-shin-spring-joint" value="-1.79084209e-07"/>
+ <joint name="left-knee-op" value="-0.22740812"/>
+ <joint name="left-pitch-op" value="0.19239359743669127"/>
+ <joint name="left-yaw-op" value="-4.059968e-05"/>
+ <joint name="left-roll-op" value="-6.46156049e-06"/>
+ <joint name="right-roll-ip" value="-0.01295208"/>
+ <joint name="right-roll-joint" value="0.01317795"/>
+ <joint name="right-yaw-ip" value="0.01658614"/>
+ <joint name="right-yaw-joint" value="-0.0166263"/>
+ <joint name="right-pitch-ip" value="1.5912251274366866"/>
+ <joint name="right-pitch-joint" value="-1.39653428"/>
+ <joint name="right-pitch-rod-joint" value="-0.00604403 -0.0046093 -0.08350479 0.99647839"/>
+ <joint name="right-achilles-spring-joint" value="-1.63406124e-14"/>
+ <joint name="right-knee-ip" value="1.40275306"/>
+ <joint name="right-knee-joint" value="-1.63016464"/>
+ <joint name="right-knee-spring-joint" value="-7.87499336e-06"/>
+ <joint name="right-knee-shin-joint" value="-7.87499336e-06"/>
+ <joint name="right-shin-tarsus-joint" value="0.69293781"/>
+ <joint name="right-tarsus-spring-joint" value="-0.63341554"/>
+ <joint name="right-tarsus-crank-joint" value="-1.75391974"/>
+ <joint name="right-crank-rod-joint" value="1.75706331"/>
+ <joint name="right-plantar-foot-joint" value="-4.83732315e-14 -2.80375818e-06 -7.63300126e-01 6.46044053e-01"/>
+ <joint name="right-foot-ip" value="1.39961783"/>
+ <joint name="right-foot-joint" value="-3.13328631"/>
+ <joint name="right-foot-op" value="-1.73366848"/>
+ <joint name="right-shin-spring-joint" value="-2.75627693e-07"/>
+ <joint name="right-knee-op" value="-0.22741158"/>
+ <joint name="right-pitch-op" value="0.1946908474366893"/>
+ <joint name="right-yaw-op" value="-4.01635934e-05"/>
+ <joint name="right-roll-op" value="0.00022586"/>
+</group_state>
</robot>
--
GitLab