movement_primitives.dmp.CouplingTermObstacleAvoidance3D#
- class movement_primitives.dmp.CouplingTermObstacleAvoidance3D(obstacle_position, gamma=1000.0, beta=6.366197723675814)#
Bases:
objectCoupling term for obstacle avoidance in 3D.
For
DMPandDMPWithFinalVelocity.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.
Methods
coupling(y, yd)Computes coupling term based on current state.
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
- 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.