Buckets:
| import mujoco | |
| import numpy as np | |
| from ..geometry import quat_wxyz, relative_rotation | |
| SKIN = (0.86, 0.64, 0.52) | |
| HIDDEN_HEIGHT = -10.0 | |
| RADIUS = {"palm": 0.010, "finger": 0.0078, "tip": 0.0065} | |
| class HandRig: | |
| def __init__(self, name, hand, color=SKIN): | |
| self.name = name | |
| self.hand = hand | |
| self.skeleton = hand.skeleton | |
| self.color = np.array([*color, 1.0]) | |
| self.offsets = self._bone_offsets() | |
| def _bone_offsets(self): | |
| poses, skeleton = self.hand.poses, self.skeleton | |
| offsets = np.zeros((skeleton.size, 3)) | |
| for joint, parent in enumerate(skeleton.parents): | |
| if parent < 0: | |
| continue | |
| delta = poses[:, joint, :3, 3] - poses[:, parent, :3, 3] | |
| offsets[joint] = np.median( | |
| np.einsum("tji,tj->ti", poses[:, parent, :3, :3], delta), axis=0 | |
| ) | |
| return offsets | |
| def body_name(self, joint): | |
| return f"{self.name}_{self.skeleton.joints[joint]}" | |
| def _geom_style(self, child): | |
| if not self.skeleton.children[child]: | |
| return RADIUS["tip"], 0.8 * self.color | |
| if self.skeleton.parents[child] == 0: | |
| return RADIUS["palm"], self.color | |
| return RADIUS["finger"], self.color | |
| def attach(self, spec): | |
| skeleton, bodies = self.skeleton, [] | |
| for joint in range(skeleton.size): | |
| if skeleton.parents[joint] < 0: | |
| body = spec.worldbody.add_body(name=self.body_name(joint)) | |
| body.add_joint( | |
| name=self.body_name(joint), type=mujoco.mjtJoint.mjJNT_FREE | |
| ) | |
| else: | |
| body = bodies[skeleton.parents[joint]].add_body( | |
| name=self.body_name(joint), pos=self.offsets[joint] | |
| ) | |
| body.add_joint( | |
| name=self.body_name(joint), type=mujoco.mjtJoint.mjJNT_BALL | |
| ) | |
| bodies.append(body) | |
| for joint, body in enumerate(bodies): | |
| for child in skeleton.children[joint]: | |
| radius, rgba = self._geom_style(child) | |
| body.add_geom( | |
| type=mujoco.mjtGeom.mjGEOM_CAPSULE, | |
| fromto=[0, 0, 0, *self.offsets[child]], | |
| size=[radius, 0, 0], | |
| rgba=rgba, | |
| ) | |
| if not skeleton.children[joint]: | |
| body.add_geom( | |
| type=mujoco.mjtGeom.mjGEOM_SPHERE, | |
| size=[RADIUS["tip"], 0, 0], | |
| rgba=0.8 * self.color, | |
| ) | |
| def qpos(self, model): | |
| poses, skeleton = self.hand.poses, self.skeleton | |
| out = np.zeros((len(poses), model.nq)) | |
| wrist = model.joint(self.body_name(0)).qposadr[0] | |
| out[:, wrist : wrist + 3] = poses[:, 0, :3, 3] | |
| out[:, wrist + 2][~self.hand.visible] = HIDDEN_HEIGHT | |
| out[:, wrist + 3 : wrist + 7] = quat_wxyz(poses[:, 0, :3, :3]) | |
| for joint in range(1, skeleton.size): | |
| address = model.joint(self.body_name(joint)).qposadr[0] | |
| out[:, address : address + 4] = quat_wxyz( | |
| relative_rotation( | |
| poses[:, skeleton.parents[joint], :3, :3], poses[:, joint, :3, :3] | |
| ) | |
| ) | |
| return out | |
Xet Storage Details
- Size:
- 3.32 kB
- Xet hash:
- 2a75f186be52d69845c4d420ee9f497d205517dcf8163fedb88a4da23ffed611
·
Xet efficiently stores files, intelligently splitting them into unique chunks and accelerating uploads and downloads. More info.