def joint_angles_from_link_position(
robot: typing.Any,
link_position: typing.Sequence[float],
link_id: int,
joint_ids: typing.Sequence[int],
position_in_world_frame=False,
base_translation: typing.Sequence[float] = (0, 0, 0),
base_rotation: typing.Sequence[float] = (0, 0, 0, 1)):
"""Uses Inverse Kinematics to calculate joint angles.
Args:
robot: A robot instance.
link_position: The (x, y, z) of the link in the body or the world frame,
depending on whether the argument position_in_world_frame is true.
link_id: The link id as returned from loadURDF.
joint_ids: The positional index of the joints. This can be different from
the joint unique ids.
position_in_world_frame: Whether the input link_position is specified
in the world frame or the robot's base frame.
base_translation: Additional base translation.
base_rotation: Additional base rotation.
Returns:
A list of joint angles.
"""
if not position_in_world_frame:
# Projects to local frame.
base_position, base_orientation = robot.GetBasePosition(
), robot.GetBaseOrientation()
base_position, base_orientation = robot.pybullet_client.multiplyTransforms(
base_position, base_orientation, base_translation, base_rotation)
# Projects to world space.
world_link_pos, _ = robot.pybullet_client.multiplyTransforms(
base_position, base_orientation, link_position,
_IDENTITY_ORIENTATION)
else:
world_link_pos = link_position
ik_solver = 0
all_joint_angles = robot.pybullet_client.calculateInverseKinematics(
robot.quadruped, link_id, world_link_pos, solver=ik_solver)
# Extract the relevant joint angles.
joint_angles = [all_joint_angles[i] for i in joint_ids]
return joint_angles
def joint_angles_from_link_position(
robot: typing.Any,
link_position: typing.Sequence[float],
link_id: int,
joint_ids: typing.Sequence[int],
base_translation: typing.Sequence[float] = (0, 0, 0),
base_rotation: typing.Sequence[float] = (0, 0, 0, 1)):
"""Uses Inverse Kinematics to calculate joint angles.
Args:
robot: A robot instance.
link_position: The (x, y, z) of the link in the body frame. This local frame
is transformed relative to the COM frame using a given translation and
rotation.
link_id: The link id as returned from loadURDF.
joint_ids: The positional index of the joints. This can be different from
the joint unique ids.
base_translation: Additional base translation.
base_rotation: Additional base rotation.
Returns:
A list of joint angles.
"""
# Projects to local frame.
base_position, base_orientation = robot.GetBasePosition(
), robot.GetBaseOrientation()
base_position, base_orientation = robot.pybullet_client.multiplyTransforms(
base_position, base_orientation, base_translation, base_rotation)
# Projects to world space.
world_link_pos, _ = robot.pybullet_client.multiplyTransforms(
base_position, base_orientation, link_position, _IDENTITY_ORIENTATION)
ik_solver = 0
all_joint_angles = robot.pybullet_client.calculateInverseKinematics(
robot.quadruped, link_id, world_link_pos, solver=ik_solver)
# Extract the relevant joint angles.
joint_angles = [all_joint_angles[i] for i in joint_ids]
return joint_angles
def link_position_in_base_frame(
robot: typing.Any,
link_id: int,
):
"""Computes the link's local position in the robot frame.
Args:
robot: A robot instance.
link_id: The link to calculate its relative position.
Returns:
The relative position of the link.
"""
base_position, base_orientation = robot.GetBasePosition(
), robot.GetBaseOrientation()
inverse_translation, inverse_rotation = robot.pybullet_client.invertTransform(
base_position, base_orientation)
link_state = robot.pybullet_client.getLinkState(robot.quadruped, link_id)
link_position = link_state[0]
link_local_position, _ = robot.pybullet_client.multiplyTransforms(
inverse_translation, inverse_rotation, link_position, (0, 0, 0, 1))
return np.array(link_local_position)
