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); }
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)); }