JointPathEx.h
Go to the documentation of this file.
1 #ifndef __JOINT_PATH_EX_H__
2 #define __JOINT_PATH_EX_H__
3 #include <hrpModel/Body.h>
4 #include <hrpModel/Link.h>
5 #include <hrpModel/JointPath.h>
6 #include <cmath>
7 #include <coil/stringutil.h>
8 
9 // hrplib/hrpUtil/MatrixSolvers.h
10 namespace hrp {
11  int calcSRInverse(const dmatrix& _a, dmatrix &_a_sr, double _sr_ratio = 1.0, dmatrix _w = dmatrix::Identity(0,0));
12 };
13 
14 // hrplib/hrpModel/JointPath.h
15 namespace hrp {
16  class JointPathEx : public JointPath {
17  public:
18  JointPathEx(BodyPtr& robot, Link* base, Link* end, double control_cycle, bool _use_inside_joint_weight_retrieval = true, const std::string& _debug_print_prefix = "");
19  bool calcJacobianInverseNullspace(dmatrix &J, dmatrix &Jinv, dmatrix &Jnull);
20  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);
21  bool calcInverseKinematics2Loop(const Vector3& end_effector_p, const Matrix33& end_effector_R,
22  const double LAMBDA, const double avoid_gain = 0.0, const double reference_gain = 0.0, const hrp::dvector* reference_q = NULL,
23  const double vel_gain = 1.0,
24  const hrp::Vector3& localPos = hrp::Vector3::Zero(), const hrp::Matrix33& localR = hrp::Matrix33::Identity());
25  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);
26  double getSRGain() { return sr_gain; }
27  bool setSRGain(double g) { sr_gain = g; return true; }
29  bool setManipulabilityLimit(double l) { manipulability_limit = l; return true; }
30  bool setManipulabilityGain(double l) { manipulability_gain = l; return true; }
31  void setMaxIKError(double epos, double erot);
32  void setMaxIKError(double e);
33  void setMaxIKIteration(int iter);
34  void setOptionalWeightVector(const std::vector<double>& _opt_w)
35  {
36  for (unsigned int i = 0 ; i < numJoints(); i++ ) {
37  optional_weight_vector[i] = _opt_w[i];
38  }
39  };
40  bool setInterlockingJointPairIndices (const std::vector<std::pair<Link*, Link*> >& pairs, const std::string& print_str = "");
41  bool setInterlockingJointPairIndices (const std::vector<std::pair<size_t, size_t> >& pairs);
42  void getInterlockingJointPairIndices (std::vector<std::pair<size_t, size_t> >& pairs);
43  void getOptionalWeightVector(std::vector<double>& _opt_w)
44  {
45  for (unsigned int i = 0 ; i < numJoints(); i++ ) {
46  _opt_w[i] = optional_weight_vector[i];
47  }
48  };
49  protected:
52  std::vector<Link*> joints;
54  // Interlocking joint pairs
55  // pair = [index of joint1, index of joint2], index is considered as index for "joints[index]"
56  // Joint angles of joint1 and joint2 has relathionships.
57  // Currently joint1 = joint2 is assumed.
58  std::vector<std::pair<size_t, size_t> > interlocking_joint_pair_indices;
60  std::string debug_print_prefix;
61  // Print message Hz management
62  std::vector<size_t> joint_limit_debug_print_counts;
65  };
66 
68 
70  int id;
74  };
75 
76  void readVirtualForceSensorParamFromProperties (std::map<std::string, hrp::VirtualForceSensorParam>& vfs,
78  const std::string& prop_string,
79  const std::string& instance_name);
80 
81  void readInterlockingJointsParamFromProperties (std::vector<std::pair<Link*, Link*> >& pairs,
83  const std::string& prop_string,
84  const std::string& instance_name);
85 };
86 
87 
88 namespace hrp {
90  public:
91  int N_DOF;
93  double DT;
94  hrp::dvector q, q_old, q_oldold, dq, ddq;
95  hrp::Vector3 base_p, base_p_old, base_p_oldold, base_v, base_dv;
96  hrp::Matrix33 base_R, base_R_old, base_dR, base_w_hat;
97  hrp::Vector3 base_w, base_w_old, base_dw;
98  InvDynStateBuffer():is_initialized(false){};
100  void setInitState(const hrp::BodyPtr _m_robot, const double _dt){
101  N_DOF = _m_robot->numJoints();
102  DT = _dt;
103  q.resize(N_DOF);
104  q_old.resize(N_DOF);
105  q_oldold.resize(N_DOF);
106  dq.resize(N_DOF);
107  ddq.resize(N_DOF);
108  for(int i=0;i<N_DOF;i++)q(i) = _m_robot->joint(i)->q;
109  q_oldold = q_old = q;
110  dq = ddq = hrp::dvector::Zero(N_DOF);
111  base_p_oldold = base_p_old = base_p = _m_robot->rootLink()->p;
112  base_R_old = base_R = _m_robot->rootLink()->R;
113  base_dR = base_w_hat = hrp::Matrix33::Zero();
114  base_w_old = base_w = base_dw = hrp::Vector3::Zero();
115  is_initialized = true;
116  };
117  };
118  // set current Body q,base_p,base_R into InvDynStateBuffer and update vel and acc
120  // set all vel and acc into Body, and call Body::calcInverseDynamics()
122  // call calcRootLinkWrenchFromInverseDynamics() and convert f,tau into ZMP
124  // increment InvDynStateBuffer for 1 step
126 }
127 
128 #include <iomanip>
129 
130 #endif //__JOINT_PATH_EX_H__
bool calcJacobianInverseNullspace(dmatrix &J, dmatrix &Jinv, dmatrix &Jnull)
double maxIKRotErrorSqr
Definition: JointPathEx.h:48
std::vector< Link * > joints
Definition: JointPathEx.h:52
Eigen::MatrixXd dmatrix
double getSRGain()
Definition: JointPathEx.h:26
bool setInterlockingJointPairIndices(const std::vector< std::pair< Link *, Link *> > &pairs, const std::string &print_str="")
JointPathEx(BodyPtr &robot, Link *base, Link *end, double control_cycle, bool _use_inside_joint_weight_retrieval=true, const std::string &_debug_print_prefix="")
Definition: JointPathEx.cpp:92
std::vector< std::pair< size_t, size_t > > interlocking_joint_pair_indices
Definition: JointPathEx.h:58
boost::shared_ptr< JointPathEx > JointPathExPtr
Definition: JointPathEx.h:67
png_uint_32 i
void setOptionalWeightVector(const std::vector< double > &_opt_w)
Definition: JointPathEx.h:34
Eigen::VectorXd dvector
std::vector< double > avoid_weight_gain
Definition: JointPathEx.h:53
double getManipulabilityLimit()
Definition: JointPathEx.h:28
void calcRootLinkWrenchFromInverseDynamics(hrp::BodyPtr _m_robot, InvDynStateBuffer &_idsb, hrp::Vector3 &_f_ans, hrp::Vector3 &_t_ans)
Eigen::Vector3d Vector3
hrp::Vector3 base_v
Definition: JointPathEx.h:95
Eigen::Matrix3d Matrix33
void setMaxIKIteration(int iter)
void calcAccelerationsForInverseDynamics(const hrp::BodyPtr _m_robot, InvDynStateBuffer &_idsb)
std::vector< size_t > joint_limit_debug_print_counts
Definition: JointPathEx.h:62
int calcSRInverse(const dmatrix &_a, dmatrix &_a_sr, double _sr_ratio, dmatrix _w)
Definition: JointPathEx.cpp:42
bool setManipulabilityLimit(double l)
Definition: JointPathEx.h:29
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)
void getInterlockingJointPairIndices(std::vector< std::pair< size_t, size_t > > &pairs)
hrp::Matrix33 base_w_hat
Definition: JointPathEx.h:96
bool use_inside_joint_weight_retrieval
Definition: JointPathEx.h:64
double manipulability_gain
Definition: JointPathEx.h:59
unsigned int numJoints() const
void readVirtualForceSensorParamFromProperties(std::map< std::string, hrp::VirtualForceSensorParam > &vfs, hrp::BodyPtr m_robot, const std::string &prop_string, const std::string &instance_name)
void setMaxIKError(double epos, double erot)
std::string debug_print_prefix
Definition: JointPathEx.h:60
void getOptionalWeightVector(std::vector< double > &_opt_w)
Definition: JointPathEx.h:43
void readInterlockingJointsParamFromProperties(std::vector< std::pair< Link *, Link *> > &pairs, hrp::BodyPtr m_robot, const std::string &prop_string, const std::string &instance_name)
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)
hrp::BodyPtr m_robot
bool setSRGain(double g)
Definition: JointPathEx.h:27
std::vector< double > optional_weight_vector
Definition: JointPathEx.h:53
hrp::Vector3 base_w_old
Definition: JointPathEx.h:97
double maxIKPosErrorSqr
Definition: JointPathEx.h:48
void updateInvDynStateBuffer(InvDynStateBuffer &_idsb)
size_t debug_print_freq_count
Definition: JointPathEx.h:63
void setInitState(const hrp::BodyPtr _m_robot, const double _dt)
Definition: JointPathEx.h:100
double manipulability_limit
Definition: JointPathEx.h:59
hrp::dvector q_oldold
Definition: JointPathEx.h:94
bool setManipulabilityGain(double l)
Definition: JointPathEx.h:30
void calcWorldZMPFromInverseDynamics(hrp::BodyPtr _m_robot, InvDynStateBuffer &_idsb, hrp::Vector3 &_zmp_ans)


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Sat Dec 17 2022 03:52:20