1 files changed, 115 insertions, 0 deletions
diff --git a/intern/itasc/ConstraintSet.hpp b/intern/itasc/ConstraintSet.hpp
new file mode 100644
index 00000000000..12bb085c911
--- /dev/null
+++ b/intern/itasc/ConstraintSet.hpp
@@ -0,0 +1,115 @@
+/* $Id: ConstraintSet.hpp 20307 2009-05-20 20:39:18Z ben2610 $
+ * ConstraintSet.hpp
+ *
+ *  Created on: Jan 5, 2009
+ *      Author: rubensmits
+ */
+
+#ifndef CONSTRAINTSET_HPP_
+#define CONSTRAINTSET_HPP_
+
+#include "kdl/frames.hpp"
+#include "eigen_types.hpp"
+#include "Cache.hpp"
+#include <vector>
+
+namespace iTaSC {
+
+enum ConstraintAction {
+	ACT_NONE=		0,
+	ACT_VALUE=		1,
+	ACT_VELOCITY=	2,
+	ACT_TOLERANCE=	4,
+	ACT_FEEDBACK=	8,
+	ACT_ALPHA=		16
+};
+
+struct ConstraintSingleValue {
+	unsigned int id;	// identifier of constraint value, depends on constraint
+	unsigned int action;// action performed, compbination of ACT_..., set on return
+	const double y;		// actual constraint value
+	const double ydot;	// actual constraint velocity
+	double yd;			// current desired constraint value, changed on return
+	double yddot;		// current desired constraint velocity, changed on return
+	ConstraintSingleValue(): id(0), action(0), y(0.0), ydot(0.0) {}
+};
+
+struct ConstraintValues {
+	unsigned int id;	// identifier of group of constraint values, depend on constraint
+	unsigned short number;		// number of constraints in list
+	unsigned short action;		// action performed, ACT_..., set on return
+	double alpha;		// constraint activation coefficient, should be [0..1]
+	double tolerance;	// current desired tolerance on constraint, same unit than yd, changed on return
+	double feedback;	// current desired feedback on error, in 1/sec, changed on return
+	struct ConstraintSingleValue* values;
+	ConstraintValues(): id(0), number(0), action(0), values(NULL) {}
+};
+
+class ConstraintSet;
+typedef bool (*ConstraintCallback)(const Timestamp& timestamp, struct ConstraintValues* const _values, unsigned int _nvalues, void* _param);
+
+class ConstraintSet {
+protected:
+    unsigned int m_nc;
+	e_scalar m_maxDeltaChi;
+	e_matrix m_Cf;
+    e_vector m_Wy,m_y,m_ydot;
+	e_vector6 m_chi,m_chidot,m_S,m_temp,m_tdelta;
+    e_matrix6 m_Jf,m_U,m_V,m_B,m_Jf_inv;
+	KDL::Frame m_internalPose,m_externalPose;
+    ConstraintCallback m_constraintCallback;
+    void* m_constraintParam;
+	void* m_poseParam;
+    bool m_toggle;
+	bool m_substep;
+    double m_threshold;
+    unsigned int m_maxIter;
+
+	friend class Scene;
+	virtual void modelUpdate(KDL::Frame& _external_pose,const Timestamp& timestamp);
+    virtual void updateKinematics(const Timestamp& timestamp)=0;
+    virtual void pushCache(const Timestamp& timestamp)=0;
+    virtual void updateJacobian()=0;
+    virtual void updateControlOutput(const Timestamp& timestamp)=0;
+	virtual void initCache(Cache *_cache) = 0;
+	virtual bool initialise(KDL::Frame& init_pose);
+	virtual void reset(unsigned int nc,double accuracy,unsigned int maximum_iterations);
+    virtual bool closeLoop();
+	virtual double getMaxTimestep(double& timestep);
+
+
+public:
+    ConstraintSet(unsigned int nc,double accuracy,unsigned int maximum_iterations);
+    ConstraintSet();
+    virtual ~ConstraintSet();
+
+	EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+	virtual bool registerCallback(ConstraintCallback _function, void* _param)
+	{
+		m_constraintCallback = _function;
+		m_constraintParam = _param;
+		return true;
+	}
+
+    virtual const e_vector& getControlOutput()const{return m_ydot;};
+	virtual const ConstraintValues* getControlParameters(unsigned int* _nvalues) = 0;
+	virtual bool setControlParameters(ConstraintValues* _values, unsigned int _nvalues, double timestep=0.0) = 0;
+	bool setControlParameter(int id, ConstraintAction action, double value, double timestep=0.0);
+
+    virtual const e_matrix6& getJf() const{return m_Jf;};
+	virtual const KDL::Frame& getPose() const{return m_internalPose;};
+    virtual const e_matrix& getCf() const{return m_Cf;};
+
+    virtual const e_vector& getWy() const {return m_Wy;};
+    virtual void setWy(const e_vector& Wy_in){m_Wy = Wy_in;};
+    virtual void setJointVelocity(const e_vector chidot_in){m_chidot = chidot_in;};
+
+    virtual unsigned int getNrOfConstraints(){return m_nc;};
+	void substep(bool _substep) {m_substep=_substep;}
+	bool substep() {return m_substep;}
+};
+
+}
+
+#endif /* CONSTRAINTSET_HPP_ */