Merge branch 'new_descriptions' into 'devel'

New descriptions See merge request gepetto/example-robot-data!21

Merge branch 'new_descriptions' into 'devel'
New descriptions See merge request gepetto/example-robot-data!21
a920b253 · Carlos Mastalli · 8884b6b7 · 8a2da0df · a920b253 · a920b253
Commit a920b253 authored Nov 13, 2019 by Carlos Mastalli
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -50,4 +50,6 @@ IF(NOT INSTALL_PYTHON_INTERFACE_ONLY)
  INSTALL(DIRECTORY tiago_description DESTINATION share/${PROJECT_NAME})
  INSTALL(DIRECTORY ur_description DESTINATION share/${PROJECT_NAME})
  INSTALL(DIRECTORY romeo_description DESTINATION share/${PROJECT_NAME})
+  INSTALL(DIRECTORY hector_description DESTINATION share/${PROJECT_NAME})
+  INSTALL(DIRECTORY double_pendulum_description DESTINATION share/${PROJECT_NAME})
 ENDIF(NOT INSTALL_PYTHON_INTERFACE_ONLY)
--- a/double_pendulum_description/meshes/base_link.STL
+++ b/double_pendulum_description/meshes/base_link.STL
--- a/double_pendulum_description/meshes/link1.STL
+++ b/double_pendulum_description/meshes/link1.STL
--- a/double_pendulum_description/meshes/link2.STL
+++ b/double_pendulum_description/meshes/link2.STL
--- a/double_pendulum_description/urdf/double_pendulum.urdf
+++ b/double_pendulum_description/urdf/double_pendulum.urdf
+<?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://2dof_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://2dof_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://2dof_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://2dof_description/meshes/link1.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint1"
+    type="revolute">
+    <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://2dof_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://2dof_description/meshes/link2.STL" />
+      </geometry>
+    </collision>
+  </link>
+  <joint
+    name="joint2"
+    type="revolute">
+    <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>
--- a/hector_description/meshes/quadrotor/quadrotor_base.dae
+++ b/hector_description/meshes/quadrotor/quadrotor_base.dae
--- a/hector_description/meshes/quadrotor/quadrotor_base.stl
+++ b/hector_description/meshes/quadrotor/quadrotor_base.stl
--- a/hector_description/package.xml
+++ b/hector_description/package.xml
+<package>
+  <name>hector-description</name>
+  <version>0.3.5</version>
+  <description>hector-description provides an URDF model of a quadrotor UAV.</description>
+  <maintainer email="meyer@fsr.tu-darmstadt.de">Johannes Meyer</maintainer>
+
+  <license>BSD</license>
+
+  <url type="website">http://ros.org/wiki/hector_quadrotor_description</url>
+  <url type="bugtracker">https://github.com/tu-darmstadt-ros-pkg/hector_quadrotor/issues</url>
+
+  <author email="meyer@fsr.tu-darmstadt.de">Johannes Meyer</author>
+  <author email="kohlbrecher@sim.tu-darmstadt.de">Stefan Kohlbrecher</author>
+
+  <!-- Dependencies which this package needs to build itself. -->
+  <buildtool_depend>catkin</buildtool_depend>
+
+  <!-- Dependencies needed to compile this package. -->
+
+  <!-- Dependencies needed after this package is compiled. -->
+  <run_depend>hector_sensors_description</run_depend>
+
+  <!-- Dependencies needed only for running tests. -->
+
+</package>
--- a/hector_description/robots/quadrotor_base.urdf
+++ b/hector_description/robots/quadrotor_base.urdf
+<?xml version="1.0" ?>
+<!-- =================================================================================== -->
+<!-- |    This document was autogenerated by xacro from quadrotor_base.xacro           | -->
+<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
+<!-- =================================================================================== -->
+<robot name="hector" xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <link name="base_link">
+    <inertial>
+      <mass value="1.477"/>
+      <origin xyz="0 0 0"/>
+      <inertia ixx="0.01152" ixy="0.0" ixz="0.0" iyy="0.01152" iyz="0.0" izz="0.0218"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hector_description/meshes/quadrotor/quadrotor_base.dae"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="package://hector_description/meshes/quadrotor/quadrotor_base.stl"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>
--- a/hector_description/robots/quadrotor_base.urdf.xacro
+++ b/hector_description/robots/quadrotor_base.urdf.xacro
+<?xml version="1.0"?>
+
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <xacro:property name="pi" value="3.1415926535897931" />
+
+  <!-- Main quadrotor link -->
+  <xacro:macro name="quadrotor_base_macro">
+    <link name="base_link">
+      <inertial>
+        <mass value="1.477" />
+        <origin xyz="0 0 0" />
+        <inertia ixx="0.01152" ixy="0.0" ixz="0.0" iyy="0.01152" iyz="0.0" izz="0.0218" />
+      </inertial>
+
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://hector-description/meshes/quadrotor/quadrotor_base.dae"/>
+        </geometry>
+      </visual>
+
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0" />
+        <geometry>
+          <mesh filename="package://hector-description/meshes/quadrotor/quadrotor_base.stl"/>
+        </geometry>
+      </collision>
+    </link>
+
+  </xacro:macro>
+</robot>
--- a/hector_description/robots/quadrotor_base.xacro
+++ b/hector_description/robots/quadrotor_base.xacro
+<?xml version="1.0" ?>
+<robot name="hector" xmlns:xacro="http://www.ros.org/wiki/xacro">
+  <xacro:include filename="package://hector-description/urdf/quadrotor_base.urdf.xacro"/>
+  <xacro:quadrotor_base_macro/>
+</robot>
--- a/python/example_robot_data/__init__.py
+++ b/python/example_robot_data/__init__.py
 # flake8: noqa
 from .robots_loader import (getModelPath, loadANYmal, loadHyQ, loadICub, loadKinova, loadRomeo, loadSolo, loadTalos,
-                            loadTalosArm, loadTalosLegs, loadTiago, loadTiagoNoHand, loadUR, readParamsFromSrdf)
+                            loadTalosArm, loadTalosLegs, loadTiago, loadTiagoNoHand, loadUR, readParamsFromSrdf, loadHector, loadDoublePendulum)
--- a/python/example_robot_data/__main__.py
+++ b/python/example_robot_data/__main__.py
@@ -3,8 +3,9 @@ from argparse import ArgumentParser
 from . import robots_loader

 ROBOTS = [
-    'anymal', 'anymal_kinova', 'hyq', 'solo', 'solo12', 'talos', 'talos_arm', 'talos_legs', 'kinova', 'tiago',
-    'tiago_no_hand', 'icub', 'ur5', 'romeo'
+    'anymal', 'anymal_kinova', 'hyq', 'solo', 'solo12', 'talos', 'talos_arm',
+    'talos_legs', 'kinova', 'tiago', 'tiago_no_hand', 'icub', 'ur5', 'romeo',
+    'hector', 'double_pendulum'
 ]

 parser = ArgumentParser()
