Simox  2.3.50
VirtualRobot::DifferentialIK Class Reference

Encapsulates a differential inverse kinematics for the virtual robot. More...

Inheritance diagram for VirtualRobot::DifferentialIK:
VirtualRobot::JacobiProvider

Public Member Functions

EIGEN_MAKE_ALIGNED_OPERATOR_NEW DifferentialIK (RobotNodeSetPtr rns, RobotNodePtr coordSystem=RobotNodePtr(), JacobiProvider::InverseJacobiMethod invJacMethod=eSVD, float invParam=0.0f)
 Initialize a Jacobian object. More...
 
virtual void setGoal (const Eigen::Matrix4f &goal, SceneObjectPtr tcp=SceneObjectPtr(), IKSolver::CartesianSelection mode=IKSolver::All, float tolerancePosition=5.0f, float toleranceRotation=3.0f/180.0f *M_PI, bool performInitialization=true)
 Sets the target position for (one of) the tcp(s). More...
 
virtual void setGoal (const Eigen::Vector3f &goal, SceneObjectPtr tcp=SceneObjectPtr(), IKSolver::CartesianSelection mode=IKSolver::Position, float tolerancePosition=5.0f, float toleranceRotation=3.0f/180.0f *M_PI, bool performInitialization=true)
 Sets the target position for (one of) the tcp(s). More...
 
virtual Eigen::MatrixXf getJacobianMatrix (SceneObjectPtr tcp, IKSolver::CartesianSelection mode)
 Returns the Jacobian matrix for a given tcp. More...
 
virtual Eigen::MatrixXf getJacobianMatrix (SceneObjectPtr tcp)
 
virtual Eigen::MatrixXf getJacobianMatrix (IKSolver::CartesianSelection mode)
 
virtual Eigen::MatrixXf getJacobianMatrix ()
 
virtual Eigen::VectorXf getError (float stepSize=1.0f)
 
void updateError (Eigen::VectorXf &error, float stepSize=1.0f)
 
virtual Eigen::MatrixXf getPseudoInverseJacobianMatrix (SceneObjectPtr tcp, IKSolver::CartesianSelection mode=IKSolver::All)
 Returns the pseudo inverse of the Jacobian matrix for a given tcp of the robot. More...
 
virtual Eigen::MatrixXf getPseudoInverseJacobianMatrix ()
 
virtual Eigen::MatrixXf getPseudoInverseJacobianMatrix (IKSolver::CartesianSelection mode)
 
void updateJacobianMatrix (Eigen::MatrixXf &jac)
 
void updateJacobianMatrix (Eigen::MatrixXf &jac, SceneObjectPtr tcp, IKSolver::CartesianSelection mode)
 
virtual Eigen::VectorXf computeStep (float stepSize=1.0f)
 Compute a single IK step. More...
 
virtual bool computeSteps (float stepSize, float minChange, int maxSteps)
 Computes the complete inverse kinematics. More...
 
virtual bool solveIK (float stepSize=0.2f, float minChange=0.0f, int maxSteps=50)
 Computes the complete inverse kinematics. More...
 
virtual RobotNodePtr getDefaultTCP ()
 Returns the default tcp of the robot. More...
 
virtual float getErrorPosition (SceneObjectPtr tcp=SceneObjectPtr())
 Returns the translation error for a given tcp (i.e., the distance to the target). More...
 
virtual float getErrorRotation (SceneObjectPtr tcp=SceneObjectPtr())
 Returns the error metric for a given tcp (i.e., the angle between the target pose). More...
 
virtual void checkImprovements (bool enable)
 
void convertModelScalingtoM (bool enable)
 
void setVerbose (bool enable)
 
virtual float getMeanErrorPosition ()
 
virtual void updateDeltaToGoal (Eigen::VectorXf &delta, SceneObjectPtr tcp=SceneObjectPtr())
 
virtual void updateDelta (Eigen::VectorXf &delta, const Eigen::Matrix4f &current, const Eigen::Matrix4f &goal, IKSolver::CartesianSelection mode=IKSolver::All)
 
virtual Eigen::VectorXf getDeltaToGoal (SceneObjectPtr tcp=SceneObjectPtr())
 
virtual Eigen::VectorXf getDelta (const Eigen::Matrix4f &current, const Eigen::Matrix4f &goal, IKSolver::CartesianSelection mode=IKSolver::All)
 
virtual void setMaxPositionStep (float s)
 When considering large errors, the translational part can be cut to this length. Set to <= 0 to ignore cutting (standard) More...
 
virtual bool checkTolerances ()
 
virtual void initialize ()
 
virtual void print ()
 print Print current status of the IK solver More...
 
- Public Member Functions inherited from VirtualRobot::JacobiProvider
 JacobiProvider (RobotNodeSetPtr rns, InverseJacobiMethod invJacMethod=eSVD)
 
virtual ~JacobiProvider ()
 
virtual Eigen::MatrixXd getJacobianMatrixD ()
 
virtual Eigen::MatrixXd getJacobianMatrixD (SceneObjectPtr tcp)
 
virtual Eigen::MatrixXf computePseudoInverseJacobianMatrix (const Eigen::MatrixXf &m) const
 
virtual Eigen::MatrixXd computePseudoInverseJacobianMatrixD (const Eigen::MatrixXd &m) const
 
virtual Eigen::MatrixXf computePseudoInverseJacobianMatrix (const Eigen::MatrixXf &m, float invParameter) const
 
virtual Eigen::MatrixXd computePseudoInverseJacobianMatrixD (const Eigen::MatrixXd &m, double invParameter) const
 
virtual void updatePseudoInverseJacobianMatrix (Eigen::MatrixXf &invJac, const Eigen::MatrixXf &m, float invParameter=0.0f) const
 
virtual void updatePseudoInverseJacobianMatrixD (Eigen::MatrixXd &invJac, const Eigen::MatrixXd &m, double invParameter=0.0) const
 
virtual Eigen::MatrixXd getPseudoInverseJacobianMatrixD ()
 
virtual Eigen::MatrixXf getPseudoInverseJacobianMatrix (SceneObjectPtr tcp)
 
virtual Eigen::MatrixXd getPseudoInverseJacobianMatrixD (SceneObjectPtr tcp)
 
VirtualRobot::RobotNodeSetPtr getRobotNodeSet ()
 
bool isInitialized ()
 
void setJointWeights (const Eigen::VectorXf &jointWeights)
 

Protected Member Functions

virtual void setNRows ()
 

Protected Attributes

float invParam
 
std::vector< SceneObjectPtrtcp_set
 
RobotNodePtr coordSystem
 
bool checkImprovement
 Indicates if the jacobian steps must improve the result, otherwise the loop is canceld. More...
 
std::size_t nRows
 
std::size_t nDoF
 
std::map< SceneObjectPtr, Eigen::Matrix4f, std::less< SceneObjectPtr >, Eigen::aligned_allocator< std::pair< const SceneObjectPtr, Eigen::Matrix4f > > > targets
 
std::map< SceneObjectPtr, Eigen::MatrixXf, std::less< SceneObjectPtr >, Eigen::aligned_allocator< std::pair< const SceneObjectPtr, Eigen::MatrixXf > > > partJacobians
 
std::map< SceneObjectPtr, IKSolver::CartesianSelectionmodes
 
std::map< SceneObjectPtr, float > tolerancePosition
 
std::map< SceneObjectPtr, float > toleranceRotation
 
bool convertMMtoM
 
std::vector< RobotNodePtrnodes
 
std::map< RobotNodePtr, std::vector< RobotNodePtr > > parents
 
Eigen::VectorXf currentError
 
Eigen::MatrixXf currentJacobian
 
Eigen::MatrixXf currentInvJacobian
 
Eigen::Matrix4f tmpDeltaOrientation
 
Eigen::AngleAxis< float > tmpDeltaAA
 
Eigen::VectorXf tmpUpdateErrorDelta
 
Eigen::Vector3f tmpUpdateErrorPosition
 
Eigen::MatrixXf tmpUpdateJacobianPosition
 
Eigen::MatrixXf tmpUpdateJacobianOrientation
 
Eigen::VectorXf tmpComputeStepTheta
 
float positionMaxStep
 
bool verbose
 
- Protected Attributes inherited from VirtualRobot::JacobiProvider
std::string name
 
RobotNodeSetPtr rns
 
InverseJacobiMethod inverseMethod
 
bool initialized
 
Eigen::VectorXf jointWeights
 

Additional Inherited Members

- Public Types inherited from VirtualRobot::JacobiProvider
enum  InverseJacobiMethod { eSVD, eSVDDamped, eTranspose }
 Several methods are offered for inverting the Jacobi (i.e. building the Pseudoinverse) More...
 

Detailed Description

Encapsulates a differential inverse kinematics for the virtual robot.

The aim of this class is to offer a convenient mechanism to solve the inverse kinematics of redundant robots. With this class it is possible to specify goals for multiple TCPs which will be be used simultaneously within the same optimization step. That way, the IK for two-armed manipulation can be solved where both arms share the same hip joint (see image). For each tcp, it is possible to select which components of the error vector should be optimized. One can restrict it to consider only the orientation, or only position, or even only the $z$-component, for instance. Any node of the virtual robot can be selected chosen tcp. For instance, the robot hand can be targed to reach for a position while its elbow should be lifted to a certain hight (see images and examples)

Despite all higher-level functionality, the standard application can still be implemented very intuitively. Example for bimanual manipulation:

// define a kinematic chain for bimanual manipulation.
std::vector<RobotNodePtr> nBi;
nBi.push_back(robot->getRobotNode(std::string("Shoulder 1 L")));
nBi.push_back(robot->getRobotNode(std::string("Shoulder 1 R")));
// ...
RobotNodeSetPtr kcBi = RobotNodeSet::createRobotNodeSet(robot,std::string("jacobiTestBi"),nBi);
// Create an IK algorithm instance. The world coordinate system is the frame of reference.
DifferentialIK dIK (kcBi);
// Set the target poses for the two end effectors.
dIK.setGoal(targetPose,tcp,DifferentialIK::Position);
dIK.setGoal(targetPose2,tcp2,DifferentialIK::Position);
// compute the IK using 0.1 stepsize and a maximum of 50 steps.
dIK.solveIK(0.1f);

Example for lifted elbow:

// should yield the kinematic chain of the left Arm
// ...
// Create an IK algorithm instance. The world coordinate system is the frame of reference.
DifferentialIK dIK(leftArm);
// should yield the target vector.
Vector3f target_position;
// ...
// Set the target for the end effector.
// This call sets the goal for the default tcp of the chain.
// For vectors, the default is to optimize the position only.
dIK.setGoal(target_position);
// Now the vector should yield the height of the elbow;
float elbow_target_z;
target_position(2) = elbow_target_z;
// Should be a reference to the elbow
dIK.setGoal(target_position, elbow,DifferentialIK::Z);
// compute the IK using 0.1 stepsize and a maximum of 50 steps.
dIK.solveIK(0.1);
// the joints of kcBi are already set to the result

Constructor & Destructor Documentation

VirtualRobot::DifferentialIK::DifferentialIK ( RobotNodeSetPtr  rns,
RobotNodePtr  coordSystem = RobotNodePtr(),
JacobiProvider::InverseJacobiMethod  invJacMethod = eSVD,
float  invParam = 0.0f 
)

Initialize a Jacobian object.

Parameters
rnsThe robotNodes (i.e., joints) for which the Jacobians should be calculated.
coordSystemThe coordinate system in which the Jacobians are defined. By default the global coordinate system is used.
invJacMethodThe method for inverting the Jacobian
invParamOnly used when != 0.0f

Member Function Documentation

void VirtualRobot::DifferentialIK::checkImprovements ( bool  enable)
virtual

Enable or disable the check for improvements during computeSteps loop. If enabled and the delta to the goal does not gets smaller, the loop is canceled. Standard: disabled

bool VirtualRobot::DifferentialIK::checkTolerances ( )
virtual
VectorXf VirtualRobot::DifferentialIK::computeStep ( float  stepSize = 1.0f)
virtual

Compute a single IK step.

Parameters
stepSizeControls the amount of error to be reduced in each step: $ 0 < \beta \leq 1 $
Returns
The changes $\Delta \theta$ in the joint angles.
Note
{Note} This does not affect the joints values of the robot.
bool VirtualRobot::DifferentialIK::computeSteps ( float  stepSize,
float  minChange,
int  maxSteps 
)
virtual

Computes the complete inverse kinematics.

Parameters
stepSizeControls the amount of error to be reduced in each step: $ 0 < \beta \leq 1 $
maxStepsMaximal numbers of steps.
minChangeThe minimal change in joint angles (euclidean distance in radians)
Note
{Note} Sets the node's joint angles automatically.
void VirtualRobot::DifferentialIK::convertModelScalingtoM ( bool  enable)

If enabled, the Jacobian is computed for [m] while assuming the kinematic definitions are given in [mm]. Standard: disabled

RobotNodePtr VirtualRobot::DifferentialIK::getDefaultTCP ( )
virtual

Returns the default tcp of the robot.

Eigen::VectorXf VirtualRobot::DifferentialIK::getDelta ( const Eigen::Matrix4f &  current,
const Eigen::Matrix4f &  goal,
IKSolver::CartesianSelection  mode = IKSolver::All 
)
virtual
Eigen::VectorXf VirtualRobot::DifferentialIK::getDeltaToGoal ( SceneObjectPtr  tcp = SceneObjectPtr())
virtual

Returns 6D workspace delta that is used for Jacobi calculation.

Returns 6D workspace delta that is used for Jacobi calculation.

VectorXf VirtualRobot::DifferentialIK::getError ( float  stepSize = 1.0f)
virtual

Computes the complete error vector, considering all TCPs and goals.

Implements VirtualRobot::JacobiProvider.

float VirtualRobot::DifferentialIK::getErrorPosition ( SceneObjectPtr  tcp = SceneObjectPtr())
virtual

Returns the translation error for a given tcp (i.e., the distance to the target).

float VirtualRobot::DifferentialIK::getErrorRotation ( SceneObjectPtr  tcp = SceneObjectPtr())
virtual

Returns the error metric for a given tcp (i.e., the angle between the target pose).

MatrixXf VirtualRobot::DifferentialIK::getJacobianMatrix ( SceneObjectPtr  tcp,
IKSolver::CartesianSelection  mode 
)
virtual

Returns the Jacobian matrix for a given tcp.

Parameters
tcpThe tcp joint that should be considered. By default the tcp joint that is defined in rns in the constructor is used.
modeAllows to include only a subset of the Cartesian coordinates in the Jacobian (e.g., X|Y if the z-component is unimportant)
Returns
The Jacobian matrix; The number of rows depends on mode.
Note
{Important} In most cases, DifferentialIK::computeSteps() should be used rather than handling the Jacobian matrix by one self.

Please note the convention used to describe the orientation: the scaled axis representation. Orientations in the target vector e (see the example below) have to be given as vectors parallel to the rotation axis. Their lengths have to be the rotation angle in radians. Given a target pose matrix and the actual tcp pose, the pseudo inverse Jacobian matrix can be used to compute the first Taylor expansion of the IK as follows:

// given pose matrices
Matrix4f target_pose, actual_pose;
// the error vector
VectorXd e(6);
// The translational error is just the vector between the actual and the target position
e.segment(0,3) = target_pose(0,3,3,1) - actual_pose(0,3,3,1);
// For the rotational error, the transformation between the poses has to be calculated and
// reformulated into the rotation axis and angle. The error is then the rotation axis scaled
// by the angle in radians.
Matrix4f orientation = targets_pose * actual_pose.inverse();
AngleAxis<float> aa(orientation.block<3,3>(0,0));
e.segment(3,3) = aa.axis()*aa.angle();
// or
AngleAxis orientation( target_pose * actual_pose.inverse() )
e.block(3,3) = orientation.axis() * orientation.angle();
// Calculate the IK
Vector Xd dTheta = dIK->getPseudoInverseJacobianMatrix() * e;

All this is done within computeSteps(). For more information regarding the differential inverse kinematics, see this lecture.

MatrixXf VirtualRobot::DifferentialIK::getJacobianMatrix ( SceneObjectPtr  tcp)
virtual
MatrixXf VirtualRobot::DifferentialIK::getJacobianMatrix ( IKSolver::CartesianSelection  mode)
virtual
MatrixXf VirtualRobot::DifferentialIK::getJacobianMatrix ( )
virtual

Computes the complete Jacobian that considers all defined TCPs and goal poses.

Implements VirtualRobot::JacobiProvider.

float VirtualRobot::DifferentialIK::getMeanErrorPosition ( )
virtual

Returns distance to goal. If multiple goals/TCPs are defined the mean distance is returned.

Eigen::MatrixXf VirtualRobot::DifferentialIK::getPseudoInverseJacobianMatrix ( SceneObjectPtr  tcp,
IKSolver::CartesianSelection  mode = IKSolver::All 
)
virtual

Returns the pseudo inverse of the Jacobian matrix for a given tcp of the robot.

See also
getJacobianMatrix

The pseudo inverse $J^{+}$ can be calculated from the Jacobian matrix $ J $ using the following formula:

\[ J^t \cdot \left( J \cdot J^t \right)^{-1}.\]

. Update: In order to improve stability, we are now using singular value decomposition (SVD).

Eigen::MatrixXf VirtualRobot::DifferentialIK::getPseudoInverseJacobianMatrix ( )
virtual

Reimplemented from VirtualRobot::JacobiProvider.

Eigen::MatrixXf VirtualRobot::DifferentialIK::getPseudoInverseJacobianMatrix ( IKSolver::CartesianSelection  mode)
virtual
void VirtualRobot::DifferentialIK::initialize ( )
virtual

Initializes the internal data structures according to setGoal setup. Usually no need to call this method explicitly, unless performInitialization was not requested in setGoal.

void VirtualRobot::DifferentialIK::print ( )
virtual

print Print current status of the IK solver

Reimplemented from VirtualRobot::JacobiProvider.

void VirtualRobot::DifferentialIK::setGoal ( const Eigen::Matrix4f &  goal,
SceneObjectPtr  tcp = SceneObjectPtr(),
IKSolver::CartesianSelection  mode = IKSolver::All,
float  tolerancePosition = 5.0f,
float  toleranceRotation = 3.0f / 180.0f * M_PI,
bool  performInitialization = true 
)
virtual

Sets the target position for (one of) the tcp(s).

Parameters
goalTarget pose of the tcp.
tcpThe tcp joint that should be considered. By default the tcp joint that is defined in rns in the constructor is used.
modeAllows to include only a subset of the Cartesian coordinates in the Jacobian (e.g., X|Y if the z-component is unimportant)
tolerancePositionThe threshold when to accept a solution.
toleranceRotationThe threshold when to accept a solution in radians.
performInitializationIf multiple goals will be set, the internal initialization can be omitted in order to speed up the setup procedure (Ensure, to call initialize() after setting all goals).
void VirtualRobot::DifferentialIK::setGoal ( const Eigen::Vector3f &  goal,
SceneObjectPtr  tcp = SceneObjectPtr(),
IKSolver::CartesianSelection  mode = IKSolver::Position,
float  tolerancePosition = 5.0f,
float  toleranceRotation = 3.0f / 180.0f * M_PI,
bool  performInitialization = true 
)
virtual

Sets the target position for (one of) the tcp(s).

Parameters
goalTarget position of the tcp.
tcpThe tcp joints that should be considered. By default the tcp joint that is defined in rns in the constructor is used.
modeAllows to include only a subset of the Cartesian coordinates in the Jacobian (e.g., X|Y if the z-component is unimportant)
tolerancePositionThe threshold when to accept a solution.
toleranceRotationThe threshold when to accept a solution in radians.
performInitializationIf multiple goals will be set, the internal initialization can be omitted in order to speed up the setup procedure (Ensure, to call initialize() after setting all goals).
void VirtualRobot::DifferentialIK::setMaxPositionStep ( float  s)
virtual

When considering large errors, the translational part can be cut to this length. Set to <= 0 to ignore cutting (standard)

void VirtualRobot::DifferentialIK::setNRows ( )
protectedvirtual
void VirtualRobot::DifferentialIK::setVerbose ( bool  enable)
bool VirtualRobot::DifferentialIK::solveIK ( float  stepSize = 0.2f,
float  minChange = 0.0f,
int  maxSteps = 50 
)
virtual

Computes the complete inverse kinematics.

Parameters
stepSizeControls the amount of error to be reduced in each step: $ 0 < \beta \leq 1 $
maxStepsMaximal numbers of steps.
minChangeThe minimal change in joint angles (euclidean distance in radians)
Note
{Note} Sets the node's joint angles automatically.
void VirtualRobot::DifferentialIK::updateDelta ( Eigen::VectorXf &  delta,
const Eigen::Matrix4f &  current,
const Eigen::Matrix4f &  goal,
IKSolver::CartesianSelection  mode = IKSolver::All 
)
virtual
void VirtualRobot::DifferentialIK::updateDeltaToGoal ( Eigen::VectorXf &  delta,
SceneObjectPtr  tcp = SceneObjectPtr() 
)
virtual

Returns 6D workspace delta that is used for Jacobi calculation. Updates the currentDeltaToGoal vector

void VirtualRobot::DifferentialIK::updateError ( Eigen::VectorXf &  error,
float  stepSize = 1.0f 
)
void VirtualRobot::DifferentialIK::updateJacobianMatrix ( Eigen::MatrixXf &  jac)
void VirtualRobot::DifferentialIK::updateJacobianMatrix ( Eigen::MatrixXf &  jac,
SceneObjectPtr  tcp,
IKSolver::CartesianSelection  mode 
)

Field Documentation

bool VirtualRobot::DifferentialIK::checkImprovement
protected

Indicates if the jacobian steps must improve the result, otherwise the loop is canceld.

bool VirtualRobot::DifferentialIK::convertMMtoM
protected
RobotNodePtr VirtualRobot::DifferentialIK::coordSystem
protected
Eigen::VectorXf VirtualRobot::DifferentialIK::currentError
protected
Eigen::MatrixXf VirtualRobot::DifferentialIK::currentInvJacobian
protected
Eigen::MatrixXf VirtualRobot::DifferentialIK::currentJacobian
protected
float VirtualRobot::DifferentialIK::invParam
protected
std::map<SceneObjectPtr, IKSolver::CartesianSelection> VirtualRobot::DifferentialIK::modes
protected
std::size_t VirtualRobot::DifferentialIK::nDoF
protected
std::vector<RobotNodePtr> VirtualRobot::DifferentialIK::nodes
protected
std::size_t VirtualRobot::DifferentialIK::nRows
protected
std::map< RobotNodePtr, std::vector<RobotNodePtr> > VirtualRobot::DifferentialIK::parents
protected
std::map<SceneObjectPtr, Eigen::MatrixXf, std::less<SceneObjectPtr>, Eigen::aligned_allocator<std::pair<const SceneObjectPtr, Eigen::MatrixXf> > > VirtualRobot::DifferentialIK::partJacobians
protected
float VirtualRobot::DifferentialIK::positionMaxStep
protected
std::map<SceneObjectPtr, Eigen::Matrix4f, std::less<SceneObjectPtr>, Eigen::aligned_allocator<std::pair<const SceneObjectPtr, Eigen::Matrix4f> > > VirtualRobot::DifferentialIK::targets
protected
std::vector<SceneObjectPtr> VirtualRobot::DifferentialIK::tcp_set
protected
Eigen::VectorXf VirtualRobot::DifferentialIK::tmpComputeStepTheta
protected
Eigen::AngleAxis<float> VirtualRobot::DifferentialIK::tmpDeltaAA
protected
Eigen::Matrix4f VirtualRobot::DifferentialIK::tmpDeltaOrientation
protected
Eigen::VectorXf VirtualRobot::DifferentialIK::tmpUpdateErrorDelta
protected
Eigen::Vector3f VirtualRobot::DifferentialIK::tmpUpdateErrorPosition
protected
Eigen::MatrixXf VirtualRobot::DifferentialIK::tmpUpdateJacobianOrientation
protected
Eigen::MatrixXf VirtualRobot::DifferentialIK::tmpUpdateJacobianPosition
protected
std::map<SceneObjectPtr, float> VirtualRobot::DifferentialIK::tolerancePosition
protected
std::map<SceneObjectPtr, float> VirtualRobot::DifferentialIK::toleranceRotation
protected
bool VirtualRobot::DifferentialIK::verbose
protected