FancySafeBot 0.0.1
A safe robotics library
Loading...
Searching...
No Matches
Inverse Kinematics

Inverse kinematics. More...

Classes

struct  fsb::OptimParameters
 
struct  fsb::InverseKinematicsResult
 

Enumerations

enum class  fsb::InverseKinematicsInfo : uint8_t {
  INVALID_INPUT = 0 , InverseKinematicsInfo::SUCCESS = 1 , InverseKinematicsInfo::WITHIN_FTOL = 2 , InverseKinematicsInfo::WITHIN_XTOL = 3 ,
  InverseKinematicsInfo::WITHIN_FTOL_XTOL = 4 , InverseKinematicsInfo::MAXIMUM_EVALUATIONS_REACHED = 5 , InverseKinematicsInfo::SINGULAR_UPDATE_MATRIX = 6
}
 

Functions

OptimParameters fsb::default_optim_parameters ()
 
InverseKinematicsResult fsb::compute_inverse_kinematics (const BodyTree &body_tree, const OptimParameters &params, const JointSpacePosition &initial_config, size_t body_index, const Transform &target_pose, const Transform &base_pose=transform_identity())
 Compute inverse kinematics for a given end-effector pose.
 

Detailed Description

Inverse kinematics.

Enumeration Type Documentation

◆ InverseKinematicsInfo

enum class fsb::InverseKinematicsInfo : uint8_t
strong
Enumerator
SUCCESS 

objective function converged

WITHIN_FTOL 

both actual and predicted relative reductions in the sum of squares are at most ftol

WITHIN_XTOL 

relative error between two consecutive iterates is at most xtol

WITHIN_FTOL_XTOL 

conditions for WITHIN_FTOL and WITHIN_XTOL both hold

MAXIMUM_EVALUATIONS_REACHED 

number of calls to fcn with iflag = 1 has reached maxfev

SINGULAR_UPDATE_MATRIX 

singular matrix encountered

Function Documentation

◆ compute_inverse_kinematics()

InverseKinematicsResult fsb::compute_inverse_kinematics ( const BodyTree body_tree,
const OptimParameters params,
const JointSpacePosition initial_config,
size_t  body_index,
const Transform target_pose,
const Transform base_pose = transform_identity() 
)

Compute inverse kinematics for a given end-effector pose.

Parameters
body_treeBody tree with link definitions
paramsOptimization parameters
initial_configInitial joint configuration
body_indexIndex of body to compute inverse kinematics for
target_poseDesired body target pose
base_poseBase pose (default is identity transform)
Returns
Result of inverse kinematics computation