concepts.dm.crowhat.world.manipulator_interface.SingleArmMotionPlanningInterface#

class SingleArmMotionPlanningInterface[source]#

Bases: object

General interface for robot arms. It specifies a set of basic operations for motion planning:

  • ik: inverse kinematics.

  • fk: forward kinematics.

  • jac: jacobian matrix.

  • coriolis: coriolis torque.

  • mass: mass matrix.

Methods

add_attachment(body[, link, self_link, ...])

calc_differential_ik_qpos_diff(current_qpos, ...)

Use the differential IK to compute the joint difference for the given pose difference.

calc_ee_pose_from_single_attached_object_pose(...)

Get the end-effector pose given the desired pose of the attached object.

calc_ee_quat_from_vectors([u, v])

Compute the quaternion from two directions (the "down" direction for the end effector and the "forward" direction for the end effector).

check_collision([qpos, ...])

Check if the current configuration is in collision.

coriolis(qpos, qvel)

fk(qpos)

gen_ik(pos[, quat, qpos, max_distance, ...])

get_attached_objects()

get_body_id()

get_collision_free_problem_space([...])

get_configuration_space([max_stepdiff])

get_default_configuration_space_max_stepdiff()

Get the default maximum step difference for the configuration space.

get_ee_default_quat()

Get the default end-effector quaternion.

get_ee_link_id()

get_ee_pose()

get_joint_limits()

get_nr_joints()

get_qpos()

ik(pos[, quat, qpos, max_distance])

jacobian(qpos)

mass(qpos)

remove_attachment(body[, link, self_link])

rrt_collision_free(qpos1[, qpos2, ...])

RRT-based collision-free path planning.

set_planning_world(planning_world)

set_qpos(qpos)

Attributes

body_id

ee_default_quat

ee_link_id

joint_limits

nr_joints

planning_world

__init__(planning_world=None)[source]#
Parameters:

planning_world (PlanningWorldInterface | None)

__new__(**kwargs)#
add_attachment(body, link=None, self_link=None, ee_to_object=None)[source]#
Parameters:
Return type:

Any

calc_differential_ik_qpos_diff(current_qpos, current_pose, next_pose)[source]#

Use the differential IK to compute the joint difference for the given pose difference.

Parameters:
Return type:

ndarray

calc_ee_pose_from_single_attached_object_pose(pos, quat)[source]#

Get the end-effector pose given the desired pose of the attached object.

Parameters:
Return type:

Tuple[Tuple[float, float, float] | List[float] | ndarray, Tuple[float, float, float, float] | List[float] | ndarray]

calc_ee_quat_from_vectors(u=(-1., 0., 0.), v=(1., 0., 0.))[source]#

Compute the quaternion from two directions (the “down” direction for the end effector and the “forward” direction for the end effector).

Parameters:
Return type:

Tuple[float, float, float, float] | List[float] | ndarray

check_collision(qpos=None, ignore_self_collision=True, ignored_collision_bodies=None, checkpoint_world_state=None, return_list=False)[source]#

Check if the current configuration is in collision.

Parameters:

  • qpos (Tuple[float, ...] | List[float] | ndarray | None) – the configuration to check. If None, it will use the current configuration.

  • ignore_self_collision (bool) – whether to ignore the collision between the robot and itself (including the attached objects).

  • ignored_collision_bodies (List[str | int] | None) – a list of identifiers of the bodies to ignore.

  • checkpoint_world_state (bool | None) – whether to checkpoint the world state before checking collision. If None, we will checkpoint the world state if qpos is not None.

  • return_list (bool) – whether to return the list of collision bodies.

Returns:

True if the configuration is in collision, False otherwise.

Return type:

bool | List[str | int]

coriolis(qpos, qvel)[source]#
Parameters:
Return type:

ndarray

fk(qpos)[source]#
Parameters:

qpos (Tuple[float, ...] | List[float] | ndarray)

Return type:

Tuple[Tuple[float, float, float] | List[float] | ndarray, Tuple[float, float, float, float] | List[float] | ndarray]

gen_ik(pos, quat=None, qpos=None, max_distance=None, max_returns=5)[source]#
Parameters:
Return type:

Iterator[ndarray]

get_attached_objects()[source]#
Return type:

List[AttachmentInfo]

get_body_id()[source]#
Return type:

int

get_collision_free_problem_space(ignored_collision_bodies=None, max_stepdiff=None)[source]#
Parameters:

  • ignored_collision_bodies (List[str | int] | None)

  • max_stepdiff (float | None)

Return type:

CollisionFreeProblemSpace

get_configuration_space(max_stepdiff=None)[source]#
Parameters:

max_stepdiff (float | None)

Return type:

BoxConfigurationSpace

get_default_configuration_space_max_stepdiff()[source]#

Get the default maximum step difference for the configuration space. Default is 0.02 rad.

Return type:

float

get_ee_default_quat()[source]#

Get the default end-effector quaternion. This is defined by the quaternion of the EE when

  1. the robot base is at the origin,

  2. the “down” direction is (0, 0, -1), and

  3. the “forward” direction is (1, 0, 0).

Returns:

The default end-effector quaternion.

Return type:

Vec4f

Return type:

int

get_ee_pose()[source]#
Return type:

Tuple[Tuple[float, float, float] | List[float] | ndarray, Tuple[float, float, float, float] | List[float] | ndarray]

get_joint_limits()[source]#
Return type:

Tuple[ndarray, ndarray]

get_nr_joints()[source]#
Return type:

int

get_qpos()[source]#
Return type:

ndarray

ik(pos, quat=None, qpos=None, max_distance=None)[source]#
Parameters:
Return type:

ndarray | None

jacobian(qpos)[source]#
Parameters:

qpos (Tuple[float, ...] | List[float] | ndarray)

Return type:

ndarray

mass(qpos)[source]#
Parameters:

qpos (Tuple[float, ...] | List[float] | ndarray)

Return type:

ndarray

remove_attachment(body, link=None, self_link=None)[source]#
Parameters:
rrt_collision_free(qpos1, qpos2=None, ignored_collision_bodies=None, smooth_fine_path=False, **kwargs)[source]#

RRT-based collision-free path planning.

Parameters:

  • qpos1 (ndarray) – Start position.

  • qpos2 (ndarray | None) – End position. If None, the current position is used.

  • ignored_collision_bodies (List[str | int] | None) – IDs of the objects to ignore in collision checking. Defaults to None.

  • smooth_fine_path (bool) – Whether to smooth the path. Defaults to False.

  • kwargs – Additional arguments.

Returns:

True if the path is collision-free. List[np.ndarray]: Joint configuration trajectory.

Return type:

bool

set_planning_world(planning_world)[source]#
Parameters:

planning_world (PlanningWorldInterface | None)

set_qpos(qpos)[source]#
Parameters:

qpos (Tuple[float, ...] | List[float] | ndarray)

property body_id: int#
property ee_default_quat: Tuple[float, float, float, float] | List[float] | ndarray#
property joint_limits: Tuple[ndarray, ndarray]#
property nr_joints: int#
property planning_world: PlanningWorldInterface | None#
On this page