This commit is contained in:
yjinzero 2025-02-16 21:29:06 +09:00
parent 940861378a
commit 6e8cb33b3f
1 changed files with 1 additions and 1 deletions

View File

@ -69,7 +69,7 @@ class FakeWorldPublisher(Node):
def pelvis_height(self, low_state: LowStateHG): def pelvis_height(self, low_state: LowStateHG):
robot = self.robot robot = self.robot
q_mot = [self.low_state.motor_state[i_mot].q for i_mot in range(29)] q_mot = [self.low_state.motor_state[i_mot].q for i_mot in range(29)]
q_pin = np.zeros_like(self.ikctrl.cfg.q) q_pin = np.zeros_like(self.robot.q0)
q_pin[self.pin_from_mot] = q_mot q_pin[self.pin_from_mot] = q_mot
cfg = pink.Configuration(robot.model, robot.data, q_pin) cfg = pink.Configuration(robot.model, robot.data, q_pin)