rofunc.utils.robolab.kinematics.robot_class
#
1. Module Contents#
1.1. Classes#
1.2. API#
- class rofunc.utils.robolab.kinematics.robot_class.RobotModel(model_path: str, robot_pose=None, solve_engine: str = 'pytorch_kinematics', device='cpu', verbose=False)[source]#
Initialization
Initialize a robot model from a URDF or MuJoCo XML file
- Parameters:
model_path – the path of the URDF or MuJoCo XML file
robot_pose – initial pose of robot base link, [x, y, z, qx, qy, qz, qw]
solve_engine – the engine to solve the kinematics, [“pytorch_kinematics”, “kinpy”, “all”]
device – the device to run the computation
verbose – whether to print the chain
- load_meshes()[source]#
Load all meshes and store them in a dictionary. Handles both complex meshes and simple shapes.
- Returns:
A dictionary where keys are mesh names and values are mesh data, and a dictionary for simple shapes.
- get_link_mesh_map()[source]#
Get the map of link and its corresponding geometries from the robot model file (either URDF or MJCF).
- Returns:
{link_body_name: {geom_name: {‘type’: geom_type, ‘params’: geom_specific_parameters}}}
If the robot model is a URDF file, it will attempt to link the geometry’s mesh paths. The URDF format relies on external mesh files, and this function assumes that any .obj mesh files are converted to .stl files.
If the robot model is an MJCF file (which has a .xml extension), it uses the MJCF-specific link-mesh mapping generated by the parser and processes different geometry types, including meshes, spheres, cylinders, boxes, and capsules.
For each geometry type: - ‘mesh’: It maps the geometry name to its corresponding mesh file path. - ‘sphere’: It maps the geometry name to a dictionary with the sphere radius and position. - ‘cylinder’: It maps the geometry name to a dictionary with the cylinder’s radius, height, and position. - ‘box’: It maps the geometry name to a dictionary with the box’s extents (x, y, z) and position. - ‘capsule’: It maps the geometry name to a dictionary with the capsule’s radius, height, and start/end positions.
- get_forward_robot_mesh(joint_value, base_trans=None)[source]#
Transform the robot mesh according to the joint values and the base pose :param joint_value: the joint values, [batch_size, num_joint] :param base_trans: transformation matrix of the base pose, [batch_size, 4, 4] :return:
- forward(joint_value, base_trans=None)[source]#
Transform the robot mesh according to the joint values and the base pose
- Parameters:
joint_value – the joint values, [batch_size, num_joint]
base_trans – transformation matrix of the base pose, [batch_size, 4, 4]
- Returns:
- get_fk(joint_value: List, export_link=None)[source]#
Get the forward kinematics from a chain
- Parameters:
joint_value – both single and batch input are supported
export_link – the name of the export link
- Returns:
the position, rotation of the end effector, and the transformation matrices of all links
- get_jacobian(joint_value: List, export_link: str, locations=None)[source]#
Get the jacobian of a chain
- Parameters:
joint_value – the joint values, [batch_size, num_joint]
export_link – the name of the export link
locations – the locations offset from the export link
- Returns:
- get_trans_dict(joint_value: List, base_trans: Union[None, torch.Tensor] = None) dict [source]#
Get the transformation matrices of all links
- Parameters:
joint_value – the joint values, [batch_size, num_joint]
base_trans – transformation matrix of the base pose, [batch_size, 4, 4]
- Returns:
A dictionary where the keys are link names and the values are transformation matrices.
- get_ik(ee_pose: Union[torch.Tensor, None, List, Tuple], export_link, goal_in_rob_tf: bool = True, cur_configs=None, num_retries=10)[source]#
Get the inverse kinematics from a chain
- Parameters:
ee_pose – the pose of the end effector, 7D vector with the first 3 elements as position and the last 4 elements as rotation
export_link – the name of the export link
goal_in_rob_tf – whether the goal pose is in the robot base frame
cur_configs – let the ik solver retry from these configurations
num_retries – the number of retries
- Returns:
the joint values