18 m_S(6),m_temp(6),m_tdelta(6),
23 m_constraintCallback(
NULL), m_constraintParam(
NULL),
24 m_toggle(false),m_substep(false),
25 m_threshold(accuracy),m_maxIter(maximum_iterations)
33 m_constraintCallback(
NULL), m_constraintParam(
NULL),
34 m_toggle(false),m_substep(false),
35 m_threshold(0.0),m_maxIter(0)
153 for(
unsigned int i=0;i<6;i++)
156 for(
unsigned int i=0;i<6;i++) {
158 m_B.row(i).setConstant(0.0);
represents a frame transformation in 3D space (rotation + translation)
Vector p
origine of the Frame
Frame Inverse() const
Gives back inverse transformation of a Frame.
represents both translational and rotational velocities.
Twist RefPoint(const Vector &v_base_AB) const
virtual void updateJacobian()=0
bool setControlParameter(int id, ConstraintAction action, double value, double timestep=0.0)
virtual double getMaxTimestep(double ×tep)
virtual bool initialise(KDL::Frame &init_pose)
KDL::Frame m_internalPose
KDL::Frame m_externalPose
virtual void modelUpdate(KDL::Frame &_external_pose, const Timestamp ×tamp)
virtual void reset(unsigned int nc, double accuracy, unsigned int maximum_iterations)
virtual bool setControlParameters(ConstraintValues *_values, unsigned int _nvalues, double timestep=0.0)=0
#define e_identity_matrix
IMETHOD Vector diff(const Vector &a, const Vector &b, double dt=1)
IMETHOD bool Equal(const VectorAcc &, const VectorAcc &, double=epsilon)
int svd_eigen_HH(const Eigen::MatrixBase< MatrixA > &A, Eigen::MatrixBase< MatrixUV > &U, Eigen::MatrixBase< VectorS > &S, Eigen::MatrixBase< MatrixUV > &V, Eigen::MatrixBase< VectorS > &tmp, int maxiter=150)
struct ConstraintSingleValue * values