Go to the documentation of this file.00001 #ifndef CARTESIAN_HYBCNTRL_H
00002 #define CARTESIAN_HYBCNTRL_H
00003
00004 #include <kdl/treefksolverpos_recursive.hpp>
00005 #include <kdl/treefksolver.hpp>
00006 #include <kdl/jntarrayvel.hpp>
00007 #include <kdl/framevel.hpp>
00008 #include <kdl/frames.hpp>
00009
00010 #include <iostream>
00011 #include <Eigen/Core>
00012
00013 #include "robodyn_controllers/JointTrajectoryFollower.h"
00014 #include "robodyn_controllers/KdlTreeUtilities.h"
00015 #include "robodyn_utilities/RateLimiter.h"
00016 #include "r2_msgs/ForceControlAxis.h"
00017 #include "robodyn_utilities/GeneralUtilities.h"
00018
00019 using namespace std;
00020 using namespace Eigen;
00021
00022 class Cartesian_HybCntrl
00023 {
00024 public:
00025
00027 struct ForceControl
00028 {
00029 std::string sensorName;
00030 bool x;
00031 bool y;
00032 bool z;
00033 bool roll;
00034 bool pitch;
00035 bool yaw;
00036 KDL::Wrench desiredForce;
00037 KDL::Wrench sensorForce;
00038 KDL::Wrench integrator;
00039 KDL::Wrench derivative;
00040 KDL::Wrench gain;
00041 KDL::Wrench error;
00042 KDL::Twist prevVelocity;
00043 KDL::Frame pose;
00044 ForceControl()
00045 {
00046 x = y = z = roll = pitch = yaw = false;
00047 desiredForce = KDL::Wrench::Zero();
00048 sensorForce = KDL::Wrench::Zero();
00049 integrator = KDL::Wrench::Zero();
00050 derivative = KDL::Wrench::Zero();
00051 gain = KDL::Wrench::Zero();
00052 error = KDL::Wrench::Zero();
00053 prevVelocity = KDL::Twist::Zero();
00054 pose = KDL::Frame::Identity();
00055 }
00056 };
00057
00058 Cartesian_HybCntrl();
00059 ~Cartesian_HybCntrl();
00060
00061 void setGain(double forceP, double torqueP, double forceI, double torqueI, double forceD, double torqueD, double forceWindup, double torqueWindup, double positionErrorLimit_in, double rotationErrorLimit_in);
00062 void setPoseSettings(double linVel, double angVel, double linAcc, double angAcc);
00063 void setFilter(double filterAlpha);
00064 void setSensorNameMap(const std::map<std::string, std::string>& sensorNameMap);
00065 void updateSensorForces(const std::map<std::string, KDL::Wrench>& sensorForces);
00066 bool setInitialPose(const std::string& frameName, const::KDL::Frame& pose);
00067 void createForceControlMap(std::map<std::string, std::pair<std::vector<int>, std::vector<double> > > forceNodes);
00068 void printFrame(std::string, KDL::Frame);
00069 void printWrench(std::string, KDL::Wrench);
00070 void limitPoseError(KDL::Frame &output_pose, KDL::Frame input_pose);
00071 double edgeLimit(double vel, double prev, double input);
00072
00073
00074 virtual void updateFrames(std::vector<std::string> nodeNames, std::map<std::string, KDL::Frame> referenceFrameMap, std::vector<KDL::Frame>& nodeFrames, double dt);
00075
00076 bool doFeedForward;
00077
00078 protected:
00079
00080 void limitVector(double minVal, double maxVal, KDL::Vector& vector);
00081 void limitVelocity(const KDL::Twist& pastVelocity, KDL::Twist& currVelocity);
00082 std::string getSensorNameFromNode(const std::string& nodeName);
00083
00084 boost::shared_ptr<RateLimiter> linearRateLimiter;
00085 boost::shared_ptr<RateLimiter> angularRateLimiter;
00086 double fKp;
00087 double tKp;
00088 double fKi;
00089 double tKi;
00090 double fKd;
00091 double tKd;
00092 double maxLinearVelocity;
00093 double maxAngularVelocity;
00094 double forceIntegralWindupLimit;
00095 double torqueIntegralWindupLimit;
00096 double positionErrorLimit;
00097 double rotationErrorLimit;
00098 double alpha;
00099
00100 std::map<std::string, ForceControl> forceCntrlMap;
00101 std::map<std::string, std::string> sensorNameMap;
00102 std::map<std::string, ForceControl>::iterator forceItr;
00103 std::map<std::string, KDL::Wrench> sensorForceMap;
00104 std::map<std::string, KDL::Wrench>::const_iterator sensorForceItr;
00105
00106 private:
00107 std::string logCategory;
00108 KDL::Wrench sensorForce;
00109 KDL::Wrench forcePID;
00110 KDL::Frame desiredPose;
00111 KDL::Twist velocity;
00112 KDL::Frame poseFrame;
00113 KDL::Frame sensorFrame;
00114 KDL::Wrench wrenchDesired;
00115 KDL::Wrench wrenchSensorFrame;
00116 KDL::Wrench wrenchPoseFrame;
00117 KDL::Wrench forceError;
00118 double edgeBuffer;
00119
00120 };
00121
00122 #endif // CARTESIAN_HYBCNTRL_H