Data types

Overview

In order to give a better overview of the data types which are defined by xamla_motion these are devided into five groups.

The groups are robot and environment description types, joint space data types, task space data types and state/result data types

Robot and environment description types

Data type

Description

xamla_motion.data_types.JointSet

Manages a list of joint names which describes a robot or part of it

xamla_motion.data_types.EndEffectorDescription

Describes an end effector of a robot

xamla_motion.data_types.MoveGroupDescription

Describes a robot or a subset of it by move groups, end effectors and joints

xamla_motion.data_types.CollisionPrimitive

Defines and describe a primitive collision object

xamla_motion.data_types.CollisionObject

Defines a collision object which consists of a single primitive or an assembly of them

Joint space data types

Data type

Description

xamla_motion.data_types.JointValues

Manages respective joint values for a specific joint set

xamla_motion.data_types.JointLimits

Manages the joint limits for a specific set of joints

xamla_motion.data_types.JointStates

Can hold a complete JointState (pos, vel, eff) for a specific joint set

xamla_motion.data_types.JointPath

Describes a path by a list of joint configurations/values

xamla_motion.data_types.JointTrajectoryPoint

Defines a point of a trajectory for a specific joint set

xamla_motion.data_types.JointTrajectory

Describes a trajectory by a list of trajectory points

xamla_motion.data_typesPlanParameters

Holds all constrains for joint space trajectory planning

Task space data types

Data type

Description

xamla_motion.data_types.Pose

Pose defined by three dimensional translation and rotation

xamla_motion.data_types.EndEffectorLimits

Holds hold task space / endeffector limits

xamla_motion.data_types.CartesianPath

Describes a path by a list of poses

xamla_motion.data_types.TaskSpacePlanParameters

Holds all constrains for task space trajectory planning

State and result data types

Data type

Description

xamla_motion.data_types.MoveGripperResult

Represents the result of a common gripper action

xamla_motion.data_types.WsgResult

Respresents the result of a WSG gripper action

xamla_motion.data_types.IkResults

Holds result of a inverse kinematics query

xamla_motion.data_types.SteppedMotionState

Carries a state snapshot of a stepped motion operation

xamla_motion.data_types.JointValuesCollisions

Represents a robot collision

Robot and environment description types

JointSet

How to use it:

Details:

class xamla_motion.data_types.JointSet(names)[source]

Manages a list of unique joint names

empty()[source]

Creates emtpy JointSet

add_prefix(prefix)[source]

Creates new JointSet where prefix is added to every joint name

is_subset(other)[source]

Checks if it is a subset of the JointSet in parameter other

is_superset(other)[source]

Checks if it is a superset of the JointSet in parameter other

is_similar(other)[source]

Checks if it contains the same joint names as in JointSet other

union(other)[source]

Creates new JointSet which contains the union of self and other joints

try_get_index_of(name)[source]

Tries to get the list index of the joint by name

get_index_of(name)[source]

Returns the list index of joint name specificed paramerter name

contains(name)

Checks if this JointSet contains a specific joint name

Initialization of the JointSet class

Parameters

names (str or Iterable[str convertable]) – The Joint set class is initializable in different ways. -by a string which only contains one joint name -by a string which contains multiple joints names separated by a comma as delimiter -Iterable container where each time is convertable to str

Returns

Return type

An instance of class JointSet

:raises TypeError : type mismatch: If input parameter names is not of type str or Iterable[str convertable]

Examples

Create an instance of JointSet by string and iterable type

>>> from xamla_motion.data_types import JointSet
>>> JointSet('Joint1, Joint2')
JointSet:
Joint1
Joint2
>>> JointSet(['Joint1','Joint2'])
JointSet:
Joint1
Joint2
>>> JointSet('Joint1,Joint1,Joint2')
JointSet:
Joint1
Joint2
add_prefix(prefix)[source]

Creates new JointSet where prefix is added to every joint name

Parameters

prefix (str) – Defines the prefix which is added in front of every joint name

:raises TypeError : type mismatch: If input parameter prefix is not of type str

Returns

The created JointSet with added prefix to joint names

Return type

JointSet

Examples

Create new JointSet instance with added prefix

>>> from xamla_motion.data_types import JointSet
>>> joint_set = JointSet('joint1, joint2')
>>> joint_set.add_prefix('robot1_')
JointSet:
robot1_joint1
robot1_joint2
difference(others)[source]

Creates new JointSet which contains the union of self and others joints

Parameters

others (JointSet) – JointSet with which the union is performed

:raises TypeError : type mismatch: If others is not one of expected type JointSet

Returns

The created JointSet with union joint names

Return type

JointSet

Examples

Create new JointSet instance which is the union of two existing ones

>>> from xamla_motion.data_types import JointSet
>>> joint_set1 = JointSet('joint1, joint2, joint3')
>>> joint_set2 = JointSet('joint1, joint2')
>>> joint_set1.difference(joint_set2)
JointSet:
joint3
static empty()[source]

Creates a empty JointSet

Returns

The created empty JointSet

Return type

JointSet

Examples

Create a empty JointSet instance

>>> from xamla_motion.data_types import JointSet
>>> JointSet.empty()
JointSet:
get_index_of(name)[source]

Returns the list index of joint name specificed paramerter name

namestr

joint name for which it finds the list index

Returns

index – List index of the searched joint name

Return type

int

:raises TypeError : type mismatch: If parameter name is not type of str :raises ValueError : value not exists: If joint name not exists

Examples

Get index of a specific joint in JointSet

>>> from xamla_motion.data_types import JointSet
>>> joint_set1 = JointSet('joint0')
>>> joint_set1.get_index_of('joint0')
0
>>> joint_set1.try_get_index_of('joint1')
ValueError: This JointSet not contains a joint with name: joint1
intersection(others)[source]

Creates new JointSet which contains the intersection of self and others joints

Parameters

others (JointSet or Iterable[JointSet]) – JointSet with which the union is performed

:raises TypeError : type mismatch: If others is not one of expected types JointSet or Iterable of JointSet

Returns

The created JointSet with intersecting joint names

Return type

JointSet

Examples

Create new JointSet instance which is the intersection of two existing ones

>>> from xamla_motion.data_types import JointSet
>>> joint_set1 = JointSet('joint1, joint2')
>>> joint_set2 = JointSet('joint2')
>>> joint_set1.intersection(joint_set2)
JointSet:
joint2
is_similar(other)[source]

Checks if it contains the same joint names as in JointSet other

The method returns true if all joint names are available in the other join set. It is not necessary that both have the same order.

Parameters

other (JointSet) – JointSet which is used for comparision

Returns

result – If this JointSet the joint names and number of joint names returns True else False

Return type

bool

:raises TypeError : type mismatch: If the parameter other is not of type JointSet

Examples

Check if one joint set is the similar to another

>>> from xamla_motion.data_types import JointSet
>>> joint_set1 = JointSet('joint1, joint2, joint3')
>>> joint_set2 = JointSet('joint1, joint3, joint2')
>>> joint_set3 = JointSet('joint1, joint3')
>>> joint_set1.is_similar(joint_set2)
True
>>> joint_set3.is_similar(joint_set1)
False
is_subset(other)[source]

Checks if it is a subset of the JointSet in parameter other

The method returns True if it is a subset or similar to other. The ordering of the joint names is not checked.

Parameters

other (JointSet) – JointSet which is used for comparision

Returns

result – If this JointSet is a subset of the other JointSet returns True else False

Return type

bool

:raises TypeError : type mismatch: If the parameter other is not of type JointSet

Examples

Check if one joint set is the subset of another

>>> from xamla_motion.data_types import JointSet
>>> joint_set1 = JointSet('joint1, joint2, joint3')
>>> joint_set2 = JointSet('joint1, joint2')
>>> joint_set2.is_subset(joint_set1)
True
is_superset(other)[source]

Checks if it is a superset of the JointSet in parameter other

The method returns True if it is a superset or similar to other. The ordering of the joint names is not checked.

Parameters

other (JointSet) – JointSet which is used for comparision

Returns

result – If this JointSet is a superset of the other JointSet returns True else False

Return type

bool

:raises TypeError : type mismatch: If the parameter other is not of type JointSet

Examples

Check if one joint set is the superset of another

>>> from xamla_motion.data_types import JointSet
>>> joint_set1 = JointSet('joint1, joint2, joint3')
>>> joint_set2 = JointSet('joint1, joint2')
>>> joint_set1.is_superset(joint_set2)
True
names

