bdk-blender/intern/itasc/Armature.hpp
Campbell Barton 5035fbdd23 License headers: use SPDX identifiers in intern/itasc
Added license headers based on the original LGPL files from:
gitlab.kuleuven.be/rob-itasc
2022-03-18 10:27:01 +11:00

140 lines
4.1 KiB
C++

/* SPDX-License-Identifier: LGPL-2.1-or-later
* Copyright 2009 Benoit Bolsee. */
/** \file
* \ingroup intern_itasc
*/
#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 bool 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;
#if 0
CacheTS m_yCTs;
#endif
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_ */