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

Controller to move planar arm to a target end effector position. More...

#include <PlanarArmController.h>

Public Member Functions

 PlanarArmController (const kinematics::ArmKinematics< 2, N > &kin_obj, const double safetyFactor)
 Construct a new controller object.
 
 PlanarArmController (PlanarArmController &&other)
 Constructs a copy of an existing PlanarArmController object.
 
bool tryInitController (const navtypes::Vectord< N > &currJointPos)
 Instantiates the PlanarArmController with the current joint positions, returning true if the joint positions are valid.
 
const kinematics::ArmKinematics< 2, N > & kinematics () const
 
void set_setpoint (const navtypes::Vectord< N > &targetJointPos)
 Sets the end effector setpoint / target position.
 
Eigen::Vector2d get_setpoint (robot::types::datatime_t currTime)
 Gets the current end effector setpoint / target position.
 
void set_x_vel (robot::types::datatime_t currTime, double targetVel, const navtypes::Vectord< N > &jointPos)
 Sets the x velocity for the end effector and returns the new command.
 
void set_y_vel (robot::types::datatime_t currTime, double targetVel, const navtypes::Vectord< N > &jointPos)
 Sets the y velocity for the end effector and returns the new command.
 
navtypes::Vectord< N > getCommand (robot::types::datatime_t currTime, const navtypes::Vectord< N > &currJointPos)
 Get the current command for the arm.
 

Static Public Member Functions

static bool is_setpoint_valid (const navtypes::Vectord< N > &targetJointPos, kinematics::ArmKinematics< 2, N > kin_obj, double safetyFactor)
 Returns whether the target joint positions are within the arm controller's radius limit.
 

Detailed Description

template<unsigned int N>
class control::PlanarArmController< N >

Controller to move planar arm to a target end effector position.

This class is thread-safe.

Template Parameters
NThe number of arm joints.

Constructor & Destructor Documentation

◆ PlanarArmController() [1/2]

template<unsigned int N>
control::PlanarArmController< N >::PlanarArmController ( const kinematics::ArmKinematics< 2, N > & kin_obj,
const double safetyFactor )
inline

Construct a new controller object.

Parameters
currJointPosThe current joint positions of the arm.
kin_objArmKinematics object for the arm (should have the same number of arm joints).
safetyFactorthe percentage factor to scale maximum arm extension radius by to prevent singularity lock. Must be in range (0, 1).

◆ PlanarArmController() [2/2]

template<unsigned int N>
control::PlanarArmController< N >::PlanarArmController ( PlanarArmController< N > && other)
inline

Constructs a copy of an existing PlanarArmController object.

Parameters
otherThe existing PlanarArmController to copy.

Member Function Documentation

◆ get_setpoint()

template<unsigned int N>
Eigen::Vector2d control::PlanarArmController< N >::get_setpoint ( robot::types::datatime_t currTime)
inline

Gets the current end effector setpoint / target position.

Parameters
currTimeThe current timestamp.
Returns
The target end effector position.

◆ getCommand()

template<unsigned int N>
navtypes::Vectord< N > control::PlanarArmController< N >::getCommand ( robot::types::datatime_t currTime,
const navtypes::Vectord< N > & currJointPos )
inline

Get the current command for the arm.

Parameters
currTimeThe current timestamp.
currJointPosThe current joint positions.
Returns
The new command, which is the new joint positions to get to the new target end effector position.

◆ is_setpoint_valid()

template<unsigned int N>
static bool control::PlanarArmController< N >::is_setpoint_valid ( const navtypes::Vectord< N > & targetJointPos,
kinematics::ArmKinematics< 2, N > kin_obj,
double safetyFactor )
inlinestatic

Returns whether the target joint positions are within the arm controller's radius limit.

Parameters
targetJointPosThe target joint positions.
Returns
whether the target joint positions are within the arm controller's radius limit.

◆ set_setpoint()

template<unsigned int N>
void control::PlanarArmController< N >::set_setpoint ( const navtypes::Vectord< N > & targetJointPos)
inline

Sets the end effector setpoint / target position.

Parameters
targetJointPosThe target joint positions.

◆ set_x_vel()

template<unsigned int N>
void control::PlanarArmController< N >::set_x_vel ( robot::types::datatime_t currTime,
double targetVel,
const navtypes::Vectord< N > & jointPos )
inline

Sets the x velocity for the end effector and returns the new command.

Parameters
currTimeThe current timestamp.
targetVelThe target x velocity.
jointPosThe current joint angles of the arm.
Returns
The new command, which is the new joint positions.

◆ set_y_vel()

template<unsigned int N>
void control::PlanarArmController< N >::set_y_vel ( robot::types::datatime_t currTime,
double targetVel,
const navtypes::Vectord< N > & jointPos )
inline

Sets the y velocity for the end effector and returns the new command.

Parameters
currTimeThe current timestamp.
targetVelThe target y velocity.
jointPosThe current joint angles of the arm.
Returns
The new command, which is the new joint positions.

◆ tryInitController()

template<unsigned int N>
bool control::PlanarArmController< N >::tryInitController ( const navtypes::Vectord< N > & currJointPos)
inline

Instantiates the PlanarArmController with the current joint positions, returning true if the joint positions are valid.

If PlanarArmController is already initialized, the PlanarArmController is reinitialized with the supplied positions and this function returns true. Otherwise, controller gets uninitialized and function returns false.

Parameters
currJointPosThe current joint positions of the arm.
Returns
true iff the joint positions are within the robot's maximum arm extension radius and the controller was successfully initialized in the function call.

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