movement_primitives.dmp.CouplingTermObstacleAvoidance3D

class movement_primitives.dmp.CouplingTermObstacleAvoidance3D(obstacle_position, gamma=1000.0, beta=6.366197723675814)

Bases: object

Coupling term for obstacle avoidance in 3D.

For DMP and DMPWithFinalVelocity.

Following [1], this coupling term adds an acceleration

\[\boldsymbol{C}_t = \gamma \boldsymbol{R} \dot{\boldsymbol{y}} \theta \exp(-\beta \theta),\]

where

\[\theta = \arccos\left( \frac{(\boldsymbol{o} - \boldsymbol{y})^T \dot{\boldsymbol{y}}}{|\boldsymbol{o} - \boldsymbol{y}| |\dot{\boldsymbol{y}}|} \right)\]

and a rotation axis \(\boldsymbol{r} = (\boldsymbol{o} - \boldsymbol{y}) \times \dot{\boldsymbol{y}}\) used to compute the rotation matrix \(\boldsymbol{R}\) that rotates about it by 90 degrees for an obstacle at position \(\boldsymbol{o}\).

Intuitively, this coupling term adds a movement perpendicular to the current velocity in the plane defined by the direction to the obstacle and the current movement direction.

Parameters:

obstacle_positionarray, shape (3,)

Position of the point obstacle.

gammafloat, optional (default: 1000)

Parameter of obstacle avoidance.

betafloat, optional (default: 20 / pi)

Parameter of obstacle avoidance.

References

[1]

Ijspeert, A. J., Nakanishi, J., Hoffmann, H., Pastor, P., Schaal, S. (2013). Dynamical Movement Primitives: Learning Attractor Models for Motor Behaviors. Neural Computation 25 (2), 328-373. DOI: 10.1162/NECO_a_00393, https://homes.cs.washington.edu/~todorov/courses/amath579/reading/DynamicPrimitives.pdf

Methods

coupling(y, yd)

Computes coupling term based on current state.

__init__(obstacle_position, gamma=1000.0, beta=6.366197723675814)

Methods

__init__(obstacle_position[, gamma, beta])
coupling(y, yd)	Computes coupling term based on current state.

coupling(y, yd)

Computes coupling term based on current state.

Parameters:

yarray, shape (n_dims,)

Current position.

ydarray, shape (n_dims,)

Current velocity.

Returns:

cdarray, shape (n_dims,)

Velocity. 0 for this coupling term.

cddarray, shape (n_dims,)

Acceleration.