dmp_lib package

Submodules

dmp_lib.canonical module

class dmp_lib.canonical.Canonical(tau, alpha_phase, alpha_stop=0)[source]

Bases: object

A class used to implement the DMP canonical system. The canonical system drives phase variable ‘s’ from 1 to 0, dictating the time for the other systems composing a DMP.

s

phase variable

Type: float

tau

time scaling parameter

Type: float

alpha_phase

phase decay parameter

Type: float

alpha_stop

phase stop parameter

Type: float

get_phase()[source]

Get current phase variable value.

Returns: phase value
Return type: float

reset()[source]: Reset phase attribute to 1.

step(dt, dist=0)[source]

Single-step canonical system update.

Parameters

dt (float) – time step
dist (float, optional) – distance between commanded and real position (default is 0)

dmp_lib.geometry module

This module contains a set of util functions for handling different rotation representations:

solve_discontinuities - solve discontinuities in axis-angle traj.

rot_to_quat - rotation matrix to quaternion conversion

quat_to_rot - quaternion to rotation matrix conversion

rot_to_axis_angle - rotation matrix to axis-angle conversion

axis_angle_to_rot - axis-angle to rotation matrix conversion

axis_angle_to_quat - axis-angle to quaternion conversion

quat_to_axis_angle - quaternion to axis-angle conversion

skew - get skew matrix form of a 3-dimensional vector

quat_product - quaternion product

quat_conj - quaternion conjugation

dmp_lib.geometry.axis_angle_to_quat(axis_angle)[source]

Axis-angle to quaternion conversion.

Parameters: axis_angle (numpy.ndarray) – rotation matrix [axis_angle.shape : (3,)]
Returns: a list containing qx, qy, qz, qw [float]
Return type: list
Raises: ValueError – If input list dimension is not 3

dmp_lib.geometry.axis_angle_to_rot(axis_angle)[source]

Axis-angle to rotation matrix conversion.

Parameters: axis_angle (numpy.ndarray) – rotation matrix [axis_angle.shape : (3,)]
Returns: rotation matrix conversion
Return type: numpy.ndarray
Raises: ValueError – If input list dimension is not 3

dmp_lib.geometry.is_rotation_matrix(R)[source]

Check if a matrix is a valid rotation matrix.

Parameters: R (numpy.ndarray) – input matrix [R.shape : (3, 3)]
Returns: input matrix is a valid rotation matrix
Return type: bool
Raises: ValueError – If R is not a 3-by-3 matrix

dmp_lib.geometry.quat_conj(quat)[source]

Quaternion conjugation.

Parameters: quat (list) – a list containing qx, qy, qz, qw [float]
Returns: a list containing qx, qy, qz, qw [float] of conjugate quaternion
Return type: list
Raises: ValueError – If input list dimension is not 4

dmp_lib.geometry.quat_product(q1, q2)[source]

Quaternion product.

Parameters

q1 (list) – a list containing qx, qy, qz, qw [float] of 1st quaternion
q2 (list) – a list containing qx, qy, qz, qw [float] of 2nd quaternion

Returns

a list containing qx, qy, qz, qw [float] of product quaternion

Return type

list

Raises

ValueError – If q1 dimension is not 4 If q2 dimension is not 4

dmp_lib.geometry.quat_to_axis_angle(quat, auto_align=False, ref_axis=array([1, 0, 0]))[source]

Quaternion to rotation matrix conversion.

Parameters

quat (list) – a list containing qx, qy, qz, qw [float]
auto_align (bool, optional) – flag to align axis-angle with reference axis (default is False)
ref_axis (numpy.ndarray, optional) – reference axis for auto alignment (default is np.array([1,0,0]))

Returns

a list containing axis [numpy.ndarray] and angle [float]

Return type

list

Raises

ValueError – If input list dimension is not 4

dmp_lib.geometry.quat_to_rot(quat)[source]

Quaternion to rotation matrix conversion.

Parameters: quat (list) – a list containing qx, qy, qz, qw [float]
Returns: R – rotation matrix [R.shape : (3, 3)]
Return type: numpy.ndarray
Raises: ValueError – If input list dimension is not 4

dmp_lib.geometry.rot_to_axis_angle(R, auto_align=False, ref_axis=array([1, 0, 0]))[source]

Rotation matrix to axis-angle conversion.

Parameters

