fastdev.robo.kinematics

Module Contents

fastdev.robo.kinematics.forward_kinematics(joint_values: jaxtyping.Float[torch.Tensor, ... total_num_joints], articulation: fastdev.robo.articulation.Articulation, root_poses: Optional[jaxtyping.Float[torch.Tensor, ... num_arti 4 4]] = None) → jaxtyping.Float[torch.Tensor, ... total_num_links 4 4]

Parameters:

• joint_values (jaxtyping.Float[torch.Tensor, ... total_num_joints])

• articulation (fastdev.robo.articulation.Articulation)

• root_poses (Optional[jaxtyping.Float[torch.Tensor, ... num_arti 4 4]])

Return type:

jaxtyping.Float[torch.Tensor, … total_num_links 4 4]

fastdev.robo.kinematics.calculate_jacobian(joint_values: jaxtyping.Float[torch.Tensor, ... total_num_joints], target_link_indices: jaxtyping.Int[torch.Tensor, calculate_jacobian.num_arti], articulation: fastdev.robo.articulation.Articulation, root_poses: Optional[jaxtyping.Float[torch.Tensor, ... num_arti 4 4]] = None, return_target_link_poses: bool = False) → Union[jaxtyping.Float[torch.Tensor, ... 6 total_num_joints], Tuple[jaxtyping.Float[torch.Tensor, ... 6 total_num_joints], jaxtyping.Float[torch.Tensor, ... num_arti 4 4]]]

Calculate the geometric Jacobian for the end-effector for each joint in the articulated robot.

The Jacobian is computed in the world frame. For revolute joints, the linear velocity component is computed as the cross product of the joint axis (in world frame) and the vector from the joint position to the end-effector position, while the angular velocity component is the joint axis. For prismatic joints, the linear velocity component is the joint axis and the angular component is zero.

Parameters:

• joint_values (jaxtyping.Float[torch.Tensor, ... total_num_joints])

• target_link_indices (jaxtyping.Int[torch.Tensor, calculate_jacobian.num_arti])

• articulation (fastdev.robo.articulation.Articulation)

• root_poses (Optional[jaxtyping.Float[torch.Tensor, ... num_arti 4 4]])

• return_target_link_poses (bool)

Return type:

Union[jaxtyping.Float[torch.Tensor, … 6 total_num_joints], Tuple[jaxtyping.Float[torch.Tensor, … 6 total_num_joints], jaxtyping.Float[torch.Tensor, … num_arti 4 4]]]

fastdev.robo.kinematics.delta_pose(T_current: torch.Tensor, pos_target: torch.Tensor, quat_target: torch.Tensor) → torch.Tensor

Compute the error between current and target poses.

Parameters:

• T_current (torch.Tensor)

• pos_target (torch.Tensor)

• quat_target (torch.Tensor)

Return type:

torch.Tensor

fastdev.robo.kinematics.inverse_kinematics(target_link_poses: jaxtyping.Float[torch.Tensor, ... num_arti 4 4], target_link_indices: jaxtyping.Int[torch.Tensor, inverse_kinematics.num_arti], articulation: fastdev.robo.articulation.Articulation, max_iterations: int = 100, learning_rate: float = 0.1, tolerance: float = 1e-06, damping: float = 0.01, num_retries: int = 50, init_joint_values: Optional[jaxtyping.Float[torch.Tensor, ... total_num_dofs]] = None, jitter_strength: float = 1.0) → Tuple[jaxtyping.Float[torch.Tensor, ... total_num_dofs], jaxtyping.Bool[torch.Tensor, ... num_arti]]

Parameters:

• target_link_poses (jaxtyping.Float[torch.Tensor, ... num_arti 4 4])

• target_link_indices (jaxtyping.Int[torch.Tensor, inverse_kinematics.num_arti])

• articulation (fastdev.robo.articulation.Articulation)

• max_iterations (int)

• learning_rate (float)

• tolerance (float)

• damping (float)

• num_retries (int)

• init_joint_values (Optional[jaxtyping.Float[torch.Tensor, ... total_num_dofs]])

• jitter_strength (float)

Return type:

Tuple[jaxtyping.Float[torch.Tensor, … total_num_dofs], jaxtyping.Bool[torch.Tensor, … num_arti]]