75 lines
2.4 KiB
C++
75 lines
2.4 KiB
C++
#ifndef ARMMODEL_H
|
|
#define ARMMODEL_H
|
|
|
|
#include <vector>
|
|
#include "common/math/robotics.h"
|
|
#include "thirdparty/quadProgpp/QuadProg++.hh"
|
|
|
|
using namespace robo;
|
|
|
|
class ArmModel{
|
|
public:
|
|
ArmModel(Vec3 endPosLocal, double endEffectorMass, Vec3 endEffectorCom, Mat3 endEffectorInertia);
|
|
~ArmModel(){};
|
|
|
|
HomoMat forwardKinematics(Vec6 q, int index = 6);
|
|
virtual bool inverseKinematics(HomoMat TDes, Vec6 qPast, Vec6& q_result, bool checkInWorkSpace = false);
|
|
Mat6 CalcJacobian(Vec6 q);
|
|
Vec6 inverseDynamics(Vec6 q, Vec6 qd, Vec6 qdd, Vec6 Ftip);
|
|
virtual void solveQP(Vec6 twist, Vec6 qPast, Vec6& qd_result, double dt) = 0;
|
|
|
|
virtual bool checkInSingularity(Vec6 q) = 0;
|
|
void jointProtect(Vec6& q, Vec6& qd);
|
|
|
|
std::vector<double> getJointQMax() {return _jointQMax;}
|
|
std::vector<double> getJointQMin() {return _jointQMin;}
|
|
std::vector<double> getJointSpeedMax() {return _jointSpeedMax;}
|
|
void addLoad(double load);
|
|
|
|
const size_t dof = 6;
|
|
protected:
|
|
bool _checkAngle(Vec6 );
|
|
void _buildModel();
|
|
// initial parameters
|
|
HomoMat _M; //End posture at the home position
|
|
std::vector<HomoMat> _Mlist;// List of link frames {i} relative to {i-1} at the home position
|
|
Vec3 _gravity;
|
|
Mat6 _Slist;// spatial twist at home position
|
|
std::vector<Mat6> _Glist;
|
|
std::vector<Vec3> _jointAxis;// omega
|
|
std::vector<Vec3> _jointPos;// p_0
|
|
std::vector<double> _jointQMax;
|
|
std::vector<double> _jointQMin;
|
|
std::vector<double> _jointSpeedMax;
|
|
Vec3 _endPosLocal; // based on mount postion
|
|
Vec3 _endMountPosGlobal;
|
|
|
|
std::vector<Vec3> _linkPosLocal;
|
|
std::vector<double> _disJoint;//distance between joint
|
|
std::vector<double> _linkMass;
|
|
std::vector<Vec3> _linkCom; // center of mass
|
|
std::vector<Mat3> _linkInertia;
|
|
double _endEffectorMass;
|
|
Vec3 _endEffectorCom;
|
|
Mat3 _endEffectorInertia;
|
|
|
|
double _load;
|
|
};
|
|
|
|
class Z1Model : public ArmModel{
|
|
public:
|
|
Z1Model(Vec3 endPosLocal = Vec3::Zero(), double endEffectorMass = 0.0,
|
|
Vec3 endEffectorCom = Vec3::Zero(), Mat3 endEffectorInertia = Mat3::Zero());
|
|
~Z1Model(){};
|
|
|
|
bool checkInSingularity(Vec6 q);
|
|
bool inverseKinematics(HomoMat TDes, Vec6 qPast, Vec6& q_result, bool checkInWorkSpace = false);
|
|
void solveQP(Vec6 twist, Vec6 qPast, Vec6& qd_result, double dt);
|
|
private:
|
|
void setParam_V3_6();
|
|
void setParam_V3_7();//long
|
|
void setParam_V3_8();
|
|
double _theta2Bias;
|
|
};
|
|
|
|
#endif |