def compute(m):
tq_vec = pin.SE3ToXYZQUAT(m)
tq_tup = pin.SE3ToXYZQUATtuple(m)
mm_vec = pin.XYZQUATToSE3(tq_vec)
mm_tup = pin.XYZQUATToSE3(tq_tup)
mm_lis = pin.XYZQUATToSE3(list(tq_tup))
return tq_vec, tq_tup, mm_vec, mm_tup, mm_lis
def poseToSE3(m):
import pinocchio, geometry_msgs.msg
if isinstance(m, geometry_msgs.msg.Transform):
return pinocchio.XYZQUATToSE3([
m.translation.x, m.translation.y, m.translation.z, m.rotation.x,
m.rotation.y, m.rotation.z, m.rotation.w
])
elif isinstance(m, geometry_msgs.msg.Pose):
return pinocchio.XYZQUATToSE3([
m.position.x, m.position.y, m.position.z, m.orientation.x,
m.orientation.y, m.orientation.z, m.orientation.w
])
def attach_to_link(model, link, gripper=None, handle=None, contact=None):
"""
Attach a gripper, handle or contact to a different link.
- model: a pinocchio.Model that represents the kinematic chain.
- link: the link onto which to attach.
- gripper, handle, contact: exactly one of them should be provided
"""
import pinocchio
if int(gripper is None) + int(handle is None) + int(contact is None) != 2:
raise ValueError(
"Exactly one of {gripper, handle, contact} should be provided")
srdf = (
contact if handle is None else handle) if gripper is None else gripper
oid = model.getFrameId(srdf['link'])
nid = model.getFrameId(link)
if oid >= model.nframes or nid >= model.nframes:
raise ValueError("Could not find one of the frames")
if oid == nid: return
of = model.frames[oid]
nf = model.frames[nid]
if of.parent != nf.parent:
raise RuntimeError("The frames are not attached to the same joint")
srdf['link'] = nf.name
if contact is not None:
# Change srdf['points'] from old link to new link
raise NotImplementedError(
"Need to change srdf['points'] from old link to new link")
else:
olMf = pinocchio.XYZQUATToSE3(srdf["position"])
jMnl = nf.placement
jMol = of.placement
nlMf = jMnl.inverse() * jMol * olMf
srdf["position"] = pinocchio.SE3ToXYZQUAT(nlMf)
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 tagVisible(self, oMt, size):
""" It assumes that updateGeometryPlacements has been called """
idc = self.model.getFrameId("xtion_rgb_optical_frame")
camera = self.model.frames[idc]
oMc = self.data.oMf[idc]
# oMc.rotation[:,2] view axis
# oMt.rotation[:,2] normal of the tag, on the tag side.
if not isinstance(oMt, pinocchio.SE3):
oMt = pinocchio.XYZQUATToSE3(oMt)
cos_theta = -np.dot(oMc.rotation[:, 2], oMt.rotation[:, 2])
if cos_theta < self.cos_angle_thr:
return False
pts = self.tagToTetahedronPts(oMt, size)
tetahedron = hppfcl.BVHModelOBBRSS()
tetahedron.beginModel(4, 5)
tetahedron.addSubModel(pts, _tetahedron_tris)
tetahedron.endModel()
request = hppfcl.CollisionRequest()
Id = hppfcl.Transform3f()
for go, oMg in zip(self.gmodel.geometryObjects, self.gdata.oMg):
# Don't check for collision with the camera, except with the field_of_view
if go.parentJoint == camera.parent and go.name != "field_of_view":
continue
result = hppfcl.CollisionResult()
hppfcl.collide(go.geometry, hppfcl.Transform3f(oMg), tetahedron,
Id, request, result)
if result.isCollision():
return False
return True
def tag(self, X, i, asSE3=True):
assert i < self.ntags
s = self.nplanes * 4 + self.ncameras * 7
if asSE3:
return pinocchio.XYZQUATToSE3(X[s + 7 * i:s + 7 * (i + 1)])
else:
return X[s + 7 * i:s + 7 * (i + 1)]
def __init__(self, name, filename, package_dirs, root_joint, consimVisualizer, visualOptions, conf):
self.name = name
self.contactNames= visualOptions["contact_names"]
self.robotColor = visualOptions["robot_color"]
self.forceColor = visualOptions["force_color"]
self.coneColor = visualOptions["cone_color"]
self.force_radius = visualOptions["force_radius"]
self.force_length = visualOptions["force_length"]
self.cone_length = visualOptions["cone_length"]
self.friction_coeff = visualOptions["friction_coeff"]
self.wireframeMode = visualOptions["wireframe_mode"]# "FILL", "WIREFRAME" or "FILL_AND_WIREFRAME".
self.cone_radius = self.cone_length * self.friction_coeff
#
self.urdfDir = filename
self.meshDir = package_dirs
self.gui = consimVisualizer.gui
self.sceneName = consimVisualizer.sceneName
self.model, self.collision_model, self.visual_model = buildModelsFromUrdf(filename, package_dirs, root_joint)
self.display_collisions = False
self.display_visuals = True
self.display_forces = True
self.display_cones = True
self.data, self.collision_data, self.visual_data = createDatas(self.model,self.collision_model,self.visual_model)
self.rootNodeName="pinocchio"
self.RootNodeName = self.sceneName + "/" + self.rootNodeName+ "_" + self.name
self.forceGroup = self.RootNodeName + "/contact_forces"
self.frictionGroup = self.RootNodeName + "/friction_cone"
self.x_axis = np.array([1., 0., 0.])
self.z_axis = np.array([0., 0., 1.])
self.totalWeight = sum(m.mass for m in self.model.inertias) * np.linalg.norm(self.model.gravity.linear)
self.parentIndex = conf.parent_frame_index
self.type=conf.TYPE
if (self.type=="Biped"):
contact_point = np.ones((3,4)) * conf.lz
contact_point[0, :] = [-conf.lxn, -conf.lxn, conf.lxp, conf.lxp]
contact_point[1, :] = [-conf.lyn, conf.lyp, -conf.lyn, conf.lyp]
for j,cf in enumerate(conf.parent_frames):
parentFrameId = self.model.getFrameId(cf)
parentJointId = self.model.frames[parentFrameId].parent
for i in range(4):
frame_name = conf.contact_frames[4*j + i]
placement = pin.XYZQUATToSE3(list(contact_point[:,i])+[0, 0, 0, 1.])
placement = self.model.frames[parentFrameId].placement * placement
fr = pin.Frame(frame_name, parentJointId, parentFrameId, placement, pin.FrameType.OP_FRAME)
self.model.addFrame(fr)
self.data = self.model.createData()
elif(self.type=="Quadruped"):
pass
else:
pass
def test_se3ToXYZQUAT_XYZQUATToSe3(self):
m = pin.SE3.Identity()
m.translation = np.matrix('1. 2. 3.').T
m.rotation = np.matrix(
'1. 0. 0.;0. 0. -1.;0. 1. 0.') # rotate('x', pi / 2)
self.assertApprox(
pin.SE3ToXYZQUAT(m).T,
[1., 2., 3., sqrt(2) / 2, 0, 0,
sqrt(2) / 2])
self.assertApprox(
pin.XYZQUATToSE3([1., 2., 3.,
sqrt(2) / 2, 0, 0,
sqrt(2) / 2]), m)
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 tagToTetahedronPts(self, oMt, size, margin = 0.002):
""" It assumes that updateGeometryPlacements has been called """
if not isinstance(oMt, pinocchio.SE3):
oMt = pinocchio.XYZQUATToSE3(oMt)
pts = hppfcl.StdVec_Vec3f()
idc = self.model.getFrameId("xtion_rgb_optical_frame")
C = self.data.oMf[idc].translation + 0.002*self.data.oMf[idc].rotation[:,2]
pts.append(C)
for pt in [
np.array(( size / 2, size / 2, 0)),
np.array((-size / 2, size / 2, 0)),
np.array((-size / 2, -size / 2, 0)),
np.array(( size / 2, -size / 2, 0)), ]:
P = oMt * pt
u = (C-P)
u /= np.linalg.norm(u)
pts.append(P + margin * u)
return pts
def compute_freeflyer(trajectory_data: TrajectoryDataType,
freeflyer_continuity: bool = True) -> None:
"""Compute the freeflyer positions and velocities.
.. warning::
This function modifies the internal robot data.
:param trajectory_data: Sequence of States for which to retrieve the
freeflyer.
:param freeflyer_continuity: Whether or not to enforce the continuity
in position of the freeflyer.
Optional: True by default.
"""
robot = trajectory_data['robot']
contact_frame_prev = None
w_M_ff_offset = pin.SE3.Identity()
w_M_ff_prev = None
for s in trajectory_data['evolution_robot']:
# Compute freeflyer using contact frame as reference frame
s.contact_frame = compute_freeflyer_state_from_fixed_body(
robot, s.q, s.v, s.a, s.contact_frame, None)
# Move freeflyer to ensure continuity over time, if requested
if freeflyer_continuity:
# Extract the current freeflyer transform
w_M_ff = pin.XYZQUATToSE3(s.q[:7])
# Update the internal buffer of the freeflyer transform
if contact_frame_prev is not None \
and contact_frame_prev != s.contact_frame:
w_M_ff_offset = w_M_ff_offset * w_M_ff_prev * w_M_ff.inverse()
contact_frame_prev = s.contact_frame
w_M_ff_prev = w_M_ff
# Add the appropriate offset to the freeflyer
w_M_ff = w_M_ff_offset * w_M_ff
s.q[:7] = pin.SE3ToXYZQUAT(w_M_ff)
def meshcat_transform(x, y, z, q, u, a, t):
return np.array(pin.XYZQUATToSE3([x, y, z, q, u, a, t]))
def test_se3ToXYZQUAT_XYZQUATToSe3(self):
m = pin.SE3.Identity()
m.translation = np.array([1., 2., 3.])
m.rotation = np.array([[1., 0., 0.],[0., 0., -1.],[0., 1., 0.]]) # rotate('x', pi / 2)
self.assertApprox(pin.SE3ToXYZQUAT(m).T, [1., 2., 3., sqrt(2) / 2, 0, 0, sqrt(2) / 2])
self.assertApprox(pin.XYZQUATToSE3([1., 2., 3., sqrt(2) / 2, 0, 0, sqrt(2) / 2]), m)
import conf_romeo_cpp as conf
elif (robot_name == 'talos'):
import conf_talos_cpp as conf
robot = RobotWrapper.BuildFromURDF(conf.urdf, [conf.modelPath],
pin.JointModelFreeFlyer())
contact_point = np.ones((3, 4)) * conf.lz
contact_point[0, :] = [-conf.lxn, -conf.lxn, conf.lxp, conf.lxp]
contact_point[1, :] = [-conf.lyn, conf.lyp, -conf.lyn, conf.lyp]
contact_frames = []
for cf in conf.contact_frames:
parentFrameId = robot.model.getFrameId(cf)
parentJointId = robot.model.frames[parentFrameId].parent
for i in range(4):
frame_name = cf + "_" + str(i)
placement = pin.XYZQUATToSE3(
list(contact_point[:, i]) + [0, 0, 0, 1.])
placement = robot.model.frames[parentFrameId].placement * placement
fr = pin.Frame(frame_name, parentJointId, parentFrameId, placement,
pin.FrameType.OP_FRAME)
robot.model.addFrame(fr)
contact_frames += [frame_name]
conf.contact_frames = contact_frames
robot.data = robot.model.createData()
#conf.unilateral_contacts = 1
PRINT_N = int(conf.PRINT_T / dt)
ground_truth_file_name = robot_name + "_" + motionName + str(dt) + "_cpp.p"
nq, nv = robot.nq, robot.nv
if conf.use_viewer:
print("Could not detect part or hole. Detected tags {}".format(
detector.getTagsId()))
J.flush()
J.getClick()
space = liegroups.SE3()
# print("Gauss-Newton method")
tagResidual = TagResidual(space, bMhs_hole)
gn = mno.GaussNewton(space.nq, space.nv)
gn.xtol = 1e-8
gn.fxtol2 = 1e-11
gn.maxIter = 40
gn.verbose = True
x0 = bMhs_hole[0]
res, x = gn.minimize(tagResidual, x0, integrate=space.integrate)
pose = pinocchio.XYZQUATToSE3(x)
bMhs.append(pose)
print("hole {:03} pose: ".format(i))
print(pose)
def tagsInUrdfFormat(tags, bMis):
tag_fmt = """
<link name="tag36_11_{id:05}">
<visual>
<geometry>
<mesh filename="package://gerard_bauzil/meshes/apriltag_36h11/tag36_11_{id:05}.dae" scale="{size} {size} 1."/>
</geometry>
</visual>
</link>
<joint name="to_tag_{id:05}" type="fixed">
