Resurgence (PY2022)
Codebase for the Husky Robotics 2021-2022 rover Resurgence
|
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. | |
Controller to move planar arm to a target end effector position.
This class is thread-safe.
N | The number of arm joints. |
|
inline |
Construct a new controller object.
currJointPos | The current joint positions of the arm. |
kin_obj | ArmKinematics object for the arm (should have the same number of arm joints). |
safetyFactor | the percentage factor to scale maximum arm extension radius by to prevent singularity lock. Must be in range (0, 1). |
|
inline |
Constructs a copy of an existing PlanarArmController object.
other | The existing PlanarArmController to copy. |
|
inline |
Gets the current end effector setpoint / target position.
currTime | The current timestamp. |
|
inline |
Get the current command for the arm.
currTime | The current timestamp. |
currJointPos | The current joint positions. |
|
inlinestatic |
Returns whether the target joint positions are within the arm controller's radius limit.
targetJointPos | The target joint positions. |
|
inline |
Sets the end effector setpoint / target position.
targetJointPos | The target joint positions. |
|
inline |
Sets the x velocity for the end effector and returns the new command.
currTime | The current timestamp. |
targetVel | The target x velocity. |
jointPos | The current joint angles of the arm. |
|
inline |
Sets the y velocity for the end effector and returns the new command.
currTime | The current timestamp. |
targetVel | The target y velocity. |
jointPos | The current joint angles of the arm. |
|
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.
currJointPos | The current joint positions of the arm. |