List[str] (readonly) List of joint names

Type

names

try_get_index_of(name)[source]

Tries to get the list index of the joint by name

Parameters

name (str) – joint name for which it tries to find the list index

Returns

  • is_found (bool) – If the index is found it resturns True else False

  • index (int) – If index is found returns the index else returns None

:raises TypeError : type mismatch: If parameter name is not type of str

Examples

Try to get index of a specific joint in JointSet

>>> from xamla_motion.data_types import JointSet
>>> joint_set1 = JointSet('joint0')
>>> joint_set1.try_get_index_of('joint0')
(True, 0)
>>> joint_set1.try_get_index_of('joint1')
(False, None)
union(others)[source]

Creates new JointSet which contains the union of self and others joints

Parameters

others (JointSet or Iterable[JointSet]) – JointSet with which the union is performed

:raises TypeError : type mismatch: If others is not one of expected types JointSet or Iterable of JointSet

Returns

The created JointSet with union joint names

Return type

JointSet

Examples

Create new JointSet instance which is the union of two existing ones

>>> from xamla_motion.data_types import JointSet
>>> joint_set1 = JointSet('joint1, joint2')
>>> joint_set2 = JointSet('joint2, joint3')
>>> joint_set1.union(joint_set2)
JointSet:
joint1
joint2
joint3

EndEffectorDescription

How to use it:

class xamla_motion.data_types.EndEffectorDescription(name, sub_move_group_ids, joint_set, move_group_name, link_name)[source]

Class which describes an end effector

Initialization of EndEffectorDescription class

Parameters
  • name (str convertable) – name of the end effector

  • sub_move_group_ids (Iterable[str convertable]) – ids of the sub move groups

  • joint_set (JointSet) – JointSet which contains all joints of the base move group (if provided)

  • move_group_name (str convertable) – name of the move group

  • link_name (str convertable) – name of the end effector link

:raises TypeError : type mismatch: If joint_set is not of type JointSet or ids or names are not iterable :raises ValueError: If a input is not convertable to str

Returns

The created EndEffectorDescription

Return type

EndEffectorDescription

Examples

Create a instance of EndEffectorDescription

>>> from xamla_motion.data_types import EndEffectorDescription
>>> joint_set = JointSet('joint0')
>>> EndEffectorDescription('tcp',[1],joint_set,['group1'],'tcp_link')
EndEffectorDescription
link_name = tcp_link
name = tcp
move_group_name = ['group1']
joint_set = JointSet:
joint0
sub_move_group_ids = ['1']
joint_set

JointSet (read only) JointSet which contains all joints of the base move group

Type

joint_set

List[str] (read only) names of the endeffector link

Type

link_name

move_group_name

str (read only) name of the move group

Type

move_group_name

name

str (read only) name of the end effector

Type

name

sub_move_group_ids

List[str] (read only) ids of the sub move groups

Type

sub_move_group_ids

Details:

MoveGroupDescription

How to use it:

Details:

class xamla_motion.data_types.MoveGroupDescription(name, sub_move_group_ids, joint_set, end_effector_names, end_effector_link_names)[source]

Class which describes a robot or a subset of it by move groups, end effectors and joints

Initialization of MoveGroupDescription class

Parameters
  • name (str convertable) – name of the base move group

  • sub_move_group_ids (Iterable[str convertable]) – ids of the sub move groups

  • joint_set (JointSet) – JointSet which contains all joints of the base move group (if provided)

  • end_effector_names (Iterable[str convertable]) – names of the end effectors

  • end_effector_link_names (Iterable[str convertable]) – names of the end effector links

:raises TypeError : type mismatch: If joint_set is not of type JointSet or ids or names are not iterable :raises ValueError: If a input is not convertable to str

Returns

The created MoveGroupDescription

Return type

MoveGroupDescription

Examples

Create a instance of MoveGroupDescription

>>> from xamla_motion.data_types import MoveGroupDescription
>>> joint_set = JointSet('joint0')
>>> MoveGroupDescription('group1',[1],joint_set,['tcp'],['tcp_link'])
MoveGroupDescription
end_effector_names = ['tcp']
sub_move_group_ids = ['1']
end_effector_link_names = ['tcp_link']
joint_set = JointSet:
joint0
name = group1

List[str](read only) names of the end effector links

Type

end_effector_link_names

end_effector_names

List[str](read only) names of the end effectors

Type

end_effector_names

joint_set

JointSet(read only) JointSet which contains all joints of the base move group

Type

joint_set

name

str (read only) name of the base move group

Type

name

sub_move_group_ids

List[str](read only) ids of the sub move groups

Type

sub_move_group_ids

CollisionPrimitive

How to use it:

Details:

class xamla_motion.data_types.CollisionPrimitive(kind, parameters, pose)[source]

Class CollisionPrimitive

At the moment following simple primitives are supported: plane, box, sphere, cylinder, cone

from_shape_msg(msg, pose)[source]

Creates an Instance of CollisionPrimitive from moveit shape msg

create_plane(a, b, c, d, pose)[source]

Creates an instance of CollisionPrimitive which represents a plane

create_box(cls, x, y, z, pose)[source]

Creates an instance of CollisionPrimitive which represents a box

create_unit_box(pose)[source]

Creates an instance of CollisionPrimitive which represents unit box

create_sphere(radius, pose)[source]

Creates an instance of CollisionPrimitive which represents a sphere

create_unit_sphere(pose)[source]

Creates an instance of CollisionPrimitive which represents unit sphere

create_cylinder(height, radius, pose)[source]

Creates an instance of CollisionPrimitive which represents a cylinder

create_cone(height, radius, pose)[source]

Creates an instance of CollisionPrimitive which represents a cone

to_shape_msg()[source]

Creates an instance of moveit/shape_msgs.Plane for a plane primitive or an instance of moveit/shape_msgs.SolidPrimitive for all other primitive

Initialization of CollisionPrimitive

Representation of a plane, using the plane equation ax + by + cz + d = 0

a := parameters[0] b := parameters[1] c := parameters[2] d := parameters[3]

Representation of a box

x := parameters[0] y := parameters[1] z := parameters[2]

Representation of a sphere radius := parameters[0]

Representation of a cylinder

height := parameters[0] radius := parameters[1]

Representation of a cone

height := parameters[0] radius := parameters[1]

Parameters
  • kind (CollisionPrimitiveKind) – Specifices which kind of primitive the new instance should represent

  • parameters (Iterable[float convertable]) – Parameters which describe the primitive

  • pose (Pose) – pose describes the position and orientation of the mid point of the primitive

Returns

An Instance of collision primitive

Return type

CollisionPrimitive

Raises
  • TypeError – If kind is not of type CollisionPrimitiveKind If pose is not of type Pose If parameters is not of type iterable of float convertable

  • ValuesError – If for a specific primitive kind the wrong number of parameters are provided If kind is CollisionPrimitiveKind.plane and the normal vector is described only by zeros If kind is not CollisionPrimitveKind.plane and a parameter in parameters is smaller than zero

Examples

Create a instance of a box collision primitive which is one meter in size in all three dimensions and the origin is the origin of the world coordinate system

>>> import numpy as np
>>> from xamla_motion.data_types import CollisionPrimitive, CollisionPrimitiveKind, Pose
>>> origin = Pose.from_transformation_matrix(np.eye(4),'world')
>>> CollisionPrimitive(CollisionPrimitiveKind.box,[1.0,1.0,1.0], origin)
CollisionPrimitive
pose = Pose:
translation.x : 0.0
translation.y : 0.0
translation.z : 0.0
quaternion.w : 1.0
quaternion.x : 0.0
quaternion.y : 0.0
quaternion.z : 0.0
frame_id : world
kind = CollisionPrimitiveKind.box
parameters = [ 1.  1.  1.]
classmethod create_box(x, y, z, pose)[source]

Creates an instance of CollisionPrimitive which represents a box

Representation of box aligned with pose xyz coordinates

Parameters
  • x (float convertable) – x coordinate of pose

  • y (float convertable) – y coordinate of pose

  • z (float convertable) – z coordinate of pose

  • pose (Pose) – Pose of the midpoint of the box

Returns

Primitive which represents a box

Return type

CollisionPrimitve