R (numpy.ndarray) – rotation matrix [R.shape : (3, 3)]
auto_align (bool, optional) – flag to align axis-angle with reference axis (default is False)
ref_axis (numpy.ndarray, optional) – reference axis for auto alignment (default is np.array([1,0,0]))

Returns

a list containing axis [numpy.ndarray] and angle [float]

Return type

list

Raises

ValueError – If R is not a valid rotation matrix

dmp_lib.geometry.rot_to_quat(R)[source]

Rotation matrix to quaternion conversion.

Parameters: R (numpy.ndarray) – rotation matrix [R.shape : (3, 3)]
Returns: a list containing qx, qy, qz, qw [float]
Return type: list
Raises: ValueError – If R is not a valid rotation matrix

dmp_lib.geometry.skew(v)[source]

Compute skew matrix form of 3D vector.

Parameters: v (numpy.ndarray) – vector [v.shape = (3,)]
Returns: skew matrix form
Return type: numpy.ndarray
Raises: ValueError – If v dimension is not 3

dmp_lib.geometry.solve_discontinuities(axis, angle, flg_plot=False)[source]

Solve discontinuities in the given axis angle trajectory.

Parameters

axis (numpy.ndarray) – trajectory of axis values [axis.shape : (num_step, 3)]
angle (numpy.ndarray) – trajectory of angle values [angle.shape : (num_step,)]
flg_plot (bool, optional) – flag to show the trajectory plot (default is False)

Returns

a list of numpy.ndarray with the continuous axis and angle

Return type

list

Raises

ValueError – If axis and angle trajectories have different lenght If axis 2nd dimension is not 3

dmp_lib.goal module

class dmp_lib.goal.Goal(g0, tau, alpha_g)[source]

Bases: object

A class used to implement the DMP goal system. The goal system drives goal variable ‘g’ towards a desired ‘g0’.

g

goal variable

Type: numpy.ndarray

g0

desired goal

Type: numpy.ndarray

tau

time scaling parameter

Type: float

alpha_g

goal advancement parameter

Type: float

get_goal()[source]

Get current goal variable value.

Returns: goal value
Return type: numpy.ndarray

set_new_goal(new_g, force_goal=True)[source]

Set new desired goal.

Parameters

new_g (numpy.ndarray) – new desired goal
force_goal (bool, optional) – force goal variable to new desired goal (default is True)

step(dt)[source]

Single-step goal system update.

Parameters: dt (float) – time step

dmp_lib.movement_primitives module

class dmp_lib.movement_primitives.Bimanual_DMP(num_basis, x0, g0, dt, tau, alpha_phase=8.333333333333334, alpha_stop=25, alpha_g=12.5, K=312.5, D=None, style='advanced', R_right=array([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]]), R_left=array([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]]))[source]

Bases: DMP

A class used to implement the Bimanual DMP that controls a virtual frame placed between the two end-effectors. Each end-effector pose is defined by a fixed rotation and variable translation from the central virtual frame. Bimanual_DMP extends the main DMP class.

canonical_sys

canonical system

Type: dmp_lib.canonical.Canonical

goal_sys

goal system

Type: dmp_lib.goal.Goal

transformation_sys

transformation system

Type: dmp_lib.transformation.Transformation

z

scaled velocity variable

Type: numpy.ndarray

x0

initial state

Type: numpy.ndarray

sys_dim

system dimension

Type: int

tau

time scaling parameter

Type: float

dt

sampling interval

Type: float

num_basis

number of basis functions

Type: int

centers

basis function centers

Type: numpy.ndarray

widths

basis function widths

Type: numpy.ndarray

weights

basis function weights

Type: numpy.ndarray

R_right

right EE rotation matrix [R_right.shape = (3,3)]

Type: numpy.ndarray

R_left

left EE rotation matrix [R_left.shape = (3,3)]

Type: numpy.ndarray

get_poses()[source]

Get end-effector positions from virtual frame pose.

Returns: list with left and right positions and quaternions
Return type: list

info()[source]

Return info about DMP type.

Returns: DMP type
Return type: string

rollout()[source]

Run a DMP rollout and reset.

Returns: list of numpy.array describing the bimanual rollout
Return type: list

class dmp_lib.movement_primitives.Bimanual_Target_DMP(num_basis, x0, g0, dt, tau, alpha_phase=8.333333333333334, alpha_stop=25, alpha_g=12.5, K=312.5, D=None, style='advanced', w_H_t=array([[1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]]), R_right=array([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]]), R_left=array([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]]))[source]

