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 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 computeCollisions(self, q, vq=None):
res = pio.computeCollisions(self.rmodel, self.rdata, self.gmodel,
self.gdata, q, False)
pio.computeDistances(self.rmodel, self.rdata, self.gmodel, self.gdata,
q)
pio.computeJointJacobians(self.rmodel, self.rdata, q)
if not (vq is None):
pio.forwardKinematics(self.rmodel, self.rdata, q, vq, 0 * vq)
return res
def testGeomConfig(x_rot, y_rot, collisionPair):
test_config = robot_config.copy()
test_config[7] = x_rot
test_config[8] = y_rot
pio.computeDistances(rmodel,rdata,gmodel,gdata, test_config)
pio.computeCollisions(rmodel, rdata, gmodel, gdata, test_config, True)
# Get distance and collision results from collision data
collisions_dist = gdata.distanceResults
collisions = gdata.collisionResults
is_col = collisions[collisionPair].isCollision() # pair 0 is between the leg and the body
dist = collisions_dist[collisionPair].min_distance
p1 = collisions_dist[collisionPair].getNearestPoint1()
p2 = collisions_dist[collisionPair].getNearestPoint2() # get nearest points on the segments
return is_col, dist, [p1,p2]
def is_auto_colliding(self, q):
"""
Check if the configuration is autocolliding
"""
model = self.wrapper.model
data = self.dummy_data
collision_model = self.wrapper.collision_model
collision_data = self.wrapper.collision_data
is_colliding = True
is_colliding = pin.computeCollisions(model, data, collision_model,
collision_data, q, True)
return is_colliding
def searchContactPosture(self, numberOfContacts, qguess=None, ntrial=100):
"""
Search (randomly) a contact configuration with a given number of contacts.
- number of contacts: between 1 and len(self.contactCandidates).
- qguess is a configuration, of dim rmodel.nq. If None, a random configuration is first
sampled.
- ntrial: number of random trial.
If successfull, set self.success to True, and store the contact posture found in self.qcontact.
If not sucessfull, set self.success to False.
Returns self.success.
"""
assert 0 < numberOfContacts < len(self.contactCandidates)
if qguess is None:
qguess = pin.randomConfiguration(self.rmodel)
for itrial in range(ntrial):
limbs = random.sample(list(self.contactCandidates.values()), numberOfContacts)
stones = random.sample(self.terrain, numberOfContacts)
constraints = [Constraint(self.rmodel, limb.frameIndex, stone, limb.contactType)
for limb, stone in zip(limbs, stones)]
self.postureGenerator.run(qguess, constraints)
if self.postureGenerator.sucess:
# We found a candidate posture, let' s check that it is acceptable.
qcandidate = self.postureGenerator.qopt.copy()
# computeCollisions check collision among the collision pairs.
collision = pin.computeCollisions(self.rmodel, self.rdata,
self.collision_model, self.collision_data, qcandidate, True)
if not collision:
self.qcontact = qcandidate
self.success = True
return True
self.success = False
return False
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
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 robotClogsFieldOfView(self):
"""whether the camera is clogged by the selected robot bodies. It assumes that updateGeometryPlacements has been called """
return pinocchio.computeCollisions(self.gmodel, self.gdata, True)
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
# 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]
cp = robot.collision_model.collisionPairs[k]
# print("collision pair:",cp.first,",",cp.second,"- collision:","Yes" if cr.isCollision() else "No") # Optionnal display of all the collision pairs
if cr.isCollision():
valid = False
print("## First configuration valid: ", valid)
# Move to a configuration in self collision
#
# Option open=True pens the visualizer.
# Note: the visualizer can also be opened seperately by visiting the provided URL.
try:
viz.initViewer(open=True)
except ImportError as err:
print(
"Error while initializing the viewer. It seems you should install Python meshcat"
)
print(err)
sys.exit(0)
# Load the robot in the viewer.
viz.loadViewerModel()
# Display a robot configuration.
q0 = pin.neutral(model)
viz.display(q0)
is_collision = False
data = model.createData()
collision_data = collision_model.createData()
while not is_collision:
q = pin.randomConfiguration(model)
is_collision = pin.computeCollisions(model, data, collision_model,
collision_data, q, True)
print("Found a configuration in collision:", q)
viz.display(q)
# 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)
