def get_model_velocity(self) -> np.ndarray:
nu = idt.VectorDynSize()
if not self.kindyn.getModelVel(nu):
raise RuntimeError("Failed to get the model velocity")
return nu.toNumPy()
def get_joint_positions(self) -> np.ndarray:
vector = idt.VectorDynSize()
if not self.kindyn.getJointPos(vector):
raise RuntimeError("Failed to extract joint positions")
return vector.toNumPy()
def get_joint_velocities(self) -> np.ndarray:
vector = idt.VectorDynSize()
if not self.kindyn.getJointVel(vector):
raise RuntimeError("Failed to extract joint velocities")
return vector.toNumPy()
def get_reduced_solution(self) -> IKSolution:
# Initialize buffers
base_transform = idt.Transform.Identity()
joint_positions = idt.VectorDynSize(
self._ik.reducedModel().getNrOfJoints())
assert len(joint_positions) == len(self._considered_joints)
# Get the solution
self._ik.getReducedSolution(base_transform, joint_positions)
# Convert to numpy objects
joint_positions = joint_positions.toNumPy()
base_position = base_transform.getPosition().toNumPy()
base_quaternion = base_transform.getRotation().asQuaternion().toNumPy()
return IKSolution(base_position=base_position,
base_quaternion=base_quaternion,
joint_configuration=joint_positions)