Bases: Target_DMP

A class used to implement the Target-Referred (TR) Bimanual DMP that controls a virtual frame placed between the two end-effectors. Each end-effector pose is defined by a fixed rotation and variable translation from the central virtual frame. The demonstration is learned expressed in a target reference frame. Bimanual_Target_DMP extends the Target_DMP class.

canonical_sys

canonical system

Type: dmp_lib.canonical.Canonical

goal_sys

goal system

Type: dmp_lib.goal.Goal

transformation_sys

transformation system

Type: dmp_lib.transformation.Transformation

z

scaled velocity variable

Type: numpy.ndarray

x0

initial state

Type: numpy.ndarray

sys_dim

system dimension

Type: int

tau

time scaling parameter

Type: float

dt

sampling interval

Type: float

num_basis

number of basis functions

Type: int

centers

basis function centers

Type: numpy.ndarray

widths

basis function widths

Type: numpy.ndarray

weights

basis function weights

Type: numpy.ndarray

w_H_t

new world-to-target hom. tr. [w_H_t.shape = (4,4)]

Type: numpy.ndarray

R_right

right EE rotation matrix [R_right.shape = (3,3)]

Type: numpy.ndarray

R_left

left EE rotation matrix [R_left.shape = (3,3)]

Type: numpy.ndarray

get_poses()[source]

Get end-effector positions from virtual frame pose.

Returns: list with left and right positions and quaternions
Return type: list

info()[source]

Return info about DMP type.

Returns: DMP type
Return type: string

rollout()[source]

Run a DMP rollout and reset.

Returns: list of numpy.array describing the bimanual rollout
Return type: list

class dmp_lib.movement_primitives.DMP(num_basis, x0, g0, dt, tau, alpha_phase=8.333333333333334, alpha_stop=25, alpha_g=12.5, K=312.5, D=None, style='advanced')[source]

Bases: object

A class used to implement the DMP comprising of a canonical, goal and transformation system working together.

canonical_sys

canonical system

Type: dmp_lib.canonical.Canonical

goal_sys

goal system

Type: dmp_lib.goal.Goal

transformation_sys

transformation system

Type: dmp_lib.transformation.Transformation

z

scaled velocity variable

Type: numpy.ndarray

x0

initial state

Type: numpy.ndarray

sys_dim

system dimension

Type: int

tau

time scaling parameter

Type: float

dt

sampling interval

Type: float

num_basis

number of basis functions

Type: int

centers

basis function centers

Type: numpy.ndarray

widths

basis function widths

Type: numpy.ndarray

weights

basis function weights

Type: numpy.ndarray

evaluate_basis(s)[source]

Evaluate basis functions at phase s.

Parameters: s (float) – phase location
Returns: basis functions values in s
Return type: numpy.ndarray

evaluate_forcing(s)[source]

Evaluate forcing term at phase s.

Parameters: s (float) – phase location
Returns: forcing term in s
Return type: numpy.ndarray

evaluate_weighted_basis(s)[source]

Evaluate weighted basis functions at phase s.

Parameters: s (float) – phase location
Returns: weighted sum of basis functions values in s
Return type: numpy.ndarray

get_goal()[source]

Get current goal from goal system.

Returns: goal value
Return type: numpy.ndarray

get_output()[source]

Get DMP output from transformation system.

Returns: a list with current DMP position and velocity
Return type: list

get_phase()[source]

Get current phase from canonical system.

Returns: phase value
Return type: float

get_weights()[source]

Get DMP weights.

Returns: basis functions weights
Return type: numpy.ndarray

import_weights(new_weights)[source]

Import DMP weights.

Parameters: new_weights (numpy.ndarray) – new DMP weights [new_weights.shape = (num_basis, sys_dim)]

info()[source]

Return info about DMP type.

Returns: DMP type
Return type: string

init_forcing(num_basis)[source]

Initialize parameters of the forcing term.

Parameters: num_basis (int) – number of basis functions

learn_from_demo(x_demo, time_steps, filter_mode='savgol')[source]

Fit forcing function to reproduce desired trajectory.

Attributes tau, x0, g0 are modified according to demo.

Parameters

x_demo (numpy.ndarray) – demo trajectory [x_demo.shape : (num_steps, sys_dim)]
time_steps (numpy.ndarray) – time steps [time_steps.shape : (num_steps,)]
filter_mode (string, optional) – either ‘savgol’ or ‘cd’ (default is ‘savgol’)

