Merge of itasc branch. Project files, scons and cmake should be working. Makefile...
[blender.git] / intern / itasc / Armature.hpp
1 /* $Id: Armature.hpp 20853 2009-06-13 12:29:46Z ben2610 $
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 void 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
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;
56                 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
93                 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         CacheTS m_yCTs;
115     JntArray m_qKdl;
116     JntArray m_oldqKdl;
117     JntArray m_newqKdl;
118     JntArray m_qdotKdl;
119     Jacobian* m_jac;
120         double m_armlength;
121
122         KDL::TreeJntToJacSolver* m_jacsolver;
123         KDL::TreeFkSolverPos_recursive* m_fksolver;
124         EffectorList m_effectors;
125         JointConstraintList m_constraints;
126         JointList m_joints;
127
128         void pushQ(CacheTS timestamp);
129         bool popQ(CacheTS timestamp);
130         //void pushConstraints(CacheTS timestamp);
131         //bool popConstraints(CacheTS timestamp);
132
133 };
134
135 }
136
137 #endif /* ARMATURE_HPP_ */