pykin.utils package

Submodules

pykin.utils.error_utils module

exception pykin.utils.error_utils.CollisionError(data)[source]

Bases: Exception

Class of custom Exception

Parameters: data (all types) – input data

exception pykin.utils.error_utils.NotFoundError(data)[source]

Bases: Exception

Class of custom Exception

Parameters: data (all types) – input data

<<<<<<< HEAD

pykin.collision.collision_manager module¶

class pykin.collision.collision_manager.ContactData(names, contact)[source]¶

pykin.utils.fcl_utils module

class pykin.utils.fcl_utils.ContactData(names, contact)[source]

Bases: object

Data structure for holding information about a collision contact.

Parameters

names (str) – The names of the two objects in order.
contact (fcl.Contact) – The contact in question.

property depth¶

property depth

The penetration depth of the 3D point of intersection for this contact.

Returns: The penetration depth.
Return type: float

index(name)[source]¶

index(name)[source]

Returns the index of the face in contact for the mesh with the given name.

Parameters: name (str) – The name of the target object.
Returns: The index of the face in collision
Return type: int

property point¶

property point

The 3D point of intersection for this contact.

Returns: The intersection point.
Return type: (3,) float

class pykin.collision.collision_manager.CollisionManager[source]¶

Bases: object

A rigid body collision manager.

add_object(name, gtype=None, gparam=None, transform=None)[source]¶

class pykin.utils.fcl_utils.FclManager[source]

Bases: object

A rigid body collision manager.

add_object(name, gtype=None, gparam=None, transform=None)[source]

Add an object to the collision manager. If an object with the given name is already in the manager, replace it.

Parameters

name (str) – An identifier for the object
gtype (str) – object type (cylinder, sphere, box)
gparam (float or tuple) – object parameter (radius, length, size)
transform (np.array) – Homogeneous transform matrix for the object

collision_check(return_names=False, return_data=False)[source]¶

collision_check(return_names=False, return_data=False)[source]

Check if any pair of objects in the manager collide with one another.

Parameters

return_names (bool) – If true, a set is returned containing the names of all pairs of objects in collision.
return_data (bool) – If true, a list of ContactData is returned as well

Returns

True if a collision occurred between any pair of objects and False otherwise names (set of 2-tup): The set of pairwise collisions. contacts (list of ContactData): All contacts detected

Return type

is_collision (bool)

remove_object(name)[source]¶

remove_object(name)[source]

Delete an object from the collision manager.

Parameters: name (str) – The identifier for the object

reset_all_object()[source]¶
reset_all_object()[source]: Reset all object from the collision manager.

set_transform(name=None, transform=array([[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0]]))[source]¶

set_transform(name=None, transform=array([[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0]]))[source]

Set the transform for one of the manager’s objects. This replaces the prior transform.

Parameters

name (str) – An identifier for the object already in the manager
transform (np.array) – A new homogeneous transform matrix for the object

pykin.utils.kin_utils module

class pykin.utils.kin_utils.Baxter[source]

Bases: object

static add_visual_link(link_transforms, f)[source]

left_e0_fixed_offset = pykin.kinematics.transform.Transform()

left_w0_fixed_offset = pykin.kinematics.transform.Transform()

right_e0_fixed_offset = pykin.kinematics.transform.Transform()

right_w0_fixed_offset = pykin.kinematics.transform.Transform()

class pykin.utils.kin_utils.ShellColors[source]

Bases: object

BOLD = '\x1b[1m'

ENDC = '\x1b[0m'

FAIL = '\x1b[91m'

HEADER = '\x1b[95m'

OKBLUE = '\x1b[94m'

OKCYAN = '\x1b[96m'

OKGREEN = '\x1b[92m'

UNDERLINE = '\x1b[4m'

WARNING = '\x1b[93m'

pykin.utils.kin_utils.calc_pose_error(tar_pose, cur_pose, EPS)[source]

Parameters

tar_pos (np.array) – target pose
cur_pos (np.array) – current pose
EPS (float) – epsilon

Returns

Returns pose error

Return type

np.array

pykin.utils.kin_utils.convert_string_to_narray(str_input)[source]

Parameters: str_input (str) – string
Returns: Returns string to np.array
Return type: np.array

pykin.utils.kin_utils.convert_thetas_to_dict(active_joint_names, thetas)[source]

Check if any pair of objects in the manager collide with one another.

Parameters

active_joint_names (list) – actuated joint names
thetas (sequence of float) – If not dict, convert to dict ex. {joint names : thetas}

Returns

Dictionary of actuated joint angles

Return type

thetas (dict)

pykin.utils.kin_utils.convert_transform(origin)[source]

