code-HFD / scripts /viz /fk.py
Tinghong-Ye22's picture
Add files using upload-large-folder tool
9fc9873 verified
Raw History Blame Contribute Delete
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()