Simox  2.3.74.0
VirtualRobot::GenericIKSolver Class Reference
Inheritance diagram for VirtualRobot::GenericIKSolver:
VirtualRobot::AdvancedIKSolver VirtualRobot::IKSolver

Public Member Functions

 GenericIKSolver (RobotNodeSetPtr rns, JacobiProvider::InverseJacobiMethod invJacMethod=JacobiProvider::eSVD)
 Initialize an IK solver without collision detection. More...
 
bool solve (const Eigen::Matrix4f &globalPose, CartesianSelection selection=All, int maxLoops=1) override
 
GraspPtr solve (ManipulationObjectPtr object, CartesianSelection selection=All, int maxLoops=1) override
 
bool solve (ManipulationObjectPtr object, GraspPtr grasp, CartesianSelection selection=All, int maxLoops=1) override
 
void setupJacobian (float stepSize, int maxLoops)
 
void setVerbose (bool enable)
 
DifferentialIKPtr getDifferentialIK ()
 
void setupTranslationalJoint (RobotNodePtr rn, float initialValue)
 
- Public Member Functions inherited from VirtualRobot::AdvancedIKSolver
 AdvancedIKSolver (RobotNodeSetPtr rns)
 Initialize a IK solver without collision detection. More...
 
virtual void collisionDetection (SceneObjectPtr avoidCollisionsWith)
 
virtual void collisionDetection (ObstaclePtr avoidCollisionsWith)
 
virtual void collisionDetection (SceneObjectSetPtr avoidCollisionsWith)
 
virtual void collisionDetection (CDManagerPtr avoidCollision)
 
virtual void setMaximumError (float maxErrorPositionMM=1.0f, float maxErrorOrientationRad=0.02)
 
virtual void setReachabilityCheck (ReachabilityPtr reachabilitySpace)
 
virtual std::vector< float > solveNoRNSUpdate (const Eigen::Matrix4f &globalPose, CartesianSelection selection=All)
 
virtual bool solve (const Eigen::Vector3f &globalPosition)
 
virtual GraspPtr sampleSolution (ManipulationObjectPtr object, GraspSetPtr graspSet, CartesianSelection selection=All, bool removeGraspFromSet=false, int maxLoops=1)
 
virtual bool checkReachable (const Eigen::Matrix4f &globalPose)
 
RobotNodeSetPtr getRobotNodeSet ()
 
RobotNodePtr getTcp ()
 
void setVerbose (bool enable)
 
- Public Member Functions inherited from VirtualRobot::IKSolver
 IKSolver ()
 

Protected Member Functions

virtual void _init ()
 This method is called by the constructor and can be used in derived classes for initialization. More...
 
bool _sampleSolution (const Eigen::Matrix4f &globalPose, CartesianSelection selection, int maxLoops=1) override
 This method should deliver solution sample out of the set of possible solutions. More...
 
bool trySolve ()
 
void setJointsRandom ()
 

Protected Attributes

RobotNodePtr coordSystem
 
JacobiProvider::InverseJacobiMethod invJacMethod
 
DifferentialIKPtr jacobian
 
float jacobianStepSize
 
int jacobianMaxLoops
 
RobotNodePtr translationalJoint
 
float initialTranslationalJointValue
 
- Protected Attributes inherited from VirtualRobot::AdvancedIKSolver
RobotNodeSetPtr rns
 
RobotNodePtr tcp
 
CDManagerPtr cdm
 
float maxErrorPositionMM
 
float maxErrorOrientationRad
 
ReachabilityPtr reachabilitySpace
 
bool verbose
 

Additional Inherited Members

- Public Types inherited from VirtualRobot::IKSolver
enum  CartesianSelection {
  X = 1, Y = 2, Z = 4, Position = 7,
  Orientation = 8, All = 15
}
 Flags for the selection of the target components. More...
 

Constructor & Destructor Documentation

◆ GenericIKSolver()

VirtualRobot::GenericIKSolver::GenericIKSolver ( RobotNodeSetPtr  rns,
JacobiProvider::InverseJacobiMethod  invJacMethod = JacobiProvider::eSVD 
)

