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 MotionConstr::update(const rbd::MultiBody& mb, const rbd::MultiBodyConfig& mbc,
const SolverData& /* data */)
{
computeMatrix(mb, mbc);
AL_.segment(mb.joint(0).dof(), nrTor_) += torqueL_;
AU_.segment(mb.joint(0).dof(), nrTor_) += torqueU_;
}
void MotionConstrCommon::torque(const rbd::MultiBody& mb, rbd::MultiBodyConfig& mbc) const
{
int pos = mb.joint(0).dof();
for(std::size_t i = 1; i < mbc.jointTorque.size(); ++i)
{
for(double& d: mbc.jointTorque[i])
{
d = curTorque_(pos);
++pos;
}
}
}
void MotionSpringConstr::update(const rbd::MultiBody& mb, const rbd::MultiBodyConfig& mbc,
const SolverData& /* data */)
{
computeMatrix(mb, mbc);
for(const SpringJointData& sj: springs_)
{
double spring = mbc.q[sj.index][0]*sj.K + mbc.alpha[sj.index][0]*sj.C + sj.O;
torqueL_(sj.posInDof) = -spring;
torqueU_(sj.posInDof) = -spring;
}
AL_.segment(mb.joint(0).dof(), nrTor_) += torqueL_;
AU_.segment(mb.joint(0).dof(), nrTor_) += torqueU_;
}
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_);
}
MotionPolyConstr::MotionPolyConstr(const rbd::MultiBody& mb,
const std::vector<std::vector<Eigen::VectorXd>>& lTorqueBounds,
const std::vector<std::vector<Eigen::VectorXd>>& uTorqueBounds):
MotionConstrCommon(mb),
torqueL_(),
torqueU_()
{
// remove non managed joint
for(int i = 0; i < mb.nrJoints(); ++i)
{
if(mb.joint(i).dof() == 1)
{
jointIndex_.push_back(i);
torqueL_.push_back(lTorqueBounds[i][0]);
torqueU_.push_back(uTorqueBounds[i][0]);
}
}
}
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;
}
}
}
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();
}
}
std::string MotionConstrCommon::descInEq(const rbd::MultiBody& mb, int line)
{
int jIndex = findJointFromVector(mb, line, true);
return std::string("Joint: ") + mb.joint(jIndex).name();
}
void writeUrdf(const std::string& filename, const std::string& robotName,
const rbd::MultiBody& mb, const Limits& limits)
{
urdf::ModelInterface model;
model.name_ = robotName;
for(const rbd::Body& b: mb.bodies())
{
urdf::Link link;
link.name = b.name();
if(b.inertia().mass() != 0.)
{
urdf::Inertial inertial;
Eigen::Vector3d com = b.inertia().momentum()/b.inertia().mass();
Eigen::Matrix3d inertiaInCoM = sva::inertiaToOrigin<double>(
b.inertia().inertia(), -b.inertia().mass(), com,
Eigen::Matrix3d::Identity());
inertial.ixx = inertiaInCoM(0,0);
inertial.ixy = inertiaInCoM(0,1);
inertial.ixz = inertiaInCoM(0,2);
inertial.iyy = inertiaInCoM(1,1);
inertial.iyz = inertiaInCoM(1,2);
inertial.izz = inertiaInCoM(2,2);
inertial.origin = transformToPose(sva::PTransformd(com));
inertial.mass = b.inertia().mass();
link.inertial = boost::make_shared<urdf::Inertial>(inertial);
}
model.links_[b.name()] = boost::make_shared<urdf::Link>(link);
}
// don't read the first joint (that a virtual joint add by MultiBodyGraph)
for(int i = 1; i < mb.nrJoints(); ++i)
{
const rbd::Joint& j = mb.joint(i);
urdf::Joint joint;
joint.name = j.name();
joint.child_link_name = mb.body(mb.successor(i)).name();
joint.parent_link_name = mb.body(mb.predecessor(i)).name();
joint.parent_to_joint_origin_transform = transformToPose(mb.transform(i));
fillUrdfJoint(j, limits, joint);
// only export limits if there is position limits or
// velocity and effort limits
if((exist(limits.ql, j.id()) && exist(limits.qu, j.id())) ||
((exist(limits.vl, j.id()) && exist(limits.vu, j.id())) &&
(exist(limits.tl, j.id()) && exist(limits.tu, j.id()))))
{
urdf::JointLimits urdfLimits;
if(exist(limits.ql, j.id()) && exist(limits.qu, j.id()))
{
urdfLimits.lower = limits.ql.at(j.id());
urdfLimits.upper = limits.qu.at(j.id());
}
if(exist(limits.vl, j.id()) && exist(limits.vu, j.id()))
{
urdfLimits.velocity = std::min(std::abs(limits.vl.at(j.id())),
limits.vu.at(j.id()));
}
if(exist(limits.tl, j.id()) && exist(limits.tu, j.id()))
{
urdfLimits.effort = std::min(std::abs(limits.tl.at(j.id())),
limits.tu.at(j.id()));
}
joint.limits = boost::make_shared<urdf::JointLimits>(urdfLimits);
}
model.joints_[j.name()] = boost::make_shared<urdf::Joint>(joint);
}
model.root_link_ = model.links_[mb.body(0).name()];
writeUrdf(filename, model);
}