Raises
  • TypeError – If pose is not of type Pose If a parameter is not float convertable

  • ValuesError – If a parameter in is smaller than zero

classmethod create_cone(height, radius, pose)[source]

Creates an instance of CollisionPrimitive which represents a cone

Parameters
  • height (float convertable) – height of the cone

  • radius (float convertable) – radius of the cone

  • pose (Pose) – Pose of the midpoint of the cone

Returns

Primitive which represents a cone

Return type

CollisionPrimitve

Raises
  • TypeError – If pose is not of type Pose If a parameter is not float convertable

  • ValuesError – If a parameter in is smaller than zero

classmethod create_cylinder(height, radius, pose)[source]

Creates an instance of CollisionPrimitive which represents a cylinder

Parameters
  • height (float convertable) – height of the cylinder

  • radius (float convertable) – radius of the cylinder

  • pose (Pose) – Pose of the midpoint of the cylinder

Returns

Primitive which represents a cylinder

Return type

CollisionPrimitve

Raises
  • TypeError – If pose is not of type Pose If a parameter is not float convertable

  • ValuesError – If a parameter in is smaller than zero

classmethod create_plane(a, b, c, d, pose)[source]

Creates an instance of CollisionPrimitive which represents a plane

Representation of a plane, using the plane equation ax + by + cz + d = 0

Parameters
  • a (float convertable) – a parameter of plane equation

  • b (float convertable) – b parameter of plane equation

  • c (float convertable) – c parameter of plane equation

  • d (float convertable) – d parameter of plane equation

  • pose (Pose) – Pose of the midpoint of the plane

Returns

Primitive which represents a plane

Return type

CollisionPrimitve

Raises
  • TypeError – If pose is not of type Pose If a parameter is not float convertable

  • ValuesError – If the normal vector is described only by zeros If kind is not CollisionPrimitveKind.plane and a parameter in parameters is smaller than zero

classmethod create_sphere(radius, pose)[source]

Creates an instance of CollisionPrimitive which represents a sphere

Parameters
  • radius (float convertable) – radius of the sphere

  • pose (Pose) – Pose of the midpoint of the sphere

Returns

Primitive which represents a sphere

Return type

CollisionPrimitve

Raises
  • TypeError – If pose is not of type Pose If a parameter is not float convertable

  • ValuesError – If a parameter in is smaller than zero

classmethod create_unit_box(pose)[source]

Creates an instance of CollisionPrimitive which represents unit box

Parameters

pose (Pose) – Pose of the midpoint of the box

Returns

Primitive which represents a unit box

Return type

CollisionPrimitve

Raises
  • TypeError – If pose is not of type Pose If a parameter is not float convertable

  • ValuesError – If a parameter in is smaller than zero

classmethod create_unit_sphere(pose)[source]

Creates an instance of CollisionPrimitive which represents unit sphere

Parameters

pose (Pose) – Pose of the midpoint of the sphere

Returns

Primitive which represents a unit sphere

Return type

CollisionPrimitve

Raises
  • TypeError – If pose is not of type Pose If a parameter is not float convertable

  • ValuesError – If a parameter in is smaller than zero

classmethod from_shape_msg(msg, pose)[source]

Creates an Instance of CollisionPrimitive from moveit shape msg

At the moment it can handle moveit plane and solid primitive

Parameters
  • msg (message type from moveit/shape_msgs) – Message from which a CollisionPrimitive should be created

  • pose (Pose) – Pose instance which describes the midpoint pose of primitive

kind

CollisionPrimitiveKind kind of the collision primitive

Type

kind

parameters

numpy.array(dtype.floating) parameters which describe the primitive

Type

parameters

pose

Pose Describes the position and orientation of the primitive for the primitive midpoint

Type

pose

to_shape_msg()[source]

Creates an instance of moveit/shape_msgs.Plane for a plane primitive or an instance of moveit/shape_msgs.SolidPrimitive for all other primitive

CollisionObject

How to use it:

Details:

class xamla_motion.data_types.CollisionObject(primitives, frame_id='world')[source]

Class CollisionObject

An Instance of this class represent a collision object. A collision object can be a simple primitive or a assembly of many primitives

from_collision_object_msg(msg)[source]

Create a Pose from an instance of moveit_msgs/CollisionObject

to_collision_object_msg(self, action):

Creates a ROS collision object message from this instance

Initialization of CollisionObject class

Parameters
  • primitives (Iterable[CollisionPrimitives] or None) – primitives which describe the collision object

  • frame_id (str) – Frame / coordinate system in which the collision object is described

Returns

Instance of CollisionObject

Return type

CollisionObject

Raises

TypeError – If primitives are not of type iterable of CollisionPrimitives or None or if frame_id is not str convertable

frame_id

str (read only) Frame / coordinate system in which the collision object is described

Type

frame_id

classmethod from_collision_object_msg(msg)[source]

Create a Pose from an instance of moveit_msgs/CollisionObject

Parameters

msg (ros message moveit_msgs/CollisionObject) – Message of moveit_msgs/CollisionObject

Returns

New instance of collision object from msg

Return type

CollisionObject

Raises

TypeError – If msg is not of type moveit_msgs/CollisionObject

primitives

List[CollisionPrimitives] or None (read only) primitives which describe the collision object

Type

primitives

to_collision_object_msg(action)[source]

Creates a ROS collision object message from this instance

Parameters

action (moveit_msg/CollisionObject operation) – Defines which action/opertation should be performed (add, remove, append, move)

Returns

Return type

moveit_msgs/CollisionObject

Joint space data types

JointValues

How to use it:

Details:

class xamla_motion.data_types.JointValues(joint_set, values)[source]

Manages a set of joints and the respective joint values

empty()[source]

Creates empty instance of JointValues

zero(joint_set)[source]

Creates instance of JointValues with all values 0.0

try_get_value(name)[source]

Tries to get joint value by joint name

reorder(new_order)[source]

Creates reordered instance of JointValues by the order of new_order

transform(transform_function)[source]

Creates a transformed version of JointValues

select(names)[source]

Creates a JointValue instance which only contains selected joints

set_values(joint_set, values)[source]

Create a new instance of JointValues with modified values

merge(others)[source]

Merge JointValues instance with others JointValues

from_joint_path_point_msg(joint_set, msg)[source]

Creates an instance of JointValues from a JointPathPoint ros message

to_joint_path_point_msg()[source]

Transform to xamlamoveit_msgs JointPathMessage

Initialization of the JointValues class

Parameters
  • joint_set (JoinSet) – JointSet for which also the values should be managed

  • values (iterable[float castable]) – The JointValue class can be initialized in different ways -JointSet + only on float convertable value then all joint values are set to the same float value -JointSet + an iterable type with the same number of items as number of joints in joint set. (mapping one to one)

Returns

An instance of class JointValues

Return type

JointValues

:raises TypeError : type mismatch: If joint_set is not of type JointSet or type in values is not float castable :raises ValueError : not same size: If values is list of floats and not contains the same number of items as joint_set or numpy array that has not the correct size

static empty()[source]

Creates a empty JointValues instance

Returns

joint_values – An empty instance of JointValues

Return type

JointValues

classmethod from_joint_path_point_msg(joint_set, msg)[source]

Creates an instance of JointValues from a JointPathPoint ros message

Parameters
  • joint_set (JointSet) – Joint set to assign joint with values

  • msg (JointPathPoint ros message) – Message which should be transformed to JointValues

Returns

New instance of JointValues

Return type

JointValues

Raises

TypeError – If joint_set is not of type JointSet

classmethod from_joint_values_point_msg(msg)[source]

Creates an instance of JointValues from JointValuesPoint message

Parameters

msg (xamlamoveit_msgs JointValuesPoint message) – Message which should be transformed to JointValues

Returns

New instance of JointValues

Return type

JointValues

Raises

AttributeError – If msg not provide needed properties and methods

joint_set

JointSet (readonly) A instance of JointSet managing joint names

Type

joint_set

merge(others)[source]

Merge JointValues instance with others JointValues

Parameters

others (JointValues or Iterable[JointValues]) – JointValues which are merge with current instance

Returns

New instance of JointValues with contains Values for all Joints defined in this and others JointValues instances

Return type

JointValues

:raises TypeError : type mismatch: If others is not one of expected types JointValues or Iterable[JointValues] :raises ValueError : merge conflict: If values for some joints are defined in multiple instances of JointValues which should be merged

