RobWorkProject
23.9.11-
|
Implements a Gradient Projection Method (GPM) More...
#include <BasicGPM.hpp>
Public Types | |
enum | ProjectionFrame { BaseFrame = 0 , ControlFrame } |
Enumeration used to specify frame associated with the projection. More... | |
Public Member Functions | |
BasicGPM (rw::models::Device *device, rw::core::Ptr< rw::kinematics::Frame > controlFrame, const rw::kinematics::State &state, const rw::math::Q &qhome, double dt) | |
Constructs BasicGPM object. More... | |
virtual | ~BasicGPM () |
Destructor. | |
rw::math::Q | solve (const rw::math::Q &q, const rw::math::Q &dq, const rw::math::VelocityScrew6D<> &tcpvel) |
Solves for joint velocities given a desired tool velocity. More... | |
void | setUseJointLimitsCost (bool use) |
Specifies whether to use joint limit avoidance. More... | |
void | setUseSingularityCost (bool use) |
Specifies whether to use singularity avoidance. More... | |
void | setJointLimitsWeight (double w) |
Sets the weight of the joint limits. More... | |
void | setJointLimitThreshold (double thresholdLowerRatio, double thresholdUpperRatio) |
Sets the threshold for the joint limits. More... | |
void | setSingularityWeight (double w) |
Sets the weight of the singularity avoidance task. More... | |
void | setProjection (const Eigen::MatrixXd &P, ProjectionFrame space) |
Setup the projection. More... | |
Implements a Gradient Projection Method (GPM)
GPM implements the basic Gradient Projection Method with joint limit and singularity avoidance presented in [1]: "Using the task function approach to avoid robot joint limits and kinematic singularities in visual servoing" by Marchand, Chaumette and Rizzo, IEEE/RSJ Int. Conf. on Intelligent Robots and Systems, 1996, vol. 3, pp. 1083-1090.
enum ProjectionFrame |
BasicGPM | ( | rw::models::Device * | device, |
rw::core::Ptr< rw::kinematics::Frame > | controlFrame, | ||
const rw::kinematics::State & | state, | ||
const rw::math::Q & | qhome, | ||
double | dt | ||
) |
Constructs BasicGPM object.
device | [in] Device to work with |
controlFrame | [in] The frame on the device to control. Usually this will be the tool, but not necessarily. |
state | [in] Default state |
qhome | [in] Configuration somewhere between the lower and upper limit and towards which the joints should move |
dt | [in] Step size |
void setJointLimitsWeight | ( | double | w | ) |
Sets the weight of the joint limits.
w | [in] Weight of the joint limit |
void setJointLimitThreshold | ( | double | thresholdLowerRatio, |
double | thresholdUpperRatio | ||
) |
Sets the threshold for the joint limits.
Given an upper and a lower bound \(upper\) and \(lower\) the proximity of the joint limits is defined as \(upper-\tau_{max} (upper-lower)\) and \(lower+\tau_{min} (upper-lower)\) where \(\tau_{min}\) and \(\tau_{max}\) are the thresholds specified here.
thresholdLowerRatio | [in] Relative threshold for the lower joint limits |
thresholdUpperRatio | [in] Relative threshold for the upper joint limits |
void setProjection | ( | const Eigen::MatrixXd & | P, |
ProjectionFrame | space | ||
) |
Setup the projection.
The traditional relationship between device Jacobian, joint velocities and tool velocity is given by \(J\dot{q}=\dot{q}\). To ignore certain degrees of freedom or put more emphasis (with respect to the least square solution) on some we can multiply with \(P\) to get \(P J\dot{q}=P \dot{x}\).
\(P\) needs to have exactly 6 columns, however the number of row may be less than 6. Use the space flag to specify in which space the projection should occur.
Usage: Setup to ignore tool roll
P | [in] The projection matrix |
space | [in] The space in which to apply the projection |
void setSingularityWeight | ( | double | w | ) |
Sets the weight of the singularity avoidance task.
w | [in] The weight |
void setUseJointLimitsCost | ( | bool | use | ) |
Specifies whether to use joint limit avoidance.
The method implements the method with Activation threshold from the [1]
use | [in] True to use joint limit avoidance |
void setUseSingularityCost | ( | bool | use | ) |
Specifies whether to use singularity avoidance.
The singularity avoidance as described in [1]
use | [in] True to use singularity avoidance |
rw::math::Q solve | ( | const rw::math::Q & | q, |
const rw::math::Q & | dq, | ||
const rw::math::VelocityScrew6D<> & | tcpvel | ||
) |
Solves for joint velocities given a desired tool velocity.
q | [in] The current joint configuration |
dq | [in] The current joint velocity |
tcpvel | [in] The desired tool velocity seen in the base frame |