`sapien_utils`¶

class mplib.sapien_utils.SapienPlanningWorld( sim_scene: Scene, planned_articulations: list[PhysxArticulation] = [], )[source]¶

Bases: PlanningWorld

__init__( sim_scene: Scene, planned_articulations: list[PhysxArticulation] = [], )[source]¶

Creates an mplib.PlanningWorld from a sapien.Scene.

Parameters:: planned_articulations – list of planned articulations.

update_from_simulation( *, update_attached_object: bool = True, ) → None[source]¶

Updates PlanningWorld’s articulations/objects pose with current Scene state. Note that shape’s local_pose is not updated. If those are changed, please recreate a new SapienPlanningWorld instance.

Parameters:: update_attached_object – whether to update the attached pose of all attached objects

check_collision_between( obj_A: PhysxArticulation | Entity, obj_B: PhysxArticulation | Entity, *, acm: AllowedCollisionMatrix = AllowedCollisionMatrix(), ) → list[WorldCollisionResult][source]¶

Check collision between two objects, which can either be a PhysxArticulation or an Entity.

Parameters:

obj_A – object A to check for collision.
obj_B – object B to check for collision.
acm – allowed collision matrix.

Returns:

a list of WorldCollisionResult. Empty if there’s no collision.

distance_between( obj_A: PhysxArticulation | Entity, obj_B: PhysxArticulation | Entity, *, acm: AllowedCollisionMatrix = AllowedCollisionMatrix(), return_distance_only: bool = True, ) → WorldDistanceResult | float[source]¶

Check distance-to-collision between two objects, which can either be a PhysxArticulation or an Entity.

Parameters:

obj_A – object A to check for collision.
obj_B – object B to check for collision.
acm – allowed collision matrix.
return_distance_only – if True, return distance only.

Returns:

a WorldDistanceResult or a float if return_distance_only==True.

static convert_physx_component( comp: PhysxRigidBaseComponent, ) → FCLObject | None[source]¶

Converts a SAPIEN physx.PhysxRigidBaseComponent to an FCLObject. All shapes in the returned FCLObject are already set at their world poses.

Parameters:: comp – a SAPIEN physx.PhysxRigidBaseComponent.
Returns:: an FCLObject containing all collision shapes in the Physx component. If the component has no collision shapes, return None.

add_articulation( self: mplib.pymp.PlanningWorld, model: mplib.pymp.ArticulatedModel, planned: bool = False, ) → None¶

Adds an articulation (ArticulatedModelPtr) to world

Parameters:

model – articulated model to be added
planned – whether the articulation is being planned

add_object(*args, **kwargs)¶

Overloaded function.

add_object(self: mplib.pymp.PlanningWorld, fcl_obj: mplib.pymp.collision_detection.fcl.FCLObject) -> None

Adds an non-articulated object containing multiple collision objects (FCLObjectPtr) to world

Parameters:: fcl_obj – FCLObject to be added

add_object(self: mplib.pymp.PlanningWorld, name: str, collision_object: mplib.pymp.collision_detection.fcl.CollisionObject) -> None

Adds an non-articulated object (CollisionObjectPtr) with given name to world

Parameters:

name – name of the collision object
collision_object – collision object to be added

add_point_cloud( self: mplib.pymp.PlanningWorld, name: str, vertices: numpy.ndarray[numpy.float64[m, 3]], resolution: float = 0.01, ) → None¶

Adds a point cloud as a collision object with given name to world

Parameters:

name – name of the point cloud collision object
vertices – point cloud vertices matrix
resolution – resolution of the point in octomap::OcTree

attach_box( self: mplib.pymp.PlanningWorld, size: numpy.ndarray[numpy.float64[3, 1]], art_name: str, link_id: int, pose: mplib.pymp.Pose, ) → None¶

Attaches given box to specified link of articulation (auto touch_links)

Parameters:

size – box side length
art_name – name of the planned articulation to attach to
link_id – index of the link of the planned articulation to attach to
pose – attached pose (relative pose from attached link to object)

attach_mesh( self: mplib.pymp.PlanningWorld, mesh_path: str, art_name: str, link_id: int, pose: mplib.pymp.Pose, *, convex: bool = False, ) → None¶

Attaches given mesh to specified link of articulation (auto touch_links)

Parameters:

