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});
}
}
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();
}
}