Commit a920b253 authored by Carlos Mastalli's avatar Carlos Mastalli
Browse files

Merge branch 'new_descriptions' into 'devel'

New descriptions

See merge request gepetto/example-robot-data!21
parents 8884b6b7 8a2da0df
Pipeline #6800 passed with stage
in 2 minutes and 52 seconds
......@@ -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)
<?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>
This diff is collapsed.
<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>
<?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>
<?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>
<?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>
# 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)
......@@ -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)
......@@ -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())
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment