CoMTask::CoMTask(const rbd::MultiBody& mb, const Eigen::Vector3d& com):
com_(com),
jac_(mb),
eval_(3),
speed_(3),
normalAcc_(3),
jacMat_(3, mb.nrDof()),
jacDotMat_(3, mb.nrDof())
{}
CoMTask::CoMTask(const rbd::MultiBody& mb, const Eigen::Vector3d& com,
std::vector<double> weight):
com_(com),
jac_(mb, std::move(weight)),
eval_(3),
speed_(3),
normalAcc_(3),
jacMat_(3, mb.nrDof()),
jacDotMat_(3, mb.nrDof())
{}
OrientationTask::OrientationTask(const rbd::MultiBody& mb, int bodyId, const Eigen::Matrix3d& ori):
ori_(ori),
bodyIndex_(mb.bodyIndexById(bodyId)),
jac_(mb, bodyId),
eval_(3),
speed_(3),
normalAcc_(3),
jacMat_(3, mb.nrDof()),
jacDotMat_(3, mb.nrDof())
{
}
PositionTask::PositionTask(const rbd::MultiBody& mb, int bodyId,
const Eigen::Vector3d& pos, const Eigen::Vector3d& bodyPoint):
pos_(pos),
point_(bodyPoint),
bodyIndex_(mb.bodyIndexById(bodyId)),
jac_(mb, bodyId, bodyPoint),
eval_(3),
speed_(3),
normalAcc_(3),
jacMat_(3, mb.nrDof()),
jacDotMat_(3, mb.nrDof())
{
}
MotionConstrCommon::MotionConstrCommon(const rbd::MultiBody& mb):
fd_(mb),
cont_(),
fullJac_(6, mb.nrDof()),
A_(),
AL_(),
AU_(),
XL_(),
XU_(),
curTorque_(mb.nrDof()),
nrDof_(0),
nrFor_(0),
nrTor_(0)
{
}
LinVelocityTask::LinVelocityTask(const rbd::MultiBody& mb, int bodyId,
const Eigen::Vector3d& v, const Eigen::Vector3d& bodyPoint):
vel_(v),
point_(bodyPoint),
bodyIndex_(mb.bodyIndexById(bodyId)),
jac_(mb, bodyId, bodyPoint),
eval_(3),
speed_(3),
normalAcc_(3),
jacMat_(3, mb.nrDof()),
jacDotMat_(3, mb.nrDof())
{
// this task don't have any derivative
speed_.setZero();
}
MotionSpringConstr::MotionSpringConstr(const rbd::MultiBody& mb,
std::vector<std::vector<double>> lTorqueBounds,
std::vector<std::vector<double>> uTorqueBounds,
const std::vector<SpringJoint>& springs):
MotionConstrCommon(mb),
torqueL_(),
torqueU_(),
springs_()
{
int vars = mb.nrDof() - mb.joint(0).dof();
torqueL_.resize(vars);
torqueU_.resize(vars);
// remove the joint 0
lTorqueBounds[0] = {};
uTorqueBounds[0] = {};
rbd::paramToVector(lTorqueBounds, torqueL_);
rbd::paramToVector(uTorqueBounds, torqueU_);
springs_.reserve(springs.size());
for(const SpringJoint& sj: springs)
{
int index = mb.jointIndexById(sj.jointId);
int posInDof = mb.jointPosInDof(index) - mb.joint(0).dof();
springs_.push_back({index, posInDof, sj.K, sj.C, sj.O});
}
}
void QPSolver::nrVars(const rbd::MultiBody& mb,
std::vector<UnilateralContact> uni,
std::vector<BilateralContact> bi)
{
data_.normalAccB_.resize(mb.nrBodies());
data_.alphaD_ = mb.nrDof();
data_.lambda_ = 0;
data_.torque_ = (mb.nrDof() - mb.joint(0).dof());
data_.uniCont_ = uni;
data_.biCont_ = bi;
// counting unilateral contact
for(const UnilateralContact& c: data_.uniCont_)
{
for(std::size_t i = 0; i < c.points.size(); ++i)
{
data_.lambda_ += c.nrLambda(int(i));
}
}
data_.lambdaUni_ = data_.lambda_;
// counting bilateral contact
for(const BilateralContact& c: data_.biCont_)
{
for(std::size_t i = 0; i < c.points.size(); ++i)
{
data_.lambda_ += c.nrLambda(int(i));
}
}
data_.lambdaBi_ = data_.lambda_ - data_.lambdaUni_;
data_.nrVars_ = data_.alphaD_ + data_.lambda_;
for(Task* t: tasks_)
{
t->updateNrVars(mb, data_);
}
for(Constraint* c: constr_)
{
c->updateNrVars(mb, data_);
}
solver_->updateSize(data_.nrVars_, maxEqLines_, maxInEqLines_, maxGenInEqLines_);
}
void checkMultiBodyEq(const rbd::MultiBody& mb, std::vector<rbd::Body> bodies,
std::vector<rbd::Joint> joints, std::vector<int> pred, std::vector<int> succ,
std::vector<int> parent,
std::vector<sva::PTransformd> Xt)
{
// bodies
BOOST_CHECK_EQUAL_COLLECTIONS(mb.bodies().begin(), mb.bodies().end(),
bodies.begin(), bodies.end());
// joints
BOOST_CHECK_EQUAL_COLLECTIONS(mb.joints().begin(), mb.joints().end(),
joints.begin(), joints.end());
// pred
BOOST_CHECK_EQUAL_COLLECTIONS(mb.predecessors().begin(), mb.predecessors().end(),
pred.begin(), pred.end());
// succ
BOOST_CHECK_EQUAL_COLLECTIONS(mb.successors().begin(), mb.successors().end(),
succ.begin(), succ.end());
// parent
BOOST_CHECK_EQUAL_COLLECTIONS(mb.parents().begin(), mb.parents().end(),
parent.begin(), parent.end());
// Xt
BOOST_CHECK_EQUAL_COLLECTIONS(mb.transforms().begin(),
mb.transforms().end(),
Xt.begin(), Xt.end());
// nrBodies
BOOST_CHECK_EQUAL(mb.nrBodies(), bodies.size());
// nrJoints
BOOST_CHECK_EQUAL(mb.nrJoints(), bodies.size());
int params = 0, dof = 0;
for(int i = 0; i < static_cast<int>(joints.size()); ++i)
{
BOOST_CHECK_EQUAL(mb.jointPosInParam(i), params);
BOOST_CHECK_EQUAL(mb.jointsPosInParam()[i], params);
BOOST_CHECK_EQUAL(mb.sJointPosInParam(i), params);
BOOST_CHECK_EQUAL(mb.jointPosInDof(i), dof);
BOOST_CHECK_EQUAL(mb.jointsPosInDof()[i], dof);
BOOST_CHECK_EQUAL(mb.sJointPosInDof(i), dof);
params += joints[i].params();
dof += joints[i].dof();
}
BOOST_CHECK_EQUAL(params, mb.nrParams());
BOOST_CHECK_EQUAL(dof, mb.nrDof());
BOOST_CHECK_THROW(mb.sJointPosInParam(mb.nrJoints()), std::out_of_range);
BOOST_CHECK_THROW(mb.sJointPosInDof(mb.nrJoints()), std::out_of_range);
}
OrientationTrackingTask::OrientationTrackingTask(const rbd::MultiBody& mb,
int bodyId, const Eigen::Vector3d& bodyPoint, const Eigen::Vector3d& bodyAxis,
const std::vector<int>& trackingJointsId,
const Eigen::Vector3d& trackedPoint):
bodyIndex_(mb.bodyIndexById(bodyId)),
bodyPoint_(bodyPoint),
bodyAxis_(bodyAxis),
zeroJacIndex_(),
trackedPoint_(trackedPoint),
jac_(mb, bodyId),
eval_(3),
shortJacMat_(3, jac_.dof()),
jacMat_(3, mb.nrDof()),
jacDotMat_(3, mb.nrDof())
{
std::set<int> trackingJointsIndex;
for(int id: trackingJointsId)
{
trackingJointsIndex.insert(mb.jointIndexById(id));
}
int jacPos = 0;
for(int i: jac_.jointsPath())
{
const rbd::Joint& curJoint = mb.joint(i);
if(trackingJointsIndex.find(i) == std::end(trackingJointsIndex))
{
for(int j = 0; j < curJoint.dof(); ++j)
{
zeroJacIndex_.push_back(jacPos + j);
}
}
jacPos += curJoint.dof();
}
}
MotionConstr::MotionConstr(const rbd::MultiBody& mb,
std::vector<std::vector<double>> lTorqueBounds,
std::vector<std::vector<double>> uTorqueBounds):
MotionConstrCommon(mb),
torqueL_(),
torqueU_()
{
int vars = mb.nrDof() - mb.joint(0).dof();
torqueL_.resize(vars);
torqueU_.resize(vars);
// remove the joint 0
lTorqueBounds[0] = {};
uTorqueBounds[0] = {};
rbd::paramToVector(lTorqueBounds, torqueL_);
rbd::paramToVector(uTorqueBounds, torqueU_);
}
PostureTask::PostureTask(const rbd::MultiBody& mb, std::vector<std::vector<double> > q):
q_(q),
eval_(mb.nrDof()),
jacMat_(mb.nrDof(), mb.nrDof()),
jacDotMat_(mb.nrDof(), mb.nrDof())
{
eval_.setZero();
jacMat_.setIdentity();
jacDotMat_.setZero();
if(mb.nrDof() > 0 && mb.joint(0).type() == rbd::Joint::Free)
{
for(int i = 0; i < 6; ++i)
{
jacMat_(i, i) = 0;
}
}
}
IDIM::IDIM(const rbd::MultiBody& mb):
Y_(Eigen::MatrixXd::Zero(mb.nrDof(), mb.nrBodies()*10))
{ }