Download scripts/viz/fk.py from Tinghong-Ye22/code-Psi0: direct link, hf CLI and curl.
- Browser
- Download file 6.17 kB
-
https://huggingface.co/Tinghong-Ye22/code-Psi0/resolve/main/scripts/viz/fk.py
- Command line
-
hf download hf://Tinghong-Ye22/code-Psi0/scripts/viz/fk.py
-
curl -L -o fk.py https://huggingface.co/Tinghong-Ye22/code-Psi0/resolve/main/scripts/viz/fk.py
6.17 kB
| import argparse | |
| import json | |
| import os | |
| import sys | |
| import numpy as np | |
| import pinocchio as pin | |
| LOCKED_MIXED = [ | |
| "left_hip_pitch_joint", | |
| "left_hip_roll_joint", | |
| "left_hip_yaw_joint", | |
| "left_knee_joint", | |
| "left_ankle_pitch_joint", | |
| "left_ankle_roll_joint", | |
| "right_hip_pitch_joint", | |
| "right_hip_roll_joint", | |
| "right_hip_yaw_joint", | |
| "right_knee_joint", | |
| "right_ankle_pitch_joint", | |
| "right_ankle_roll_joint", | |
| "waist_yaw_joint", | |
| "waist_roll_joint", | |
| "waist_pitch_joint", | |
| "left_hand_thumb_0_joint", | |
| "left_hand_thumb_1_joint", | |
| "left_hand_thumb_2_joint", | |
| "left_hand_middle_0_joint", | |
| "left_hand_middle_1_joint", | |
| "left_hand_index_0_joint", | |
| "left_hand_index_1_joint", | |
| "right_hand_thumb_0_joint", | |
| "right_hand_thumb_1_joint", | |
| "right_hand_thumb_2_joint", | |
| "right_hand_index_0_joint", | |
| "right_hand_index_1_joint", | |
| "right_hand_middle_0_joint", | |
| "right_hand_middle_1_joint", | |
| ] | |
| LOCKED_FINGERS_ONLY = [ | |
| "left_hand_thumb_0_joint", | |
| "left_hand_thumb_1_joint", | |
| "left_hand_thumb_2_joint", | |
| "left_hand_middle_0_joint", | |
| "left_hand_middle_1_joint", | |
| "left_hand_index_0_joint", | |
| "left_hand_index_1_joint", | |
| "right_hand_thumb_0_joint", | |
| "right_hand_thumb_1_joint", | |
| "right_hand_thumb_2_joint", | |
| "right_hand_index_0_joint", | |
| "right_hand_index_1_joint", | |
| "right_hand_middle_0_joint", | |
| "right_hand_middle_1_joint", | |
| ] | |
| class G1FK: | |
| def __init__(self, urdf_path, mesh_dir=None, mode="reduced"): | |
| if mesh_dir is None: | |
| mesh_dir = os.path.dirname(urdf_path) | |
| self.robot_full = pin.RobotWrapper.BuildFromURDF(urdf_path, mesh_dir) | |
| if mode == "reduced": | |
| to_lock = LOCKED_MIXED | |
| elif mode == "whole": | |
| to_lock = LOCKED_FINGERS_ONLY | |
| elif mode == "default": | |
| to_lock = [] | |
| else: | |
| raise ValueError("mode must be 'reduced' or 'whole'") | |
| if len(to_lock) > 0: | |
| qref = np.zeros(self.robot_full.model.nq) | |
| self.model = self.robot_full.buildReducedRobot( | |
| list_of_joints_to_lock=to_lock, reference_configuration=qref | |
| ).model | |
| else: | |
| self.model = self.robot_full.model | |
| self.add_frame("l_ee", "left_wrist_yaw_joint", [0.0, 0.0, 0.0]) | |
| self.add_frame("r_ee", "right_wrist_yaw_joint", [0.0, 0.0, 0.0]) | |
| self.add_frame("l_foot", "left_ankle_pitch_joint", [0.0, 0.0, -0.0]) | |
| self.add_frame("r_foot", "right_ankle_pitch_joint", [0.0, 0.0, -0.0]) | |
| self.data = self.model.createData() | |
| def add_frame(self, frame_name, parent_joint, offset): | |
| fid = self.model.getFrameId(frame_name) | |
| exists = fid < self.model.nframes and self.model.frames[fid].name == frame_name | |
| if not exists: | |
| jid = self.model.getJointId(parent_joint) | |
| if jid == 0: | |
| raise ValueError(f"Joint not found: {parent_joint}") | |
| self.model.addFrame( | |
| pin.Frame( | |
| frame_name, | |
| jid, | |
| pin.SE3( | |
| np.eye(3), | |
| np.array(offset, dtype=float).reshape( | |
| 3, | |
| ), | |
| ), | |
| pin.FrameType.OP_FRAME, | |
| ) | |
| ) | |
| def joints_to_q(self, joint_dict): | |
| q = pin.neutral(self.model) | |
| for name, val in joint_dict.items(): | |
| jid = self.model.getJointId(name) | |
| if jid == 0: | |
| print(f"[warn] joint not in model: {name}", file=sys.stderr) | |
| continue | |
| j = self.model.joints[jid] | |
| if j.nq == 1: | |
| q[j.idx_q] = float(val) | |
| else: | |
| print(f"[warn] joint {name} has nq={j.nq}; skipping", file=sys.stderr) | |
| return q | |
| def fk(self, joint_dict, base_frame="world"): | |
| q = self.joints_to_q(joint_dict) | |
| data = self.model.createData() | |
| pin.forwardKinematics(self.model, data, q) | |
| pin.updateFramePlacements(self.model, data) | |
| def pose_for(frame_name): | |
| fid = self.model.getFrameId(frame_name) | |
| if fid >= len(data.oMf): | |
| raise RuntimeError( | |
| f"Frame {frame_name} id={fid} >= nframes={len(data.oMf)}" | |
| ) | |
| oMf = data.oMf[fid] # world->frame | |
| if base_frame == "base": | |
| oMb = data.oMi[1] # root joint | |
| T = oMb.inverse() * oMf | |
| else: | |
| T = oMf | |
| return { | |
| "matrix": T.homogeneous.tolist(), | |
| "position": T.translation.tolist(), | |
| "quat_xyzw": T.rotation.tolist(), | |
| } | |
| return { | |
| "l_ee": pose_for("l_ee"), | |
| "r_ee": pose_for("r_ee"), | |
| "l_foot": pose_for("l_foot"), | |
| "r_foot": pose_for("r_foot") | |
| } | |
| def _parse_q(args): | |
| if args.qjson: | |
| with open(args.qjson, "r") as f: | |
| return json.load(f) | |
| if args.q: | |
| return json.loads(args.q) | |
| return {} | |
| def main(): | |
| ap = argparse.ArgumentParser(description="Unitree G1 FK for hands") | |
| ap.add_argument("--urdf", required=True, help="Path to g1_body29_hand14.urdf") | |
| ap.add_argument( | |
| "--meshdir", default=None, help="Directory with meshes (defaults to urdf dir)" | |
| ) | |
| ap.add_argument( | |
| "--mode", | |
| choices=["reduced", "whole"], | |
| default="reduced", | |
| help="reduced: lock hips+waist+fingers; whole: lock only fingers", | |
| ) | |
| ap.add_argument("--base_frame", choices=["world", "base"], default="world") | |
| group = ap.add_mutually_exclusive_group() | |
| group.add_argument("--q", help="JSON string of {joint_name: angle_rad}") | |
| group.add_argument("--qjson", help="Path to JSON file with {joint_name: angle_rad}") | |
| args = ap.parse_args() | |
| qdict = _parse_q(args) | |
| fk_solver = G1FK(args.urdf, args.meshdir, args.mode) | |
| out = fk_solver.fk(qdict, base_frame=args.base_frame) | |
| json.dump(out, sys.stdout, indent=2) | |
| if __name__ == "__main__": | |
| main() | |