def appendUrdfModel(self, urdfFilename, frame_name, fMm, prefix=None):
if not isinstance(fMm, pinocchio.SE3):
fMm = pinocchio.XYZQUATToSE3(fMm)
id_parent_frame = self.model.getFrameId(frame_name)
model, gmodel = pinocchio.buildModelsFromUrdf(
hpp.rostools.retrieve_resource(urdfFilename),
geometry_types=pinocchio.GeometryType.COLLISION)
if prefix is not None:
for i in range(1, len(model.names)):
model.names[i] = prefix + model.names[i]
for f in model.frames:
f.name = prefix + f.name
for go in gmodel.geometryObjects:
go.name = prefix + go.name
igeom = self.gmodel.ngeoms
nmodel, ngmodel = pinocchio.appendModel(self.model, model, self.gmodel,
gmodel, id_parent_frame, fMm)
self.gid_field_of_view = ngmodel.getGeometryId("field_of_view")
for go in gmodel.geometryObjects:
ngmodel.addCollisionPair(
pinocchio.CollisionPair(self.gid_field_of_view,
ngmodel.getGeometryId(go.name)))
self.model, self.gmodel = nmodel, ngmodel
self.data = pinocchio.Data(self.model)
self.gdata = pinocchio.GeometryData(self.gmodel)
def loadSoloLeg(solo8=True):
if solo8:
URDF_FILENAME = "solo.urdf"
legMaxId = 4
else:
URDF_FILENAME = "solo12.urdf"
legMaxId = 5
SRDF_FILENAME = "solo.srdf"
SRDF_SUBPATH = "/solo_description/srdf/" + SRDF_FILENAME
URDF_SUBPATH = "/solo_description/robots/" + URDF_FILENAME
modelPath = example_robot_data.getModelPath(URDF_SUBPATH)
robot = example_robot_data.loadSolo(solo8)
m1 = robot.model
m2 = pinocchio.Model()
for index, [j, M, name, parent, Y] in enumerate(
zip(m1.joints, m1.jointPlacements, m1.names, m1.parents,
m1.inertias)):
if j.id < legMaxId:
jointType = j.shortname()
if jointType == 'JointModelFreeFlyer':
# for the freeflyer we just take reduced mass and zero inertia
jointType = 'JointModelPZ'
Y.mass = Y.mass / 4
Y.inertia = np.diag(1e-6 * np.ones(3))
M = pinocchio.SE3.Identity()
if index == 2:
# start with the prismatic joint on the sliding guide
M.translation = np.zeros(3)
# 2D model, flatten y axis
vector2d = M.translation
vector2d[1] = 0.0
M.translation = vector2d
jid = m2.addJoint(parent, getattr(pinocchio, jointType)(), M, name)
assert (jid == j.id)
m2.appendBodyToJoint(jid, Y, pinocchio.SE3.Identity())
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 < legMaxId:
g2.addGeometryObject(g)
robot.model = m2
robot.data = m2.createData()
robot.visual_model = g2
robot.visual_data = pinocchio.GeometryData(g2)
# Load SRDF file
#q0 = example_robot_data.readParamsFromSrdf(robot.model, modelPath + SRDF_SUBPATH, False, False, 'standing')
robot.q0 = np.zeros(m2.nq + 1)
assert ((m2.rotorInertia[:m2.joints[1].nq] == 0.).all())
return robot
def __init__(self):
super(TalosLegsLoader, self).__init__()
legMaxId = 14
m1 = self.robot.model
m2 = pin.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(pin, j.shortname())(), M, name)
upperPos = m2.upperPositionLimit
lowerPos = m2.lowerPositionLimit
effort = m2.effortLimit
upperPos[m2.joints[jid].idx_q:m2.joints[jid].idx_q +
j.nq] = m1.upperPositionLimit[j.idx_q:j.idx_q + j.nq]
lowerPos[m2.joints[jid].idx_q:m2.joints[jid].idx_q +
j.nq] = m1.lowerPositionLimit[j.idx_q:j.idx_q + j.nq]
effort[m2.joints[jid].idx_v:m2.joints[jid].idx_v +
j.nv] = m1.effortLimit[j.idx_v:j.idx_v + j.nv]
m2.upperPositionLimit = upperPos
m2.lowerPositionLimit = lowerPos
m2.effortLimit = effort
assert jid == j.id
m2.appendBodyToJoint(jid, Y, pin.SE3.Identity())
upperPos = m2.upperPositionLimit
upperPos[:7] = 1
m2.upperPositionLimit = upperPos
lowerPos = m2.lowerPositionLimit
lowerPos[:7] = -1
m2.lowerPositionLimit = lowerPos
effort = m2.effortLimit
effort[:6] = np.inf
m2.effortLimit = effort
# q2 = self.robot.q0[:19]
for f in m1.frames:
if f.parent < legMaxId:
m2.addFrame(f)
g2 = pin.GeometryModel()
for g in self.robot.visual_model.geometryObjects:
if g.parentJoint < 14:
g2.addGeometryObject(g)
self.robot.model = m2
self.robot.data = m2.createData()
self.robot.visual_model = g2
# self.robot.q0=q2
self.robot.visual_data = pin.GeometryData(g2)
# Load SRDF file
self.q0 = readParamsFromSrdf(self.robot.model, self.srdf_path,
self.verbose, self.has_rotor_parameters,
self.ref_posture)
assert (m2.armature[:6] == 0.).all()
# Add the free-flyer joint limits to the new model
self.addFreeFlyerJointLimits()
def reduceModel(self, jointsToRemove, config, len_prefix=0):
jointIds = [
self.model.getJointId(jn[len_prefix:]) for jn in jointsToRemove
]
self.model, self.gmodel = pinocchio.buildReducedModel(
self.model, self.gmodel, jointIds, np.array(config))
self.data = pinocchio.Data(self.model)
self.gdata = pinocchio.GeometryData(self.gmodel)
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.array(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 __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 createRobotWithObstacles(robotname='ur5'):
### Robot
# Load the robot
robot = robex.load(robotname)
### Obstacle map
# Capsule obstacles will be placed at these XYZ-RPY parameters
oMobs = [[0.40, 0., 0.30, np.pi / 2, 0, 0],
[-0.08, -0., 0.69, np.pi / 2, 0, 0],
[0.23, -0., 0.04, np.pi / 2, 0, 0],
[-0.32, 0., -0.08, np.pi / 2, 0, 0]]
# Load visual objects and add them in collision/visual models
color = [1.0, 0.2, 0.2, 1.0] # color of the capsules
rad, length = .1, 0.4 # radius and length of capsules
for i, xyzrpy in enumerate(oMobs):
obs = pin.GeometryObject.CreateCapsule(
rad, length) # Pinocchio obstacle object
obs.meshColor = np.array(
[1.0, 0.2, 0.2,
1.0]) # Don't forget me, otherwise I am transparent ...
obs.name = "obs%d" % i # Set object name
obs.parentJoint = 0 # Set object parent = 0 = universe
obs.placement = XYZRPYtoSE3(xyzrpy) # Set object placement wrt parent
robot.collision_model.addGeometryObject(
obs) # Add object to collision model
robot.visual_model.addGeometryObject(obs) # Add object to visual model
### Collision pairs
nobs = len(oMobs)
nbodies = robot.collision_model.ngeoms - nobs
robotBodies = range(nbodies)
envBodies = range(nbodies, nbodies + nobs)
for a, b in itertools.product(robotBodies, envBodies):
robot.collision_model.addCollisionPair(pin.CollisionPair(a, b))
### Geom data
# Collision/visual models have been modified => re-generate corresponding data.
robot.collision_data = pin.GeometryData(robot.collision_model)
robot.visual_data = pin.GeometryData(robot.visual_model)
return robot
def addDriller(self, mesh, frame_name, fMm):
if not isinstance(fMm, pinocchio.SE3):
fMm = pinocchio.XYZQUATToSE3(fMm)
id_parent_frame = self.model.getFrameId(frame_name)
parent_frame = self.model.frames[id_parent_frame]
go = pinocchio.GeometryObject("driller", id_parent_frame, parent_frame.parent,
hppfcl.MeshLoader().load(hpp.rostools.retrieve_resource(mesh)),
parent_frame.placement * fMm)
self.gmodel.addGeometryObject(go, self.model)
self.gmodel.addCollisionPair(pinocchio.CollisionPair(self.gid_field_of_view, self.gmodel.ngeoms-1))
self.gdata = pinocchio.GeometryData(self.gmodel)
def __init__(self):
self.viewer = GepettoViewerServer()
#self.visuals = []
self.model = pio.Model()
self.gmodel = pio.GeometryModel()
self.createHand()
self.addCollisionPairs()
self.data = self.model.createData()
self.gdata = pio.GeometryData(self.gmodel)
self.gdata.collisionRequest.enable_contact=True
self.q0 = zero(self.model.nq)
self.q0[0] = np.pi
self.q0[-2] = -np.pi/3
self.q0[2:-4] = -np.pi/6
self.v0 = zero(self.model.nv)
self.collisionPairs = []
def loadTalosLegs(modelPath='/opt/openrobots/share/example-robot-data'):
from pinocchio import JointModelFreeFlyer, JointModelRX, JointModelRY, JointModelRZ
robot = loadTalos(modelPath=modelPath)
SRDF_FILENAME = "talos.srdf"
SRDF_SUBPATH = "/talos_data/srdf/" + SRDF_FILENAME
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, locals()[j.shortname()](), M, name)
assert (jid == j.id)
m2.appendBodyToJoint(jid, Y, pinocchio.SE3.Identity())
m2.upperPositionLimit = np.matrix([1.] * 19).T
m2.lowerPositionLimit = np.matrix([-1.] * 19).T
#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
pinocchio.getNeutralConfiguration(m2, modelPath + SRDF_SUBPATH, False)
pinocchio.loadRotorParameters(m2, modelPath + SRDF_SUBPATH, False)
m2.armature = \
np.multiply(m2.rotorInertia.flat, np.square(m2.rotorGearRatio.flat))
assert ((m2.armature[:6] == 0.).all())
robot.q0 = m2.neutralConfiguration.copy()
return robot
def __init__(self, controller=None):
"""
Description:
1. Initiates, Builds & Starts Robot Model components.
2. Initializes 'NYUFingerSimulator()' as Visualizer.
Args:
1. connect_hardware = True (default).
Connects to the robot hardware if enabled
else, runs as simulation.
Keyword Args:
1. controller = 'None': Does not initialize a joystick.
'keyboard': Enables joystick mode on the keyboard.
"""
# Load the .urdf files
self.package_dir = os.path.abspath(os.getcwd())
self.urdf_path = self.package_dir + '/model/fingeredu.urdf'
# Create Models from the .urdf files
self.model, self.collision_model, self.visual_model = \
pin.buildModelsFromUrdf(self.urdf_path, self.package_dir)
self.collision_model.addAllCollisionPairs()
# Create datas from models
self.model_data = self.model.createData()
self.collision_model.addAllCollisionPairs()
self.collision_data = pin.GeometryData(self.collision_model)
# Display the models in viewer
self.simulator = NYUFingerSimulator(self.package_dir, self.urdf_path)
# Get the FrameID of the EOAT
self.EOAT_ID = self.model.getFrameId('finger_tip_link')
# Get the FrameID of the Base
self.Base_ID = self.model.getFrameId('base_link')
# Read Initial Joint Values
init_q, init_v = self.simulator.get_state()
# Initial Position
memory.pose = self.ForwardKinematics(init_q, init_v)[0]
# PD Controller
self.control_freq = 0.001
self.PD = SimPD(self.control_freq, self.simulator, self.model,
self.model_data, self.ForwardKinematics, self.Jacobian,
self.TimeDerivativeJacobian, 9500, 35, 1.5, 18000, 100,
50)
# Direct Joint Limits
J1_limits = np.pi / 2
J2_limits = np.pi
J3_limits = np.pi
self.JointLimits = np.array([J1_limits, J2_limits, J3_limits])
# Init. Motion Planner
self.Motion = MotionPlanner(self.model, self.model_data, self.PD,
self.simulator, self.ForwardKinematics,
self.Jacobian, self.JointLimits)
# Feature Configuration Spaces
self.StablePose = init_q
self.parking_pose = np.array([0, -np.pi / 4, np.pi / 4])
# Use Keyboard as a controller
if controller == 'keyboard':
TeleKeyboard = NumStick(self.Motion.LinearPlanner)
TeleKeyboard.run()
geom_model = pin.buildGeomFromUrdf(model,urdf_model_path,model_path,pin.GeometryType.COLLISION)
# Add collisition pairs
geom_model.addAllCollisionPairs()
print("num collision pairs - initial:",len(geom_model.collisionPairs))
# Remove collision pairs listed in the SRDF file
srdf_filename = "romeo.srdf"
srdf_model_path = model_path + "/romeo_description/srdf/" + srdf_filename
pin.removeCollisionPairs(model,geom_model,srdf_model_path)
print("num collision pairs - final:",len(geom_model.collisionPairs))
# Load reference configuration
pin.loadReferenceConfigurations(model,srdf_model_path)
# Retrieve the half sitting position from the SRDF file
q = model.referenceConfigurations["half_sitting"]
# Create data structures
data = model.createData()
geom_data = pin.GeometryData(geom_model)
# Compute all the collisions
pin.computeCollisions(model,data,geom_model,geom_data,q,False)
# Compute for a single pair of collision
pin.updateGeometryPlacements(model,data,geom_model,geom_data,q)
pin.computeCollision(geom_model,geom_data,0)
np.matrix(xyzrpy[:3]).T ) # Set object placement wrt parent
robot.collision_model.addGeometryObject(obs,robot.model,False) # Add object to collision model
robot.visual_model .addGeometryObject(obs,robot.model,False) # Add object to visual model
# Also create a geometric object in gepetto viewer, with according name.
try: robot.viewer.gui.addCapsule( "world/pinocchio/"+obs.name, rad,length, [ 1.0, 0.2, 0.2, 1.0 ] )
except: pass
# Add all collision pairs
robot.collision_model.addAllCollisionPairs()
# Remove collision pairs that can not be relevant (forearm with upper arm, forearm with wrist, etc).
for idx in [ 56,35,23 ]:
#print "Remove pair",robot.collision_model.collisionPairs[idx]
robot.collision_model.removeCollisionPair(robot.collision_model.collisionPairs[idx])
# Collision/visual models have been modified => re-generate corresponding data.
robot.collision_data = se3.GeometryData(robot.collision_model)
robot.visual_data = se3.GeometryData(robot.visual_model )
# Display the robot will also update capsule placements.
for iloop in range(100):
q = rand(6)*2*np.pi-np.pi
se3.updateGeometryPlacements(robot.model,robot.data,robot.collision_model,robot.collision_data,q)
if se3.computeCollisions(robot.collision_model,robot.collision_data,False):
for i,p in enumerate(robot.collision_model.collisionPairs):
if se3.computeCollision(robot.collision_model,robot.collision_data,i):
print i,p, robot.collision_model.geometryObjects[p.first].name, \
robot.collision_model.geometryObjects[p.second].name
robot.display(q)
break
# Find the absolute path to the srdf file
srdf_path = find_paths("solo12")["srdf"]
# Disable collision pairs specified in the srdf
pin.removeCollisionPairs(robot.model, robot.collision_model, srdf_path)
# Read the reference configurations defined in the srdf
pin.loadReferenceConfigurations(robot.model, srdf_path)
q = robot.model.referenceConfigurations["straight_standing"]
# Display the configuration in the viewer.
robot.display(q)
# Initialize the collision data
robot.collision_data = pin.GeometryData(robot.collision_model)
# Compute all the collisions
pin.computeCollisions(
robot.model,
robot.data,
robot.collision_model,
robot.collision_data,
q,
False,
)
# Print the status of collision for all collision pairs
valid = True
for k in range(len(robot.collision_model.collisionPairs)):
cr = robot.collision_data.collisionResults[k]
def loadTalos(legs=False, arm=False):
URDF_FILENAME = "talos_left_arm.urdf" if arm else "talos_reduced.urdf"
SRDF_FILENAME = "talos.srdf"
robot = robot_loader('talos_data', URDF_FILENAME, SRDF_FILENAME, free_flyer=not arm)
assert (robot.model.armature[:6] == 0.).all()
if legs:
legMaxId = 14
m1 = robot.model
m2 = pin.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(pin, j.shortname())(), M, name)
upperPos = m2.upperPositionLimit
lowerPos = m2.lowerPositionLimit
effort = m2.effortLimit
upperPos[m2.joints[jid].idx_q:m2.joints[jid].idx_q + j.nq] = m1.upperPositionLimit[j.idx_q:j.idx_q +
j.nq]
lowerPos[m2.joints[jid].idx_q:m2.joints[jid].idx_q + j.nq] = m1.lowerPositionLimit[j.idx_q:j.idx_q +
j.nq]
effort[m2.joints[jid].idx_v:m2.joints[jid].idx_v + j.nv] = m1.effortLimit[j.idx_v:j.idx_v + j.nv]
m2.upperPositionLimit = upperPos
m2.lowerPositionLimit = lowerPos
m2.effortLimit = effort
assert jid == j.id
m2.appendBodyToJoint(jid, Y, pin.SE3.Identity())
upperPos = m2.upperPositionLimit
upperPos[:7] = 1
m2.upperPositionLimit = upperPos
lowerPos = m2.lowerPositionLimit
lowerPos[:7] = -1
m2.lowerPositionLimit = lowerPos
effort = m2.effortLimit
effort[:6] = np.inf
m2.effortLimit = effort
# q2 = robot.q0[:19]
for f in m1.frames:
if f.parent < legMaxId:
m2.addFrame(f)
g2 = pin.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 = pin.GeometryData(g2)
# Load SRDF file
robot.q0 = robot.q0[:robot.model.nq]
model_path = getModelPath(join('talos_data/robots', URDF_FILENAME))
robot.q0 = readParamsFromSrdf(robot.model, join(model_path, 'talos_data/srdf', SRDF_FILENAME), False)
assert (m2.armature[:6] == 0.).all()
# Add the free-flyer joint limits to the new model
addFreeFlyerJointLimits(robot.model)
return robot
