(self, leg_id, position, position_in_world_frame)
return joint_position_idxs, joint_angles.tolist()
Calculate the joint positions from the end effector position.
Calculate the joint positions from the end effector position.
Calculate the joint positions from the end effector position .
def _EndEffectorIK(self, leg_id, position, position_in_world_frame):
    """Calculate the joint positions from the end effector position."""
    assert len(self._foot_link_ids) == self.num_legs
    toe_id = self._foot_link_ids[leg_id]
    motors_per_leg = self.num_motors // self.num_legs
    joint_position_idxs = [
        i for i in range(leg_id * motors_per_leg, leg_id * motors_per_leg +
                         motors_per_leg)
    ]
    joint_angles = self.joint_angles_from_link_position(
        robot=self,
        link_position=position,
        link_id=toe_id,
        joint_ids=joint_position_idxs,
        position_in_world_frame=position_in_world_frame)
    # Joint offset is necessary for Laikago.
    joint_angles = np.multiply(
        np.asarray(joint_angles) -
        np.asarray(self._motor_offset)[joint_position_idxs],
        self._motor_direction[joint_position_idxs])
    # Return the joing index (the same as when calling GetMotorAngles) as well
    # as the angles.
    return joint_position_idxs, joint_angles.tolist()
def _EndEffectorIK ( self , leg_id , position , position_in_world_frame ) : assert len ( self . _foot_link_ids ) == self . num_legs toe_id = self . _foot_link_ids [ leg_id ] motors_per_leg = self . num_motors // self . num_legs joint_position_idxs = [ i for i in range ( leg_id * motors_per_leg , leg_id * motors_per_leg + motors_per_leg ) ] joint_angles = self . joint_angles_from_link_position ( robot = self , link_position = position , link_id = toe_id , joint_ids = joint_position_idxs , position_in_world_frame = position_in_world_frame ) # Joint offset is necessary for Laikago. joint_angles = np . multiply ( np . asarray ( joint_angles ) - np . asarray ( self . _motor_offset ) [ joint_position_idxs ] , self . _motor_direction [ joint_position_idxs ] ) # Return the joing index (the same as when calling GetMotorAngles) as well # as the angles. return joint_position_idxs , joint_angles . tolist ( )