def display(self, q, force):
"""Display the robot at configuration q in the viewer by placing all the bodies."""
gui = self.gui
pin.framesForwardKinematics(self.model,self.data,q)
if self.display_visuals:
pin.updateGeometryPlacements(self.model, self.data, self.visual_model, self.visual_data)
gui.applyConfigurations (
[ self.getViewerNodeName(visual,pin.GeometryType.VISUAL) for visual in self.visual_model.geometryObjects ],
[ pin.SE3ToXYZQUATtuple(self.visual_data.oMg[self.visual_model.getGeometryId(visual.name)]) for visual in self.visual_model.geometryObjects ]
)
for contactIndex, contactName in enumerate(self.contactNames):
forceVector = np.zeros(3)
if(self.type=="Biped"):
for i in range(4):
forceVector += force[:,4*contactIndex+i]
elif(self.type=="Quadruped"):
forceVector =force[:,contactIndex]
forceNorm = np.linalg.norm(forceVector)
if forceNorm<=1.e-3:
forceVisiblity = "OFF"
else:
forceVisiblity = "ON"
self.displayContact(contactName, forceVector, forceVisiblity)
gui.refresh()
def collisionDistance(q):
'''Return the minimal distance between robot and environment. '''
threshold = 1e-2
pin.updateGeometryPlacements(robot.model,robot.data,robot.collision_model,robot.collision_data,q)
if pin.computeCollisions(robot.collision_model,robot.collision_data,False): return -threshold
idx = pin.computeDistances(robot.collision_model,robot.collision_data)
return pin.computeDistance(robot.collision_model,robot.collision_data,idx).min_distance - threshold
def display(self,q):
pio.forwardKinematics(self.model,self.data,q)
pio.updateGeometryPlacements(self.model,self.data,self.gmodel,self.gdata)
if self.viewer is None: return
for i,g in enumerate(self.gmodel.geometryObjects):
self.viewer.applyConfiguration(g.name, pio.se3ToXYZQUATtuple(self.gdata.oMg[i]))
self.viewer.refresh()
def collision(self, qw):
if isinstance(qw, ConfigurationWrapper):
q = qw.q
elif isinstance(qw, np.ndarray):
qw = ConfigurationWrapper(self, qw)
q = qw.q
else:
raise ValueError(
"qw should either be a ConfigurationWrapper or a numpy array.")
if q.shape[0] != self._model.nq:
raise ValueError(
f"The given configuration vector is of shape {q.shape[0]} while \
the model requires a configuration vector of shape {self._model.nq}"
)
model = self._model
data = self._data
geom_model = self._geom_model
geom_data = self._geom_data
pin.forwardKinematics(model, data, q)
pin.updateGeometryPlacements(model, data, geom_model, geom_data)
qw.oMi = data.oMi.tolist()
qw.oMg = geom_data.oMg.tolist()
# stop at the first collision
collide = pin.computeCollisions(geom_model, geom_data, True)
return collide
def compute_oMg(self, qw):
q = qw.q
model = self._model
data = self._data
geom_model = self._geom_model
geom_data = self._geom_data
pin.forwardKinematics(model, data, q)
pin.updateGeometryPlacements(model, data, geom_model, geom_data, q)
qw.oMg = geom_data.oMg.tolist()
def display(self, q):
"""Display the robot at configuration q in the viewer by placing all the bodies."""
pin.forwardKinematics(self.model, self.data, q)
pin.updateGeometryPlacements(self.model, self.data, self.visual_model,
self.visual_data)
for visual in self.visual_model.geometryObjects:
# Get mesh pose.
M = self.visual_data.oMg[self.visual_model.getGeometryId(visual.name)]
# Manage scaling
S = np.diag(np.concatenate((visual.meshScale, np.array([1.0]))).flat)
T = np.array(M.homogeneous).dot(S)
# Update viewer configuration.
self.viewer[self.getViewerNodeName(
visual, pin.GeometryType.VISUAL)].set_transform(T)
def _updateGeometryPlacements(self, visual=False):
"""
@brief Update the generalized position of a geometry object.
@remark This is a hidden function not intended to be called manually.
@param[in] visual Wether it is a visual or collision update
"""
if visual:
geom_model = self._rb.visual_model
geom_data = self._rb.visual_data
else:
geom_model = self._rb.collision_model
geom_data = self._rb.collision_data
pin.updateGeometryPlacements(self.pinocchio_model, self.pinocchio_data,
geom_model, geom_data)
def display(self, q: np.ndarray):
"""Display the robot at configuration q in the viewer by placing all
the bodies."""
pin.forwardKinematics(self.model, self.data, q)
if self.display_visuals:
pin.updateGeometryPlacements(self.model, self.data,
self.visual_model, self.visual_data)
for i, visual in enumerate(self.visual_model.geometryObjects):
# Get mesh pose
M = self.visual_data.oMg[i]
# Manage scaling
S = np.diag(
np.concatenate((visual.meshScale, np.array([1.0]))).flat)
T = np.array(M.homogeneous).dot(S)
# Update viewer configuration
nodeName = self.getViewerNodeName(visual,
pin.GeometryType.VISUAL)
self.viewer[nodeName].set_transform(T)
if self.display_collisions:
pin.updateGeometryPlacements(self.model, self.data,
self.collision_model,
self.collision_data)
for i, collision in enumerate(
self.collision_model.geometryObjects):
# Get mesh pose
M = self.collision_data.oMg[i]
# Manage scaling
S = np.diag(
np.concatenate(
(collision.meshScale, np.array([1.0]))).flat)
T = np.array(M.homogeneous).dot(S)
# Update viewer configuration
nodeName = self.getViewerNodeName(collision,
pin.GeometryType.collision)
self.viewer[nodeName].set_transform(T)
def reduceGeomModel(model,
data,
modelReduced,
dataReduced,
geomModel,
geomData,
q=None,
qReduced=None):
'''
Given a kinematic model and its reduced version, a geometry model corresponding
to the unreduced model, the corresponding data and a reference configuration (in
both unreduced and reduced version), modify the geometry model to make it corresponds
to the reduced kinematic tree.
'''
if q is not None:
pio.forwardKinematics(model, data, q)
pio.updateGeometryPlacements(model, data, geomModel, geomData)
if qReduced is not None:
pio.forwardKinematics(modelReduced, dataReduced, qReduced)
for igeom, geom in enumerate(geomModel.geometryObjects):
jname = model.names[geom.parentJoint]
if (modelReduced.existJointName(jname)):
# Case 1: the parent joint just change ID, not name
geom.parentJoint = modelReduced.getJointId(jname)
else:
# Case 2: Geom is now orphan'
# Get joint ID from the parent frame
frame = modelReduced.frames[modelReduced.getFrameId(
model.frames[geom.parentFrame].name)]
geom.parentJoint = frame.parent
# Change geom placement wrt new joint
oMg = geomData.oMg[igeom]
oMj = dataReduced.oMi[geom.parentJoint]
jMg = oMj.inverse() * oMg
geom.placement = jMg
def update_quantities(robot: jiminy.Model,
position: np.ndarray,
velocity: Optional[np.ndarray] = None,
acceleration: Optional[np.ndarray] = None,
update_physics: bool = True,
update_com: bool = True,
update_energy: bool = True,
update_jacobian: bool = False,
update_collisions: bool = True,
use_theoretical_model: bool = True) -> None:
"""Compute all quantities using position, velocity and acceleration
configurations.
Run multiple algorithms to compute all quantities which can be known with
the model position, velocity and acceleration.
This includes:
- body spatial transforms,
- body spatial velocities,
- body spatial drifts,
- body transform acceleration,
- body transform jacobians,
- center-of-mass position,
- center-of-mass velocity,
- center-of-mass drift,
- center-of-mass acceleration,
- center-of-mass jacobian,
- articular inertia matrix,
- non-linear effects (Coriolis + gravity)
- collisions and distances
.. note::
Computation results are stored internally in the robot, and can
be retrieved with associated getters.
.. warning::
This function modifies the internal robot data.
.. warning::
It does not called overloaded pinocchio methods provided by
`jiminy_py.core` but the original pinocchio methods instead. As a
result, it does not take into account the rotor inertias / armatures.
One is responsible of calling the appropriate methods manually instead
of this one if necessary. This behavior is expected to change in the
future.
:param robot: Jiminy robot.
:param position: Robot position vector.
:param velocity: Robot velocity vector.
:param acceleration: Robot acceleration vector.
:param update_physics: Whether or not to compute the non-linear effects and
internal/external forces.
Optional: True by default.
:param update_com: Whether or not to compute the COM of the robot AND each
link individually. The global COM is the first index.
Optional: False by default.
:param update_energy: Whether or not to compute the energy of the robot.
Optional: False by default
:param update_jacobian: Whether or not to compute the jacobians.
Optional: False by default.
:param use_theoretical_model: Whether the state corresponds to the
theoretical model when updating and fetching
the robot's state.
Optional: True by default.
"""
if use_theoretical_model:
pnc_model = robot.pinocchio_model_th
pnc_data = robot.pinocchio_data_th
else:
pnc_model = robot.pinocchio_model
pnc_data = robot.pinocchio_data
if (update_physics and update_com and
update_energy and update_jacobian and
velocity is not None and acceleration is None):
pin.computeAllTerms(pnc_model, pnc_data, position, velocity)
else:
if velocity is None:
pin.forwardKinematics(pnc_model, pnc_data, position)
elif acceleration is None:
pin.forwardKinematics(pnc_model, pnc_data, position, velocity)
else:
pin.forwardKinematics(
pnc_model, pnc_data, position, velocity, acceleration)
if update_com:
if velocity is None:
pin.centerOfMass(pnc_model, pnc_data, position)
elif acceleration is None:
pin.centerOfMass(pnc_model, pnc_data, position, velocity)
else:
pin.centerOfMass(
pnc_model, pnc_data, position, velocity, acceleration)
if update_jacobian:
if update_com:
pin.jacobianCenterOfMass(pnc_model, pnc_data)
pin.computeJointJacobians(pnc_model, pnc_data)
if update_physics:
if velocity is not None:
pin.nonLinearEffects(pnc_model, pnc_data, position, velocity)
pin.crba(pnc_model, pnc_data, position)
if update_energy:
if velocity is not None:
pin.computeKineticEnergy(pnc_model, pnc_data)
pin.computePotentialEnergy(pnc_model, pnc_data)
pin.updateFramePlacements(pnc_model, pnc_data)
if update_collisions:
pin.updateGeometryPlacements(
pnc_model, pnc_data, robot.collision_model, robot.collision_data)
pin.computeCollisions(
robot.collision_model, robot.collision_data,
stop_at_first_collision=False)
pin.computeDistances(robot.collision_model, robot.collision_data)
for dist_req in robot.collision_data.distanceResults:
if np.linalg.norm(dist_req.normal) < 1e-6:
pin.computeDistances(
robot.collision_model, robot.collision_data)
break
urdf_model_path, mesh_dir)
print('model name: ' + model.name)
# Create data required by the algorithms
data, collision_data, visual_data = pinocchio.createDatas(
model, collision_model, visual_model)
# Sample a random configuration
q = pinocchio.randomConfiguration(model)
print('q: %s' % q.T)
# Perform the forward kinematics over the kinematic tree
pinocchio.forwardKinematics(model, data, q)
# Update Geometry models
pinocchio.updateGeometryPlacements(model, data, collision_model,
collision_data)
pinocchio.updateGeometryPlacements(model, data, visual_model, visual_data)
# Print out the placement of each joint of the kinematic tree
print("\nJoint placements:")
for name, oMi in zip(model.names, data.oMi):
print(("{:<24} : {: .2f} {: .2f} {: .2f}".format(name,
*oMi.translation.T.flat)))
# Print out the placement of each collision geometry object
print("\nCollision object placements:")
for k, oMg in enumerate(collision_data.oMg):
print(("{:d} : {: .2f} {: .2f} {: .2f}".format(k,
*oMg.translation.T.flat)))
# Print out the placement of each visual geometry object
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)
def coll(q):
'''Return true if in collision, false otherwise.'''
pin.updateGeometryPlacements(robot.model,robot.data,robot.collision_model,robot.collision_data,q)
return pin.computeCollisions(robot.collision_model,robot.collision_data,False)
def updateGeometryPlacements(self, q):
pinocchio.framesForwardKinematics(self.model, self.data, np.array(q))
pinocchio.updateGeometryPlacements(self.model, self.data, self.gmodel,
self.gdata)
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
# Load collision geometries geom_model = pin.buildGeomFromUrdf(model,urdf_model_path,[model_path],pin.GeometryType.COLLISION) # Add collisition pairs geom_model.addAllCollisionPairs() # Remove collision pairs listed in the SRDF file srdf_filename = "romeo_small.srdf" srdf_model_path = model_path + "/romeo_description/srdf/" + srdf_filename pin.removeCollisionPairs(model,geom_model,srdf_model_path) # 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)
