Commit 3bd689f4 authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

Merge branch 'topic/talos-legs' into 'devel'

Added the loading function for talos legs + removed an path append inside display

See merge request gepetto/example-robot-data!10
parents 34d3182c 0629d81e
Pipeline #5372 passed with stage
in 33 seconds
from robots_loader import (getModelPath, loadHyQ, loadICub, loadTalos, loadTalosArm, loadTiago, loadTiagoNoHand,
readParamsFromSrdf)
from robots_loader import (getModelPath, loadHyQ, loadICub, loadTalos, loadTalosArm, loadTalosLegs, loadTiago,
loadTiagoNoHand, readParamsFromSrdf)
import sys
from example_robot_data import loadHyQ, loadICub, loadTalos, loadTalosArm, loadTiago, loadTiagoNoHand
sys.path.append('/opt/openrobots/share/example-robot-data/unittest/')
import example_robot_data
DISPLAY_HYQ = 'hyq' in sys.argv
DISPLAY_TALOS = 'talos' in sys.argv
DISPLAY_TALOS_ARM = 'talos_arm' in sys.argv
DISPLAY_TALOS_LEGS = 'talos_legs' in sys.argv
DISPLAY_TIAGO = 'tiago' in sys.argv
DISPLAY_TIAGO_NO_HAND = 'tiago_no_hand' in sys.argv
DISPLAY_ICUB = 'icub' in sys.argv
if DISPLAY_HYQ:
hyq = loadHyQ()
hyq.initDisplay(loadModel=True)
hyq = example_robot_data.loadHyQ()
hyq.initViewer(loadModel=True)
hyq.display(hyq.q0)
if DISPLAY_TALOS:
talos = loadTalos()
talos.initDisplay(loadModel=True)
talos = example_robot_data.loadTalos()
talos.initViewer(loadModel=True)
talos.display(talos.q0)
if DISPLAY_TALOS_ARM:
talos_arm = loadTalosArm()
talos_arm.initDisplay(loadModel=True)
talos_arm = example_robot_data.loadTalosArm()
talos_arm.initViewer(loadModel=True)
talos_arm.display(talos_arm.q0)
if DISPLAY_TALOS_LEGS:
talos_legs = example_robot_data.loadTalosLegs()
talos_legs.initViewer(loadModel=True)
talos_legs.display(talos_legs.q0)
if DISPLAY_TIAGO:
tiago = loadTiago()
tiago.initDisplay(loadModel=True)
tiago = example_robot_data.loadTiago()
tiago.initViewer(loadModel=True)
tiago.display(tiago.q0)
if DISPLAY_TIAGO_NO_HAND:
tiago_no_hand = loadTiagoNoHand()
tiago_no_hand.initDisplay(loadModel=True)
tiago_no_hand = example_robot_data.loadTiagoNoHand()
tiago_no_hand.initViewer(loadModel=True)
tiago_no_hand.display(tiago_no_hand.q0)
if DISPLAY_ICUB:
icub = loadICub()
icub.initDisplay(loadModel=True)
icub = example_robot_data.loadICub()
icub.initViewer(loadModel=True)
icub.display(icub.q0)
......@@ -66,6 +66,61 @@ def loadTalos():
addFreeFlyerJointLimits(robot)
return robot
def loadTalosLegs():
robot = loadTalos()
URDF_FILENAME = "talos_reduced.urdf"
SRDF_FILENAME = "talos.srdf"
SRDF_SUBPATH = "/talos_data/srdf/" + SRDF_FILENAME
URDF_SUBPATH = "/talos_data/robots/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH)
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):
if j.id < legMaxId:
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]
m2.upperPositionLimit = up
m2.lowerPositionLimit = down
assert (jid == j.id)
m2.appendBodyToJoint(jid, Y, pinocchio.SE3.Identity())
u = m2.upperPositionLimit
u[:7] = 1
m2.upperPositionLimit = u
limit = m2.lowerPositionLimit
limit[:7] = -1
m2.lowerPositionLimit = limit
# q2 = robot.q0[:19]
for f in m1.frames:
if f.parent < legMaxId:
m2.addFrame(f)
g2 = pinocchio.GeometryModel()
for g in robot.visual_model.geometryObjects:
if g.parentJoint < 14:
g2.addGeometryObject(g)
robot.model = m2
robot.data = m2.createData()
robot.visual_model = g2
# robot.q0=q2
robot.visual_data = pinocchio.GeometryData(g2)
# Load SRDF file
robot.q0 = np.matrix(np.resize(robot.q0, robot.model.nq)).T
readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False)
assert ((m2.armature[:6] == 0.).all())
# Add the free-flyer joint limits
addFreeFlyerJointLimits(robot)
return robot
def loadHyQ():
URDF_FILENAME = "hyq_no_sensors.urdf"
......
#!/usr/bin/env python2
import unittest
from example_robot_data import loadHyQ, loadICub, loadTalos, loadTalosArm, loadTiago, loadTiagoNoHand
import example_robot_data
class RobotTestCase(unittest.TestCase):
......@@ -23,43 +22,47 @@ class RobotTestCase(unittest.TestCase):
class TalosArmTest(RobotTestCase):
RobotTestCase.ROBOT = loadTalosArm()
RobotTestCase.ROBOT = example_robot_data.loadTalosArm()
RobotTestCase.NQ = 7
RobotTestCase.NV = 7
class TalosArmFloatingTest(RobotTestCase):
RobotTestCase.ROBOT = loadTalosArm()
RobotTestCase.ROBOT = example_robot_data.loadTalosArm()
RobotTestCase.NQ = 14
RobotTestCase.NV = 13
class TalosTest(RobotTestCase):
RobotTestCase.ROBOT = loadTalos()
RobotTestCase.ROBOT = example_robot_data.loadTalos()
RobotTestCase.NQ = 39
RobotTestCase.NV = 38
class TalosLegsTest(RobotTestCase):
RobotTestCase.ROBOT = example_robot_data.loadTalosLegs()
RobotTestCase.NQ = 19
RobotTestCase.NV = 18
class HyQTest(RobotTestCase):
RobotTestCase.ROBOT = loadHyQ()
RobotTestCase.ROBOT = example_robot_data.loadHyQ()
RobotTestCase.NQ = 19
RobotTestCase.NV = 18
class TiagoTest(RobotTestCase):
RobotTestCase.ROBOT = loadTiago()
RobotTestCase.ROBOT = example_robot_data.loadTiago()
RobotTestCase.NQ = 50
RobotTestCase.NV = 48
class TiagoNoHandTest(RobotTestCase):
RobotTestCase.ROBOT = loadTiagoNoHand()
RobotTestCase.ROBOT = example_robot_data.loadTiagoNoHand()
RobotTestCase.NQ = 14
RobotTestCase.NV = 12
class ICubTest(RobotTestCase):
RobotTestCase.ROBOT = loadICub(reduced=False)
RobotTestCase.ROBOT = example_robot_data.loadICub(reduced=False)
RobotTestCase.NQ = 39
RobotTestCase.NV = 38
......
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