Returns

list with (processed) x_demo, dx_demo, ddx_demo, time_steps

Return type

list

Raises

ValueError – if x_demo and time_steps have different length

reset(x0)[source]

Reset DMP at initial state x0.

Parameters: x0 (numpy.ndarray) – initial state

rollout()[source]

Run a DMP rollout and reset.

Returns: list of numpy.array describing the rollout
Return type: list

set_new_goal(new_g, force_goal=True)[source]

Set new desired goal.

Parameters

new_g (numpy.ndarray) – new goal
force_goal (bool, optional) – choose if override g variable or only g0 (default is True)

set_tau(new_tau)[source]

Set time scaling parameter.

Parameters: new_tau (float) – new time scaling parameter

step(dist=0)[source]

Single-step DMP update.

Parameters: dist (float) –

class dmp_lib.movement_primitives.Target_DMP(num_basis, x0, g0, dt, tau, alpha_phase=8.333333333333334, alpha_stop=25, alpha_g=12.5, K=312.5, D=None, style='advanced', w_H_t=array([[1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 1., 0.], [0., 0., 0., 1.]]))[source]

Bases: DMP

Target-Referred DMP subclass (extension of DMP). A class used to implement the Target-Referred DMP (TR-DMP) that learned demo trajectory w.r.t. a target reference frame that can be changed at execution time.

canonical_sys

canonical system

Type: dmp_lib.canonical.Canonical

goal_sys

goal system

Type: dmp_lib.goal.Goal

transformation_sys

transformation system

Type: dmp_lib.transformation.Transformation

z

scaled velocity variable

Type: numpy.ndarray

x0

initial state

Type: numpy.ndarray

sys_dim

system dimension

Type: int

tau

time scaling parameter

Type: float

dt

sampling interval

Type: float

num_basis

number of basis functions

Type: int

centers

basis function centers

Type: numpy.ndarray

widths

basis function widths

Type: numpy.ndarray

weights

basis function weights

Type: numpy.ndarray

w_H_t

world-to-target homogeneous transformation

Type: numpy.ndarray

get_goal(in_world_frame=False)[source]

Set new desired goal.

Parameters: in_world_frame (bool, optional) – pose expressed in world frame (default is False)

get_pose()[source]

Get end-effector pose w.r.t. world frame

Returns: list with EE position and quaternion (in world-frame)
Return type: list

learn_from_demo(x_demo, time_steps, filter_mode='cd')[source]

Fit forcing function to reproduce desired trajectory.

Attributes tau, x0, g0 are modified according to demo.

Parameters

x_demo (numpy.ndarray) – demo trajectory [x_demo.shape : (num_steps, sys_dim)]
time_steps (numpy.ndarray) – time steps [time_steps.shape : (num_steps,)]
filter_mode (string, optional) – either ‘savgol’ or ‘cd’ (default is ‘savgol’)

Returns

list with (processed) x_demo, dx_demo, ddx_demo, time_steps

Return type

list

Raises

ValueError – if x_demo and time_steps have different length

reset(x0, in_world_frame=True)[source]

Reset DMP at initial state x0.

Parameters

x0 (numpy.ndarray) – initial state
in_world_frame (bool, optional) – pose expressed in world frame (default is True)

rollout()[source]

Run a DMP rollout and reset.

Returns: list of numpy.array describing the rollout
Return type: list

set_new_goal(new_g, force_goal=True, in_world_frame=True)[source]

Set new desired goal.

Parameters

new_g (numpy.ndarray) – new goal
force_goal (bool, optional) – choose if override g variable or only g0 (default is True)
in_world_frame (bool, optional) – pose expressed in world frame (default is True)

set_new_target_frame(w_H_t)[source]

Set new world-to-target transform

Parameters: w_H_t (numpy.ndarray) – new world-to-target hom. tr. [w_H_t.shape = (4,4)]
Raises: ValueError – if w_H_t is not a proper hom. tr. matrix

dmp_lib.transformation module

class dmp_lib.transformation.Transformation(x0, tau, K, D=None, style='advanced')[source]

Bases: object

A class used to implement the DMP transformation system. The transformation system drives the DMP state ‘x’ towards a goal ‘g’ following a mass-spring-damper dynamics perturbed by a nonlinear forcing term. The ‘classic’ formulation refers to the one in Schaal et al., 2006. The ‘advanced’ formulation refers to the one in Pastor et al., 2009.

x

state variable