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