Parameters: origin (None or Transform) – offset of object
Returns: Returns Transform if origin is None
Return type: Transform

pykin.utils.kin_utils.get_robot_collision_geom(link)[source]¶

pykin.utils.kin_utils.get_robot_geom(link)[source]

Get robot geometry from link

Parameters: link (Link) – robot’s link
Returns: geom’s name gtype: geom’s type gparam: geom’s param
Return type: name (str)

pykin.utils.kin_utils.limit_joints(joint_angles, lower, upper)[source]

Set joint angle limit

Parameters

joint_angles (sequence of float) – joint angles
lower (sequence of float) – lower limit
upper (sequence of float) – upper limit

Returns

Returns limited joint angle

Return type

joint_angles (sequence of float)

pykin.utils.kin_utils.logging_time(original_fn)[source]: Decorator to check time of function

pykin.utils.object_utils module

class pykin.utils.object_utils.Obstacle[source]

Bases: object

Obstacle class Obstacles are noe of three types(sphere, box, cylinder)

add_obstacles(name=None, gtype=None, gparam=None, gpose=None)[source]

Add obstacles

Parameters

name (str) – An identifier for the object
gtype (str) – object type (cylinder, sphere, box)
gparam (float or tuple) – object parameter (radius, length, size)
transform (np.array) – Homogeneous transform matrix for the object

obstacle_types = ['sphere', 'box', 'cylinder']

property obstacles

pykin.utils.plot_utils module

pykin.utils.plot_utils.convert_trimesh_scene(scene, filename=None, A2B=array([[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0]]), color='k')[source]: Convert scene from mesh to trimesh

pykin.utils.plot_utils.init_3d_figure(name=None, figsize=(15, 7.5), dpi=80)[source]: Initializes 3d figure

pykin.utils.plot_utils.plot_animation(robot, trajectory, fig=None, ax=None, eef_poses=None, obstacles=None, visible_obstacles=False, only_visible_geom=False, visible_text=True, visible_scatter=True, interval=100, repeat=False, result=None)[source]: Plot animation

pykin.utils.plot_utils.plot_basis(robot=None, ax=None)[source]: Plot a frame fitted to the robot size

pykin.utils.plot_utils.plot_baxter(nodes, ax, visible_text=True, visible_scatter=True)[source]: Plot baxter robot

pykin.utils.plot_utils.plot_box(ax=None, size=array([1.0, 1.0, 1.0]), alpha=1.0, A2B=array([[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0]]), color='k')[source]: Plot box

pykin.utils.plot_utils.plot_collision(robot, transformations, ax, alpha=0.8)[source]: Plot robot’s collision

pykin.utils.plot_utils.plot_cylinder(ax=None, length=1.0, radius=1.0, A2B=array([[1.0, 0.0, 0.0, 0.0], [0.0, 1.0, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0], [0.0, 0.0, 0.0, 1.0]]), n_steps=100, alpha=1.0, color='k')[source]: Plot cylinder

pykin.utils.plot_utils.plot_mesh(robot, transformations, mesh_path)[source]: Plot mesh

pykin.utils.plot_utils.plot_obstacles(obstacles, ax)[source]: Plot obstacles

pykin.utils.plot_utils.plot_path_planner(path, ax)[source]: Plot rrt* path planner

pykin.utils.plot_utils.plot_robot(robot, ax=None, transformations=None, visible_visual=False, only_visible_geom=False, mesh_path='../assets/urdf/baxter/', visible_text=True, visible_scatter=True)[source]: Plot robot

pykin.utils.plot_utils.plot_rrt_vertices(vertices, ax)[source]: Plot rrt* trees

pykin.utils.plot_utils.plot_sphere(ax=None, radius=1.0, p=array([0.0, 0.0, 0.0]), n_steps=20, alpha=1.0, color='k')[source]: Plot sphere

pykin.utils.plot_utils.plot_trajectories(ax, path)[source]: Plot plot_trajectories

pykin.utils.plot_utils.show_figure()[source]: Show figure

pykin.utils.transform_utils module

pykin.utils.transform_utils.get_h_mat(position, orientation)[source]¶
pykin.utils.transform_utils.get_homogeneous_matrix(position=array([0.0, 0.0, 0.0]), orientation=array([1.0, 0.0, 0.0, 0.0]))[source]: Returns homogeneous matrix from position and orientation

pykin.utils.transform_utils.get_h_mat_from_quaternion(quaternion)[source]¶
pykin.utils.transform_utils.get_homogeneous_matrix_from_quaternion(quaternion)[source]: Returns homogeneous rotation matrix from quaternion.

