101 lines
3.5 KiB
C++
101 lines
3.5 KiB
C++
/* SPDX-FileCopyrightText: 2009 Benoit Bolsee
|
|
*
|
|
* SPDX-License-Identifier: LGPL-2.1-or-later */
|
|
|
|
/** \file
|
|
* \ingroup intern_itasc
|
|
*/
|
|
|
|
#ifndef COPYPOSE_H_
|
|
#define COPYPOSE_H_
|
|
|
|
#include "ConstraintSet.hpp"
|
|
namespace iTaSC{
|
|
|
|
using namespace KDL;
|
|
|
|
class CopyPose: public iTaSC::ConstraintSet
|
|
{
|
|
protected:
|
|
virtual void updateKinematics(const Timestamp& timestamp);
|
|
virtual void pushCache(const Timestamp& timestamp);
|
|
virtual void updateJacobian();
|
|
virtual bool initialise(Frame& init_pose);
|
|
virtual void initCache(Cache *_cache);
|
|
virtual void updateControlOutput(const Timestamp& timestamp);
|
|
virtual void modelUpdate(Frame& _external_pose,const Timestamp& timestamp);
|
|
virtual double getMaxTimestep(double& timestep);
|
|
|
|
public:
|
|
enum ID { // constraint ID in callback and setControlParameter
|
|
ID_POSITION=0,
|
|
ID_POSITIONX=1,
|
|
ID_POSITIONY=2,
|
|
ID_POSITIONZ=3,
|
|
ID_ROTATION=4,
|
|
ID_ROTATIONX=5,
|
|
ID_ROTATIONY=6,
|
|
ID_ROTATIONZ=7,
|
|
};
|
|
enum CTL { // control ID in constructor to specify which output is constrainted
|
|
CTL_NONE=0x00,
|
|
CTL_POSITIONX=0x01, // the bit order is important: it matches the y output order
|
|
CTL_POSITIONY=0x02,
|
|
CTL_POSITIONZ=0x04,
|
|
CTL_POSITION=0x07,
|
|
CTL_ROTATIONX=0x08,
|
|
CTL_ROTATIONY=0x10,
|
|
CTL_ROTATIONZ=0x20,
|
|
CTL_ROTATION=0x38,
|
|
CTL_ALL=0x3F,
|
|
};
|
|
|
|
// use a combination of CTL_.. in control_output to specify which
|
|
CopyPose(unsigned int control_output=CTL_ALL, unsigned int dynamic_output=CTL_NONE, double armlength=1.0, double accuracy=1e-6, unsigned int maximum_iterations=100);
|
|
virtual ~CopyPose();
|
|
|
|
virtual bool setControlParameters(struct ConstraintValues* _values, unsigned int _nvalues, double timestep);
|
|
virtual const ConstraintValues* getControlParameters(unsigned int* _nvalues);
|
|
|
|
private:
|
|
struct ConstraintSingleValue m_posData[3]; // index = controlled output in X,Y,Z order
|
|
struct ConstraintSingleValue m_rotData[3];
|
|
struct ConstraintValues m_values[2]; // index = group of controlled output, in position, rotation order
|
|
Cache* m_cache;
|
|
int m_poseCCh;
|
|
CacheTS m_poseCTs;
|
|
unsigned int m_poseCacheSize;
|
|
unsigned int m_outputDynamic; // combination of CTL_... determine which variables are dynamically controlled by the application
|
|
unsigned int m_outputControl; // combination of CTL_... determine which output are constrained
|
|
unsigned int m_nvalues; // number of elements used in m_values[]
|
|
double m_maxerror;
|
|
|
|
struct ControlState {
|
|
int firsty; // first y index
|
|
int ny; // number of y in output
|
|
double alpha;
|
|
double K;
|
|
double tolerance;
|
|
struct ControlValue {
|
|
double yddot;
|
|
double yd;
|
|
double nextyd;
|
|
double nextyddot;
|
|
} output[3]; // inded numbex = same as m_rotData
|
|
} m_rot, m_pos;
|
|
|
|
void pushPose(CacheTS timestamp);
|
|
bool popPose(CacheTS timestamp);
|
|
int nBitsOn(unsigned int v)
|
|
{ int n=0; while(v) { if (v&1) n++; v>>=1; } return n; }
|
|
double* restoreValues(double* item, ConstraintValues* _values, ControlState* _state, unsigned int mask);
|
|
double* pushValues(double* item, ControlState* _state, unsigned int mask);
|
|
void updateState(ConstraintValues* _values, ControlState* _state, unsigned int mask, double timestep);
|
|
void updateValues(Vector& vel, ConstraintValues* _values, ControlState* _state, unsigned int mask);
|
|
void updateOutput(Vector& vel, ControlState* _state, unsigned int mask);
|
|
void interpolateOutput(ControlState* _state, unsigned int mask, const Timestamp& timestamp);
|
|
|
|
};
|
|
}
|
|
#endif /* COPYROTATION_H_ */
|