Skip to content
Snippets Groups Projects
Commit 8a2da0df authored by Pep Marti Saumell's avatar Pep Marti Saumell
Browse files

Cmake fixes and formatting

parent a6279e8b
No related branches found
No related tags found
1 merge request!21New descriptions
Hide whitespace changes
Inline Side-by-side
CMakeLists.txt
+ 1
− 0
View file @ 8a2da0df
......@@ -49,6 +49,7 @@ IF(NOT INSTALL_PYTHON_INTERFACE_ONLY)
INSTALL(DIRECTORY kinova_description DESTINATION share/${PROJECT_NAME})
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)
hector_description/meshes/quadrotor/blender/quadrotor_base.blend deleted 100644 → 0
+ 0
− 0
View file @ a6279e8b
File deleted
python/example_robot_data/__main__.py
+ 4
− 4
View file @ 8a2da0df
......@@ -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', 'hector', 'double_pendulum'
'anymal', 'anymal_kinova', 'hyq', 'solo', 'solo12', 'talos', 'talos_arm',
'talos_legs', 'kinova', 'tiago', 'tiago_no_hand', 'icub', 'ur5', 'romeo',
'hector', 'double_pendulum'
]
parser = ArgumentParser()
......@@ -76,7 +77,7 @@ elif args.robot == 'ur5':
ur5 = robots_loader.loadUR()
ur5.initViewer(loadModel=True)
ur5.display(ur5.q0)
elif args.robot == 'romeo':
romeo = robots_loader.loadRomeo()
romeo.initViewer(loadModel=True)
......@@ -91,4 +92,3 @@ if args.robot == 'double_pendulum':
planar2dof = robots_loader.load2dof()
planar2dof.initViewer(loadModel=True)
planar2dof.display(planar2dof.q0)
python/example_robot_data/robots_loader.py
+ 46
− 20
View file @ 8a2da0df
......@@ -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,7 +260,8 @@ 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])
......@@ -248,7 +271,8 @@ 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())
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath],
pinocchio.JointModelFreeFlyer())
return robot
......@@ -258,9 +282,11 @@ def loadDoublePendulum():
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())
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