mesh_path – path to a mesh file
art_name – name of the planned articulation to attach to
link_id – index of the link of the planned articulation to attach to
pose – attached pose (relative pose from attached link to object)
convex – whether to load mesh as a convex mesh. Default: False.

attach_object(*args, **kwargs)¶

Overloaded function.

attach_object(self: mplib.pymp.PlanningWorld, name: str, art_name: str, link_id: int, touch_links: list[str]) -> None

Attaches existing non-articulated object to specified link of articulation at its current pose. If the object is currently attached, disallow collision between the object and previous touch_links.

Updates acm_ to allow collisions between attached object and touch_links.

Parameters:

name – name of the non-articulated object to attach
art_name – name of the planned articulation to attach to
link_id – index of the link of the planned articulation to attach to
touch_links – link names that the attached object touches

Raises:

ValueError – if non-articulated object with given name does not exist or if planned articulation with given name does not exist

attach_object(self: mplib.pymp.PlanningWorld, name: str, art_name: str, link_id: int) -> None

Attaches existing non-articulated object to specified link of articulation at its current pose. If the object is not currently attached, automatically sets touch_links as the name of self links that collide with the object in the current state.

Updates acm_ to allow collisions between attached object and touch_links.

If the object is already attached, the touch_links of the attached object is preserved and acm_ remains unchanged.

Parameters:

name – name of the non-articulated object to attach
art_name – name of the planned articulation to attach to
link_id – index of the link of the planned articulation to attach to

Raises:

ValueError – if non-articulated object with given name does not exist or if planned articulation with given name does not exist

attach_object(self: mplib.pymp.PlanningWorld, name: str, art_name: str, link_id: int, pose: mplib.pymp.Pose, touch_links: list[str]) -> None

Attaches existing non-articulated object to specified link of articulation at given pose. If the object is currently attached, disallow collision between the object and previous touch_links.

Updates acm_ to allow collisions between attached object and touch_links.

Parameters:

name – name of the non-articulated object to attach
art_name – name of the planned articulation to attach to
link_id – index of the link of the planned articulation to attach to
pose – attached pose (relative pose from attached link to object)
touch_links – link names that the attached object touches

Raises:

ValueError – if non-articulated object with given name does not exist or if planned articulation with given name does not exist

attach_object(self: mplib.pymp.PlanningWorld, name: str, art_name: str, link_id: int, pose: mplib.pymp.Pose) -> None

Attaches existing non-articulated object to specified link of articulation at given pose. If the object is not currently attached, automatically sets touch_links as the name of self links that collide with the object in the current state.

Updates acm_ to allow collisions between attached object and touch_links.

If the object is already attached, the touch_links of the attached object is preserved and acm_ remains unchanged.

Parameters:

name – name of the non-articulated object to attach
art_name – name of the planned articulation to attach to
link_id – index of the link of the planned articulation to attach to
pose – attached pose (relative pose from attached link to object)

Raises:

ValueError – if non-articulated object with given name does not exist or if planned articulation with given name does not exist

attach_object(self: mplib.pymp.PlanningWorld, name: str, p_geom: mplib.pymp.collision_detection.fcl.CollisionGeometry, art_name: str, link_id: int, pose: mplib.pymp.Pose, touch_links: list[str]) -> None

Parameters:

name – name of the non-articulated object to attach
p_geom – pointer to a CollisionGeometry object
art_name – name of the planned articulation to attach to
link_id – index of the link of the planned articulation to attach to
pose – attached pose (relative pose from attached link to object)
touch_links – link names that the attached object touches

attach_object(self: mplib.pymp.PlanningWorld, name: str, p_geom: mplib.pymp.collision_detection.fcl.CollisionGeometry, art_name: str, link_id: int, pose: mplib.pymp.Pose) -> None

Attaches given object (w/ p_geom) to specified link of articulation at given pose. This is done by removing the object and then adding and attaching object. As a result, all previous acm_ entries with the object are removed. Automatically sets touch_links as the name of self links that collide with the object in the current state (auto touch_links).

Parameters:

name – name of the non-articulated object to attach
p_geom – pointer to a CollisionGeometry object
art_name – name of the planned articulation to attach to
link_id – index of the link of the planned articulation to attach to
pose – attached pose (relative pose from attached link to object)

attach_sphere( self: mplib.pymp.PlanningWorld, radius: float, art_name: str, link_id: int, pose: mplib.pymp.Pose, ) → None¶

