Code Documentation
- class trip_kinematics.Utility.Rotation(quat)[source]
Represents a 3D rotation.
Can be initialized from quaternions, rotation matrices, or Euler angles, and can be represented as quaternions. Reimplements a (small) part of the scipy.spatial.transform.Rotation API and is meant to be used for converting between rotation representations to avoid depending on SciPy. This class is not meant to be instantiated directly using __init__; use the methods from_[representation] instead.
- as_quat(scalar_first=True)[source]
Represents the object as a quaternion.
- Parameters
scalar_first (bool, optional) – Represent the quaternion in scalar-first (w, x, y, z) or scalar-last (x, y, z, w) format. Defaults to True.
- Returns
Quaternion.
- Return type
np.array
- classmethod from_euler(seq, rpy, degrees=False)[source]
Creates a :py:class`Rotation` object from Euler angles.
- Parameters
seq (str) – Included for compatibility with the SciPy API. Required to be set to ‘xyz’.
rpy (np.array) – Euler angles.
degrees (bool, optional) – True for degrees and False for radians. Defaults to False.
- Returns
Rotation object.
- Return type
- classmethod from_matrix(matrix)[source]
Creates a :py:class`Rotation` object from a rotation matrix.
Uses a very similar algorithm to scipy.spatial.transform.Rotation.from_matrix(). See https://github.com/scipy/scipy/blob/22f66bbd83867459f1491abf01b860b5ef4e026e/ scipy/spatial/transform/_rotation.pyx
- Parameters
matrix (np.array) – Rotation matrix.
- Returns
Rotation object.
- Return type
- classmethod from_quat(xyzw, scalar_first=True)[source]
Creates a :py:class`Rotation` object from a quaternion.
- Parameters
xyzw (np.array) – Quaternion.
scalar_first (bool, optional) – Whether the quaternion is in scalar-first (w, x, y, z) or scalar-last (x, y, z, w) format. Defaults to True.
- Returns
Rotation object.
- Return type
- trip_kinematics.Utility.get_rotation(matrix)[source]
Returns the 3x3 rotation matrix of the :py:class`TransformationMatrix`
- Returns
The 3x3 rotation matrix
- Return type
numpy.array
- trip_kinematics.Utility.get_translation(matrix)[source]
Returns the translation of the :py:class`TransformationMatrix`
- Returns
The 3 dimensional translation
- Return type
numpy array
- trip_kinematics.Utility.hom_rotation(rotation_matrix)[source]
Converts a 3x3 rotation matrix into a 4x4 homogenous rotation mtrix
- Parameters
rotation_matrix (numpy.array) – A 3x3 rotation matrix
- Returns
A 4x4 homogenous rotation matrix
- Return type
numpy.array
- trip_kinematics.Utility.hom_translation_matrix(t_x=0, t_y=0, t_z=0)[source]
Returns a homogenous translation matrix
- Parameters
t_x (int, optional) – Translation along the x axis. Defaults to 0.
t_y (int, optional) – Translation along the y axis. Defaults to 0.
t_z (int, optional) – Translation along the z axis. Defaults to 0.
- Returns
A 4x4 homogenous translation matrix
- Return type
numpy.array
- trip_kinematics.Utility.identity_transformation()[source]
Returns a 4x4 identity matix
- Returns
a 4x4 identity matrix
- Return type
numpy.array
- trip_kinematics.Utility.quat_rotation_matrix(q_w, q_x, q_y, q_z) <Mock name='mock.array' id='139641357108560'>[source]
Generates a 3x3 rotation matrix from q quaternion
- Parameters
q_w (float) – part of a quaternion [q_w,q_x,q_y,q_z]
q_x (float) – part of a quaternion [q_w,q_x,q_y,q_z]
q_y (float) – part of a quaternion [q_w,q_x,q_y,q_z]
q_z (float) – part of a quaternion [q_w,q_x,q_y,q_z]
- Returns
A 3x3 rotation matrix
- Return type
numpy.array
- trip_kinematics.Utility.x_axis_rotation_matrix(theta)[source]
Generates a matrix rotating around the x axis
- Parameters
theta (float) – The angle of rotation in rad
- Returns
A 3x3 rotation matrix
- Return type
numpy.array
- trip_kinematics.Utility.y_axis_rotation_matrix(theta)[source]
Generates a matrix rotating around the y axis
- Parameters
theta (float) – The angle of rotation in rad
- Returns
A 3x3 rotation matrix
- Return type
numpy.array
- trip_kinematics.Utility.z_axis_rotation_matrix(theta)[source]
Generates a matrix rotating around the z axis
- Parameters
theta (float) – The angle of rotation in rad
- Returns
A 3x3 rotation matrix
- Return type
numpy.array
- class trip_kinematics.Transformation.Transformation(name: str, values: Dict[str, float], state_variables: Optional[List[str]] = None, parent=None)[source]
Initializes the
Transformationclass.- Parameters
name (str) – The unique name identifying the Transformation. No two
Transformationobjects of a :py:class`Robot` should have the same namevalues (Dict[str, float]) – A parametric description of the transformation.
state_variables (List[str], optional) – This list describes which state variables are dynamically changable. This is the case if the
Transformationrepresents a joint. Defaults to [].
- Raises
ValueError – A dynamic state was declared that does not correspond to a parameter declared in
values.
- add_children(child: str)[source]
- Adds the name of a
KinematicGrouporTransformation as a child.
- Parameters
child (str) – the name of a
KinematicGrouporTransformation
- Adds the name of a
- static get_convention(state: Dict[str, float])[source]
- Returns the connvention which describes how the matrix
of a
Transformationis build from its state.
- Parameters
state (Dict[str, float]) – :py:attr:’state’
- Raises
ValueError – “Invalid key.” If the dictionary kontains keys that dont correspond to a parameter of the transformation.
ValueError – “State can’t have euler angles and quaternions!” If the dictionary contains keys correspondig to multiple mutually exclusive conventions.
- Returns
A string describing the convention
- Return type
[type]
- get_name()[source]
Returns the
_nameof theTransformation- Returns
a copy the
_nameattribute- Return type
str
- get_state()[source]
- Returns a copy of the
_state attribute of the
Transformationobject.
- Returns
a copy of the
_state- Return type
Dict[str,float]
- Returns a copy of the
- get_transformation_matrix()[source]
- Returns a homogeneous transformation matrix build
from the
stateandconstants
- Raises
RuntimeError – If the convention used in
stateis not supported. Should normally be catched during initialization.- Returns
- A transformation matrix build using the parameters
of the
Transformationstate
- Return type
[type]
- set_state(state: Dict[str, float])[source]
Sets the state of the
Transformationobject.- Parameters
state (Dict[str, float]) – Dictionary with states that should be set. Does not have to be the full state.
- Raises
KeyError – If a key in the argument is not valid state parameter name.
- trip_kinematics.Transformation.array_find(arr, obj) int[source]
- A helpher function which finds the index of an object in an array.
Instead of throwing an error when no index can be found it returns -1.
- Parameters
arr – the array to be searched
obj – the object whose index is to be found.
- Returns
The index of obj in the array. -1 if the object is not in the array
- Return type
int
- class trip_kinematics.KinematicGroup.KinematicGroup(name: str, virtual_chain: List[trip_kinematics.Transformation.Transformation], actuated_state: Dict[str, float], actuated_to_virtual: Callable, virtual_to_actuated: Callable, act_to_virt_args=None, virt_to_act_args=None, parent=None)[source]
Initializes a
KinematicGroupobject.- Parameters
name (str) – The unique name identifying the group. No two
KinematicGroupobjects of a :py:class`Robot` should have the same namevirtual_chain (List[Transformation]) – A list of
Transformationobjects forming a serial Kinematic chain.actuated_state (List[Dict[str, float]]) – The State of the Groups actuated joints.
actuated_to_virtual (Callable) – Maps the
actuated_stateto thevirtual_stateof thevirtual_chain.virtual_to_actuated (Callable) – Maps the
virtual_stateof thevirtual_chainto theactuated_state.act_to_virt_args ([type], optional) – Arguments that can be passed to
actuated_to_virtualduring the initial testing of the function. Defaults to None.virt_to_act_args ([type], optional) – Arguments that can be passed to
virtual_to_actuatedduring the initial testing of the function. Defaults to None.parent (Union(Transformation,KinematicGroup), optional) – The transformation or group preceding the
KinematicGroup. Defaults to None.
- Raises
ValueError – ‘Error: Actuated state is missing. You provided a mapping to actuate the group but no state to be actuated.’ if there is no
actuated_statedespite a mapping being passedValueError – ‘Error: Only one mapping provided. You need mappings for both ways. Consider to pass a trivial mapping.’ if either
actuated_to_virtualorvirtual_to_actuatedwas not set despite providing aactuated_state.ValueError – ‘Error: Mappings missing. You provided an actuated state but no mappings. If you want a trivial mapping you don’t need to pass an actuated state. Trip will generate one for you.’ if both
actuated_to_virtualandvirtual_to_actuatedwere not set despite providing aactuated_state.RuntimeError – “actuated_to_virtual does not fit virtual state” if the
actuated_to_virtualfunction does not return a validvirtual_statedictionary.RuntimeError – “virtual_to_actuated does not fit actuated state” if the
virtual_to_actuatedfunction does not return a validactuated_statedictionary.
- add_children(child: str)[source]
- Adds the name of a
KinematicGrouporTransformation as a child.
- Parameters
child (str) – the name of a
KinematicGrouporTransformation
- Adds the name of a
- get_actuated_state()[source]
- Returns a copy of the
actuated_state attribute of the
KinematicGroupobject.
- Returns
a copy of the
actuated_state- Return type
Dict[str,float]
- Returns a copy of the
- get_name()[source]
Returns the
_nameof theKinematicGroup- Returns
the
_nameattribute- Return type
str
- get_transformation_matrix()[source]
- Calculates the full transformationmatrix from
the start of the virtual chain to its endeffector.
- Returns
- The homogenous transformation matrix from
the start of the virtual chain to its endeffector.
- Return type
array
- get_virtual_chain()[source]
- Returns a copy of the
_virtual_chain attribute of a
KinematicGroupobject.
- Returns
a copy of the
_virtual_chain- Return type
Dict[str,Transformation]
- Returns a copy of the
- get_virtual_state()[source]
- Returns a copy of the
virtual_state attribute of the
KinematicGroupobject.
- Returns
a copy of the
virtual_state- Return type
Dict[str,Dict[str,float]]
- Returns a copy of the
- static object_list_to_key_lists(object_lst)[source]
Helper function which transforms dictionary into list of keys.
- Parameters
object_lst (Dict) – The dictionary to be transformed
- Returns
A list of keys
- Return type
list(str)
- pass_arg_a_to_v(argv)[source]
Allows arguments to be passed the
actuated_to_virtualmapping.- Parameters
argv ([type]) – arguments to be passed.
- pass_arg_v_to_a(argv)[source]
Allows arguments to be passed the
virtual_to_actuatedmapping.- Parameters
argv ([type]) – arguments to be passed.
- set_actuated_state(state: Dict[str, float])[source]
- Sets the
actuated_stateof the Group and automatically updates the corresponding
virtual_state.
- Parameters
state (Dict[str, float]) – A dictionary containing the members of
actuated_statethat should be set.- Raises
RuntimeError – if all
Transformationobjects of_virtual_chainare static.ValueError – if the state to set is not part of keys of
actuated_state
- Sets the
- set_virtual_state(state: Dict[str, Dict[str, float]])[source]
- Sets the
virtual_stateof the Group and automatically updates the corresponding
actuated_state.
- Parameters
state (Dict[str,Dict[str, float]]) – A dictionary containing the members of
virtual_statethat should be set. The new values need to be valid state for the state of the joint.- Raises
RuntimeError – if all
Transformationobjects of_virtual_chainare static.ValueError – if the state to set is not part of keys of
virtual_state
- Sets the
- class trip_kinematics.KinematicGroup.OpenKinematicGroup(name: str, virtual_chain: List[trip_kinematics.Transformation.Transformation], parent=None)[source]
- A subclass of the
KinematicGroupthat assumes that all states of the virtual_chain are actuated and automatically generates mappings. Typically only used internally by the :py:class`Robot` class to convert :py:class`Transformation` objects to :py:class`KinematicGroup`s.
- Parameters
name (str) – The unique name identifying the group. No two
KinematicGroupobjects of a :py:class`Robot` should have the same namevirtual_chain (List[Transformation]) – A list of
Transformationobjects forming a serial Kinematic chain.parent (Union(Transformation,KinematicGroup), optional) – The transformation or group preceding the
KinematicGroup. Defaults to None.
- A subclass of the
- class trip_kinematics.Robot.Robot(kinematic_chain: List[trip_kinematics.KinematicGroup.KinematicGroup])[source]
A class representing the kinematic model of a robot.
- Parameters
kinematic_chain (List[KinematicGroup]) – A list of Kinematic Groups and Transformations which make up the robot. Transformations are automatically converted to groups
- Raises
KeyError – “More than one robot actuator has the same name! Please give each actuator a unique name” if there are actuated states with the same names between the :py:class`KinematicGroup` objects of the :py:class`Robot`
KeyError – if there are joints with the same names between the :py:class`KinematicGroup` objects of the :py:class`Robot`
- get_actuated_state()[source]
- Returns the actuated state of the :py:class`Robot` comprised
of the actuated states of the individual :py:class`KinematicGroup`.
- Returns
combined actuated state of all :py:class`KinematicGroup` objects.
- Return type
Dict[str, float]
- get_endeffectors()[source]
- Returns a list of possible endeffectors.
These are the names of all
KinematicGroupobjects. Since Transformations are internally converted to Groups, this includes the names of all Transformations.
- Returns
list of possible endeffectors.
- Return type
list(str)
- get_groups()[source]
- Returns a dictionary of the py:class`KinematicGroup` managed by the :py:class`Robot`_
Since Transformations are internally converted to Groups, this also returns all Transformations.
- Returns
The dictionary of py:class`KinematicGroup` objects.
- Return type
Dict[str, KinematicGroup]
- get_symbolic_rep(endeffector: str)[source]
This Function returnes a symbolic representation of the virtual chain.
- Parameters
endeffector (str) – The name of the group whose virtual chain models the desired endeffector
- Raises
KeyError – If the endeffector argument is not the name of a transformation or group
- Returns
A 4x4 symbolic casadi matrix containing the transformation from base to endeffector
- Return type
SX
- get_virtual_state()[source]
- Returns the virtual state of the :py:class`Robot` comprised
of the virtual states of the individual :py:class`KinematicGroup`.
- Returns
- combined virtual state of all
:py:class`KinematicGroup` objects.
- Return type
Dict[str,Dict[str, float]]
- pass_group_arg_a_to_v(argv_dict)[source]
- Passes optional actuated_to_virtual mapping arguments
to :py:class`KinematicGroup` objects of the robot.
- Parameters
argv_dict (Dict) – A dictionary containing the mapping arguments keyed with the :py:class`KinematicGroup` names.
- Raises
KeyError – If no group with the name given in the argument is part of the robot.
- pass_group_arg_v_to_a(argv_dict: Dict)[source]
- Passes optional virtual_to_actuated mapping arguments
to :py:class`KinematicGroup` objects of the robot.
- Parameters
argv_dict (Dict) – A dictionary containing the mapping arguments keyed with the :py:class`KinematicGroup` names.
- Raises
KeyError – If no group with the name given in the argument is part of the robot.
- set_actuated_state(state: Dict[str, float])[source]
Sets the virtual state of multiple actuated joints of the robot.
- Parameters
state (Dict[str, float]) – A dictionary containing the members of
__actuated_statethat should be set.
- set_virtual_state(state: Dict[str, Dict[str, float]])[source]
Sets the virtual state of multiple virtual joints of the robot.
- Parameters
state (Dict[str,Dict[str, float]]) –
- A dictionary containing the members of
__virtual_statethat should be set.
The new values need to be valid state for the state of the joint.
- trip_kinematics.Robot.forward_kinematics(robot: trip_kinematics.Robot.Robot, endeffector)[source]
Calculates a robots transformation from base to endeffector using its current state
- Parameters
robot (Robot) – The robot for which the forward kinematics should be computed
- Returns
The Transformation from base to endeffector
- Return type
numpy.array
- class trip_kinematics.Solver.CCDSolver(robot: trip_kinematics.Robot.Robot, endeffector: str, orientation=False, update_robot=False, options=None)[source]
- A Cyclical Coordinate Descent based Kinematic Solver Class that calculates
the inverse kinematics for a given endeffector.
- Parameters
robot (Robot) – The Robot for which the kinematics should be calculated
endeffector (str) – the name of the endeffector
orientation (bool, optional) – Boolean flag deciding if inverse kinematics targets also specify orientation. Defaults to False.
update_robot (bool, optional) – Boolean flag decding if the inverse kinematics should immediately update the robot model. Defaults to False.
options (Dict, optional) –
A dictionary containing options for the CCD solver. Possible keys: stepsize: the step length along the gradient max_iterations: the maximum number of iterations
before terminating
- precision: the minimum amount of joint value change
before terminating
- solve_actuated(target: <Mock name='mock.array' id='139641357108560'>, initial_tip=None, mapping_argument=None)[source]
Returns the actuated state needed for the endeffector to be in the target position :param target: The target state of the endeffector.
Either a 3 dimensional position or a 4x4 homogenous transformation
- Parameters
initial_tip (Dict[str,Dict[str, float]], optional) – Initial state of the solver. In this case refers to a virtual state. Defaults to None in which case zeros are used.
mapping_argument ([type], optional) – Optional arguments for the virtual_to_actuated mappings of the robot. Defaults to None.
- Returns
The actuated state leading the endeffector to the target position.
- Return type
Dict(str,float)
- solve_virtual(target: <Mock name='mock.array' id='139641357108560'>, initial_tip=None)[source]
Returns the virtual state needed for the endeffector to be in the target position :param target: The target state of the endeffector.
Either a 3 dimensional position or a 4x4 homogenous transformation
- Parameters
initial_tip ((Dict(str,Dict(str,float))), optional) – Initial state of the solver as a virtual state. Defaults to None in which case zeros are used.
- Returns
- The virtual state leading the endeffector
to the target position.
- Return type
Dict(str,Dict(str,float))
- class trip_kinematics.Solver.SimpleInvKinSolver(robot: trip_kinematics.Robot.Robot, endeffector: str, orientation=False, update_robot=False)[source]
- A Simple Kinematic Solver Class that calculates
the inverse kinematics for a given endeffector.
- Parameters
robot (Robot) – The Robot for which the kinematics should be calculated
endeffector (str) – the name of the endeffector
orientation (bool, optional) – Boolean flag deciding if inverse kinematics targets also specify orientation. Defaults to False.
update_robot (bool, optional) – Boolean flag decding if the inverse kinematics should immediately update the robot model. Defaults to False.
- solve_actuated(target: <Mock name='mock.array' id='139641357108560'>, initial_tip=None, mapping_argument=None)[source]
Returns the actuated state needed for the endeffector to be in the target position :param target: The target state of the endeffector.
Either a 3 dimensional position or a 4x4 homogenous transformation
- Parameters
initial_tip (Dict[str,Dict[str, float]], optional) – Initial state of the solver. In this case refers to a virtual state. Defaults to None in which case zeros are used.
mapping_argument ([type], optional) – Optional arguments for the virtual_to_actuated mappings of the robot. Defaults to None.
- Returns
The actuated state leading the endeffector to the target position.
- Return type
Dict(str,float)
- solve_virtual(target: <Mock name='mock.array' id='139641357108560'>, initial_tip=None)[source]
Returns the virtual state needed for the endeffector to be in the target position :param target: The target state of the endeffector.
Either a 3 dimensional position or a 4x4 homogenous transformation
- Parameters
initial_tip ((Dict(str,Dict(str,float))), optional) – Initial state of the solver as a virtual state. Defaults to None in which case zeros are used.
- Returns
- The virtual state leading the endeffector
to the target position.
- Return type
Dict(str,Dict(str,float))