5 from .
import pinocchio_pywrap_default
as pin
7 from .shortcuts
import buildModelsFromUrdf, createDatas, buildModelsFromSdf
15 filename, package_dirs=None, root_joint=None, verbose=False, meshLoader=None
18 robot.initFromURDF(filename, package_dirs, root_joint, verbose, meshLoader)
30 filename, package_dirs, root_joint, verbose, meshLoader
32 RobotWrapper.__init__(
35 collision_model=collision_model,
36 visual_model=visual_model,
80 RobotWrapper.__init__(
83 collision_model=collision_model,
84 visual_model=visual_model,
89 self, model=pin.Model(), collision_model=
None, visual_model=
None, verbose=
False
96 model, collision_model, visual_model
99 self.
v0 = utils.zero(self.
nv)
112 def com(self, q=None, v=None, a=None):
115 return self.
data.com[0]
118 pin.centerOfMass(self.
model, self.
data, q, v)
119 return self.
data.com[0], self.
data.vcom[0]
120 pin.centerOfMass(self.
model, self.
data, q, v, a)
121 return self.
data.com[0], self.
data.vcom[0], self.
data.acom[0]
122 return pin.centerOfMass(self.
model, self.
data, q)
125 pin.centerOfMass(self.
model, self.
data, q, v)
126 return self.
data.vcom[0]
129 pin.centerOfMass(self.
model, self.
data, q, v, a)
130 return self.
data.acom[0]
133 return pin.computeCentroidalMomentum(self.
model, self.
data, q, v)
137 Computes the centroidal momentum matrix which maps from the joint velocity vector to the centroidal momentum expressed around the center of mass.
139 return pin.computeCentroidalMap(self.
model, self.
data, q)
143 Computes all the quantities related to the centroidal dynamics (hg, Ag and Ig), corresponding to the centroidal momentum, the centroidal map and the centroidal rigid inertia.
149 return pin.computeCentroidalMomentumTimeVariation(
154 return pin.jacobianCenterOfMass(self.
model, self.
data, q)
157 return pin.crba(self.
model, self.
data, q)
160 return pin.nonLinearEffects(self.
model, self.
data, q, v)
163 return pin.computeGeneralizedGravity(self.
model, self.
data, q)
168 pin.forwardKinematics(self.
model, self.
data, q, v, a)
170 pin.forwardKinematics(self.
model, self.
data, q, v)
172 pin.forwardKinematics(self.
model, self.
data, q)
175 if update_kinematics:
176 pin.forwardKinematics(self.
model, self.
data, q)
177 return self.
data.oMi[index]
184 update_kinematics=True,
185 reference_frame=pin.ReferenceFrame.LOCAL,
187 if update_kinematics:
188 pin.forwardKinematics(self.
model, self.
data, q, v)
189 return pin.getVelocity(self.
model, self.
data, index, reference_frame)
197 update_kinematics=True,
198 reference_frame=pin.ReferenceFrame.LOCAL,
200 if update_kinematics:
201 pin.forwardKinematics(self.
model, self.
data, q, v, a)
202 return pin.getAcceleration(self.
model, self.
data, index, reference_frame)
210 update_kinematics=True,
211 reference_frame=pin.ReferenceFrame.LOCAL,
213 if update_kinematics:
214 pin.forwardKinematics(self.
model, self.
data, q, v, a)
215 return pin.getClassicalAcceleration(
216 self.
model, self.
data, index, reference_frame
220 if update_kinematics:
221 pin.forwardKinematics(self.
model, self.
data, q)
222 return pin.updateFramePlacement(self.
model, self.
data, index)
229 update_kinematics=True,
230 reference_frame=pin.ReferenceFrame.LOCAL,
232 if update_kinematics:
233 pin.forwardKinematics(self.
model, self.
data, q, v)
234 return pin.getFrameVelocity(self.
model, self.
data, index, reference_frame)
242 update_kinematics=True,
243 reference_frame=pin.ReferenceFrame.LOCAL,
245 if update_kinematics:
246 pin.forwardKinematics(self.
model, self.
data, q, v, a)
247 return pin.getFrameAcceleration(self.
model, self.
data, index, reference_frame)
255 update_kinematics=True,
256 reference_frame=pin.ReferenceFrame.LOCAL,
258 if update_kinematics:
259 pin.forwardKinematics(self.
model, self.
data, q, v, a)
260 return pin.getFrameClassicalAcceleration(
261 self.
model, self.
data, index, reference_frame
265 return pin.computeJointJacobian(self.
model, self.
data, q, index)
268 return pin.getJointJacobian(self.
model, self.
data, index, rf_frame)
271 return pin.computeJointJacobians(self.
model, self.
data, q)
282 pin.updateGeometryPlacements(
283 self.
model, self.
data, geom_model, geom_data, q
286 pin.updateGeometryPlacements(self.
model, self.
data, geom_model, geom_data)
289 pin.framesForwardKinematics(self.
model, self.
data, q)
293 Build a reduced robot model given a list of joints to lock.
295 \tlist_of_joints_to_lock: list of joint indexes/names to lock.
296 \treference_configuration: reference configuration to compute the
297 placement of the lock joints. If not provided, reference_configuration
298 defaults to the robot's neutral configuration.
300 Returns: a new robot model.
305 for jnt
in list_of_joints_to_lock:
307 if isinstance(jnt, str):
308 idx = self.
model.getJointId(jnt)
309 lockjoints_idx.append(idx)
311 if reference_configuration
is None:
312 reference_configuration = pin.neutral(self.
model)
314 model, geom_models = pin.buildReducedModel(
317 list_of_joints_to_lock=lockjoints_idx,
318 reference_configuration=reference_configuration,
322 model=model, visual_model=geom_models[0], collision_model=geom_models[1]
327 It computes the Jacobian of frame given by its id (frame_id) either expressed in the
328 local coordinate frame or in the world coordinate frame.
330 return pin.getFrameJacobian(self.
model, self.
data, frame_id, rf_frame)
334 Similar to getFrameJacobian but does not need pin.computeJointJacobians and
335 pin.updateFramePlacements to update internal value of self.data related to frames.
337 return pin.computeFrameJacobian(self.
model, self.
data, q, frame_id)
340 """Re-build the data objects. Needed if the models were modified.
341 Warning: this will delete any information stored in all data objects."""
345 if self.
viz is not None:
352 self.
viz.collision_data = collision_data
353 self.
viz.visual_data = visual_data
363 return self.
model.getJointId(name)
370 return self.
viz.viewer
373 """Set the visualizer. If init is True, the visualizer is initialized with this wrapper's models.
374 If copy_models is also True, the models are copied. Otherwise, they are simply kept as a reference.
380 self.
viz = visualizer
383 """For each geometry object, returns the corresponding name of the node in the display."""
387 """Init the viewer"""
390 from .visualize
import GepettoVisualizer
392 data, collision_data, visual_data =
None,
None,
None
397 self.
viz = GepettoVisualizer(
410 """Create the scene displaying the robot meshes in gepetto-viewer"""
414 """Display the robot at configuration q in the viewer by placing all the bodies."""
418 """Set whether to diplay collision objects or not"""
422 """Set whether to diplay visual objects or not"""
425 def play(self, q_trajectory, dt):
426 """Play a trajectory with given time step"""
427 self.
viz.
play(q_trajectory, dt)
430 __all__ = [
"RobotWrapper"]