Blender  V3.3
Armature.hpp
Go to the documentation of this file.
1 /* SPDX-License-Identifier: LGPL-2.1-or-later
2  * Copyright 2009 Benoit Bolsee. */
3 
8 #ifndef ARMATURE_HPP_
9 #define ARMATURE_HPP_
10 
11 #include "ControlledObject.hpp"
12 #include "ConstraintSet.hpp"
15 #include <vector>
16 
17 namespace iTaSC {
18 
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 
47  virtual double getArmLength()
48  {
49  return m_armlength;
50  }
51 
52  struct Effector_struct {
53  std::string name;
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 {
68  };
70  SegmentMap::const_iterator segment;
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);
81  };
82  typedef std::vector<JointConstraint_struct*> JointConstraintList;
83 
84  struct Joint_struct {
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 #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_ */
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:526
static Inertia Zero()
Definition: inertia.hpp:46
This class encapsulates a simple joint, that is with one parameterized degree of freedom and with sca...
Definition: joint.hpp:43
This class encapsulates a tree kinematic interconnection structure. It is build out of segments.
Definition: tree.hpp:68
virtual void initCache(Cache *_cache)
Definition: Armature.cpp:135
std::vector< Joint_struct > JointList
Definition: Armature.hpp:96
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())
Definition: Armature.cpp:231
int addLimitConstraint(const std::string &segment_name, unsigned int dof, double _min, double _max)
Definition: Armature.cpp:335
virtual void updateControlOutput(const Timestamp &timestamp)
Definition: Armature.cpp:668
virtual void pushCache(const Timestamp &timestamp)
Definition: Armature.cpp:419
virtual bool updateJoint(const Timestamp &timestamp, JointLockCallback &callback)
Definition: Armature.cpp:443
virtual void updateKinematics(const Timestamp &timestamp)
Definition: Armature.cpp:621
bool getRelativeFrame(Frame &result, const std::string &segment_name, const std::string &base_name=m_root)
Definition: Armature.cpp:661
virtual const Frame & getPose(const unsigned int end_effector)
Definition: Armature.cpp:654
virtual ~Armature()
Definition: Armature.cpp:48
std::vector< JointConstraint_struct * > JointConstraintList
Definition: Armature.hpp:82
int addConstraint(const std::string &segment_name, ConstraintCallback _function, void *_param=NULL, bool _freeParam=false, bool _substep=false)
Definition: Armature.cpp:297
virtual int addEndEffector(const std::string &name)
Definition: Armature.cpp:354
std::vector< Effector_struct > EffectorList
Definition: Armature.hpp:58
virtual bool finalize()
Definition: Armature.cpp:373
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)
Definition: Armature.cpp:248
virtual void updateJacobian()
Definition: Armature.cpp:635
virtual bool setControlParameter(unsigned int constraintId, unsigned int valueId, ConstraintAction action, double value, double timestep=0.0)
Definition: Armature.cpp:714
virtual double getArmLength()
Definition: Armature.hpp:47
double getMaxEndEffectorChange()
Definition: Armature.cpp:278
virtual bool setJointArray(const KDL::JntArray &joints)
Definition: Armature.cpp:427
double getMaxJointChange()
Definition: Armature.cpp:264
virtual const KDL::JntArray & getJointArray()
Definition: Armature.cpp:438
DEGForeachIDComponentCallback callback
#define M
const Frame F_identity
bool(* ConstraintCallback)(const Timestamp &timestamp, struct ConstraintValues *const _values, unsigned int _nvalues, void *_param)
unsigned int CacheTS
Definition: Cache.hpp:32
Definition: DNA_ID.h:368
Effector_struct(const std::string &_name)
Definition: Armature.hpp:56
JointConstraint_struct(SegmentMap::const_iterator _segment, unsigned int _y_nr, ConstraintCallback _function, void *_param, bool _freeParam, bool _substep)
Definition: Armature.cpp:65
ConstraintSingleValue value[3]
Definition: Armature.hpp:71
SegmentMap::const_iterator segment
Definition: Armature.hpp:70
KDL::Joint::JointType type
Definition: Armature.hpp:85
Joint_struct(KDL::Joint::JointType _type, unsigned int _ndof, double _rest)
Definition: Armature.hpp:93