Attaches given sphere to specified link of articulation (auto touch_links)

Parameters:

radius – sphere radius
art_name – name of the planned articulation to attach to
link_id – index of the link of the planned articulation to attach to
pose – attached pose (relative pose from attached link to object)

check_collision( self: mplib.pymp.PlanningWorld, request: mplib.pymp.collision_detection.fcl.CollisionRequest = <mplib.pymp.collision_detection.fcl.CollisionRequest object at 0x7f172b1ab7b0>, ) → list[mplib.pymp.collision_detection.WorldCollisionResult]¶

Check full collision (calls checkSelfCollision() and checkRobotCollision())

Parameters:: request – collision request params.
Returns:: List of WorldCollisionResult objects

check_robot_collision( self: mplib.pymp.PlanningWorld, request: mplib.pymp.collision_detection.fcl.CollisionRequest = <mplib.pymp.collision_detection.fcl.CollisionRequest object at 0x7f17372cb630>, ) → list[mplib.pymp.collision_detection.WorldCollisionResult]¶

Check collision with other scene bodies in the world (planned articulations with attached objects collide against unplanned articulations and scene objects)

Parameters:: request – collision request params.
Returns:: List of WorldCollisionResult objects

check_self_collision( self: mplib.pymp.PlanningWorld, request: mplib.pymp.collision_detection.fcl.CollisionRequest = <mplib.pymp.collision_detection.fcl.CollisionRequest object at 0x7f1729f5f6b0>, ) → list[mplib.pymp.collision_detection.WorldCollisionResult]¶

Check for self collision (including planned articulation self-collision, planned articulation-attach collision, attach-attach collision)

Parameters:: request – collision request params.
Returns:: List of WorldCollisionResult objects

detach_object( self: mplib.pymp.PlanningWorld, name: str, also_remove: bool = False, ) → bool¶

Detaches object with given name. Updates acm_ to disallow collision between the object and touch_links.

Parameters:

name – name of the non-articulated object to detach
also_remove – whether to also remove object from world

Returns:

True if success, False if the object with given name is not attached

distance( self: mplib.pymp.PlanningWorld, request: mplib.pymp.collision_detection.fcl.DistanceRequest = <mplib.pymp.collision_detection.fcl.DistanceRequest object at 0x7f172bb494f0>, ) → mplib.pymp.collision_detection.WorldDistanceResult¶

Compute the minimum distance-to-all-collision (calls distanceSelf() and distanceRobot())

Parameters:: request – distance request params.
Returns:: a WorldDistanceResult object

distance_robot( self: mplib.pymp.PlanningWorld, request: mplib.pymp.collision_detection.fcl.DistanceRequest = <mplib.pymp.collision_detection.fcl.DistanceRequest object at 0x7f1729f6e630>, ) → mplib.pymp.collision_detection.WorldDistanceResult¶

Compute the minimum distance-to-collision between a robot and the world

Parameters:: request – distance request params.
Returns:: a WorldDistanceResult object

distance_self( self: mplib.pymp.PlanningWorld, request: mplib.pymp.collision_detection.fcl.DistanceRequest = <mplib.pymp.collision_detection.fcl.DistanceRequest object at 0x7f1729f80930>, ) → mplib.pymp.collision_detection.WorldDistanceResult¶

Get the minimum distance to self-collision given the robot in current state

Parameters:: request – distance request params.
Returns:: a WorldDistanceResult object

distance_to_collision(self: mplib.pymp.PlanningWorld) → float¶

Compute the minimum distance-to-all-collision. Calls distance().

Note that this is different from MoveIt2’s planning_scene::PlanningScene::distanceToCollision() where self-collisions are ignored.

Returns:: minimum distance-to-all-collision

distance_to_robot_collision(self: mplib.pymp.PlanningWorld) → float¶

The distance between the robot model at current state to the nearest collision (ignoring self-collisions). Calls distanceRobot().

Returns:: minimum distance-to-robot-collision

distance_to_self_collision(self: mplib.pymp.PlanningWorld) → float¶

The minimum distance to self-collision given the robot in current state. Calls distanceSelf().

Returns:: minimum distance-to-self-collision

get_allowed_collision_matrix( self: mplib.pymp.PlanningWorld, ) → mplib.pymp.collision_detection.AllowedCollisionMatrix¶: Get the current allowed collision matrix

