Blender V2.61 - r43446

Armature.hpp

Go to the documentation of this file.
00001 /* 
00002  * Armature.hpp
00003  *
00004  *  Created on: Feb 3, 2009
00005  *      Author: benoitbolsee
00006  */
00007 
00008 #ifndef ARMATURE_HPP_
00009 #define ARMATURE_HPP_
00010 
00011 #include "ControlledObject.hpp"
00012 #include "ConstraintSet.hpp"
00013 #include "kdl/treejnttojacsolver.hpp"
00014 #include "kdl/treefksolverpos_recursive.hpp"
00015 #include <vector>
00016 
00017 namespace iTaSC {
00018 
00019 class Armature: public iTaSC::ControlledObject {
00020 public:
00021     Armature();
00022     virtual ~Armature();
00023 
00024     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());
00025     // general purpose constraint on joint
00026     int addConstraint(const std::string& segment_name, ConstraintCallback _function, void* _param=NULL, bool _freeParam=false, bool _substep=false);
00027     // specific limit constraint on joint
00028     int addLimitConstraint(const std::string& segment_name, unsigned int dof, double _min, double _max);
00029     double getMaxJointChange();
00030     double getMaxEndEffectorChange();
00031     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);
00032     bool getRelativeFrame(Frame& result, const std::string& segment_name, const std::string& base_name=m_root);
00033 
00034     virtual void finalize();
00035 
00036     virtual int addEndEffector(const std::string& name);
00037     virtual const Frame& getPose(const unsigned int end_effector);
00038     virtual bool updateJoint(const Timestamp& timestamp, JointLockCallback& callback);
00039     virtual void updateKinematics(const Timestamp& timestamp);
00040     virtual void pushCache(const Timestamp& timestamp);
00041     virtual void updateControlOutput(const Timestamp& timestamp);
00042     virtual bool setControlParameter(unsigned int constraintId, unsigned int valueId, ConstraintAction action, double value, double timestep=0.0);
00043     virtual void initCache(Cache *_cache);
00044     virtual bool setJointArray(const KDL::JntArray& joints);
00045     virtual const KDL::JntArray& getJointArray();
00046 
00047     virtual double getArmLength()
00048     {
00049         return m_armlength;
00050     }
00051 
00052     struct Effector_struct {
00053         std::string name;
00054         Frame oldpose;
00055         Frame pose;
00056         Effector_struct(const std::string& _name) {name = _name; oldpose = pose = F_identity;}
00057     };
00058     typedef std::vector<Effector_struct> EffectorList;
00059 
00060     enum ID  {
00061         ID_JOINT=1,
00062         ID_JOINT_RX=2,
00063         ID_JOINT_RY=3,
00064         ID_JOINT_RZ=4,
00065         ID_JOINT_TX=2,
00066         ID_JOINT_TY=3,
00067         ID_JOINT_TZ=4,
00068     };
00069     struct JointConstraint_struct {
00070         SegmentMap::const_iterator segment;
00071         ConstraintSingleValue value[3];
00072         ConstraintValues values[3];
00073         ConstraintCallback function;
00074         unsigned int v_nr;
00075         unsigned int y_nr;  // first coordinate of constraint in Y vector
00076         void* param;
00077         bool freeParam;
00078         bool substep;
00079         JointConstraint_struct(SegmentMap::const_iterator _segment, unsigned int _y_nr, ConstraintCallback _function, void* _param, bool _freeParam, bool _substep);
00080         ~JointConstraint_struct();
00081     };
00082     typedef std::vector<JointConstraint_struct*> JointConstraintList;   
00083 
00084     struct Joint_struct {
00085         KDL::Joint::JointType   type;
00086         unsigned short          ndof;
00087         bool                    useLimit;
00088         bool                    locked;
00089         double                  rest;
00090         double                  min;
00091         double                  max;
00092 
00093         Joint_struct(KDL::Joint::JointType _type, unsigned int _ndof, double _rest) :
00094             type(_type), ndof(_ndof), rest(_rest)  { useLimit=locked=false; min=0.0; max=0.0; }
00095     };
00096     typedef std::vector<Joint_struct> JointList;
00097     
00098 protected:
00099     virtual void updateJacobian();
00100 
00101 private:
00102     static std::string m_root;
00103     Tree m_tree;
00104     unsigned int m_njoint;
00105     unsigned int m_nconstraint;
00106     unsigned int m_noutput;
00107     unsigned int m_neffector;
00108     bool m_finalized;
00109     Cache* m_cache;
00110     double *m_buf;
00111     int m_qCCh;
00112     CacheTS m_qCTs;
00113     int m_yCCh;
00114     CacheTS m_yCTs;
00115     JntArray m_qKdl;
00116     JntArray m_oldqKdl;
00117     JntArray m_newqKdl;
00118     JntArray m_qdotKdl;
00119     Jacobian* m_jac;
00120     double m_armlength;
00121 
00122     KDL::TreeJntToJacSolver* m_jacsolver;
00123     KDL::TreeFkSolverPos_recursive* m_fksolver;
00124     EffectorList m_effectors;
00125     JointConstraintList m_constraints;
00126     JointList m_joints;
00127 
00128     void pushQ(CacheTS timestamp);
00129     bool popQ(CacheTS timestamp);
00130     //void pushConstraints(CacheTS timestamp);
00131     //bool popConstraints(CacheTS timestamp);
00132 
00133 };
00134 
00135 }
00136 
00137 #endif /* ARMATURE_HPP_ */