Forward kinematics can be performed using several methods, returning poses as 4x4 homogenous transform matrices relative to the robot's base link.
Link FK
robot.link_fk(cfg=None) returns a map from Link objects to their 4x4 transform matrices.
- If
cfg is provided, it should be a dictionary mapping joint names (or joint objects) to their configuration values.
Trimesh FK
To get the poses of the actual geometries, use:
robot.visual_trimesh_fk(): Returns a map from trimesh.base.Trimesh objects to their poses.robot.collision_trimesh_fk(): Returns a map from trimesh.base.Trimesh objects to their poses.
# Get link poses at default configuration
fk = robot.link_fk()
print(fk[robot.links[1]])
# Get link poses with a specific joint configuration
# cfg is a dict: {joint_name: value}
fk = robot.link_fk(cfg={'shoulder_pan_joint': 1.0})
print(fk[robot.links[1]])
# Get poses for visual/collision trimeshes
visual_fk = robot.visual_trimesh_fk()
collision_fk = robot.collision_trimesh_fk()