get_articulation( self: mplib.pymp.PlanningWorld, name: str, ) → mplib.pymp.ArticulatedModel¶

Gets the articulation (ArticulatedModelPtr) with given name

Parameters:: name – name of the articulated model
Returns:: the articulated model with given name or None if not found.

get_articulation_names(self: mplib.pymp.PlanningWorld) → list[str]¶: Gets names of all articulations in world (unordered)

get_attached_object( self: mplib.pymp.PlanningWorld, name: str, ) → mplib.pymp.AttachedBody¶

Gets the attached body (AttachedBodyPtr) with given name

Parameters:: name – name of the attached body
Returns:: the attached body with given name or None if not found.

get_object( self: mplib.pymp.PlanningWorld, name: str, ) → mplib.pymp.collision_detection.fcl.FCLObject¶

Gets the non-articulated object (FCLObjectPtr) with given name

Parameters:: name – name of the non-articulated object
Returns:: the object with given name or None if not found.

get_object_names(self: mplib.pymp.PlanningWorld) → list[str]¶: Gets names of all objects in world (unordered)

get_planned_articulations( self: mplib.pymp.PlanningWorld, ) → list[mplib.pymp.ArticulatedModel]¶: Gets all planned articulations (ArticulatedModelPtr)

has_articulation(self: mplib.pymp.PlanningWorld, name: str) → bool¶

Check whether the articulation with given name exists

Parameters:: name – name of the articulated model
Returns:: True if exists, False otherwise.

has_object(self: mplib.pymp.PlanningWorld, name: str) → bool¶

Check whether the non-articulated object with given name exists

Parameters:: name – name of the non-articulated object
Returns:: True if exists, False otherwise.

is_articulation_planned( self: mplib.pymp.PlanningWorld, name: str, ) → bool¶

Check whether the articulation with given name is being planned

Parameters:: name – name of the articulated model
Returns:: True if exists, False otherwise.

is_object_attached( self: mplib.pymp.PlanningWorld, name: str, ) → bool¶

Check whether the non-articulated object with given name is attached

Parameters:: name – name of the non-articulated object
Returns:: True if it is attached, False otherwise.

is_state_colliding(self: mplib.pymp.PlanningWorld) → bool¶

Check if the current state is in collision (with the environment or self collision).

Returns:: True if collision exists

print_attached_body_pose(self: mplib.pymp.PlanningWorld) → None¶: Prints global pose of all attached bodies

remove_articulation( self: mplib.pymp.PlanningWorld, name: str, ) → bool¶

Removes the articulation with given name if exists. Updates acm_

Parameters:: name – name of the articulated model
Returns:: True if success, False if articulation with given name does not exist

remove_object(self: mplib.pymp.PlanningWorld, name: str) → bool¶

Removes (and detaches) the collision object with given name if exists. Updates acm_

Parameters:: name – name of the non-articulated collision object
Returns:: True if success, False if the non-articulated object with given name does not exist

set_articulation_planned( self: mplib.pymp.PlanningWorld, name: str, planned: bool, ) → None¶

Sets articulation with given name as being planned

Parameters:

name – name of the articulated model
planned – whether the articulation is being planned

Raises:

ValueError – if the articulation with given name does not exist

set_qpos( self: mplib.pymp.PlanningWorld, name: str, qpos: numpy.ndarray[numpy.float64[m, 1]], ) → None¶

Set qpos of articulation with given name

Parameters:

name – name of the articulated model
qpos – joint angles of the movegroup only // FIXME: double check

set_qpos_all( self: mplib.pymp.PlanningWorld, state: numpy.ndarray[numpy.float64[m, 1]], ) → None¶: Set qpos of all planned articulations

class mplib.sapien_utils.SapienPlanner( sapien_planning_world: SapienPlanningWorld, move_group: str, *, joint_vel_limits: Sequence[float] | ndarray | None = None, joint_acc_limits: Sequence[float] | ndarray | None = None, )[source]¶

Bases: Planner

__init__( sapien_planning_world: SapienPlanningWorld, move_group: str, *, joint_vel_limits: Sequence[float] | ndarray | None = None, joint_acc_limits: Sequence[float] | ndarray | None = None, )[source]¶

Creates an mplib.planner.Planner from a SapienPlanningWorld.

Parameters:

sapien_planning_world – a SapienPlanningWorld created from sapien.Scene
move_group – name of the move group (end effector link)
joint_vel_limits – joint velocity limits for time parameterization
joint_acc_limits – joint acceleration limits for time parameterization

