void PostureTask::update(const rbd::MultiBody& mb, const rbd::MultiBodyConfig& mbc)
{
using namespace Eigen;
int pos = mb.jointPosInDof(1);
// we drop the first joint (fixed or free flyier).
for(int i = 1; i < mb.nrJoints(); ++i)
{
// if dof == 1 is a prismatic/revolute joint
// else if dof == 4 is a spherical one
// else is a fixed one
if(mb.joint(i).dof() == 1)
{
eval_(pos) = q_[i][0] - mbc.q[i][0];
++pos;
}
else if(mb.joint(i).dof() == 4)
{
Matrix3d orid(
Quaterniond(q_[i][0], q_[i][1], q_[i][2], q_[i][3]).matrix());
Vector3d err = sva::rotationError(mbc.jointConfig[i].rotation(), orid);
eval_.segment(pos, 3) = err;
pos += 3;
}
}
}
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 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);
}
void MotionPolyConstr::update(const rbd::MultiBody& mb, const rbd::MultiBodyConfig& mbc,
const SolverData& /* data */)
{
computeMatrix(mb, mbc);
for(std::size_t i = 0; i < jointIndex_.size(); ++i)
{
int index = jointIndex_[i];
int dofPos = mb.jointPosInDof(index);
AL_(dofPos) += Eigen::poly_eval(torqueL_[i], mbc.q[index][0]);
AU_(dofPos) += Eigen::poly_eval(torqueU_[i], mbc.q[index][0]);
}
}