reorder(new_order)[source]

Creates reordered instance of JointValue by the order of new_order

Parameters

new_order (JointSet) – JointSet which defines the new order

Returns

A new Instance of JointValues containing the joints in the order definied by new_order (it is ok when new_order only holds a subset of joints)

Return type

JointValues

:raises TypeError : type mismatch: If input parameter new_order is not of type JointSet :raises ValueError :: If joint name from new_order not exists

select(names)[source]

Creates a JointValue instance which only contains selected joints

Parameters

names (str or list of str or JointSet) – Joint names which should be in the new JointValues instance

Returns

New instance of JointValues with selected joints

Return type

JointValues

:raises TypeError : type mismatch: If names is not of type str or list of str :raises ValueError :: If name not exist in joint names

set_values(joint_set, values)[source]

Create a new instance of JointValues with modified values

Parameters
  • joint_set (str, Iterable[str], JointSet) – joint_set defined by single string List of strings or JointSet

  • values (float, Iterable[float], np.ndarray) –

Returns

New instance of JointValues with modified values

Return type

JointValues

Raises
  • TypeError – If joint_set is not str, Iterable[str] or JointSet If values is not iterable

  • ValueError – If joint_set and values unequal length If joint defined in joint set not exist in the JointSet of this JointValues instance

to_joint_path_point_msg()[source]

Transform to xamlamoveit_msgs JointPathPoint message

to_joint_values_point_msg()[source]

Transform to xamlamoveit_msgs JointValuesPoint message

transform(transform_function)[source]

Creates a transformed version of JointValues

The transformation which is applied to every value in JointValues is defined by the transform function

Parameters

transform_function (callable or numpy.ufunc) – Function which is applied to every value in join values

Returns

A new Instance of JointValues with transformed joint values

Return type

JointValues

:raises TypeError : type mismatch: If transform function is not callable or not a numpy.ufunc and if the function dont has the signature input : floating , output : floating

try_get_value(joint_name)[source]

Tries to get joint value by joint name

Parameters

joint_name (str) – joint name for which it tries to get joint value

Returns

  • is_found (bool) – If joint name exists and therefore values is found True else False

  • value (float) – Joint value if joint name existes else None

values

numpy.ndarray(dtype=float64) (readonly) One dimensional numpy array holding the respective joint values

Type

valules

static zero(joint_set)[source]

Creates instance of JointValues with all values 0.0

Parameters

joint_set – JointSet which should be managed by JointValues

Returns

An JointValue instance with values initialized with 0.0

Return type

JointValues

Raises

TypeError – type mismatch: If input parameter join_set is not of type JointSet

JointLimits

How to use it:

Details:

class xamla_motion.data_types.JointLimits(joint_set, max_velocity=None, max_acceleration=None, min_position=None, max_position=None)[source]

Manages the joint limits for a set of joints

select(names)[source]

Creates a JointLimits instance which only contains selected joints

Initialization of JointLimits class

If a specific joint has no limits for velocity, acceleration or position than please set them to None or numpy.nan (for numpy.ndarray) the specific joint has than no constrain for this limit

Parameters
  • joint_set (JointSet) – Set of joints for which joint limits are required

  • max_velocity (Iterable[float convertable] or None) – One dimension array which defines the maximal velocity for each joint

  • max_acceleration (Iterable[float convertable] or None) – One dimension array which defines the maximal acceleration for each joint

  • min_position (Iterable[float convertable] or None) – One dimension array which defines the mininmal position for each joint

  • max_position (Iterable[float convertable] or None) – One dimension array which defines the maximal position for each joint

Returns

An instance of class JointLimits

Return type

JointLimits

:raises TypeError : type mismatch: If joint_set is not of type JointSet or max/min values are not of type list of float or numpy array of type floating :raises ValueError : not same size: If max/min values are list of float and not contains the same number of items as joint_set or numpy array that has not the correct size

joint_set

JointSet (readonly) A instance of JointSet managing joint names

Type

joint_set

max_acceleration

numpy.array(dtype=floating) One dimension array which defines the maximal acceleration for each joint(readonly)

Type

max_acceleration

max_position

numpy.array(dtype=floating) One dimension array which defines the maximal position for each joint(readonly)

Type

max_position

max_velocity

numpy.array(dtype=floating) One dimension array which defines the maximal velocity for each joint(readonly)

Type

max_velocity

min_position

numpy.array(dtype=floating) One dimension array which defines the mininmal position for each joint(readonly)

Type

min_position

select(names)[source]

Creates a JointLimits instance which only contains selected joints

Parameters

names (str or list of str) – Joint names which from the new JointLimit instance

Returns

New instance of JointLimits with selected joints

Return type

JointLimits

:raises TypeError : type mismatch: If names is not of type str or list of str :raises ValueError :: If name not exist in joint names

JointStates

How to use it:

Details:

class xamla_motion.data_types.JointStates(positions, velocities=None, efforts=None)[source]

Class which can hold a complete JointState (pos, vel, eff)

The JointState class can hold the complete information about a the state of joints of a robot. For this purpose it holds an instance of JointValues for positions, velocities, and efforts (velocities and efforts are optional)

reorder(new_order)[source]

Creates reordered instance of JointValue by the order of new_order

select(names)[source]

Creates a JointStates instance which only contains selected joints

Initialization of JointStates class

Parameters
  • positions (Joint_Values) – A instance of JointValues which represent the positions

  • velocities (Joint_Values or None (optional)) – A instance of JointValues which represent the velocities

  • efforts (Joint_Values or None (optional)) – A instance of JointValues which represent the efforts

Returns

Creates an instance of JointStates

Return type

JointStates

:raises TypeError : type mismatch: If arguments are not of type JointValues :raises ValueError: If instances of JointValues does not hold the same JointSets (joint names and order)

efforts

JointValues (read only) Joint Values which defines the efforts / acceleration of the joints

Type

efforts

joint_set

JointSet (read only) Set of joints for which it holds the values

Type

joint_set

positions

JointValues (read only) Joint Values which defines the positions of the joints

Type

positions

reorder(new_order)[source]

Creates reordered instance of JointValue by the order of new_order

Parameters

new_order (JointSet) – JointSet which defines the new order

Returns

A new Instance of JointStates containing the joints in the order definied by new_order (it is ok when new_order only holds a subset of joints)

Return type

JointStates

:raises TypeError : type mismatch: If input parameter new_order is not of type JointSet :raises ValueError :: If joint name from new_order not exists

select(names)[source]

Creates a JointStates instance which only contains selected joints

Parameters

names (str or list of str or JointSet) – Joint names which should be in the new JointValues instance

Returns

New instance of JointValues with selected joints

Return type

JointValues

:raises TypeError : type mismatch: If names is not of type str or list of str :raises ValueError :: If name not exist in joint names

velocities

JointValues (read only) Joint Values which defines the velocities of the joints

Type

velocities

JointPath

How to use it:

Details:

class xamla_motion.data_types.JointPath(joints, points)[source]

JointPath class describes a path by a list of joint configurations

from_one_point(point)[source]

Initialization of JointPath class with only one Point

from_start_stop_point(start, stop)[source]

Initialization of JointPath class with start, stop point

prepend(points)[source]

Creates new JointPath with points added in front of path points

append(points)[source]

Creates new JointPath with points added behind the path points

concat(other)[source]

Creates new JointPath with concatenated path points

transform(transform_function)[source]

Creates a transformed version of JointPath

Initialization of JointPath class

Parameters
  • joints (JointSet) – Set of joints

  • points (Iterable[JointValues]) – Iterable of JointValues where each JointValues instance describes a point of the path

Returns

Instance of JointPath

Return type

JointPath

:raises TypeError : type mismatch: If joints is not of type JointSet or if points is not of type list of JointValues

append(points)[source]

Creates new JointPath with points added behind the path points

Parameters

points (JointValues or Iterable[JointValues]) – Points which are added behind path points

Returns

Instance of JointPath with added points

Return type

JointPath

Raises

TypeError – If points is not one of expected types JointValues or Iterable of JointValues

concat(other)[source]

Creates new JointPath with concatenated path points

Parameters

other (JointPath) –

Raises

TypeError – If other is not of type JointPath

classmethod from_one_point(point)[source]

Initialization of JointPath class with only one Point

Parameters

point (JointValues) – Single point to initialize JointPath

Returns

Instance of JointPath

Return type

JointPath

:raises TypeError : type mismatch: If points is not of type JointValues

classmethod from_start_stop_point(start, stop)[source]

Initialization of JointPath class with start, stop point

Parameters
Returns

Instance of JointPath

Return type

JointPath

:raises TypeError : type mismatch: If start or stop is not of type JointValues

joint_set

JointSet (readonly) JointSet which represent the joints for which JointPath describes a path

Type

joint_set

points

Deque[JointValues] (readonly) A deque of JointValues which represent the joint configurations which are describe the path

Type

points

prepend(points)[source]

Creates new JointPath with points added in front of path points

Parameters

points (JointValues or Iterable[JointValues]) – Points which are added in front of path points

Returns

Instance of JointPath with added points

Return type

JointPath

Raises

TypeError – If points is not one of expected types JointValues or Iterable of JointValues

transform(transform_function)[source]

Creates a transformed version of JointPath

The transformation which is applied to every point in JointPath is defined by the transform function

Parameters

transform_function (callable or numpy.ufunc) – Function which is applied to every point value

Returns

A new Instance of JointPath with transformed point values

Return type

JointPath

:raises TypeError : type mismatch: If transform function is not callable or not a numpy.ufunc and if the function dont has the signature input : floating , output : floating

JointTrajectoryPoint

How to use it:

Details:

class xamla_motion.data_types.JointTrajectoryPoint(time_from_start, positions, velocities=None, accelertations=None, efforts=None)[source]

Class JointTrajectoryPoint

from_joint_trajectory_point_msg(joint_set, msg)[source]

Creates an instance of JointTrajectoryPoint from ros message

add_time_offset(offset)[source]

Creates a new instance of this JointTrajectoryPoint with same position, vel, acc, eff but with a modified time_from_start

merge(others)[source]

Creates a new instance of JointTrajectory as a result of the merge operation

to_joint_trajectory_point_msg()[source]

Converts JointTrajectoryPoint to JointTrajectoryPoint ros message

Initialize JointTrajectoryPoint class

Parameters
  • time_from_start (datatime.timedelta) – duration between start of trajectory and this point

  • positions (JointValues) – joint positions

  • velocities (JointValues or None) – joint velocities

  • accelerations (JointValues) – joint accelerations

  • efforts (JointValues) – joint force for a prismatic joint or torque for a rotational joint

Returns

Instance of JointTrajectoryPoint

Return type

JointTrajectoryPoint

:raises TypeError : type mismatch: If time_from_start is not of expected type datatime.timedelta or if positions is not of expected type JointValues or if velocities, accelerations or efforts ist not one of the expected types None or JointValues :raises ValueError: If the joint set of positions and if not None velocities, accelerations and efforts is not equal

accelerations

JointValues (readonly) If defined available joint accelerations of this trajectory point else None

Type

accelerations

add_time_offset(offset)[source]

Creates a new instance of JointTrajectoryPoint with applied time offset

adds a time offset to time_from_start

Parameters

offset (datatime.timedelta) – time offset as datatime timedelta

Returns

Return type

Instance of JointTrajectoryPoint with modified time_from_start

Raises

TypeError – If offset ist not of expected type datatime.timedelta

efforts

JointValues (readonly) If defined available joint efforts of this trajectory point else None

Type

efforts

classmethod from_joint_trajectory_point_msg(joint_set, msg)[source]

Creates an instance of JointTrajectoryPoint from ros message

Creates an instance of JointTrajectoryPoint from the ros message trajectory_msgs/JointTrajectoryPoint

Parameters
  • joint_set (JointSet) – Set of joints for which the trajectroy point is defined

  • msg (trajectory_msgs/JointTrajectoryPoint) – Instance of ros message that should be converted to JointTrajectoryPoint

Returns

New instance of JointTrajectoryPoint with the values of the ros message

Return type

JointTrajectoryPoint

:raises TypeError : type mismatch: If values from msg are not convertable to numpy array of type floating or if joint_set is not of expected type JointSet

interpolate_cubic(other, time)[source]

Interpolate cubic between self and other trajectory point

Parameters
  • other (JointTrajectoryPoint) – Second trajectory point for interpolation

  • time (timedelta) – time for which interpolation is performed

Returns

Return type

JointTrajectoryPoint

Raises

ValueError – If time from start of point is smaller than time from start of self If joint sets of point and self are not equal

joint_set

JointSet Set of joints for which the trajectory point contains values

Type

joint_set

merge(others)[source]

Creates a new instance of JointTrajectoryPoint as a result of the merge operation

Parameters

others (JointTrajectoryPoint or Iterable[JointTrajectoryPoint]) – TrajectoryPoints to merge with the current TrajectoryPoint

Returns

New instance of JointTrajectoryPoint which contains the merged JointTrajectoryPoints

Return type

JointTrajectoryPoint

:raises TypeError : type mismatch: If other not one of excpeted types JointTrajectoryPoint or Iterable[JointTrajectoryPoint] :raises ValueError: If time_from_start in self and others dont match If JointSets of self and others are not mergeable If self and others define more or less properties of JointTrajectoryPoint

positions

JointValues (readonly) Joint positions of this trajectory point

Type

positions

time_from_start

datetime.timedelta (readonly) Time span between trajectory start and this trajectory point as a instance of datetime.timedelta

Type

time_from_start

to_joint_trajectory_point_msg()[source]

Converts JointTrajectoryPoint to JointTrajectoryPoint ros message

trajectory_msgs/JointTrajectoryPoint.msg

Returns

New instance of JointTrajectoryPoint with the values of the ros message

Return type

JointTrajectoryPoint msg

velocities

JointValues (readonly) If defined available joint velocites of this trajectory point else None

Type

velocities

with_time_from_start(time)[source]

Set time from start

Parameters

time (timedelta) – time form start to set

Returns

Return type

JointTrajectoryPoint

JointTrajectory

How to use it:

Details:

class xamla_motion.data_types.JointTrajectory(joint_set, points, valid=True)[source]

Class JointTrajectory

empty()[source]

Creates a empty JointTrajectory instance

to_joint_trajectory_msg(self, seq=0, frame_id='')[source]

Converts JointTrajectory to JointTrajectory ros message

Initialize class JointTrajectory

Parameters
  • joint_set (JointSet) – Set of joints for which JointTrajectory should be defined

  • points (Iterable[JointTrajectoryPoint]) – An Iterable of JointTrajectoryPoint which concretely defines the trajectory

  • valid (bool (optinal)) – Defines if trajectory is avalid trajectory or not

Returns

An instance of JointTrajectory

Return type

JointTrajectory

Raises
  • TypeError – If joint_set is not of type JointSet or if points is not of type iterable of JointTrajectoryPoints or None

  • ValueError – If time from start is no ascending between points or joint set of points is not the same as of this JointTrajectory

accelerations

List[JointValues] List of JointValues where each item is the accelerations field of JointTrajectoryPoints

Type

accelerations

append(other, delay=datetime.timedelta(0))[source]

Append a JointTrajectory to current trajectory

Parameters
  • other (JointTrajectory) – joint trajectory to append

  • delay (timedelta) – time between last point of current trajectory and first point of other trajectory

Returns

Return type

JointTrajectory

duration

timedelta duration of trajectory

Type

duration

efforts

List[JointValues] List of JointValues where each item is the efforts field of JointTrajectoryPoints

Type

efforts

static empty()[source]

Creates a empty JointTrajectory instance

evaluate_at(time)[source]

Evaluates the trajectory at a given time

Parameters

time (timedetla) – The time at which the trajectory should be evaluated

get_point_before(time, return_index=False)[source]

Find and return JointTrajectoryPoint nearest and before defined time

Parameters
  • time (timedelta) – defines time for which the nearest and before point is required

  • return_index (bool (default False)) – If True return index instead of JointTrajectoryPoint

Returns

If return_index True returns index instead of JointTrajectoryPoint

Return type

JointTrajectoryPoint or int

has_acceleration

bool (readonly) True if trajectory has accelerations for every point

Type

is_acceleration

has_effort

bool (readonly) True if trajectory has efforts for every point

Type

has_effort

has_velocity

bool (readonly) True if trajectory has velocities for every point

Type

has_velocity

is_valid

bool (readonly) True if trajectory is a valid trajectory

Type

is_valid

joint_set

JointSet (readonly) Set of joints for which the trajectory contains values