update_from_simulation(*, update_attached_object: bool = True) → None[source]¶

Updates PlanningWorld’s articulations/objects pose with current Scene state. Note that shape’s local_pose is not updated. If those are changed, please recreate a new SapienPlanningWorld instance.

Directly calls SapienPlanningWorld.update_from_simulation()

Parameters:: update_attached_object – whether to update the attached pose of all attached objects

IK( goal_pose: Pose, start_qpos: ndarray, mask: Sequence[bool] | ndarray | None = None, *, n_init_qpos: int = 20, threshold: float = 1e-3, return_closest: bool = False, verbose: bool = False, ) → tuple[str, list[ndarray] | ndarray | None]¶

Compute inverse kinematics

Parameters:

goal_pose – goal pose
start_qpos – initial configuration, (ndof,) np.floating np.ndarray.
mask – qpos mask to disable IK sampling, (ndof,) bool np.ndarray.
n_init_qpos – number of random initial configurations to sample.
threshold – distance threshold for marking sampled IK as success. distance is position error norm + quaternion error norm.
return_closest – whether to return the qpos that is closest to start_qpos, considering equivalent joint values.
verbose – whether to print collision info if any collision exists.

Returns:

(status, q_goals)

status: IK status, “Success” if succeeded.

q_goals: list of sampled IK qpos, (ndof,) np.floating np.ndarray.: IK is successful if q_goals is not None. If return_closest, q_goals is np.ndarray if successful and None if not successful.

TOPP(path, step=0.1, verbose=False)¶

Time-Optimal Path Parameterization

Parameters:

path – numpy array of shape (n, dof)
step – step size for the discretization
verbose – if True, will print the log of TOPPRA

check_for_collision( collision_function, state: ndarray | None = None, ) → list[WorldCollisionResult]¶

Helper function to check for collision

Parameters:: state – all planned articulations qpos state. If None, use current qpos.
Returns:: A list of collisions.

check_for_env_collision( state: ndarray | None = None, ) → list[WorldCollisionResult]¶

Check if the robot is in collision with the environment

Parameters:: state – all planned articulations qpos state. If None, use current qpos.
Returns:: A list of collisions.

check_for_self_collision( state: ndarray | None = None, ) → list[WorldCollisionResult]¶

Check if the robot is in self-collision.

Parameters:: state – all planned articulations qpos state. If None, use current qpos.
Returns:: A list of collisions.

detach_object(name='attached_geom', also_remove=False) → bool¶

Detact the attached object with given name

Parameters:

name – object name to detach
also_remove – whether to also remove object from world

Returns:

True if success, False if the object with given name is not attached

pad_move_group_qpos(qpos, articulation=None)¶

If qpos contains only the move_group joints, return qpos padded with current values of the remaining joints of articulation. Otherwise, verify number of joints and return.

Parameters:

qpos – joint positions
articulation – the articulation to get qpos from. If None, use self.robot

Returns:

joint positions with full dof

plan_pose( goal_pose: Pose, current_qpos: ndarray, mask: list[bool] | ndarray | None = None, *, time_step: float = 0.1, rrt_range: float = 0.1, planning_time: float = 1, fix_joint_limits: bool = True, wrt_world: bool = True, simplify: bool = True, constraint_function: Callable | None = None, constraint_jacobian: Callable | None = None, constraint_tolerance: float = 1e-3, verbose: bool = False, ) → dict[str, str | ndarray | float64]¶

plan from a start configuration to a goal pose of the end-effector

Parameters:

goal_pose – pose of the goal
current_qpos – current joint configuration (either full or move_group joints)
mask – if the value at a given index is True, the joint is not used in the IK
time_step – time step for TOPPRA (time parameterization of path)
rrt_range – step size for RRT
planning_time – time limit for RRT
fix_joint_limits – if True, will clip the joint configuration to be within the joint limits
wrt_world – if true, interpret the target pose with respect to the world frame instead of the base frame
verbose – if True, will print the log of OMPL and TOPPRA

plan_qpos( goal_qposes: list[ndarray], current_qpos: ndarray, *, time_step: float = 0.1, rrt_range: float = 0.1, planning_time: float = 1, fix_joint_limits: bool = True, fixed_joint_indices: list[int] | None = None, simplify: bool = True, constraint_function: Callable[[ndarray, ndarray], None] | None = None, constraint_jacobian: Callable[[ndarray, ndarray], None] | None = None, constraint_tolerance: float = 1e-3, verbose: bool = False, ) → dict[str, str | ndarray | float64]¶

