diff options
Diffstat (limited to 'intern/itasc/Armature.hpp')
-rw-r--r-- | intern/itasc/Armature.hpp | 137 |
1 files changed, 137 insertions, 0 deletions
diff --git a/intern/itasc/Armature.hpp b/intern/itasc/Armature.hpp new file mode 100644 index 00000000000..312ca1b28c3 --- /dev/null +++ b/intern/itasc/Armature.hpp @@ -0,0 +1,137 @@ +/* $Id: Armature.hpp 20853 2009-06-13 12:29:46Z ben2610 $ + * Armature.hpp + * + * Created on: Feb 3, 2009 + * Author: benoitbolsee + */ + +#ifndef ARMATURE_HPP_ +#define ARMATURE_HPP_ + +#include "ControlledObject.hpp" +#include "ConstraintSet.hpp" +#include "kdl/treejnttojacsolver.hpp" +#include "kdl/treefksolverpos_recursive.hpp" +#include <vector> + +namespace iTaSC { + +class Armature: public iTaSC::ControlledObject { +public: + Armature(); + virtual ~Armature(); + + 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()); + // general purpose constraint on joint + int addConstraint(const std::string& segment_name, ConstraintCallback _function, void* _param=NULL, bool _freeParam=false, bool _substep=false); + // specific limit constraint on joint + int addLimitConstraint(const std::string& segment_name, unsigned int dof, double _min, double _max); + double getMaxJointChange(); + double getMaxEndEffectorChange(); + 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); + bool getRelativeFrame(Frame& result, const std::string& segment_name, const std::string& base_name=m_root); + + virtual void finalize(); + + virtual int addEndEffector(const std::string& name); + virtual const Frame& getPose(const unsigned int end_effector); + virtual bool updateJoint(const Timestamp& timestamp, JointLockCallback& callback); + virtual void updateKinematics(const Timestamp& timestamp); + virtual void pushCache(const Timestamp& timestamp); + virtual void updateControlOutput(const Timestamp& timestamp); + virtual bool setControlParameter(unsigned int constraintId, unsigned int valueId, ConstraintAction action, double value, double timestep=0.0); + virtual void initCache(Cache *_cache); + virtual bool setJointArray(const KDL::JntArray& joints); + virtual const KDL::JntArray& getJointArray(); + + virtual double getArmLength() + { + return m_armlength; + } + + struct Effector_struct { + std::string name; + Frame oldpose; + Frame pose; + Effector_struct(const std::string& _name) {name = _name; oldpose = pose = F_identity;} + }; + typedef std::vector<Effector_struct> EffectorList; + + enum ID { + ID_JOINT=1, + ID_JOINT_RX=2, + ID_JOINT_RY=3, + ID_JOINT_RZ=4, + ID_JOINT_TX=2, + ID_JOINT_TY=3, + ID_JOINT_TZ=4, + }; + struct JointConstraint_struct { + SegmentMap::const_iterator segment; + ConstraintSingleValue value[3]; + ConstraintValues values[3]; + ConstraintCallback function; + unsigned int v_nr; + unsigned int y_nr; // first coordinate of constraint in Y vector + void* param; + bool freeParam; + bool substep; + JointConstraint_struct(SegmentMap::const_iterator _segment, unsigned int _y_nr, ConstraintCallback _function, void* _param, bool _freeParam, bool _substep); + ~JointConstraint_struct(); + }; + typedef std::vector<JointConstraint_struct*> JointConstraintList; + + struct Joint_struct { + KDL::Joint::JointType type; + unsigned short ndof; + bool useLimit; + bool locked; + double rest; + double min; + double max; + + Joint_struct(KDL::Joint::JointType _type, unsigned int _ndof, double _rest) : + type(_type), ndof(_ndof), rest(_rest) { useLimit=locked=false; min=0.0; max=0.0; } + }; + typedef std::vector<Joint_struct> JointList; + +protected: + virtual void updateJacobian(); + +private: + static std::string m_root; + Tree m_tree; + unsigned int m_njoint; + unsigned int m_nconstraint; + unsigned int m_noutput; + unsigned int m_neffector; + bool m_finalized; + Cache* m_cache; + double *m_buf; + int m_qCCh; + CacheTS m_qCTs; + int m_yCCh; + CacheTS m_yCTs; + JntArray m_qKdl; + JntArray m_oldqKdl; + JntArray m_newqKdl; + JntArray m_qdotKdl; + Jacobian* m_jac; + double m_armlength; + + KDL::TreeJntToJacSolver* m_jacsolver; + KDL::TreeFkSolverPos_recursive* m_fksolver; + EffectorList m_effectors; + JointConstraintList m_constraints; + JointList m_joints; + + void pushQ(CacheTS timestamp); + bool popQ(CacheTS timestamp); + //void pushConstraints(CacheTS timestamp); + //bool popConstraints(CacheTS timestamp); + +}; + +} + +#endif /* ARMATURE_HPP_ */ |