Kinematics

class pyrobot.algorithms.kinematics.Kinematics(configs, world, ros_launch_manager=None, robots={}, sensors={}, algorithms={})

Bases: pyrobot.algorithms.algorithm.Algorithm

Base class of Kinematics algorithms. Specifically, this algorithm handles computing of forward/inverse kinematics, and jacobian.

The algorithm should take a robot model urdf, specified in a tree of joint frames, in the constructor, and compute forward/inverse kinematics and jacobian correspond to that urdf.

__init__(configs, world, ros_launch_manager=None, robots={}, sensors={}, algorithms={})

Initialize self. See help(type(self)) for accurate signature.

check_cfg()

This function checks required configs shared by all kinematics instances in constructor.

Specifically, it checks for:

1. The kinematic algorithm handles one robot, and one robot only

2. The robot has an arm

3. The arm has a base frame, an end-effector frame, and urdf description.

For any algorithm specific config checks, please extend the check_cfg function

with customized algorithm config checks after calling this function using super().check_cfg()

forward_kinematics(joint_pos, target_frame)

This function takes in a list of joint positions and name of target frame, and map to transformation

of the target frame w.r.t base frame.

Args:

joint_pos: list-like object of joint angles correspond to the queried end effector, Unit: radius.

for a 5DoF arm with (joint1, joint2, …, joint5) in urdf, this function should return a vector with size 5 of (angle1, angle2, …, angle5)

target_frame: string of target frame name

Returns:

position: list of position vector in the form of [x, y, z]. Unit: meters. quarternion: list of quaternion vector in the form of [x, y, z, w]

If a forward kinematics solution cannot be found, this function should raise an error.

inverse_kinematics(position, orientation)

This function takes in position and orientation of end effector, and map to joint angles.

Args:

position: position vector as list-like objects in the form of [x, y, z]. Unit: meters. orientation: orientation vector or matrix with one of the following forms:

1. euler angles, as list-like object of form [x, y, z]

2. quaternion, as list-like object of form [x, y, z, w]

3. a 3x3 3D rotation matrix

Returns:

joint_angles: list of joint angles correspond to the queried end effector. Unit: radius.

i.e. for a 5DoF arm with (joint1, joint2, …, joint5) in urdf, this function should return a vector with size 5 of (angle1, angle2, …, angle5)

If an inverse kinematics solution cannot be found, this function should raise an error.