Plan a path from a specified joint position to a goal pose

Parameters:

goal_qposes – list of target joint configurations, [(ndof,)]
current_qpos – current joint configuration (either full or move_group joints)
mask – mask for IK. When set, the IK will leave certain joints out of planning
time_step – time step for TOPP
rrt_range – step size for RRT
planning_time – time limit for RRT
fix_joint_limits – if True, will clip the joint configuration to be within the joint limits
simplify – whether the planned path will be simplified. (constained planning does not support simplification)
constraint_function – evals to 0 when constraint is satisfied
constraint_jacobian – jacobian of constraint_function
constraint_tolerance – tolerance for constraint_function
fixed_joint_indices – list of indices of joints that are fixed during planning
verbose – if True, will print the log of OMPL and TOPPRA

plan_screw( goal_pose: Pose, current_qpos: ndarray, *, qpos_step: float = 0.1, time_step: float = 0.1, wrt_world: bool = True, verbose: bool = False, ) → dict[str, str | ndarray | float64]¶

Plan from a start configuration to a goal pose of the end-effector using screw motion

Parameters:

goal_pose – pose of the goal
current_qpos – current joint configuration (either full or move_group joints)
qpos_step – size of the random step
time_step – time step for the discretization
wrt_world – if True, interpret the target pose with respect to the world frame instead of the base frame
verbose – if True, will print the log of TOPPRA

remove_object(name) → bool¶: returns true if the object was removed, false if it was not found

remove_point_cloud(name='scene_pcd') → bool¶

Removes the point cloud collision object with given name

Parameters:: name – name of the point cloud collision object
Returns:: True if success, False if the non-articulation object with given name does not exist

set_base_pose(pose: Pose)¶

tell the planner where the base of the robot is w.r.t the world frame

Parameters:: pose – pose of the base

update_attached_box( size: Sequence[float] | ndarray[tuple[Literal[3], Literal[1]], dtype[floating]], pose: Pose, art_name=None, link_id=-1, )¶

Attach a box to some link

Parameters:

size – box side length
pose – attaching pose (relative pose from attached link to object)
art_name – name of the articulated object to attach to. If None, attach to self.robot.
link_id – if not provided, the end effector will be the target.

update_attached_mesh( mesh_path: str, pose: Pose, art_name=None, link_id=-1, )¶

Attach a mesh to some link

Parameters:

mesh_path – path to a mesh file
pose – attaching pose (relative pose from attached link to object)
art_name – name of the articulated object to attach to. If None, attach to self.robot.
link_id – if not provided, the end effector will be the target.

update_attached_object( collision_geometry: CollisionGeometry, pose: Pose, name='attached_geom', art_name=None, link_id=-1, )¶

Attach given object (w/ collision geometry) to specified link of articulation

Parameters:

collision_geometry – FCL collision geometry
pose – attaching pose (relative pose from attached link to object)
name – name of the attached geometry.
art_name – name of the articulated object to attach to. If None, attach to self.robot.
link_id – if not provided, the end effector will be the target.

update_attached_sphere( radius: float, pose: Pose, art_name=None, link_id=-1, )¶

Attach a sphere to some link

Parameters:

radius – radius of the sphere
pose – attaching pose (relative pose from attached link to object)
art_name – name of the articulated object to attach to. If None, attach to self.robot.
link_id – if not provided, the end effector will be the target.

update_point_cloud(points, resolution=1e-3, name='scene_pcd')¶

Adds a point cloud as a collision object with given name to world. If the name is the same, the point cloud is simply updated.

Parameters:

points – points, numpy array of shape (n, 3)
resolution – resolution of the point OcTree
name – name of the point cloud collision object

wrap_joint_limit(qpos: ndarray) → bool¶

Checks if the joint configuration can be wrapped to be within the joint limits. For revolute joints, the joint angle is wrapped to be within [q_min, q_min+2*pi)

Parameters:: qpos – joint positions, angles of revolute joints might be modified. If not within_limits (returns False), qpos might not be fully wrapped.
Returns:: whether qpos can be wrapped to be within the joint limits.

sapien_utils¶

`sapien_utils`¶