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);
}
MovingFrame::MovingFrame(const Frame& frame):UncontrolledObject(),
m_function(NULL), m_param(NULL), m_velocity(), m_poseCCh(-1), m_poseCTs(0)
{
m_internalPose = m_nextPose = frame;
initialize(6, 1);
e_matrix& Ju = m_JuArray[0];
Ju = e_identity_matrix(6,6);
}
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;
}
void ControlledObject::initialize(unsigned int _nq,unsigned int _nc, unsigned int _nee)
{
assert(_nee >= 1);
m_nq = _nq;
m_nc = _nc;
m_nee = _nee;
if (m_nq > 0) {
m_Wq = e_identity_matrix(m_nq,m_nq);
m_qdot = e_zero_vector(m_nq);
}
if (m_nc > 0) {
m_Wy = e_scalar_vector(m_nc,1.0);
m_ydot = e_zero_vector(m_nc);
}
if (m_nc > 0 && m_nq > 0)
m_Cq = e_zero_matrix(m_nc,m_nq);
// clear all Jacobian if any
m_JqArray.clear();
// reserve one more to have a zero matrix handy
if (m_nq > 0)
m_JqArray.resize(m_nee+1, e_zero_matrix(6,m_nq));
}
CopyPose::CopyPose(unsigned int control_output, unsigned int dynamic_output, double armlength, double accuracy, unsigned int maximum_iterations):
ConstraintSet(),
m_cache(NULL),
m_poseCCh(-1),m_poseCTs(0)
{
m_maxerror = armlength/2.0;
m_outputControl = (control_output & CTL_ALL);
unsigned int _nc = nBitsOn(m_outputControl);
if (!_nc)
return;
// reset the constraint set
reset(_nc, accuracy, maximum_iterations);
_nc = 0;
m_nvalues = 0;
int nrot = 0, npos = 0;
int nposCache = 0, nrotCache = 0;
m_outputDynamic = (dynamic_output & m_outputControl);
memset(m_values, 0, sizeof(m_values));
memset(m_posData, 0, sizeof(m_posData));
memset(m_rotData, 0, sizeof(m_rotData));
memset(&m_rot, 0, sizeof(m_rot));
memset(&m_pos, 0, sizeof(m_pos));
if (m_outputControl & CTL_POSITION) {
m_pos.alpha = 1.0;
m_pos.K = 20.0;
m_pos.tolerance = 0.05;
m_values[m_nvalues].alpha = m_pos.alpha;
m_values[m_nvalues].feedback = m_pos.K;
m_values[m_nvalues].tolerance = m_pos.tolerance;
m_values[m_nvalues].id = ID_POSITION;
if (m_outputControl & CTL_POSITIONX) {
m_Wy(_nc) = m_pos.alpha/*/(m_pos.tolerance*m_pos.K)*/;
m_Cf(_nc++,0)=1.0;
m_posData[npos++].id = ID_POSITIONX;
if (m_outputDynamic & CTL_POSITIONX)
nposCache++;
}
if (m_outputControl & CTL_POSITIONY) {
m_Wy(_nc) = m_pos.alpha/*/(m_pos.tolerance*m_pos.K)*/;
m_Cf(_nc++,1)=1.0;
m_posData[npos++].id = ID_POSITIONY;
if (m_outputDynamic & CTL_POSITIONY)
nposCache++;
}
if (m_outputControl & CTL_POSITIONZ) {
m_Wy(_nc) = m_pos.alpha/*/(m_pos.tolerance*m_pos.K)*/;
m_Cf(_nc++,2)=1.0;
m_posData[npos++].id = ID_POSITIONZ;
if (m_outputDynamic & CTL_POSITIONZ)
nposCache++;
}
m_values[m_nvalues].number = npos;
m_values[m_nvalues++].values = m_posData;
m_pos.firsty = 0;
m_pos.ny = npos;
}
if (m_outputControl & CTL_ROTATION) {
m_rot.alpha = 1.0;
m_rot.K = 20.0;
m_rot.tolerance = 0.05;
m_values[m_nvalues].alpha = m_rot.alpha;
m_values[m_nvalues].feedback = m_rot.K;
m_values[m_nvalues].tolerance = m_rot.tolerance;
m_values[m_nvalues].id = ID_ROTATION;
if (m_outputControl & CTL_ROTATIONX) {
m_Wy(_nc) = m_rot.alpha/*/(m_rot.tolerance*m_rot.K)*/;
m_Cf(_nc++,3)=1.0;
m_rotData[nrot++].id = ID_ROTATIONX;
if (m_outputDynamic & CTL_ROTATIONX)
nrotCache++;
}
if (m_outputControl & CTL_ROTATIONY) {
m_Wy(_nc) = m_rot.alpha/*/(m_rot.tolerance*m_rot.K)*/;
m_Cf(_nc++,4)=1.0;
m_rotData[nrot++].id = ID_ROTATIONY;
if (m_outputDynamic & CTL_ROTATIONY)
nrotCache++;
}
if (m_outputControl & CTL_ROTATIONZ) {
m_Wy(_nc) = m_rot.alpha/*/(m_rot.tolerance*m_rot.K)*/;
m_Cf(_nc++,5)=1.0;
m_rotData[nrot++].id = ID_ROTATIONZ;
if (m_outputDynamic & CTL_ROTATIONZ)
nrotCache++;
}
m_values[m_nvalues].number = nrot;
m_values[m_nvalues++].values = m_rotData;
m_rot.firsty = npos;
m_rot.ny = nrot;
}
assert(_nc == m_nc);
m_Jf=e_identity_matrix(6,6);
m_poseCacheSize = ((nrotCache)?(3+nrotCache*2):0)+((nposCache)?(3+nposCache*2):0);
}