Type

joint_set

merge(other, delay_self=None, delay_other=None)[source]

Merge self and other joint trajectory

Parameters
  • other (JointTrajectory) – Other trajectory to merge

  • delay_self (Union[timedelta,None] (default=None)) – If defined delay which is applied to self trajectory

  • delay_other (Union[timedelta,None] (default=None)) – If defined delay which is applied to other trajectory

Returns

Merged joint trajectory

Return type

JointTrajectory

points

List[JointTrajectoryPoints] (readonly) List of JointTrajectoryPoints which define the trajectory

Type

points

positions

List[JointValues] List of JointValues where each item is the positions field of JointTrajectoryPoints

Type

positions

prepend(other, delay=datetime.timedelta(0))[source]

Prepend a JointTrajectory to current trajectory

Parameters
  • other (JointTrajectory) – joint trajectory to prepend

  • delay (timedelta) – time between last point of other trajectory and first point of current trajectory

Returns

Return type

JointTrajectory

time_from_start

List[datatime.timedelta] List of timedeltas where each item is the time_from_start field of JointTrajectoryPoints

Type

timedelta

to_joint_trajectory_msg(seq=0, frame_id='')[source]

Converts JointTrajectory to JointTrajectory ros message

trajectory_msgs/JointTrajectory.msg

Parameters
  • seq (int(default 0)) – set seq field of message header

  • frame_id (int(default 0)) – set frame_id field of message header

Returns

Instance of JointTrajectory ros message

Return type

JointTrajectory ros message

transform(transform_function)[source]

Apply transformation to trajectory points

Parameters

transform_function (Callable[[JointTrajectoryPoint], JointTrajectoryPoint]) – Function which defines the transformation which is applied on each joint trajectory point

Returns

Joint Trajectory with transformed points

Return type

JointTrajectory

velocities

List[JointValues] List of JointValues where each item is the velocities field of JointTrajectoryPoints

Type

velocities

PlanParameters

How to use it:

Details:

class xamla_motion.data_types.PlanParameters(move_group_name, joint_limits, **kwargs)[source]

PlanParameter holds all constrains for joint space trajectory planning

from_arguments(move_group_name, joint_set, max_velocity,

max_acceleration, min_position, max_position, **kwargs)

Creates instance of PlanParameters from limit numpy arrays

Initialization of PlanParameters class

Parameters
  • move_group_name (str convertable) – name of the move group for which the plan parameters are created

  • joint_limits (JointLimits) – Defines the joint limits of the move group

  • argv (dict) – The argv dict is used to set parameters which have default values this are sample_resolution (default = 0.008 / 125 hz), collision_check (default = True), max_deviation (default = 0.2)

Returns

Return type

Instance of PlanParameters

:raises TypeError : type mismatch: If joint_limits is not of expected type JointLimts

Examples

>>> joints = ['Joint1','Joint2']
>>> joint_set = JointSet(joints)
>>> max_v = np.array([1.0, 1.5])
>>> max_a = np.array([0.2, 0.4])
>>> min_p = np.array([10, None])
>>> max_p = np.array([None, None])
>>> joint_limits = JointLimits(joint_set, max_v, max_a, min_p, max_p)
>>> p = PlanParameters('left_arm', joint_Limits, scale_velocity=0.5)
collision_check

bool defines if collision check should be performed

Type

collision_check

classmethod from_arguments(move_group_name, joint_set, max_velocity, max_acceleration, min_position, max_position, **kwargs)[source]

Initialization of PlanParameters class limit arrays

Parameters
  • move_group_name (str convertable) – name of the move group for which the plan parameters are created

  • joint_set (JointSet) – Set of joints for which joint limits are required

  • max_velocity (list of float or numpy.array(dtype=floating)) – One dimension array which defines the maximal velocity for each joint

  • max_acceleration (list of float or numpy.array(dtype=floating)) – One dimension array which defines the maximal acceleration for each joint

  • min_position (list of float or numpy.array(dtype=floating)) – One dimension array which defines the mininmal position for each joint

  • max_position (list of float or numpy.array(dtype=floating)) – One dimension array which defines the maximal position for each joint

  • kwargs (dict) – The argv dict is used to set parameters which have default values this are sample_resolution (default = 0.008), collision_check (default = True), max_deviation (default = 0.2)

Returns

Return type

Instance of PlanParameters

classmethod from_builder(builder)[source]

Initilization of PlanParameters from PlanParameters Builder

Parameters

builder – Instance of PlanParameter Builder

Returns

Return type

Instance of PlanParameters

Raises

TypeError – If builder is not of expected type PlanParameters Builder

joint_limits

JointLimits(read only) define the joint constraints

Type

joint_limits

joint_set

JointSet (read only) set of joints for which the limits are defined

Type

joint_set

max_acceleration

numpy.array(dtype=floating) (read only) maximal acceleration

Type

max_acceleration

max_deviation

float defines the maximal deviation from trajectory points when it is a fly-by-point in joint space

Type

max_deviation

max_position

numpy.array(dtype=floating) (read only) position limit maximum

Type

max_position

max_velocity

numpy.array(dtype=floating) (read only) maximal velocities

Type

max_velocity

min_position

numpy.array(dtype=floating) (read only) position limit minimum

Type

min_position

move_group_name

str (read only) Name of the move group for which plan parameters are applied

Type

move_group_name

sample_resolution

float sampling resolution in Hz

Type

sample_resolution

to_builder()[source]

Create an instance of PlanParameter Builder

Returns

Instance of PlanParameter Builder

Return type

PlanParameter Builder

with_collision_check(value)[source]

Create an instance of PlanParameters with collision check value

Parameters

value (bool) – New value of collision check

Returns

Instance of PlanParameters with new collision check value

Return type

PlanParameters

Raises

TypeError – If value is not of type bool

with_max_deviation(value)[source]

Create an instance of PlanParameters with max deviation value

Parameters

value (bool) – New value of max deviation

Returns

Instance of PlanParameters with new max deviation value

Return type

PlanParameters

Raises

TypeError – If value is not convertable to float

with_sample_resolution(value)[source]

Create an instance of PlanParameters with sample resolution value

Parameters

value (bool) – New value of sample resolution

Returns

Instance of PlanParameters with new sample resolution value

Return type

PlanParameters

Raises

TypeError – If value is not convertable to float

Task space data types

Pose

How to use it:

Details:

class xamla_motion.data_types.Pose(translation, rotation, frame_id='world', normalize_rotation=False)[source]

Pose defined by three dimensional translation and rotation

The translation is representated by a three dimensional row vector. The rotation is represented by a quaternion. The pose itself is defined in coordinate system which is defined in frame_id (default world)

from_transformation_matrix(matrix, frame_id='', normalize_rotation=False)[source]

Creates an instance of Pose from a transformation matrix

from_posestamped_msg(msg)[source]

Initialize Pose from ROS posestamped message

from_pose_msg(msg, frame_id='')[source]

Initialize Pose from ROS pose message

normalize_rotation()[source]

Creates an instance of Pose with normalized quaternion

is_rotation_normalized()[source]

Return True if quaternion is normalized

rotation_matrix()[source]

Returns the rotation matrix(3x3 numpy array)

transformation_matrix()[source]

Returns the transformation matrix in homogenous coordinates (4x4 numpy array)

inverse(new_frame_id)[source]

Creates an instance which contains the inverse of this pose

translate(translation)[source]

Translate pose

rotate(rotation)[source]

Rotate pose

to_posestamped_msg()[source]

Creates an instance of the ROS message PoseStamped from Pose

Initialization of the pose class

For the internal quaternion representation and also to initialize the the class the library pyquaternion is used : https://kieranwynn.github.io/pyquaternion/

Parameters
  • translation (convertable to numpy array of shape (3,)) – translation or position the pose describes

  • rotation (pyquaternion.Quaternion) – rotation the pose describes as Quaternion

  • frame_id (str (optinal defaul = 'world')) – name of the coordinate system the pose is defined

  • normalize_rotation (bool (optinal default = False)) – If true quaternion normalization is performed in the initialization process

Returns

pose – An instance of class Pose

Return type

Pose

:raises TypeError : type mismatch: If tranlation vector could not converted to a numpy array of shape (3,) with d type floating and the quaternion is not of type Quaternion. If frame_id is not of type str If normalize_rotation is not of type bool :raises ValueError: If the quaternion initalization from translation matrix went wrong

