diff options
Diffstat (limited to 'intern/itasc/ConstraintSet.cpp')
| -rw-r--r-- | intern/itasc/ConstraintSet.cpp | 170 |
1 files changed, 170 insertions, 0 deletions
diff --git a/intern/itasc/ConstraintSet.cpp b/intern/itasc/ConstraintSet.cpp new file mode 100644 index 00000000000..a38db445ea2 --- /dev/null +++ b/intern/itasc/ConstraintSet.cpp @@ -0,0 +1,170 @@ +/* $Id$ + * ConstraintSet.cpp + * + * Created on: Jan 5, 2009 + * Author: rubensmits + */ + +#include "ConstraintSet.hpp" +#include "kdl/utilities/svd_eigen_HH.hpp" + +namespace iTaSC { + +ConstraintSet::ConstraintSet(unsigned int _nc,double accuracy,unsigned int maximum_iterations): + m_nc(_nc), + m_Cf(e_zero_matrix(m_nc,6)), + m_Wy(e_scalar_vector(m_nc,1.0)), + m_y(m_nc),m_ydot(e_zero_vector(m_nc)),m_chi(e_zero_vector(6)), + m_S(6),m_temp(6),m_tdelta(6), + m_Jf(e_identity_matrix(6,6)), + m_U(e_identity_matrix(6,6)),m_V(e_identity_matrix(6,6)),m_B(e_zero_matrix(6,6)), + m_Jf_inv(e_zero_matrix(6,6)), + m_internalPose(F_identity), m_externalPose(F_identity), + m_constraintCallback(NULL), m_constraintParam(NULL), + m_toggle(false),m_substep(false), + m_threshold(accuracy),m_maxIter(maximum_iterations) +{ + m_maxDeltaChi = e_scalar(0.52); +} + +ConstraintSet::ConstraintSet(): + m_nc(0), + m_internalPose(F_identity), m_externalPose(F_identity), + m_constraintCallback(NULL), m_constraintParam(NULL), + m_toggle(false),m_substep(false), + m_threshold(0.0),m_maxIter(0) +{ + m_maxDeltaChi = e_scalar(0.52); +} + +void ConstraintSet::reset(unsigned int _nc,double accuracy,unsigned int maximum_iterations) +{ + m_nc = _nc; + m_Jf = e_identity_matrix(6,6); + m_Cf = e_zero_matrix(m_nc,6); + m_U = e_identity_matrix(6,6); + m_V = e_identity_matrix(6,6); + m_B = e_zero_matrix(6,6); + m_Jf_inv = e_zero_matrix(6,6), + m_Wy = e_scalar_vector(m_nc,1.0), + m_chi = e_zero_vector(6); + m_chidot = e_zero_vector(6); + m_y = e_zero_vector(m_nc); + m_ydot = e_zero_vector(m_nc); + m_S = e_zero_vector(6); + m_temp = e_zero_vector(6); + m_tdelta = e_zero_vector(6); + m_threshold = accuracy; + m_maxIter = maximum_iterations; +} + +ConstraintSet::~ConstraintSet() { + +} + +void ConstraintSet::modelUpdate(Frame& _external_pose,const Timestamp& timestamp) +{ + m_chi+=m_chidot*timestamp.realTimestep; + m_externalPose = _external_pose; + + //update the internal pose and Jf + updateJacobian(); + //check if loop is already closed, if not update the pose and Jf + unsigned int iter=0; + while(iter<5&&!closeLoop()) + iter++; +} + +double ConstraintSet::getMaxTimestep(double& timestep) +{ + e_scalar maxChidot = m_chidot.cwise().abs().maxCoeff(); + if (timestep*maxChidot > m_maxDeltaChi) { + timestep = m_maxDeltaChi/maxChidot; + } + return timestep; +} + +bool ConstraintSet::initialise(Frame& init_pose){ + m_externalPose=init_pose; + // get current Jf + updateJacobian(); + + unsigned int iter=0; + while(iter<m_maxIter&&!closeLoop()){ + iter++; + } + if (iter<m_maxIter) + return true; + else + return false; +} + +bool ConstraintSet::setControlParameter(int id, ConstraintAction action, double data, double timestep) +{ + ConstraintValues values; + ConstraintSingleValue value; + values.values = &value; + values.number = 0; + values.action = action; + values.id = id; + value.action = action; + value.id = id; + switch (action) { + case ACT_NONE: + return true; + case ACT_VALUE: + value.yd = data; + values.number = 1; + break; + case ACT_VELOCITY: + value.yddot = data; + values.number = 1; + break; + case ACT_TOLERANCE: + values.tolerance = data; + break; + case ACT_FEEDBACK: + values.feedback = data; + break; + case ACT_ALPHA: + values.alpha = data; + break; + default: + assert(action==ACT_NONE); + } + return setControlParameters(&values, 1, timestep); +} + +bool ConstraintSet::closeLoop(){ + //Invert Jf + //TODO: svd_boost_Macie has problems if Jf contains zero-rows + //toggle=!toggle; + //svd_boost_Macie(Jf,U,S,V,B,temp,1e-3*threshold,toggle); + int ret = KDL::svd_eigen_HH(m_Jf,m_U,m_S,m_V,m_temp); + if(ret<0) + return false; + + // the reference point and frame of the jacobian is the base frame + // m_externalPose-m_internalPose is the twist to extend the end effector + // to get the required pose => change the reference point to the base frame + Twist twist_delta(diff(m_internalPose,m_externalPose)); + twist_delta=twist_delta.RefPoint(-m_internalPose.p); + for(unsigned int i=0;i<6;i++) + m_tdelta(i)=twist_delta(i); + //TODO: use damping in constraintset inversion? + for(unsigned int i=0;i<6;i++) + if(m_S(i)<m_threshold){ + m_B.row(i).setConstant(0.0); + }else + m_B.row(i) = m_U.col(i)/m_S(i); + + m_Jf_inv=(m_V*m_B).lazy(); + + m_chi+=(m_Jf_inv*m_tdelta).lazy(); + updateJacobian(); + // m_externalPose-m_internalPose in end effector frame + // this is just to compare the pose, a different formula would work too + return Equal(m_internalPose.Inverse()*m_externalPose,F_identity,m_threshold); + +} +} |