177 lines
4.5 KiB
C++
177 lines
4.5 KiB
C++
/** \file itasc/ConstraintSet.cpp
|
|
* \ingroup itasc
|
|
*/
|
|
/*
|
|
* 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.array().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);
|
|
break;
|
|
}
|
|
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.noalias()=m_V*m_B;
|
|
|
|
m_chi.noalias()+=m_Jf_inv*m_tdelta;
|
|
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);
|
|
|
|
}
|
|
}
|