Examples

Create a Pose by translation and quaternion and perform normalization >>> quaternion = Quaternion(matrix=np.eye(3)) >>> translation = np.array([1.0,1.0,1.0]) >>> p1 = Pose(translation, quaternion, _, True)

frame_id

str (readonly) Id of the coordinate system / frame

Type

frame_id

classmethod from_pose_msg(msg, frame_id='world')[source]

Initialize Pose from ros geometry_msgs/Pose

Parameters

msg (Pose from geometry_msgs) – pose message

Returns

pose – Instance of Pose from pose message

Return type

Pose

Raises

TypeError – If msg is not of type Pose

classmethod from_posestamped_msg(msg)[source]

Initialize Pose from ROS posestamped message

Parameters

msg (PoseStamped from ROS geometry_msgs) – posestamped message

Returns

pose – Instance of Pose generated from PoseStamped message

Return type

Pose

Raises

TypeError – If msg is not of type PoseStamped

classmethod from_transformation_matrix(matrix, frame_id='world', normalize_rotation=False)[source]

Initialization of the pose class from transformation matrix

Parameters
  • matrix (numpy.ndarray((4,4),np.dtype=floating)) – A transformation matrix in homogenous coordinates

  • frame_id (str (defaul = 'world')) – name of the coordinate system the pose is defined

  • normalize_rotation (bool (optional default = False)) – If true quaternion normalization is performed in the initialization process

Returns

pose – An instance of class Pose

Return type

Pose

:raises TypeError : type mismatch: If tranformation matrix is not a 4x4 numpy array with dtype floating

Examples

Create a Pose instance from transformation matrix and set frame_id >>> transformation_matrix = np.eye(4) >>> p0 = Pose.from_transformation_matrix(transformation_matrix, “global”)

classmethod identity(frame_id='world')[source]

Creates a instance of Pose from idenity matrix

Parameters

frame_id (str (optional defaul = 'world')) – name of the coordinate system the pose is defined

Returns

pose – An instance of class Pose

Return type

Pose

inverse(new_frame_id)[source]

Creates an instance which contains the inverse of this pose

Parameters

new_frame_id (str convertable) – name of the coordinate system in which pose is now defined

Returns

inv_pose – pose which is the inverse of self

Return type

Pose

:raises TypeError : type mismatch: If new_frame_id is not of type str

is_rotation_normalized()[source]

Returns True is rotation is normalized

Returns

result – True is rotation is normalized else False

Return type

bool

normalize_rotation()[source]

Creates an instance of Pose with normalized quaternion

Returns

pose – Instance of Pose with normalized quaternion / rotation

Return type

Pose

quaternion

Quaternion(pyquaternion lib) (readonly) Quaternion which describes the rotation

Type

quaternion

rotate(rotation)[source]

Rotate pose

Parameters

rotation ((Quaternion or np.ndarray)) – defines rotation by Quaternion or 3x3 rotation matrix

Returns

rotated_pose – pose which is rotated by rotation

Return type

Pose

:raises TypeError : type mismatch: If rotation is not one of expected types 3x3 np.ndarray or pyquaternion.Quaternion :raises ValueError: If initialization of Quaternion is not possible

rotation_matrix()[source]

Returns the rotation matrix (3x3 numpy array)

Returns

rotation_matrix – A 3x3 numpy array with dtype float which represents the rotation matrix

Return type

np.ndarray

to_pose_msg()[source]

Creates an instance of the ROS message Pose

Returns

  • Instance of ROS message Pose

  • geometry_msgs/Pose

to_posestamped_msg()[source]

Creates an instance of the ROS message PoseStamped

Returns

  • Instance of ROS message PoseStamped (seq and time are not set)

  • docs.ros.org/kinetic/api/geometry_msgs/html/msg/PoseStamped.html

transformation_matrix()[source]

Return the transformation martix in homogenous coordinates (4x4 numpy array)

Returns

transformation_matrix – A 4x4 numpy array with dtype float which represents the transformation matrix in homogenous coordinates

Return type

np.ndarray

translate(translation)[source]

Translate pose

Parameters

translation (Iterable) – translation which defines the change in x,y,z

Returns

translated_pose – pose which is translated by translate vector

Return type

Pose

:raises TypeError : type mismatch: If translation is not convertable to np.ndarray :raises ValueError: If translation is not of size 3 (x,y,z)

translation

numpy.array((3,) dtype=floating) (readonly) numpy row array of size 3 which describes the translation

Type

translation

EndEffectorLimits

How to use it:

Details:

class xamla_motion.data_types.EndEffectorLimits(max_xyz_velocity, max_xyz_acceleration, max_angular_velocity, max_angular_acceleration)[source]

Class which holds hold task space / endeffector limits

Initialization of class EndeffectorLimits

Parameters
  • max_xyz_velocity (float convertable or None) – Defines the maximal xyz velocity [m/s]

  • max_xyz_acceleration (float convertable) – Defines the maximal xyz acceleration [m/s^2]

  • max_angular_velocity (float convertable or None) – Defines the maximal angular velocity [rad/s]

  • max_angular_acceleration (float convertable or None) – Defines the maximal angular acceleration [rad/s^2]

Returns

Instance of class EndeffectorLimits

Return type

EndEffectorLimits

:raises TypeError : type mismatch: If one of the parameters is not convertable to float

max_angular_acceleration

float or None(read only) Max angular acceleration[rad/(s ^ 2)]

Type

max_angular_acceleration

max_angular_velocity

float or None(read only) Max angular velocity[rad/s]

Type

max_angular_velocity

max_xyz_acceleration

float or None(read only) Max xyz acceleration [m/(s^2)]

Type

max_xyz_acceleration

max_xyz_velocity

float or None(read only) Maximal xyz velocity [m/s]

Type

max_xyz_velocity

CartesianPath

How to use it:

Details:

class xamla_motion.data_types.CartesianPath(points)[source]

CartesianPath class describes a path by a list of joint configurations

from_one_point(point)[source]

Initialization of CartesianPath class with only one Point

from_start_stop_point(start, stop)[source]

Initialization of CartesianPath class with start, stop point

prepend(points)[source]

Creates new CartesianPath with points added in front of path points

append(points)[source]

Creates new CartesianPath with points added behind the path points

concat(other)[source]

Creates new CartesianPath with concatenated path points

transform(transform_function)[source]

Creates a transformed version of CartesianPath

Initialization of CartesianPath class

Parameters

points (Iterable[Pose]) – List of Pose where each Pose instance describes a point of the path

Returns

Instance of CartesianPath

Return type

CartesianPath

:raises TypeError : type mismatch: If points is not of type iterable of Pose

append(points)[source]

Creates new CartesianPath with points added behind the path points

Parameters

points (Pose or Iterable[Pose]) – Points which are added behind path points

Returns

Instance of CartesianPath with added points

Return type

CartesianPath

Raises

TypeError – If points is not one of expected types Pose or Iterable of Pose

concat(other)[source]

Creates new CartesianPath with concatenated path points

Parameters

other (CartesianPath) –

Raises

TypeError – If other is not of type CartesianPath

classmethod from_cartesian_path_msg(msg)[source]

Create instance from cartesian path message

Parameters

msg (xamlamoveit_msgs.msg CartesianPath.msg) – ROS cartesian path message

Returns

New instance of CartesianPath

Return type

CartesianPath

classmethod from_one_point(point)[source]

Initialization of CartesianPath class with only one Point

Parameters

point (Pose) – Single point to initialize CartesianPath

Returns

Instance of CartesianPath

Return type

CartesianPath

:raises TypeError : type mismatch: If points is not of type Pose

classmethod from_start_stop_point(start, stop)[source]

Initialization of CartesianPath class with start, stop point

Parameters
  • start (Pose) – start point of the joint path

  • stop (Pose) – stop point of the joint path

Returns

Instance of CartesianPath

Return type

CartesianPath

:raises TypeError : type mismatch: If start or stop is not of type Pose

orientations

List[pyquaternion.Quaternion] orientations as Quaternion from pyquaternion

Type

orientations

points

Deque[Pose] (readonly) A deque of Pose which represent the cartesian poses which are describe the path

Type

points

positions

