1 /* 2 * Armature.hpp 3 * 4 * Created on: Feb 3, 2009 5 * Author: benoitbolsee 6 */ 7 8 #ifndef ARMATURE_HPP_ 9 #define ARMATURE_HPP_ 10 11 #include "ControlledObject.hpp" 12 #include "ConstraintSet.hpp" 13 #include "kdl/treejnttojacsolver.hpp" 14 #include "kdl/treefksolverpos_recursive.hpp" 15 #include <vector> 16 17 namespace iTaSC { 18 19 class Armature: public iTaSC::ControlledObject { 20 public: 21 Armature(); 22 virtual ~Armature(); 23 24 bool addSegment(const std::string& segment_name, const std::string& hook_name, const Joint& joint, const double& q_rest, const Frame& f_tip=F_identity, const Inertia& M = Inertia::Zero()); 25 // general purpose constraint on joint 26 int addConstraint(const std::string& segment_name, ConstraintCallback _function, void* _param=NULL, bool _freeParam=false, bool _substep=false); 27 // specific limit constraint on joint 28 int addLimitConstraint(const std::string& segment_name, unsigned int dof, double _min, double _max); 29 double getMaxJointChange(); 30 double getMaxEndEffectorChange(); 31 bool getSegment(const std::string& segment_name, const unsigned int q_size, const Joint* &p_joint, double &q_rest, double &q, const Frame* &p_tip); 32 bool getRelativeFrame(Frame& result, const std::string& segment_name, const std::string& base_name=m_root); 33 34 virtual bool finalize(); 35 36 virtual int addEndEffector(const std::string& name); 37 virtual const Frame& getPose(const unsigned int end_effector); 38 virtual bool updateJoint(const Timestamp& timestamp, JointLockCallback& callback); 39 virtual void updateKinematics(const Timestamp& timestamp); 40 virtual void pushCache(const Timestamp& timestamp); 41 virtual void updateControlOutput(const Timestamp& timestamp); 42 virtual bool setControlParameter(unsigned int constraintId, unsigned int valueId, ConstraintAction action, double value, double timestep=0.0); 43 virtual void initCache(Cache *_cache); 44 virtual bool setJointArray(const KDL::JntArray& joints); 45 virtual const KDL::JntArray& getJointArray(); 46 getArmLength()47 virtual double getArmLength() 48 { 49 return m_armlength; 50 } 51 52 struct Effector_struct { 53 std::string name; 54 Frame oldpose; 55 Frame pose; Effector_structiTaSC::Armature::Effector_struct56 Effector_struct(const std::string& _name) {name = _name; oldpose = pose = F_identity;} 57 }; 58 typedef std::vector<Effector_struct> EffectorList; 59 60 enum ID { 61 ID_JOINT=1, 62 ID_JOINT_RX=2, 63 ID_JOINT_RY=3, 64 ID_JOINT_RZ=4, 65 ID_JOINT_TX=2, 66 ID_JOINT_TY=3, 67 ID_JOINT_TZ=4, 68 }; 69 struct JointConstraint_struct { 70 SegmentMap::const_iterator segment; 71 ConstraintSingleValue value[3]; 72 ConstraintValues values[3]; 73 ConstraintCallback function; 74 unsigned int v_nr; 75 unsigned int y_nr; // first coordinate of constraint in Y vector 76 void* param; 77 bool freeParam; 78 bool substep; 79 JointConstraint_struct(SegmentMap::const_iterator _segment, unsigned int _y_nr, ConstraintCallback _function, void* _param, bool _freeParam, bool _substep); 80 ~JointConstraint_struct(); 81 }; 82 typedef std::vector<JointConstraint_struct*> JointConstraintList; 83 84 struct Joint_struct { 85 KDL::Joint::JointType type; 86 unsigned short ndof; 87 bool useLimit; 88 bool locked; 89 double rest; 90 double min; 91 double max; 92 Joint_structiTaSC::Armature::Joint_struct93 Joint_struct(KDL::Joint::JointType _type, unsigned int _ndof, double _rest) : 94 type(_type), ndof(_ndof), rest(_rest) { useLimit=locked=false; min=0.0; max=0.0; } 95 }; 96 typedef std::vector<Joint_struct> JointList; 97 98 protected: 99 virtual void updateJacobian(); 100 101 private: 102 static std::string m_root; 103 Tree m_tree; 104 unsigned int m_njoint; 105 unsigned int m_nconstraint; 106 unsigned int m_noutput; 107 unsigned int m_neffector; 108 bool m_finalized; 109 Cache* m_cache; 110 double *m_buf; 111 int m_qCCh; 112 CacheTS m_qCTs; 113 int m_yCCh; 114 #if 0 115 CacheTS m_yCTs; 116 #endif 117 JntArray m_qKdl; 118 JntArray m_oldqKdl; 119 JntArray m_newqKdl; 120 JntArray m_qdotKdl; 121 Jacobian* m_jac; 122 double m_armlength; 123 124 KDL::TreeJntToJacSolver* m_jacsolver; 125 KDL::TreeFkSolverPos_recursive* m_fksolver; 126 EffectorList m_effectors; 127 JointConstraintList m_constraints; 128 JointList m_joints; 129 130 void pushQ(CacheTS timestamp); 131 bool popQ(CacheTS timestamp); 132 //void pushConstraints(CacheTS timestamp); 133 //bool popConstraints(CacheTS timestamp); 134 135 }; 136 137 } 138 139 #endif /* ARMATURE_HPP_ */ 140