117 lines
3.9 KiB
C++
117 lines
3.9 KiB
C++
/* SPDX-FileCopyrightText: 2009 Ruben Smits
|
|
*
|
|
* SPDX-License-Identifier: LGPL-2.1-or-later */
|
|
|
|
/** \file
|
|
* \ingroup intern_itasc
|
|
*/
|
|
|
|
#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_ */
|