def loadTiago(initViewer=True):
'''Load the Tiago robot, and add two operation frames:
- at the front of the basis, named framebasis.
- at the end effector, name frametool.
Take care: Tiago as 3 continuous joints (one for the basis, two for the wheels), which makes
nq == nv+3.
'''
URDF_FILENAME = "tiago_no_hand.urdf"
URDF_SUBPATH = "/tiago_description/robots/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH)
robot = pio.RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH,
[modelPath], pio.JointModelPlanar())
robot.model.addFrame(
pio.Frame('framebasis', 1, 1,
pio.SE3(numpy.eye(3),
numpy.matrix([0.28, 0, 0.1]).T),
pio.FrameType.OP_FRAME))
robot.model.addFrame(
pio.Frame(
'frametool', 9, 51,
pio.SE3(numpy.matrix([[0, 1., 0], [0, 0, 1], [1, 0, 0]]),
numpy.matrix([0., 0, 0.06]).T), pio.FrameType.OP_FRAME))
robot.model.addFrame(
pio.Frame('framegaze', 11, 57,
pio.SE3(numpy.eye(3),
numpy.matrix([0.4, 0, 0]).T),
pio.FrameType.OP_FRAME))
robot.data = robot.model.createData()
jllow = robot.model.lowerPositionLimit
jllow[:2] = -2
robot.model.lowerPositionLimit = jllow
jlupp = robot.model.upperPositionLimit
jlupp[:2] = 2
robot.model.upperPositionLimit = jlupp
def display(robot, q):
robot.realdisplay(q)
pio.updateFramePlacements(robot.model, robot.viz.data)
robot.viewer.gui.applyConfiguration(
'world/' + robot.model.frames[-2].name,
list(pio.se3ToXYZQUAT(robot.viz.data.oMf[-2]).flat))
robot.viewer.gui.applyConfiguration(
'world/' + robot.model.frames[-1].name,
list(pio.se3ToXYZQUAT(robot.viz.data.oMf[-1]).flat))
robot.viewer.gui.refresh()
if initViewer:
robot.initViewer(loadModel=True)
gv = robot.viewer.gui
gv.addFloor('world/floor')
gv.addXYZaxis('world/framebasis', [1., 0., 0., 1.], .03, .1)
gv.addXYZaxis('world/frametool', [1., 0., 0., 1.], .03, .1)
#gv.addXYZaxis('world/framegaze', [1., 0., 0., 1.], .03, .1)
robot.realdisplay = robot.display
import types
robot.display = types.MethodType(display, robot)
return robot
def __init__(
self,
urdfString=None,
urdfFilename=None,
fov=np.radians((60., 90.)),
# fov = np.radians((49.5, 60)),
geoms=["arm_3_link_0"],
):
if isinstance(fov, str):
self.fov = fov
fov_fcl = hppfcl.MeshLoader().load(
hpp.rostools.retrieve_resource(fov))
else:
# build FOV tetahedron
dx, dy = np.sin(fov)
pts = hppfcl.StdVec_Vec3f()
self.fov = [
(0, 0, 0),
(dx, dy, 1),
(-dx, dy, 1),
(-dx, -dy, 1),
(dx, -dy, 1),
]
pts.extend([np.array(e) for e in self.fov])
self.fov.append(self.fov[1])
fov_fcl = hppfcl.BVHModelOBBRSS()
fov_fcl.beginModel(4, 5)
fov_fcl.addSubModel(pts, _tetahedron_tris)
fov_fcl.endModel()
self.cos_angle_thr = np.cos(np.radians(70))
if urdfFilename is None:
assert urdfString is not None
# Pinocchio does not allow to build a GeometryModel from a XML string.
urdfFilename = '/tmp/tmp.urdf'
with open(urdfFilename, 'w') as f:
f.write(urdfString)
self.model, self.gmodel = pinocchio.buildModelsFromUrdf(
hpp.rostools.retrieve_resource(urdfFilename),
root_joint=pinocchio.JointModelPlanar(),
geometry_types=pinocchio.GeometryType.COLLISION)
id_parent_frame = self.model.getFrameId("xtion_rgb_optical_frame")
parent_frame = self.model.frames[id_parent_frame]
go = pinocchio.GeometryObject("field_of_view", id_parent_frame,
parent_frame.parent, fov_fcl,
parent_frame.placement)
self.gid_field_of_view = self.gmodel.addGeometryObject(go, self.model)
for n in geoms:
assert self.gmodel.existGeometryName(n)
self.gmodel.addCollisionPair(
pinocchio.CollisionPair(self.gmodel.getGeometryId(n),
self.gid_field_of_view))
self.data = pinocchio.Data(self.model)
self.gdata = pinocchio.GeometryData(self.gmodel)
def __init__(self, urdf, pkgs):
RobotWrapper.__init__(self, urdf, pkgs, pinocchio.JointModelPlanar())
M0 = pinocchio.SE3(eye(3), np.matrix([0., 0., .6]).T)
self.model.jointPlacements[2] = M0 * self.model.jointPlacements[2]
self.visual_model.geometryObjects[0].placement = M0 * self.visual_model.geometryObjects[0].placement
self.visual_data.oMg[0] = M0 * self.visual_data.oMg[0]
# Placement of the "mobile" frame wrt basis center.
basisMop = pinocchio.SE3(eye(3), np.matrix([.3, .0, .1]).T)
self.model.addFrame(pinocchio.Frame('mobile', 1, 1, basisMop, pinocchio.FrameType.OP_FRAME))
# Placement of the tool frame wrt end effector frame (located at the center of the wrist)
effMop = pinocchio.SE3(eye(3), np.matrix([.0, .08, .095]).T)
self.model.addFrame(pinocchio.Frame('tool', 6, 6, effMop, pinocchio.FrameType.OP_FRAME))
# Create data again after setting frames
self.data = self.model.createData()