Commit 9ff10f3d authored by Carlos Mastalli's avatar Carlos Mastalli
Browse files

[cleanup] Removed package of hector robot + formatted the code

parent a920b253
<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>
# flake8: noqa # flake8: noqa
from .robots_loader import (getModelPath, loadANYmal, loadHyQ, loadICub, loadKinova, loadRomeo, loadSolo, loadTalos, from .robots_loader import (getModelPath, loadANYmal, loadHyQ, loadICub, loadKinova, loadRomeo, loadSolo, loadTalos,
loadTalosArm, loadTalosLegs, loadTiago, loadTiagoNoHand, loadUR, readParamsFromSrdf, loadHector, loadDoublePendulum) loadTalosArm, loadTalosLegs, loadTiago, loadTiagoNoHand, loadUR, readParamsFromSrdf,
loadHector, loadDoublePendulum)
...@@ -3,9 +3,8 @@ from argparse import ArgumentParser ...@@ -3,9 +3,8 @@ from argparse import ArgumentParser
from . import robots_loader from . import robots_loader
ROBOTS = [ ROBOTS = [
'anymal', 'anymal_kinova', 'hyq', 'solo', 'solo12', 'talos', 'talos_arm', 'anymal', 'anymal_kinova', 'hyq', 'solo', 'solo12', 'talos', 'talos_arm', 'talos_legs', 'kinova', 'tiago',
'talos_legs', 'kinova', 'tiago', 'tiago_no_hand', 'icub', 'ur5', 'romeo', 'tiago_no_hand', 'icub', 'ur5', 'romeo', 'hector', 'double_pendulum'
'hector', 'double_pendulum'
] ]
parser = ArgumentParser() parser = ArgumentParser()
......
...@@ -8,10 +8,8 @@ from pinocchio.robot_wrapper import RobotWrapper ...@@ -8,10 +8,8 @@ from pinocchio.robot_wrapper import RobotWrapper
def getModelPath(subpath, printmsg=False): def getModelPath(subpath, printmsg=False):
base = '../../../share/example-robot-data' base = '../../../share/example-robot-data'
for path in [ for path in [dirname(dirname(dirname(dirname(__file__)))),
dirname(dirname(dirname(dirname(__file__)))), dirname(dirname(dirname(__file__)))] + [join(p, base.strip('/')) for p in sys.path]:
dirname(dirname(dirname(__file__)))
] + [join(p, base.strip('/')) for p in sys.path]:
if exists(join(path, subpath.strip('/'))): if exists(join(path, subpath.strip('/'))):
if printmsg: if printmsg:
print("using %s as modelPath" % path) print("using %s as modelPath" % path)
...@@ -19,17 +17,12 @@ def getModelPath(subpath, printmsg=False): ...@@ -19,17 +17,12 @@ def getModelPath(subpath, printmsg=False):
raise IOError('%s not found' % (subpath)) raise IOError('%s not found' % (subpath))
def readParamsFromSrdf(robot, def readParamsFromSrdf(robot, SRDF_PATH, verbose, has_rotor_parameters=True, referencePose='half_sitting'):
SRDF_PATH,
verbose,
has_rotor_parameters=True,
referencePose='half_sitting'):
rmodel = robot.model rmodel = robot.model
if has_rotor_parameters: if has_rotor_parameters:
pinocchio.loadRotorParameters(rmodel, SRDF_PATH, verbose) pinocchio.loadRotorParameters(rmodel, SRDF_PATH, verbose)
rmodel.armature = np.multiply(rmodel.rotorInertia.flat, rmodel.armature = np.multiply(rmodel.rotorInertia.flat, np.square(rmodel.rotorGearRatio.flat))
np.square(rmodel.rotorGearRatio.flat))
pinocchio.loadReferenceConfigurations(rmodel, SRDF_PATH, verbose) pinocchio.loadReferenceConfigurations(rmodel, SRDF_PATH, verbose)
robot.q0.flat[:] = rmodel.referenceConfigurations[referencePose].copy() robot.q0.flat[:] = rmodel.referenceConfigurations[referencePose].copy()
return return
...@@ -59,14 +52,9 @@ def loadANYmal(withArm=None): ...@@ -59,14 +52,9 @@ def loadANYmal(withArm=None):
SRDF_SUBPATH = "/anymal_b_simple_description/srdf/" + SRDF_FILENAME SRDF_SUBPATH = "/anymal_b_simple_description/srdf/" + SRDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH) modelPath = getModelPath(URDF_SUBPATH)
# Load URDF file # Load URDF file
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
pinocchio.JointModelFreeFlyer())
# Load SRDF file # Load SRDF file
readParamsFromSrdf(robot, readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False, False, referencePose=REF_POSTURE)
modelPath + SRDF_SUBPATH,
False,
False,
referencePose=REF_POSTURE)
# Add the free-flyer joint limits # Add the free-flyer joint limits
addFreeFlyerJointLimits(robot) addFreeFlyerJointLimits(robot)
return robot return robot
...@@ -93,8 +81,7 @@ def loadTalos(): ...@@ -93,8 +81,7 @@ def loadTalos():
URDF_SUBPATH = "/talos_data/robots/" + URDF_FILENAME URDF_SUBPATH = "/talos_data/robots/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH) modelPath = getModelPath(URDF_SUBPATH)
# Load URDF file # Load URDF file
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
pinocchio.JointModelFreeFlyer())
# Load SRDF file # Load SRDF file
readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False) readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False)
assert ((robot.model.armature[:6] == 0.).all()) assert ((robot.model.armature[:6] == 0.).all())
...@@ -114,17 +101,13 @@ def loadTalosLegs(): ...@@ -114,17 +101,13 @@ def loadTalosLegs():
legMaxId = 14 legMaxId = 14
m1 = robot.model m1 = robot.model
m2 = pinocchio.Model() m2 = pinocchio.Model()
for j, M, name, parent, Y in zip(m1.joints, m1.jointPlacements, m1.names, for j, M, name, parent, Y in zip(m1.joints, m1.jointPlacements, m1.names, m1.parents, m1.inertias):
m1.parents, m1.inertias):
if j.id < legMaxId: if j.id < legMaxId:
jid = m2.addJoint(parent, jid = m2.addJoint(parent, getattr(pinocchio, j.shortname())(), M, name)
getattr(pinocchio, j.shortname())(), M, name)
up = m2.upperPositionLimit up = m2.upperPositionLimit
down = m2.lowerPositionLimit down = m2.lowerPositionLimit
up[m2.joints[jid].idx_q:m2.joints[jid].idx_q + up[m2.joints[jid].idx_q:m2.joints[jid].idx_q + j.nq] = m1.upperPositionLimit[j.idx_q:j.idx_q + j.nq]
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]
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.upperPositionLimit = up
m2.lowerPositionLimit = down m2.lowerPositionLimit = down
assert (jid == j.id) assert (jid == j.id)
...@@ -170,11 +153,9 @@ def loadHyQ(): ...@@ -170,11 +153,9 @@ def loadHyQ():
URDF_SUBPATH = "/hyq_description/robots/" + URDF_FILENAME URDF_SUBPATH = "/hyq_description/robots/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH) modelPath = getModelPath(URDF_SUBPATH)
# Load URDF file # Load URDF file
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
pinocchio.JointModelFreeFlyer())
# Load SRDF file # Load SRDF file
readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False, False, readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False, False, "standing")
"standing")
# Add the free-flyer joint limits # Add the free-flyer joint limits
addFreeFlyerJointLimits(robot) addFreeFlyerJointLimits(robot)
return robot return robot
...@@ -190,11 +171,9 @@ def loadSolo(solo=True): ...@@ -190,11 +171,9 @@ def loadSolo(solo=True):
URDF_SUBPATH = "/solo_description/robots/" + URDF_FILENAME URDF_SUBPATH = "/solo_description/robots/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH) modelPath = getModelPath(URDF_SUBPATH)
# Load URDF file # Load URDF file
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
pinocchio.JointModelFreeFlyer())
# Load SRDF file # Load SRDF file
readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False, False, readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False, False, "standing")
"standing")
# Add the free-flyer joint limits # Add the free-flyer joint limits
addFreeFlyerJointLimits(robot) addFreeFlyerJointLimits(robot)
return robot return robot
...@@ -249,8 +228,7 @@ def loadICub(reduced=True): ...@@ -249,8 +228,7 @@ def loadICub(reduced=True):
URDF_SUBPATH = "/icub_description/robots/" + URDF_FILENAME URDF_SUBPATH = "/icub_description/robots/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH) modelPath = getModelPath(URDF_SUBPATH)
# Load URDF file # Load URDF file
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
pinocchio.JointModelFreeFlyer())
# Load SRDF file # Load SRDF file
readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False, False) readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False, False)
# Add the free-flyer joint limits # Add the free-flyer joint limits
...@@ -260,8 +238,7 @@ def loadICub(reduced=True): ...@@ -260,8 +238,7 @@ def loadICub(reduced=True):
def loadUR(robot=5, limited=False, gripper=False): def loadUR(robot=5, limited=False, gripper=False):
assert (not (gripper and (robot == 10 or limited))) assert (not (gripper and (robot == 10 or limited)))
URDF_FILENAME = "ur%i%s_%s.urdf" % (robot, "_joint_limited" if limited else URDF_FILENAME = "ur%i%s_%s.urdf" % (robot, "_joint_limited" if limited else '', 'gripper' if gripper else 'robot')
'', 'gripper' if gripper else 'robot')
URDF_SUBPATH = "/ur_description/urdf/" + URDF_FILENAME URDF_SUBPATH = "/ur_description/urdf/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH) modelPath = getModelPath(URDF_SUBPATH)
return RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath]) return RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath])
...@@ -271,8 +248,7 @@ def loadHector(): ...@@ -271,8 +248,7 @@ def loadHector():
URDF_FILENAME = "quadrotor_base.urdf" URDF_FILENAME = "quadrotor_base.urdf"
URDF_SUBPATH = "/hector_description/robots/" + URDF_FILENAME URDF_SUBPATH = "/hector_description/robots/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH) modelPath = getModelPath(URDF_SUBPATH)
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
pinocchio.JointModelFreeFlyer())
return robot return robot
...@@ -288,5 +264,4 @@ def loadRomeo(): ...@@ -288,5 +264,4 @@ def loadRomeo():
URDF_FILENAME = "romeo.urdf" URDF_FILENAME = "romeo.urdf"
URDF_SUBPATH = "/romeo_description/urdf/" + URDF_FILENAME URDF_SUBPATH = "/romeo_description/urdf/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH) modelPath = getModelPath(URDF_SUBPATH)
return RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], return RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
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