Initialize an IK solver without collision detection.

Parameters
rnsThe robotNodes (i.e., joints) for which the Jacobians should be calculated.
invJacMethodThe method that should be used to compute the inverse of the Jacobian.

Member Function Documentation

◆ _init()

void VirtualRobot::GenericIKSolver::_init ( )
protectedvirtual

This method is called by the constructor and can be used in derived classes for initialization.

◆ _sampleSolution()

bool VirtualRobot::GenericIKSolver::_sampleSolution ( const Eigen::Matrix4f &  globalPose,
CartesianSelection  selection,
int  maxLoops = 1 
)
overrideprotectedvirtual

This method should deliver solution sample out of the set of possible solutions.

Reimplemented from VirtualRobot::AdvancedIKSolver.

◆ getDifferentialIK()

DifferentialIKPtr VirtualRobot::GenericIKSolver::getDifferentialIK ( )

◆ setJointsRandom()

void VirtualRobot::GenericIKSolver::setJointsRandom ( )
protected

◆ setupJacobian()

void VirtualRobot::GenericIKSolver::setupJacobian ( float  stepSize,
int  maxLoops 
)

◆ setupTranslationalJoint()

void VirtualRobot::GenericIKSolver::setupTranslationalJoint ( RobotNodePtr  rn,
float  initialValue 
)

◆ setVerbose()

void VirtualRobot::GenericIKSolver::setVerbose ( bool  enable)

◆ solve() [1/3]

bool VirtualRobot::GenericIKSolver::solve ( const Eigen::Matrix4f &  globalPose,
CartesianSelection  selection = All,
int  maxLoops = 1 
)
overridevirtual

This method solves the IK up to the specified max error. On success, the joints of the the corresponding RobotNodeSet are set to the IK solution.

Parameters
globalPoseThe target pose given in global coordinate system.
selectionSelect the parts of the global pose that should be used for IK solving. (e.g. you can just consider the position and ignore the target orientation)
maxLoopsHow often should we try.
Returns
true on success

Implements VirtualRobot::AdvancedIKSolver.

◆ solve() [2/3]

VirtualRobot::GraspPtr VirtualRobot::GenericIKSolver::solve ( ManipulationObjectPtr  object,
CartesianSelection  selection = All,
int  maxLoops = 1 
)
overridevirtual

This method solves the IK up to the specified max error. On success, the joints of the the corresponding RobotNodeSet are set to the IK solution.

Parameters
objectThe grasps of this object are checked if the stored TCP is identical with teh TCP of teh current RobotNodeSet, and the an IK solution for one of remaining grasps is searched.
selectionSelect the parts of the global pose that should be used for IK solving. (e.g. you can just consider the position and ignore the target orientation)
maxLoopsHow often should we try.
Returns
On success: The grasp for which an IK-solution was found, otherwise an empty GraspPtr

Reimplemented from VirtualRobot::AdvancedIKSolver.

◆ solve() [3/3]

bool VirtualRobot::GenericIKSolver::solve ( ManipulationObjectPtr  object,
GraspPtr  grasp,
CartesianSelection  selection = All,
int  maxLoops = 1 
)
overridevirtual

Reimplemented from VirtualRobot::AdvancedIKSolver.

◆ trySolve()

bool VirtualRobot::GenericIKSolver::trySolve ( )
protected

Field Documentation

◆ coordSystem

RobotNodePtr VirtualRobot::GenericIKSolver::coordSystem
protected

◆ initialTranslationalJointValue

float VirtualRobot::GenericIKSolver::initialTranslationalJointValue
protected

◆ invJacMethod

JacobiProvider::InverseJacobiMethod VirtualRobot::GenericIKSolver::invJacMethod
protected

◆ jacobian

DifferentialIKPtr VirtualRobot::GenericIKSolver::jacobian
protected

◆ jacobianMaxLoops

int VirtualRobot::GenericIKSolver::jacobianMaxLoops
protected

◆ jacobianStepSize

float VirtualRobot::GenericIKSolver::jacobianStepSize
protected

◆ translationalJoint

RobotNodePtr VirtualRobot::GenericIKSolver::translationalJoint
protected