List[np.array((3,),dtype=floating) positions in x,y,z coordinates

Type

positions

prepend(points)[source]

Creates new CartesianPath with points added in front of path points

Parameters

points (Pose or Iterable[Pose]) – Points which are added in front of path points

Returns

Instance of CartesianPath with added points

Return type

CartesianPath

Raises

TypeError – If points is not one of expected types Pose or Iterable of Pose

to_cartesian_path_msg()[source]

Generates a xamlamoveit_msgs.msg CartesianPath.msg from this CartesianPath instance

transform(transform_function)[source]

Creates a transformed version of CartesianPath

The transformation which is applied to every point in CartesianPath is defined by the transform function

Parameters

transform_function (callable) – Function which is applied to every pose

Returns

A new Instance of CartesianPath with transformed poses

Return type

CartesianPath

:raises TypeError : type mismatch: If transform function is not callable and if the function dont has the signature input : Pose , output : Pose

TaskSpacePlanParameters

How to use it:

Details:

class xamla_motion.data_types.TaskSpacePlanParameters(end_effector_name, end_effector_limits, **kwargs)[source]

TaskSpace holds all constrains for task space trajectory planning

from_arguments(end_effector_name, max_xyz_velocity,

max_xyz_acceleration, max_angular_velocity, max_angular_acceleration, **kwargs)

Creates a instance or TaskSpacePlanParameters from single limit arguments

Initialization of TaskSpacePlanParameters class

Parameters
  • end_effector_name (str convertable) – Name of the end effector

  • end_effector_limits (EndEffectorLimits) – Limits of the end effector in task space

  • kwargs (dict) – The argv dict is used to set parameters which have default values this are sample_resolution (default = 0.008 / 125 Hz), collision_check (default = True), max_deviation (default = 0.2), ik_jump_threshold (default = 1.2)

Returns

Return type

Instance of TaskSpacePlanParameters

:raises TypeError : type mismatch: If end_effector_limits is not of expected type EndEffectorLimits

Examples

>>> end_effector_limits = EndeffectorLimits(1.0, 1.0, 1.0, 1.0)
>>> p = TaskSpacePlanParameters('tool1', end_effector_limits, max_deviation=0.1)
collision_check

bool defines if collision check should be performed

Type

collision_check

end_effector_limits

EndeffectorLimits(read only) define the task space constraints

Type

end_effector_limits

end_effector_name

str (read only) Name of the move group for which plan parameters are applied

Type

end_effector_name

classmethod from_arguments(end_effector_name, max_xyz_velocity, max_xyz_acceleration, max_angular_velocity, max_angular_acceleration, **kwargs)[source]

Initialization of TaskSpacePlanParameters by single limits

Parameters
  • end_effector_name (str) – Name of the end effector

  • max_xyz_velocity (float convertable) – Defines the maximal xyz velocity [m/s]

  • max_xyz_acceleration (float convertable (read only)) – Defines the maximal xyz acceleration [m/s^2]

  • max_angular_velocity (float convertable (read only)) – Defines the maximal angular velocity [rad/s]

  • max_angular_acceleration (float convertable (read only)) – Defines the maximal angular acceleration [rad/s^2]

  • kwargs (dict) – The argv dict is used to set parameters which have default values this are sample_resolution (default = 0.008), collision_check (default = True), max_deviation (default = 0.2), ik_jump_threshold (default = 1.2)

Returns

Instance of TaskSpacePlanParameters

Return type

TaskSpacePlanParameters

Raises

ArgumentError – If instance of EndeffectorLimits could not be created due to wrong type or format of the arguments

Examples

>>> p = TaskSpacePlanParameters('tool1', 1.0, 1.0, 1.0, 1.0, max_deviation=0.1)
classmethod from_builder(builder)[source]

Initilization of TaskSpacePlanParameters from TaskSpacePlanParameters Builder

Parameters

builder – Instance of TaskSpacePlanParameter Builder

Returns

Return type

Instance of TaskSpacePlanParameters

Raises

TypeError – If builder is not of expected type TaskSpacePlanParameters Builder

ik_jump_threshold

float defines the inverse kinematic jump threshold

Type

ik_jump_threshold

max_angular_acceleration

float(read only) max angular acceleration

Type

max_angular_acceleration

max_angular_velocity

float(read only) max angular velocity

Type

max_angular_velocity

max_deviation

float defines the maximal deviation from trajectory points when fly-by-points in joint space

Type

max_deviation

max_xyz_acceleration

float(read only) max translational acceleration

Type

max_xyz_acceleration

max_xyz_velocity

float(read only) max translational velocity

Type

max_xyz_velocity

sample_resolution

float sampling resolution in Hz

Type

sample_resolution

to_builder()[source]

Create an instance of TaskSpacePlanParameters Builder

Returns

Instance of TaskSpacePlanParameters Builder

Return type

TaskSpacePlanParameters Builder

with_collision_check(value)[source]

Create an instance of TaskSpacePlanParameters with collision check value

Parameters

value (bool) – New value of collision check

Returns

Instance of TaskSpacePlanParameters with new collision check value

Return type

TaskSpacePlanParameters

Raises

TypeError – If value is not of type bool

with_ik_jump_threshold(value)[source]

Create an instance of TaskSpacePlanParameters with ik jump threshold value

Parameters

value (bool) – New value of ik jump threshold

Returns

Instance of TaskSpacePlanParameters with new ik jump threshold value

Return type

TaskSpacePlanParameters

Raises

TypeError – If value is not convertable to float

with_max_deviation(value)[source]

Create an instance of TaskSpacePlanParameters with max deviation value

Parameters

value (bool) – New value of max deviation

Returns

Instance of TaskSpacePlanParameters with new max deviation value

Return type

TaskSpacePlanParameters

Raises

TypeError – If value is not convertable to float

with_sample_resolution(value)[source]

Create an instance of TaskSpacePlanParameters with sample resolution value

Parameters

value (bool) – New value of sample resolution

Returns

Instance of TaskSpacePlanParameters with new sample resolution value

Return type

TaskSpacePlanParameters

Raises

TypeError – If value is not convertable to float

State and result data types

MoveGripperResult

How to use it:

Details:

class xamla_motion.data_types.MoveGripperResult(position, reached_goal, stalled, effort)[source]

Class which represents the result of a gripper action

effort

float The current effort exerted (in Newtons)

Type

effort

position

float The current gripper gap size (in meters)

Type

position

reached_goal

bool True iff the gripper position has reached the commanded setpoint

Type

reached_goal

stalled

bool True iff the gripper is exerting max effort and not moving

Type

stalled

WsgResult

How to use it:

Details:

class xamla_motion.data_types.WsgResult(state, width, force, status)[source]

Class which respresents the result of a WSG gripper action

force

float The current effort exerted (in Newtons)

Type

force

width

float The current gripper gap size (in meters)

Type

width

IkResults

How to use it:

Details:

class xamla_motion.data_types.IkResults(path, error_codes)[source]

Class with hold result of a inverse kinematics query

error_codes

List[ErrorCodes] error codes

Type

error_code

path

JointPath solutions from the inverse kinematics

Type

path

SteppedMotionState

How to use it:

Details:

class xamla_motion.data_types.SteppedMotionState(goal_id, error_message, error_code, progress)[source]

Class which carries a state snapshot of a stepped motion operation

Initialize class SteppedMotionState

Parameters
  • goal_id (str) – Goal id of the ROS action which is used to perform the stepped motion operation

  • error_message (str) – String representation of the error

  • error_code (int) – Numerical representation if the error

  • progress (float) – Progess of the stepped motion operation in percent range [0.0-1.0]

Returns

An instance of SteppedMotionState

Return type

SteppedMotionState

error_code

xamla_motion.data_types.ErroCodes Numerical representation if the error

Type

error_code

error_message

str String representation of the error

Type

error_message

goal_id

str Goal id of the ROS action which is used to perform the stepped motion operation

Type

goal_id

progress

float Progess of the stepped motion operation in percent range [0.0-1.0]

Type

progress

JointValuesCollisions

How to use it:

Details:

class xamla_motion.data_types.JointValuesCollisions(index, error_code, error_message)[source]

Class which represents a robot collision

error_code

int (readonly) STATE_VALID = 1 STATE_SELF_COLLISION = -1 STATE_SCENE_COLLISION =-2 INVALID_JOINTS = -11 INVALID_MOVE_GROUP = -12

Type

error_code

error_message

str error message of state

Type

error_message

index

int (readonly) value index where the collision occurs

Type

index