pykin.utils.transform_utils.get_identity_h_mat()[source]¶
pykin.utils.transform_utils.get_identity_homogeneous_matrix()[source]: Returns identity matrix

pykin.utils.transform_utils.get_inverse_homogeneous(matrix)[source]: Returns homogeneous inverse

pykin.utils.transform_utils.get_matrix_from_axis_angle(axis, angle)[source]: Returns rotation matrix from axis angle

pykin.utils.transform_utils.get_matrix_from_quaternion(q, convention='wxyz')[source]: Returns rotation matrix from quaternion

pykin.utils.transform_utils.get_matrix_from_rpy(rpy)[source]: Returns rotation matrix from rpy

<<<<<<< HEAD pykin.utils.transform_utils.get_pos_mat_from_homogeneous(h_mat)[source]¶: Returns position matrix from homogeneous matrix

<<<<<<< HEAD pykin.utils.transform_utils.get_pose_from_homogeneous(h_mat)[source]¶: Returns (7,1) pose from homogeneous matrix

pykin.utils.transform_utils.get_quaternion(orientation, convention='wxyz')[source]: Returns quaternion from orientation

pykin.utils.transform_utils.get_quaternion_about_axis(angle, axis)[source]: Returns quaternion for rotation about axis.

pykin.utils.transform_utils.get_quaternion_from_axis_angle(axis, angle, convention='wxyz')[source]: Returns quaternion from axis angle

pykin.utils.transform_utils.get_quaternion_from_matrix(R, convention='wxyz')[source]: Returns quaternion from rotation matrix

pykin.utils.transform_utils.get_quaternion_from_rpy(rpy, convention='wxyz')[source]: Returns quaternion from rpy

pykin.utils.transform_utils.get_quaternion_inverse(quaternion)[source]: Returns quaternion inverse

<<<<<<< HEAD pykin.utils.transform_utils.get_rot_mat_from_homogeneous(h_mat)[source]¶: Returns rotation matrix from homogeneous matrix

pykin.utils.transform_utils.get_rotation_matrix(orientation)[source]: Returns rotation matrix from orientation

pykin.utils.transform_utils.get_rpy_from_matrix(R)[source]: Returns roll pitch, yaw from Rotation matrix

pykin.utils.transform_utils.get_rpy_from_quaternion(q, convention='wxyz')[source]: Returns roll pitch, yaw from quaternion

<<<<<<< HEAD pykin.utils.transform_utils.homogeneous_to_pose(matrix)[source]¶: Returns pose from h_mat
======= pykin.utils.transform_utils.homogeneous_to_pose(matrix)[source]; Returns pose from homogeneous_matrix
>>>>>>> ea49476fc7b0492d09cb4006dd56bb6aba81ff14

<<<<<<< HEAD pykin.utils.transform_utils.pose_to_homogeneous(pose)[source]¶: Returns h_mat from pose
======= pykin.utils.transform_utils.pose_to_homogeneous(pose)[source]; Returns homogeneous_matrix from pose
>>>>>>> ea49476fc7b0492d09cb4006dd56bb6aba81ff14

pykin.utils.transform_utils.quaternion_multiply(quaternion1, quaternion0)[source]: Returns multiplication of two quaternions.

pykin.utils.transform_utils.vector_norm(data, axis=None, out=None)[source]: Returns length, i.e. Euclidean norm, of ndarray along axis.

pykin.utils.urdf_utils module

class pykin.utils.urdf_utils.URDF_Joint[source]

Bases: object

Class of parsing joint info described in URDF

static set_axis(elem_joint, frame)[source]: Set joint’s axis

static set_limit(elem_joint, frame)[source]: Set joint’s limit

static set_origin(elem_joint, frame)[source]: Set joint’s origin

class pykin.utils.urdf_utils.URDF_Link[source]

Bases: object

Class of parsing link info described in URDF

static set_collision(elem_link, link_frame)[source]: Set link collision

static set_collision_geometry(elem_collision, frame)[source]: Set link collision’s geometry

static set_collision_origin(elem_collision, frame)[source]: Set link collision’s origin

static set_visual(elem_link, link_frame)[source]: Set link visual

static set_visual_color(elem_visual, frame)[source]: Set link visual’s color

static set_visual_geometry(elem_visual, frame)[source]: Set link visual’s geometry

static set_visual_origin(elem_visual, frame)[source]: Set link visual’s origin

pykin.utils package

Submodules

pykin.utils.error_utils module

pykin.collision.collision_manager module¶

pykin.utils.fcl_utils module

pykin.utils.kin_utils module

pykin.utils.object_utils module

pykin.utils.plot_utils module

pykin.utils.transform_utils module

pykin.utils.urdf_utils module

Module contents