skrobot.planner.SweptSphereSdfCollisionChecker

class skrobot.planner.SweptSphereSdfCollisionChecker(sdf, robot_model)

Collision checker between swept spheres and sdf

Methods

add_coll_spheres_to_viewer(viewer)

Add collision sheres to viewer

Parameters:

viewer (skrobot.viewers._trimesh.TrimeshSceneViewer) – viewer

add_collision_link(coll_link)

Add link for which collision with sdf is checked

The given coll_link will be approximated by swept-spheres and these spheres will be added to collision sphere’s list.

Parameters:

coll_link (skrobot.model.Link) – link for which collision with sdf is checked

add_collision_links(coll_links)

Add links for which collisions with SDF is checked.

The given coll_links will be approximated by swept-spheres and these spheres will be added to collision sphere’s list.

Parameters:

coll_links (list[skrobot.model.Link]) – link list for which collisions with sdf is checked.

collision_check()

Check collision between links and collision spheres.

Returns:

is_collision – True if a collision occurred between any pair of links and collision spheres and False otherwise.

Return type:

bool

compute_batch_sd_vals(joint_list, angle_vector_seq, with_base=False, with_jacobian=False)

Compute sd signed distances of collision spheres

This method is the core of this class. We assume that this method is mainly called from a trajecotyr optimizer or path planner.

Let \(n_{wp}\) be the number of way-points of a trajectory. Let \(n_{f}\) be the number of collision feature points on the robot links.

Let \(f_{i, j} : \mathbb{R}^{n_{dof}} \ni q \mapsto x \in \mathbb{R}^3\) be the forward kinematics of collision features point \(j\) at waypoint \(i\) where \(q\) is the angle vector and \(n_{dof}\) is the dimension of the angle vector.

Let \(c : \mathbb{R}^3 \ni x \mapsto sd \in \mathbb{R}\) be the signd distance function.

Then, this fucntion is defined as \(F : \mathbb{R}^{n_{wp} n_{dof}} \ni \xi \mapsto [ [f_{1, 1}(q_1), \ldots, f_{1, n_{f}}(q_1)]^T, \ldots, [f_{n_{wp}, 1}(q_{n_{wp}}),\ldots,f_{n_{wp}, n_{f}}(q_{n_{wp}})]^T ]^T \in \mathbb{R}^{n_{wp} n_{f}}\) where \(\xi = [q_1^T, \ldots, q_{n_{wp}}^T]^T\) be the angle vector sequence of the trajectory. The corresponding jacobian is defined as \(\frac{\partial F}{\partial \xi}\).

Parameters:

• joint_list (list[skrobot.model.Joint]) – joint list to be set

• angle_vector_seq (numpy.ndarray[float](n_wp, n_dof)) – angle vector sequence.

• with_base (bool) – hoge

• with_jacobian (bool) – if True, jacobian is copmuted at the same time.

Returns:

• sd_vals (numpy.ndarray[float](n_wp * n_feature,)) – signed distnaces for all feature points through the trajectory

• sd_vals_jacobi (numpy.ndarray[float](n_wp * n_feature, n_wp * n_dof)) – jacobain of sd_vals with respect to DOF (i.e. n_wp * n_dof) of the trajectory

delete_coll_spheres_from_viewer(viewer)

Delete collision sheres from viewer

Parameters:

viewer (skrobot.viewers._trimesh.TrimeshSceneViewer) – viewer

update_color()

Update the color of links under collision

This method checks the collision between the collision spheres and registered sdf. If collision spheres are found to be under collision, the color of the spheres will be changed to color_collision_sphere.

Returns:

dists – array of the signed distances for each sphere against sdf.

Return type:

numpy.ndarray(n_sphere,)

__eq__(value, /)

Return self==value.

__ne__(value, /)

Return self!=value.

__lt__(value, /)

Return self<value.

__le__(value, /)

Return self<=value.

__gt__(value, /)

Return self>value.

__ge__(value, /)

Return self>=value.

Attributes

n_feature

Return number of collision sphere.

Returns:

n_feature – number of collision spheres.

Return type:

int