bool WSDLSSolver::init(unsigned int _nq, unsigned int _nc, const std::vector<bool>& gc) { if (_nc == 0 || _nq == 0 || gc.size() != _nc) return false; m_nc = _nc; m_nq = _nq; m_ns = std::min(m_nc,m_nq); m_AWq = e_zero_matrix(m_nc,m_nq); m_WyAWq = e_zero_matrix(m_nc,m_nq); m_WyAWqt = e_zero_matrix(m_nq,m_nc); m_S = e_zero_vector(std::max(m_nc,m_nq)); m_Wy_ydot = e_zero_vector(m_nc); m_ytask = gc; if (m_nq > m_nc) { m_transpose = true; m_temp = e_zero_vector(m_nc); m_U = e_zero_matrix(m_nc,m_nc); m_V = e_zero_matrix(m_nq,m_nc); m_WqV = e_zero_matrix(m_nq,m_nc); } else { m_transpose = false; m_temp = e_zero_vector(m_nq); m_U = e_zero_matrix(m_nc,m_nq); m_V = e_zero_matrix(m_nq,m_nq); m_WqV = e_zero_matrix(m_nq,m_nq); } return true; }
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)); }
void UncontrolledObject::initialize(unsigned int _nu, unsigned int _nf) { assert (_nf >= 1); m_nu = _nu; m_nf = _nf; if (_nu > 0) m_xudot = e_zero_vector(_nu); // clear all Jacobian if any m_JuArray.clear(); // reserve one more to have an zero matrix handy if (m_nu > 0) m_JuArray.resize(m_nf+1, e_zero_matrix(6,m_nu)); }