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;
}
void MovingFrame::updateCoordinates(const Timestamp& timestamp)
{
// don't compute the velocity during substepping, it is assumed constant.
if (!timestamp.substep) {
bool cacheAvail = true;
if (!timestamp.reiterate) {
cacheAvail = popInternalFrame(timestamp.cacheTimestamp);
if (m_function)
(*m_function)(timestamp, m_internalPose, m_nextPose, m_param);
}
// only compute velocity if we have a previous pose
if (cacheAvail && timestamp.interpolate) {
unsigned int iXu;
m_velocity = diff(m_internalPose, m_nextPose, timestamp.realTimestep);
for (iXu=0; iXu<6; iXu++)
m_xudot(iXu) = m_velocity(iXu);
} else if (!timestamp.reiterate) {
// new position is forced, no velocity as we cannot interpolate
m_internalPose = m_nextPose;
m_velocity = Twist::Zero();
m_xudot = e_zero_vector(6);
// recompute the jacobian
updateJacobian();
}
}
}
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 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));
}
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;
}
