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

A class that combines the forward and inverse kinematics of an arm. More...

#include <ArmKinematics.h>

Inheritance diagram for kinematics::ArmKinematics< D, N >:
Collaboration diagram for kinematics::ArmKinematics< D, N >:

Public Member Functions

 ArmKinematics (std::shared_ptr< const ForwardArmKinematics< D, N > > forwardKin, std::shared_ptr< const InverseArmKinematics< D, N > > inverseKin)
 
bool satisfiesConstraints (const navtypes::Vectord< N > &jointPos) const override
 Check if the given joint configuration is valid.
 
navtypes::Vectord< N > getSegLens () const override
 Get the segment lengths for this arm.
 
navtypes::Vectord< D > jointPosToEEPos (const navtypes::Vectord< N > &jointPos) const override
 Given a joint position, calculate the current EE position.
 
navtypes::Matrixd< D, N > getJacobian (const navtypes::Vectord< N > &jointPos) const override
 Get the jacobian matrix for the arm at the given joint angles.
 
- Public Member Functions inherited from kinematics::ForwardArmKinematics< D, N >
constexpr unsigned int getNumSegments () const
 Get the number of segments in this arm.
 
constexpr unsigned int getNumDimensions () const
 Get the number of dimensions of the end effector space.
 
navtypes::Vectord< D > jointVelToEEVel (const navtypes::Vectord< N > &jointPos, const navtypes::Vectord< N > &jointVel) const
 Calculate the EE velocity.
 
navtypes::Vectord< N > eeVelToJointVel (const navtypes::Vectord< N > &jointPos, const Eigen::Vector2d &eeVel) const
 Calculates the joint velocity that yields the desired EE velocity.
 
- Public Member Functions inherited from kinematics::InverseArmKinematics< D, 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.
 

Protected Member Functions

navtypes::Vectord< N > solve (const navtypes::Vectord< D > &eePos, const navtypes::Vectord< N > &jointAngles, bool &success) const override
 Solve for the joint angles that yield the given EE position.
 

Additional Inherited Members

- Protected Attributes inherited from kinematics::InverseArmKinematics< D, N >
const std::shared_ptr< const ForwardArmKinematics< D, N > > fk
 

Detailed Description

template<unsigned int D, unsigned int N>
class kinematics::ArmKinematics< D, N >

A class that combines the forward and inverse kinematics of an arm.

Template Parameters
DThe number of dimensions of the end effector space.
NThe number of joints in the arm.

Member Function Documentation

◆ getJacobian()

template<unsigned int D, unsigned int N>
navtypes::Matrixd< D, N > kinematics::ArmKinematics< D, N >::getJacobian ( const navtypes::Vectord< N > & jointPos) const
inlineoverridevirtual

Get the jacobian matrix for the arm at the given joint angles.

Parameters
jointPosThe joint angles of the arm.
Returns
navtypes::Matrixd<D, N> The jacobian matrix at the point.
See also
https://en.wikipedia.org/wiki/Jacobian_matrix_and_determinant

Implements kinematics::ForwardArmKinematics< D, N >.

◆ getSegLens()

template<unsigned int D, unsigned int N>
navtypes::Vectord< N > kinematics::ArmKinematics< D, N >::getSegLens ( ) const
inlineoverridevirtual

Get the segment lengths for this arm.

Returns
The vector of segment lengths

Implements kinematics::ForwardArmKinematics< D, N >.

◆ jointPosToEEPos()

template<unsigned int D, unsigned int N>
navtypes::Vectord< D > kinematics::ArmKinematics< D, N >::jointPosToEEPos ( const navtypes::Vectord< N > & jointPos) const
inlineoverridevirtual

Given a joint position, calculate the current EE position.

Parameters
jointPosCurrent joint position.
Returns
navtypes::Vectord<D> The EE position.

Implements kinematics::ForwardArmKinematics< D, N >.

◆ satisfiesConstraints()

template<unsigned int D, unsigned int N>
bool kinematics::ArmKinematics< D, N >::satisfiesConstraints ( const navtypes::Vectord< N > & jointPos) const
inlineoverridevirtual

Check if the given joint configuration is valid.

Parameters
jointPosThe joint positions to check.
Returns
true iff the joint configuration is valid, false otherwise.

Implements kinematics::ForwardArmKinematics< D, N >.

◆ solve()

template<unsigned int D, unsigned int N>
navtypes::Vectord< N > kinematics::ArmKinematics< D, N >::solve ( const navtypes::Vectord< D > & eePos,
const navtypes::Vectord< N > & jointAngles,
bool & success ) const
inlineoverrideprotectedvirtual

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.

Parameters
eePosThe desired end effector position.
currJointPosThe current joint angles. Used as a starting guess for the IK solver.
[out]successOutput parameter that signals if the IK solver succeeded. Failure may occur if the target is unreachable or the IK solver exceeds the iteration limit.
Returns
navtypes::Vectord<N> The target joint angles that achieve the desired EE position.

Implements kinematics::InverseArmKinematics< D, N >.


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