#!/usr/bin/env python # Copyright 2025 The HuggingFace Inc. team. All rights reserved. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # # http://www.apache.org/licenses/LICENSE-2.0 # # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. import logging import os import sys import numpy as np logger = logging.getLogger(__name__) parent2_dir = os.path.dirname(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) sys.path.append(parent2_dir) class WeightedMovingFilter: def __init__(self, weights, data_size=14): self._window_size = len(weights) self._weights = np.array(weights) self._data_size = data_size self._filtered_data = np.zeros(self._data_size) self._data_queue = [] def _apply_filter(self): if len(self._data_queue) < self._window_size: return self._data_queue[-1] data_array = np.array(self._data_queue) temp_filtered_data = np.zeros(self._data_size) for i in range(self._data_size): temp_filtered_data[i] = np.convolve(data_array[:, i], self._weights, mode="valid")[-1] return temp_filtered_data def add_data(self, new_data): assert len(new_data) == self._data_size if len(self._data_queue) > 0 and np.array_equal( new_data, self._data_queue[-1] ): # skip duplicate data return if len(self._data_queue) >= self._window_size: self._data_queue.pop(0) self._data_queue.append(new_data) self._filtered_data = self._apply_filter() @property def filtered_data(self): return self._filtered_data class G1_29_ArmIK: # noqa: N801 def __init__(self, unit_test=False): import casadi import pinocchio as pin from huggingface_hub import snapshot_download from pinocchio import casadi as cpin self._pin = pin np.set_printoptions(precision=5, suppress=True, linewidth=200) self.unit_test = unit_test self.repo_path = snapshot_download("lerobot/unitree-g1-mujoco") urdf_path = os.path.join(self.repo_path, "assets", "g1_body29_hand14.urdf") mesh_dir = os.path.join(self.repo_path, "assets") self.robot = self._pin.RobotWrapper.BuildFromURDF(urdf_path, mesh_dir) self.mixed_jointsToLockIDs = [ "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", ] self.reduced_robot = self.robot.buildReducedRobot( list_of_joints_to_lock=self.mixed_jointsToLockIDs, reference_configuration=np.array([0.0] * self.robot.model.nq), ) # Arm joint names in G1 motor order (G1_29_JointArmIndex) self._arm_joint_names_g1 = [ "left_shoulder_pitch_joint", "left_shoulder_roll_joint", "left_shoulder_yaw_joint", "left_elbow_joint", "left_wrist_roll_joint", "left_wrist_pitch_joint", "left_wrist_yaw_joint", "right_shoulder_pitch_joint", "right_shoulder_roll_joint", "right_shoulder_yaw_joint", "right_elbow_joint", "right_wrist_roll_joint", "right_wrist_pitch_joint", "right_wrist_yaw_joint", ] # Pinocchio uses its own joint order in q; build index mapping. self._arm_joint_names_pin = sorted( self._arm_joint_names_g1, key=lambda name: self.reduced_robot.model.idx_qs[self.reduced_robot.model.getJointId(name)], ) logger.info(f"Pinocchio arm joint order: {self._arm_joint_names_pin}") self._arm_reorder_g1_to_pin = [ self._arm_joint_names_g1.index(name) for name in self._arm_joint_names_pin ] # Inverse mapping to return tau in G1 motor order. self._arm_reorder_pin_to_g1 = np.argsort(self._arm_reorder_g1_to_pin) self.reduced_robot.model.addFrame( self._pin.Frame( "L_ee", self.reduced_robot.model.getJointId("left_wrist_yaw_joint"), self._pin.SE3(np.eye(3), np.array([0.05, 0, 0]).T), self._pin.FrameType.OP_FRAME, ) ) self.reduced_robot.model.addFrame( self._pin.Frame( "R_ee", self.reduced_robot.model.getJointId("right_wrist_yaw_joint"), self._pin.SE3(np.eye(3), np.array([0.05, 0, 0]).T), self._pin.FrameType.OP_FRAME, ) ) # Creating Casadi models and data for symbolic computing self.cmodel = cpin.Model(self.reduced_robot.model) self.cdata = self.cmodel.createData() # Creating symbolic variables self.cq = casadi.SX.sym("q", self.reduced_robot.model.nq, 1) self.cTf_l = casadi.SX.sym("tf_l", 4, 4) self.cTf_r = casadi.SX.sym("tf_r", 4, 4) cpin.framesForwardKinematics(self.cmodel, self.cdata, self.cq) # Get the hand joint ID and define the error function self.L_hand_id = self.reduced_robot.model.getFrameId("L_ee") self.R_hand_id = self.reduced_robot.model.getFrameId("R_ee") self.translational_error = casadi.Function( "translational_error", [self.cq, self.cTf_l, self.cTf_r], [ casadi.vertcat( self.cdata.oMf[self.L_hand_id].translation - self.cTf_l[:3, 3], self.cdata.oMf[self.R_hand_id].translation - self.cTf_r[:3, 3], ) ], ) self.rotational_error = casadi.Function( "rotational_error", [self.cq, self.cTf_l, self.cTf_r], [ casadi.vertcat( cpin.log3(self.cdata.oMf[self.L_hand_id].rotation @ self.cTf_l[:3, :3].T), cpin.log3(self.cdata.oMf[self.R_hand_id].rotation @ self.cTf_r[:3, :3].T), ) ], ) # Defining the optimization problem self.opti = casadi.Opti() self.var_q = self.opti.variable(self.reduced_robot.model.nq) self.var_q_last = self.opti.parameter(self.reduced_robot.model.nq) # for smooth self.param_tf_l = self.opti.parameter(4, 4) self.param_tf_r = self.opti.parameter(4, 4) self.translational_cost = casadi.sumsqr( self.translational_error(self.var_q, self.param_tf_l, self.param_tf_r) ) self.rotation_cost = casadi.sumsqr( self.rotational_error(self.var_q, self.param_tf_l, self.param_tf_r) ) self.regularization_cost = casadi.sumsqr(self.var_q) self.smooth_cost = casadi.sumsqr(self.var_q - self.var_q_last) # Setting optimization constraints and goals self.opti.subject_to( self.opti.bounded( self.reduced_robot.model.lowerPositionLimit, self.var_q, self.reduced_robot.model.upperPositionLimit, ) ) self.opti.minimize( 50 * self.translational_cost + self.rotation_cost + 0.02 * self.regularization_cost + 0.1 * self.smooth_cost ) opts = { "ipopt": {"print_level": 0, "max_iter": 50, "tol": 1e-6}, "print_time": False, # print or not "calc_lam_p": False, # https://github.com/casadi/casadi/wiki/FAQ:-Why-am-I-getting-%22NaN-detected%22in-my-optimization%3F } self.opti.solver("ipopt", opts) self.init_data = np.zeros(self.reduced_robot.model.nq) self.smooth_filter = WeightedMovingFilter(np.array([0.4, 0.3, 0.2, 0.1]), 14) def solve_ik(self, left_wrist, right_wrist, current_lr_arm_motor_q=None, current_lr_arm_motor_dq=None): if current_lr_arm_motor_q is not None: self.init_data = current_lr_arm_motor_q self.opti.set_initial(self.var_q, self.init_data) self.opti.set_value(self.param_tf_l, left_wrist) self.opti.set_value(self.param_tf_r, right_wrist) self.opti.set_value(self.var_q_last, self.init_data) # for smooth try: self.opti.solve() sol_q = self.opti.value(self.var_q) self.smooth_filter.add_data(sol_q) sol_q = self.smooth_filter.filtered_data if current_lr_arm_motor_dq is not None: v = current_lr_arm_motor_dq * 0.0 else: v = (sol_q - self.init_data) * 0.0 self.init_data = sol_q sol_tauff = self._pin.rnea( self.reduced_robot.model, self.reduced_robot.data, sol_q, v, np.zeros(self.reduced_robot.model.nv), ) return sol_q, sol_tauff except Exception as e: logger.error(f"ERROR in convergence, plotting debug info.{e}") sol_q = self.opti.debug.value(self.var_q) self.smooth_filter.add_data(sol_q) sol_q = self.smooth_filter.filtered_data if current_lr_arm_motor_dq is not None: v = current_lr_arm_motor_dq * 0.0 else: v = (sol_q - self.init_data) * 0.0 self.init_data = sol_q logger.error( f"sol_q:{sol_q} \nmotorstate: \n{current_lr_arm_motor_q} \nleft_pose: \n{left_wrist} \nright_pose: \n{right_wrist}" ) return current_lr_arm_motor_q, np.zeros(self.reduced_robot.model.nv) def solve_tau(self, current_lr_arm_motor_q=None, current_lr_arm_motor_dq=None): try: q_g1 = np.array(current_lr_arm_motor_q, dtype=float) if q_g1.shape[0] != len(self._arm_joint_names_g1): raise ValueError(f"Expected {len(self._arm_joint_names_g1)} arm joints, got {q_g1.shape[0]}") q_pin = q_g1[self._arm_reorder_g1_to_pin] sol_tauff = self._pin.rnea( self.reduced_robot.model, self.reduced_robot.data, q_pin, np.zeros(self.reduced_robot.model.nv), np.zeros(self.reduced_robot.model.nv), ) return sol_tauff[self._arm_reorder_pin_to_g1] except Exception as e: logger.error(f"ERROR in convergence, plotting debug info.{e}") return np.zeros(self.reduced_robot.model.nv)