Resurgence (PY2022)
Codebase for the Husky Robotics 2021-2022 rover Resurgence
Loading...
Searching...
No Matches
kinematics::FabrikSolver2D< N > Class Template Reference

Implementation of the FABRIK algorithm, in the special case of 2 dimensions. More...

#include <FabrikSolver.h>

Inheritance diagram for kinematics::FabrikSolver2D< N >:
Collaboration diagram for kinematics::FabrikSolver2D< N >:

Public Member Functions

 FabrikSolver2D (std::shared_ptr< const PlanarArmFK< N > > fk, double thresh, int maxIter)
 Construct an IK solver object.
 
navtypes::Vectord< N > solve (const Eigen::Vector2d &eePos, const navtypes::Vectord< N > &jointAngles, bool &success) const override
 
- Public Member Functions inherited from kinematics::InverseArmKinematics< 2, N >
 InverseArmKinematics (std::shared_ptr< const ForwardArmKinematics< D, N > > fk)
 Construct an IK object.
 
navtypes::Vectord< N > eePosToJointPos (const navtypes::Vectord< D > &eePos, const navtypes::Vectord< N > &currJointPos) const
 Calculate the joint angles that yield the desired EE position.
 
virtual navtypes::Vectord< N > eePosToJointPos (const navtypes::Vectord< D > &eePos, const navtypes::Vectord< N > &jointAngles, bool &success) const
 Solve for the joint angles that yield the given EE position.
 

Additional Inherited Members

- Protected Member Functions inherited from kinematics::InverseArmKinematics< 2, N >
virtual navtypes::Vectord< N > solve (const navtypes::Vectord< D > &eePos, const navtypes::Vectord< N > &jointAngles, bool &success) const=0
 Solve for the joint angles that yield the given EE position.
 
- Protected Attributes inherited from kinematics::InverseArmKinematics< 2, N >
const std::shared_ptr< const ForwardArmKinematics< D, N > > fk
 

Detailed Description

template<int N>
class kinematics::FabrikSolver2D< N >

Implementation of the FABRIK algorithm, in the special case of 2 dimensions.

Note that joint constraints are only used to verify success at the end, they are not used in intermediate parts of the algorithm.

Template Parameters
NThe number of joints.
See also
http://www.andreasaristidou.com/FABRIK.html

Constructor & Destructor Documentation

◆ FabrikSolver2D()

template<int N>
kinematics::FabrikSolver2D< N >::FabrikSolver2D ( std::shared_ptr< const PlanarArmFK< N > > fk,
double thresh,
int maxIter )
inline

Construct an IK solver object.

Parameters
fkThe forward kinematics of the arm.
threshThe IK solver will succeed when the EE position is off by at most this much.
maxIterThe maximum number of iterations to run the solver before failing.

The documentation for this class was generated from the following file: