JointPathEx.h
Go to the documentation of this file.
00001 #ifndef __JOINT_PATH_EX_H__
00002 #define __JOINT_PATH_EX_H__
00003 #include <hrpModel/Body.h>
00004 #include <hrpModel/Link.h>
00005 #include <hrpModel/JointPath.h>
00006 #include <cmath>
00007 #include <coil/stringutil.h>
00008 
00009 // hrplib/hrpUtil/MatrixSolvers.h
00010 namespace hrp {
00011     int calcSRInverse(const dmatrix& _a, dmatrix &_a_sr, double _sr_ratio = 1.0, dmatrix _w = dmatrix::Identity(0,0));
00012 };
00013 
00014 // hrplib/hrpModel/JointPath.h
00015 namespace hrp {
00016     class JointPathEx : public JointPath {
00017   public:
00018     JointPathEx(BodyPtr& robot, Link* base, Link* end, double control_cycle, bool _use_inside_joint_weight_retrieval = true, const std::string& _debug_print_prefix = "");
00019     bool calcJacobianInverseNullspace(dmatrix &J, dmatrix &Jinv, dmatrix &Jnull);
00020     bool calcInverseKinematics2Loop(const Vector3& dp, const Vector3& omega, const double LAMBDA, const double avoid_gain = 0.0, const double reference_gain = 0.0, const dvector* reference_q = NULL);
00021     bool calcInverseKinematics2Loop(const Vector3& end_effector_p, const Matrix33& end_effector_R,
00022                                     const double LAMBDA, const double avoid_gain = 0.0, const double reference_gain = 0.0, const hrp::dvector* reference_q = NULL,
00023                                     const double vel_gain = 1.0,
00024                                     const hrp::Vector3& localPos = hrp::Vector3::Zero(), const hrp::Matrix33& localR = hrp::Matrix33::Identity());
00025     bool calcInverseKinematics2(const Vector3& end_p, const Matrix33& end_R, const double avoid_gain = 0.0, const double reference_gain = 0.0, const dvector* reference_q = NULL);
00026     double getSRGain() { return sr_gain; }
00027     bool setSRGain(double g) { sr_gain = g; }
00028     double getManipulabilityLimit() { return manipulability_limit; }
00029     bool setManipulabilityLimit(double l) { manipulability_limit = l; }
00030     bool setManipulabilityGain(double l) { manipulability_gain = l; }
00031     void setMaxIKError(double epos, double erot);
00032     void setMaxIKError(double e);
00033     void setMaxIKIteration(int iter);
00034     void setOptionalWeightVector(const std::vector<double>& _opt_w)
00035     {
00036         for (unsigned int i = 0 ; i < numJoints(); i++ ) {
00037             optional_weight_vector[i] = _opt_w[i];
00038         }
00039     };
00040     bool setInterlockingJointPairIndices (const std::vector<std::pair<Link*, Link*> >& pairs, const std::string& print_str = "");
00041     bool setInterlockingJointPairIndices (const std::vector<std::pair<size_t, size_t> >& pairs);
00042     void getInterlockingJointPairIndices (std::vector<std::pair<size_t, size_t> >& pairs);
00043     void getOptionalWeightVector(std::vector<double>& _opt_w)
00044     {
00045         for (unsigned int i = 0 ; i < numJoints(); i++ ) {
00046             _opt_w[i] = optional_weight_vector[i];
00047         }
00048     };
00049   protected:
00050         double maxIKPosErrorSqr, maxIKRotErrorSqr;
00051         int maxIKIteration;
00052         std::vector<Link*> joints;
00053         std::vector<double> avoid_weight_gain, optional_weight_vector;
00054         // Interlocking joint pairs
00055         //  pair = [index of joint1, index of joint2], index is considered as index for "joints[index]"
00056         //  Joint angles of joint1 and joint2 has relathionships.
00057         //  Currently joint1 = joint2 is assumed.
00058         std::vector<std::pair<size_t, size_t> > interlocking_joint_pair_indices;
00059         double sr_gain, manipulability_limit, manipulability_gain, dt;
00060         std::string debug_print_prefix;
00061         // Print message Hz management
00062         std::vector<size_t> joint_limit_debug_print_counts;
00063         size_t debug_print_freq_count;
00064         bool use_inside_joint_weight_retrieval;
00065     };
00066 
00067     typedef boost::shared_ptr<JointPathEx> JointPathExPtr;
00068 
00069     struct VirtualForceSensorParam {
00070         int id;
00071         hrp::Link* link;
00072         hrp::Vector3 localPos;
00073         hrp::Matrix33 localR;
00074     };
00075 
00076     void readVirtualForceSensorParamFromProperties (std::map<std::string, hrp::VirtualForceSensorParam>& vfs,
00077                                                     hrp::BodyPtr m_robot,
00078                                                     const std::string& prop_string,
00079                                                     const std::string& instance_name);
00080 
00081     void readInterlockingJointsParamFromProperties (std::vector<std::pair<Link*, Link*> >& pairs,
00082                                                     hrp::BodyPtr m_robot,
00083                                                     const std::string& prop_string,
00084                                                     const std::string& instance_name);
00085 };
00086 
00087 
00088 namespace hrp {
00089   class InvDynStateBuffer{
00090     public:
00091       int N_DOF;
00092       bool is_initialized;
00093       double DT;
00094       hrp::dvector q, q_old, q_oldold, dq, ddq;
00095       hrp::Vector3 base_p, base_p_old, base_p_oldold, base_v, base_dv;
00096       hrp::Matrix33 base_R, base_R_old, base_dR, base_w_hat;
00097       hrp::Vector3 base_w, base_w_old, base_dw;
00098       InvDynStateBuffer():is_initialized(false){};
00099       ~InvDynStateBuffer(){};
00100       void setInitState(const hrp::BodyPtr _m_robot, const double _dt){
00101         N_DOF = _m_robot->numJoints();
00102         DT = _dt;
00103         q.resize(N_DOF);
00104         q_old.resize(N_DOF);
00105         q_oldold.resize(N_DOF);
00106         dq.resize(N_DOF);
00107         ddq.resize(N_DOF);
00108         for(int i=0;i<N_DOF;i++)q(i) = _m_robot->joint(i)->q;
00109         q_oldold = q_old = q;
00110         dq = ddq = hrp::dvector::Zero(N_DOF);
00111         base_p_oldold = base_p_old = base_p = _m_robot->rootLink()->p;
00112         base_R_old = base_R = _m_robot->rootLink()->R;
00113         base_dR = base_w_hat = hrp::Matrix33::Zero();
00114         base_w_old = base_w = base_dw = hrp::Vector3::Zero();
00115         is_initialized = true;
00116       };
00117   };
00118   // set current Body q,base_p,base_R into InvDynStateBuffer and update vel and acc
00119   void calcAccelerationsForInverseDynamics(const hrp::BodyPtr _m_robot, InvDynStateBuffer& _idsb);
00120   // set all vel and acc into Body, and call Body::calcInverseDynamics()
00121   void calcRootLinkWrenchFromInverseDynamics(hrp::BodyPtr _m_robot, InvDynStateBuffer& _idsb, hrp::Vector3& _f_ans, hrp::Vector3& _t_ans);
00122   // call calcRootLinkWrenchFromInverseDynamics() and convert f,tau into ZMP
00123   void calcWorldZMPFromInverseDynamics(hrp::BodyPtr _m_robot, InvDynStateBuffer& _idsb, hrp::Vector3& _zmp_ans);
00124   // increment InvDynStateBuffer for 1 step
00125   void updateInvDynStateBuffer(InvDynStateBuffer& _idsb);
00126 }
00127 
00128 #include <iomanip>
00129 
00130 #endif //__JOINT_PATH_EX_H__


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed Sep 6 2017 02:35:55