#include <Cartesian_HybCntrl.h>
Classes | |
struct | ForceControl |
defines the current force control in the force control frame More... | |
Public Member Functions | |
Cartesian_HybCntrl () | |
void | createForceControlMap (std::map< std::string, std::pair< std::vector< int >, std::vector< double > > > forceNodes) |
creates the map between node name, axis index, and desired force magnitude | |
double | edgeLimit (double vel, double prev, double input) |
void | limitPoseError (KDL::Frame &output_pose, KDL::Frame input_pose) |
void | printFrame (std::string, KDL::Frame) |
void | printWrench (std::string, KDL::Wrench) |
void | setFilter (double filterAlpha) |
void | setGain (double forceP, double torqueP, double forceI, double torqueI, double forceD, double torqueD, double forceWindup, double torqueWindup, double positionErrorLimit_in, double rotationErrorLimit_in) |
defines the default gains for force control | |
bool | setInitialPose (const std::string &frameName, const ::KDL::Frame &pose) |
void | setPoseSettings (double linVel, double angVel, double linAcc, double angAcc) |
defines the maximum velocities and accelerations for force control | |
void | setSensorNameMap (const std::map< std::string, std::string > &sensorNameMap) |
replaces the current sensor-name map with a new map | |
virtual void | updateFrames (std::vector< std::string > nodeNames, std::map< std::string, KDL::Frame > referenceFrameMap, std::vector< KDL::Frame > &nodeFrames, double dt) |
void | updateSensorForces (const std::map< std::string, KDL::Wrench > &sensorForces) |
~Cartesian_HybCntrl () | |
Public Attributes | |
bool | doFeedForward |
Protected Member Functions | |
std::string | getSensorNameFromNode (const std::string &nodeName) |
void | limitVector (double minVal, double maxVal, KDL::Vector &vector) |
void | limitVelocity (const KDL::Twist &pastVelocity, KDL::Twist &currVelocity) |
Protected Attributes | |
double | alpha |
boost::shared_ptr< RateLimiter > | angularRateLimiter |
double | fKd |
double | fKi |
double | fKp |
std::map< std::string, ForceControl > | forceCntrlMap |
double | forceIntegralWindupLimit |
std::map< std::string, ForceControl >::iterator | forceItr |
boost::shared_ptr< RateLimiter > | linearRateLimiter |
double | maxAngularVelocity |
double | maxLinearVelocity |
double | positionErrorLimit |
double | rotationErrorLimit |
std::map< std::string, KDL::Wrench >::const_iterator | sensorForceItr |
std::map< std::string, KDL::Wrench > | sensorForceMap |
std::map< std::string, std::string > | sensorNameMap |
double | tKd |
double | tKi |
double | tKp |
double | torqueIntegralWindupLimit |
Private Attributes | |
KDL::Frame | desiredPose |
double | edgeBuffer |
KDL::Wrench | forceError |
KDL::Wrench | forcePID |
std::string | logCategory |
KDL::Frame | poseFrame |
KDL::Wrench | sensorForce |
KDL::Frame | sensorFrame |
KDL::Twist | velocity |
KDL::Wrench | wrenchDesired |
KDL::Wrench | wrenchPoseFrame |
KDL::Wrench | wrenchSensorFrame |
Definition at line 22 of file Cartesian_HybCntrl.h.
Definition at line 3 of file Cartesian_HybCntrl.cpp.
Definition at line 10 of file Cartesian_HybCntrl.cpp.
void Cartesian_HybCntrl::createForceControlMap | ( | std::map< std::string, std::pair< std::vector< int >, std::vector< double > > > | forceNodes | ) |
creates the map between node name, axis index, and desired force magnitude
Definition at line 87 of file Cartesian_HybCntrl.cpp.
double Cartesian_HybCntrl::edgeLimit | ( | double | vel, |
double | prev, | ||
double | input | ||
) |
Definition at line 371 of file Cartesian_HybCntrl.cpp.
std::string Cartesian_HybCntrl::getSensorNameFromNode | ( | const std::string & | nodeName | ) | [protected] |
if sensor name in map, return sensor name
otherwise return empty string
Definition at line 449 of file Cartesian_HybCntrl.cpp.
void Cartesian_HybCntrl::limitPoseError | ( | KDL::Frame & | output_pose, |
KDL::Frame | input_pose | ||
) |
Definition at line 386 of file Cartesian_HybCntrl.cpp.
void Cartesian_HybCntrl::limitVector | ( | double | minVal, |
double | maxVal, | ||
KDL::Vector & | vector | ||
) | [protected] |
Definition at line 428 of file Cartesian_HybCntrl.cpp.
void Cartesian_HybCntrl::limitVelocity | ( | const KDL::Twist & | pastVelocity, |
KDL::Twist & | currVelocity | ||
) | [protected] |
Definition at line 438 of file Cartesian_HybCntrl.cpp.
void Cartesian_HybCntrl::printFrame | ( | std::string | name, |
KDL::Frame | frame | ||
) |
Definition at line 145 of file Cartesian_HybCntrl.cpp.
void Cartesian_HybCntrl::printWrench | ( | std::string | name, |
KDL::Wrench | wrench | ||
) |
Definition at line 167 of file Cartesian_HybCntrl.cpp.
void Cartesian_HybCntrl::setFilter | ( | double | filterAlpha | ) |
Definition at line 45 of file Cartesian_HybCntrl.cpp.
void Cartesian_HybCntrl::setGain | ( | double | forceP, |
double | torqueP, | ||
double | forceI, | ||
double | torqueI, | ||
double | forceD, | ||
double | torqueD, | ||
double | forceWindup, | ||
double | torqueWindup, | ||
double | positionErrorLimit_in, | ||
double | rotationErrorLimit_in | ||
) |
defines the default gains for force control
Definition at line 15 of file Cartesian_HybCntrl.cpp.
bool Cartesian_HybCntrl::setInitialPose | ( | const std::string & | frameName, |
const ::KDL::Frame & | pose | ||
) |
Definition at line 72 of file Cartesian_HybCntrl.cpp.
void Cartesian_HybCntrl::setPoseSettings | ( | double | linVel, |
double | angVel, | ||
double | linAcc, | ||
double | angAcc | ||
) |
defines the maximum velocities and accelerations for force control
Definition at line 34 of file Cartesian_HybCntrl.cpp.
void Cartesian_HybCntrl::setSensorNameMap | ( | const std::map< std::string, std::string > & | sensorNameMap | ) |
replaces the current sensor-name map with a new map
Definition at line 52 of file Cartesian_HybCntrl.cpp.
void Cartesian_HybCntrl::updateFrames | ( | std::vector< std::string > | nodeNames, |
std::map< std::string, KDL::Frame > | referenceFrameMap, | ||
std::vector< KDL::Frame > & | nodeFrames, | ||
double | dt | ||
) | [virtual] |
get latest sensor forces
If this is a force controlled node, modify the force controlled axes
get the force frame from base
get the sensor frame from base
Definition at line 182 of file Cartesian_HybCntrl.cpp.
void Cartesian_HybCntrl::updateSensorForces | ( | const std::map< std::string, KDL::Wrench > & | sensorForces | ) |
go through sensorForces and update map (additive, more than one topic supplies forces)
if sensor force is not in map, initialize to zero
Definition at line 57 of file Cartesian_HybCntrl.cpp.
double Cartesian_HybCntrl::alpha [protected] |
Definition at line 98 of file Cartesian_HybCntrl.h.
boost::shared_ptr<RateLimiter> Cartesian_HybCntrl::angularRateLimiter [protected] |
Definition at line 85 of file Cartesian_HybCntrl.h.
KDL::Frame Cartesian_HybCntrl::desiredPose [private] |
Definition at line 110 of file Cartesian_HybCntrl.h.
Definition at line 76 of file Cartesian_HybCntrl.h.
double Cartesian_HybCntrl::edgeBuffer [private] |
Definition at line 118 of file Cartesian_HybCntrl.h.
double Cartesian_HybCntrl::fKd [protected] |
Definition at line 90 of file Cartesian_HybCntrl.h.
double Cartesian_HybCntrl::fKi [protected] |
Definition at line 88 of file Cartesian_HybCntrl.h.
double Cartesian_HybCntrl::fKp [protected] |
Definition at line 86 of file Cartesian_HybCntrl.h.
std::map<std::string, ForceControl> Cartesian_HybCntrl::forceCntrlMap [protected] |
Definition at line 100 of file Cartesian_HybCntrl.h.
KDL::Wrench Cartesian_HybCntrl::forceError [private] |
Definition at line 117 of file Cartesian_HybCntrl.h.
double Cartesian_HybCntrl::forceIntegralWindupLimit [protected] |
Definition at line 94 of file Cartesian_HybCntrl.h.
std::map<std::string, ForceControl>::iterator Cartesian_HybCntrl::forceItr [protected] |
Definition at line 102 of file Cartesian_HybCntrl.h.
KDL::Wrench Cartesian_HybCntrl::forcePID [private] |
Definition at line 109 of file Cartesian_HybCntrl.h.
boost::shared_ptr<RateLimiter> Cartesian_HybCntrl::linearRateLimiter [protected] |
Definition at line 84 of file Cartesian_HybCntrl.h.
std::string Cartesian_HybCntrl::logCategory [private] |
Definition at line 107 of file Cartesian_HybCntrl.h.
double Cartesian_HybCntrl::maxAngularVelocity [protected] |
Definition at line 93 of file Cartesian_HybCntrl.h.
double Cartesian_HybCntrl::maxLinearVelocity [protected] |
Definition at line 92 of file Cartesian_HybCntrl.h.
KDL::Frame Cartesian_HybCntrl::poseFrame [private] |
Definition at line 112 of file Cartesian_HybCntrl.h.
double Cartesian_HybCntrl::positionErrorLimit [protected] |
Definition at line 96 of file Cartesian_HybCntrl.h.
double Cartesian_HybCntrl::rotationErrorLimit [protected] |
Definition at line 97 of file Cartesian_HybCntrl.h.
KDL::Wrench Cartesian_HybCntrl::sensorForce [private] |
Definition at line 108 of file Cartesian_HybCntrl.h.
std::map<std::string, KDL::Wrench>::const_iterator Cartesian_HybCntrl::sensorForceItr [protected] |
Definition at line 104 of file Cartesian_HybCntrl.h.
std::map<std::string, KDL::Wrench> Cartesian_HybCntrl::sensorForceMap [protected] |
Definition at line 103 of file Cartesian_HybCntrl.h.
KDL::Frame Cartesian_HybCntrl::sensorFrame [private] |
Definition at line 113 of file Cartesian_HybCntrl.h.
std::map<std::string, std::string> Cartesian_HybCntrl::sensorNameMap [protected] |
Definition at line 101 of file Cartesian_HybCntrl.h.
double Cartesian_HybCntrl::tKd [protected] |
Definition at line 91 of file Cartesian_HybCntrl.h.
double Cartesian_HybCntrl::tKi [protected] |
Definition at line 89 of file Cartesian_HybCntrl.h.
double Cartesian_HybCntrl::tKp [protected] |
Definition at line 87 of file Cartesian_HybCntrl.h.
double Cartesian_HybCntrl::torqueIntegralWindupLimit [protected] |
Definition at line 95 of file Cartesian_HybCntrl.h.
KDL::Twist Cartesian_HybCntrl::velocity [private] |
Definition at line 111 of file Cartesian_HybCntrl.h.
KDL::Wrench Cartesian_HybCntrl::wrenchDesired [private] |
Definition at line 114 of file Cartesian_HybCntrl.h.
Definition at line 116 of file Cartesian_HybCntrl.h.
Definition at line 115 of file Cartesian_HybCntrl.h.