Resurgence (PY2022)
Codebase for the Husky Robotics 2021-2022 rover Resurgence
|
A class that calculates the inverse kinematics of an arm. More...
#include <InverseArmKinematics.h>
Public Member Functions | |
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. | |
Protected Member Functions | |
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 | |
const std::shared_ptr< const ForwardArmKinematics< D, N > > | fk |
A class that calculates the inverse kinematics of an arm.
D | The number of dimensions of the end effector space. |
N | The number of joints in the arm. |
|
inline |
Construct an IK object.
fk | The forward kinematics that are inverted by this class. |
|
inline |
Calculate the joint angles that yield the desired EE position.
success
parameter.eePos | Desired EE position. |
currJointPos | The current joint angles. |
|
inlinevirtual |
Solve for the joint angles that yield the given EE position.
eePos | The desired end effector position. | |
currJointPos | The current joint angles. Used as a starting guess for the IK solver. | |
[out] | success | Output parameter that signals if the IK solver succeeded. Failure may occur if the target is unreachable or the IK solver exceeds the iteration limit. |
|
protectedpure virtual |
Solve for the joint angles that yield the given EE position.
This method does not need to verify that the solution verifies the FK constraints, or if the target position is within the arm's reach.
eePos | The desired end effector position. | |
currJointPos | The current joint angles. Used as a starting guess for the IK solver. | |
[out] | success | Output parameter that signals if the IK solver succeeded. Failure may occur if the target is unreachable or the IK solver exceeds the iteration limit. |
Implemented in kinematics::ArmKinematics< D, N >, and kinematics::ArmKinematics< 2, N >.