@@ -81,3 +82,13 @@ elif args.robot == 'romeo':
    romeo = robots_loader.loadRomeo()
    romeo.initViewer(loadModel=True)
    romeo.display(romeo.q0)
+
+if args.robot == 'hector':
+    hector = robots_loader.loadHector()
+    hector.initViewer(loadModel=True)
+    hector.display(hector.q0)
+
+if args.robot == 'double_pendulum':
+    planar2dof = robots_loader.load2dof()
+    planar2dof.initViewer(loadModel=True)
+    planar2dof.display(planar2dof.q0)
--- a/python/example_robot_data/robots_loader.py
+++ b/python/example_robot_data/robots_loader.py
@@ -8,8 +8,10 @@ from pinocchio.robot_wrapper import RobotWrapper

 def getModelPath(subpath, printmsg=False):
    base = '../../../share/example-robot-data'
-    for path in [dirname(dirname(dirname(dirname(__file__)))),
-                 dirname(dirname(dirname(__file__)))] + [join(p, base.strip('/')) for p in sys.path]:
+    for path in [
+            dirname(dirname(dirname(dirname(__file__)))),
+            dirname(dirname(dirname(__file__)))
+    ] + [join(p, base.strip('/')) for p in sys.path]:
        if exists(join(path, subpath.strip('/'))):
            if printmsg:
                print("using %s as modelPath" % path)
@@ -17,12 +19,17 @@ def getModelPath(subpath, printmsg=False):
    raise IOError('%s not found' % (subpath))


-def readParamsFromSrdf(robot, SRDF_PATH, verbose, has_rotor_parameters=True, referencePose='half_sitting'):
+def readParamsFromSrdf(robot,
+                       SRDF_PATH,
+                       verbose,
+                       has_rotor_parameters=True,
+                       referencePose='half_sitting'):
    rmodel = robot.model

    if has_rotor_parameters:
        pinocchio.loadRotorParameters(rmodel, SRDF_PATH, verbose)
-    rmodel.armature = np.multiply(rmodel.rotorInertia.flat, np.square(rmodel.rotorGearRatio.flat))
+    rmodel.armature = np.multiply(rmodel.rotorInertia.flat,
+                                  np.square(rmodel.rotorGearRatio.flat))
    pinocchio.loadReferenceConfigurations(rmodel, SRDF_PATH, verbose)
    robot.q0.flat[:] = rmodel.referenceConfigurations[referencePose].copy()
    return
@@ -52,9 +59,14 @@ def loadANYmal(withArm=None):
    SRDF_SUBPATH = "/anymal_b_simple_description/srdf/" + SRDF_FILENAME
    modelPath = getModelPath(URDF_SUBPATH)
    # Load URDF file
-    robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
+    robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath],
+                                       pinocchio.JointModelFreeFlyer())
    # Load SRDF file
-    readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False, False, referencePose=REF_POSTURE)
+    readParamsFromSrdf(robot,
+                       modelPath + SRDF_SUBPATH,
+                       False,
+                       False,
+                       referencePose=REF_POSTURE)
    # Add the free-flyer joint limits
    addFreeFlyerJointLimits(robot)
    return robot
@@ -81,7 +93,8 @@ def loadTalos():
    URDF_SUBPATH = "/talos_data/robots/" + URDF_FILENAME
    modelPath = getModelPath(URDF_SUBPATH)
    # Load URDF file
-    robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
+    robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath],
+                                       pinocchio.JointModelFreeFlyer())
    # Load SRDF file
    readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False)
    assert ((robot.model.armature[:6] == 0.).all())
@@ -101,13 +114,17 @@ def loadTalosLegs():
    legMaxId = 14
    m1 = robot.model
    m2 = pinocchio.Model()
-    for j, M, name, parent, Y in zip(m1.joints, m1.jointPlacements, m1.names, m1.parents, m1.inertias):
+    for j, M, name, parent, Y in zip(m1.joints, m1.jointPlacements, m1.names,
+                                     m1.parents, m1.inertias):
        if j.id < legMaxId:
-            jid = m2.addJoint(parent, getattr(pinocchio, j.shortname())(), M, name)
+            jid = m2.addJoint(parent,
+                              getattr(pinocchio, j.shortname())(), M, name)
            up = m2.upperPositionLimit
            down = m2.lowerPositionLimit
-            up[m2.joints[jid].idx_q:m2.joints[jid].idx_q + j.nq] = m1.upperPositionLimit[j.idx_q:j.idx_q + j.nq]
-            down[m2.joints[jid].idx_q:m2.joints[jid].idx_q + j.nq] = m1.lowerPositionLimit[j.idx_q:j.idx_q + j.nq]
+            up[m2.joints[jid].idx_q:m2.joints[jid].idx_q +
+               j.nq] = m1.upperPositionLimit[j.idx_q:j.idx_q + j.nq]
+            down[m2.joints[jid].idx_q:m2.joints[jid].idx_q +
+                 j.nq] = m1.lowerPositionLimit[j.idx_q:j.idx_q + j.nq]
            m2.upperPositionLimit = up
            m2.lowerPositionLimit = down
            assert (jid == j.id)
@@ -153,9 +170,11 @@ def loadHyQ():
    URDF_SUBPATH = "/hyq_description/robots/" + URDF_FILENAME
    modelPath = getModelPath(URDF_SUBPATH)
    # Load URDF file
-    robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
+    robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath],
+                                       pinocchio.JointModelFreeFlyer())
    # Load SRDF file
-    readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False, False, "standing")
+    readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False, False,
+                       "standing")
    # Add the free-flyer joint limits
    addFreeFlyerJointLimits(robot)
    return robot
@@ -171,9 +190,11 @@ def loadSolo(solo=True):
    URDF_SUBPATH = "/solo_description/robots/" + URDF_FILENAME
    modelPath = getModelPath(URDF_SUBPATH)
    # Load URDF file
-    robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
+    robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath],
+                                       pinocchio.JointModelFreeFlyer())
    # Load SRDF file
-    readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False, False, "standing")
+    readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False, False,
+                       "standing")
    # Add the free-flyer joint limits
    addFreeFlyerJointLimits(robot)
    return robot
@@ -228,7 +249,8 @@ def loadICub(reduced=True):
    URDF_SUBPATH = "/icub_description/robots/" + URDF_FILENAME
    modelPath = getModelPath(URDF_SUBPATH)
    # Load URDF file
-    robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
+    robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath],
+                                       pinocchio.JointModelFreeFlyer())
    # Load SRDF file
    readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False, False)
    # Add the free-flyer joint limits
@@ -238,14 +260,33 @@ def loadICub(reduced=True):

 def loadUR(robot=5, limited=False, gripper=False):
    assert (not (gripper and (robot == 10 or limited)))
-    URDF_FILENAME = "ur%i%s_%s.urdf" % (robot, "_joint_limited" if limited else '', 'gripper' if gripper else 'robot')
+    URDF_FILENAME = "ur%i%s_%s.urdf" % (robot, "_joint_limited" if limited else
+                                        '', 'gripper' if gripper else 'robot')
    URDF_SUBPATH = "/ur_description/urdf/" + URDF_FILENAME
    modelPath = getModelPath(URDF_SUBPATH)
    return RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath])


+def loadHector():
+    URDF_FILENAME = "quadrotor_base.urdf"
+    URDF_SUBPATH = "/hector_description/robots/" + URDF_FILENAME
+    modelPath = getModelPath(URDF_SUBPATH)
+    robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath],
+                                       pinocchio.JointModelFreeFlyer())
+    return robot
+
+
+def loadDoublePendulum():
+    URDF_FILENAME = "double_pendulum.urdf"
+    URDF_SUBPATH = "/double_pendulum_description/urdf/" + URDF_FILENAME
+    modelPath = getModelPath(URDF_SUBPATH)
+    robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath])
+    return robot
+
+
 def loadRomeo():
    URDF_FILENAME = "romeo.urdf"
    URDF_SUBPATH = "/romeo_description/urdf/" + URDF_FILENAME
    modelPath = getModelPath(URDF_SUBPATH)
-    return RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
+    return RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath],
+                                      pinocchio.JointModelFreeFlyer())