void operator() (Image<value_type>& fod_img, Image<value_type>& dec_img) {
if (mask_img.valid()) {
assign_pos_of(fod_img, 0, 3).to(mask_img);
if (!mask_img.value()) {
dec_img.row(3).fill(UNIT);
return;
}
}
amps.noalias() = dectrans.sht * fod_img.row(3).cast<double>();
dec.setZero();
ampsum = 0.0;
for (ssize_t i = 0; i < amps.rows(); i++) {
if (!std::isnan(dectrans.thresh) && amps(i) < dectrans.thresh)
continue;
dec += dectrans.decs.row(i) * amps(i);
ampsum += amps(i);
}
dec = dec.cwiseMax(0.0);
ampsum = std::max(ampsum, 0.0);
decnorm = dec.norm();
if (decnorm == 0.0)
dec_img.row(3).fill(UNIT);
else
dec_img.row(3) = (dec / decnorm).cast<value_type>();
if (int_img.valid()) {
assign_pos_of(fod_img, 0, 3).to(int_img);
int_img.value() = (ampsum / amps.rows()) * 4.0 * Math::pi;
}
}
IGL_INLINE void igl::signed_distance_winding_number(
const AABB<Eigen::MatrixXd,3> & tree,
const Eigen::MatrixXd & V,
const Eigen::MatrixXi & F,
const igl::WindingNumberAABB<Eigen::Vector3d> & hier,
const Eigen::RowVector3d & q,
double & s,
double & sqrd,
int & i,
Eigen::RowVector3d & c)
{
using namespace Eigen;
using namespace std;
using namespace igl;
sqrd = tree.squared_distance(V,F,q,i,c);
const double w = hier.winding_number(q.transpose());
s = 1.-2.*w;
}
virtual void operate() {
tmp_theta_pos = this->feedbackjpInput.getValue();
ThetaInput << tmp_theta_pos[0], tmp_theta_pos[1], tmp_theta_pos[2], tmp_theta_pos[3];
M_tmp = this->M.getValue();
Md_tmp = this->Md.getValue();
tmp_theta_vel = this->feedbackjvInput.getValue();
ThetadotInput << tmp_theta_vel[0], tmp_theta_vel[1], tmp_theta_vel[2], tmp_theta_vel[3];
C_tmp = this->C.getValue();
Cd_tmp = this->Cd.getValue();
// Reference position, velocity and accelerations
qd[0] = this->referencejpInput.getValue()[0];
qd[1] = this->referencejpInput.getValue()[1];
qd[2] = this->referencejpInput.getValue()[2];
qd[3] = this->referencejpInput.getValue()[3];
qdd[0] = this->referencejvInput.getValue()[0];
qdd[1] = this->referencejvInput.getValue()[1];
qdd[2] = this->referencejvInput.getValue()[2];
qdd[3] = this->referencejvInput.getValue()[3];
qddd[0] = this->referencejaInput.getValue()[0];
qddd[1] = this->referencejaInput.getValue()[1];
qddd[2] = this->referencejaInput.getValue()[2];
qddd[3] = this->referencejaInput.getValue()[3];
//
//Position error
e = ThetaInput - qd;
//Velocity error
ed = ThetadotInput - qdd;
//Eta
eta = K / b;
//The error matrix
Xtilde.resize(8, 1);
Xtilde << e, ed;
Md_tmpinverse = Md_tmp.inverse();
M_tmpinverse = M_tmp.inverse();
F = -M_tmpinverse * (C_tmp);
Fd = -Md_tmpinverse * (Cd_tmp);
rho = F - Fd;
// The fbar vector
for (i = 0; i < 4; i = i + 1) {
fbar[i] = rho[i] + eta[i] * c[i] * Xtilde[i]
+ (c[i] + eta[i]) * Xtilde[4 + i] + eta[i] * phi[i];
}
//
Ftilde1 << Xtilde[4], -eta[0] * c[0] * Xtilde[0]
- (c[0] + eta[0]) * Xtilde[4] - eta[0] * phi[0], 0;
Ftilde2 << Xtilde[5], -eta[1] * c[1] * Xtilde[1]
- (c[1] + eta[1]) * Xtilde[5] - eta[1] * phi[1], 0;
Ftilde3 << Xtilde[6], -eta[2] * c[2] * Xtilde[2]
- (c[2] + eta[2]) * Xtilde[6] - eta[2] * phi[2], 0;
Ftilde4 << Xtilde[7], -eta[3] * c[3] * Xtilde[3]
- (c[3] + eta[3]) * Xtilde[7] - eta[3] * phi[3], 0;
//The L and Lbar vectors
for (i = 0; i < 4; i = i + 1) {
L[i] = 0.5
* (Xtilde[i] * Xtilde[i] + Xtilde[4 + i] * Xtilde[4 + i]);
Lbar[i] = L[i] + fbar[i] * fbar[i];
}
// The gradient matrices
DVDX1
<< (W1[0] * Xtilde[0] + W1[1] * Xtilde[4] + W1[2] * phi[0])
* W1[0], (W1[0] * Xtilde[0] + W1[1] * Xtilde[4]
+ W1[2] * phi[0]) * W1[1], (W1[0] * Xtilde[0]
+ W1[1] * Xtilde[4] + W1[2] * phi[0]) * W1[2];
DVDX2
<< (W2[0] * Xtilde[1] + W2[1] * Xtilde[5] + W2[2] * phi[1])
* W2[0], (W2[0] * Xtilde[1] + W2[1] * Xtilde[5]
+ W2[2] * phi[1]) * W2[1], (W2[0] * Xtilde[1]
+ W2[1] * Xtilde[5] + W2[2] * phi[1]) * W2[2];
DVDX3
<< (W3[0] * Xtilde[2] + W3[1] * Xtilde[6] + W3[2] * phi[2])
* W3[0], (W3[0] * Xtilde[2] + W3[1] * Xtilde[6]
+ W3[2] * phi[2]) * W3[1], (W3[0] * Xtilde[2]
+ W3[1] * Xtilde[6] + W3[2] * phi[2]) * W3[2];
DVDX4
<< (W4[0] * Xtilde[3] + W4[1] * Xtilde[7] + W4[2] * phi[3])
* W4[0], (W4[0] * Xtilde[3] + W4[1] * Xtilde[7]
+ W4[2] * phi[3]) * W4[1], (W4[0] * Xtilde[3]
+ W4[1] * Xtilde[7] + W4[2] * phi[3]) * W4[2];
DVDW1
<< (W1[0] * Xtilde[0] + W1[1] * Xtilde[4] + W1[2] * phi[0])
* Xtilde[0] + W1[0], (W1[0] * Xtilde[0]
+ W1[1] * Xtilde[4] + W1[2] * phi[0]) * Xtilde[4] + W1[1], (W1[0]
* Xtilde[0] + W1[1] * Xtilde[4] + W1[2] * phi[0]) * phi[0]
+ W1[2];
DVDW2
<< (W2[0] * Xtilde[1] + W2[1] * Xtilde[5] + W2[2] * phi[1])
* Xtilde[1] + W2[0], (W2[0] * Xtilde[1]
+ W2[1] * Xtilde[5] + W2[2] * phi[1]) * Xtilde[5] + W2[1], (W2[0]
* Xtilde[1] + W2[1] * Xtilde[5] + W2[2] * phi[1]) * phi[1]
+ W2[2];
DVDW3
<< (W3[0] * Xtilde[2] + W3[1] * Xtilde[6] + W3[2] * phi[2])
* Xtilde[2] + W3[0], (W3[0] * Xtilde[2]
+ W3[1] * Xtilde[6] + W3[2] * phi[2]) * Xtilde[6] + W3[1], (W3[0]
* Xtilde[2] + W3[1] * Xtilde[6] + W3[2] * phi[2]) * phi[2]
+ W3[2];
DVDW4
<< (W4[0] * Xtilde[3] + W4[1] * Xtilde[7] + W4[2] * phi[3])
* Xtilde[3] + W4[0], (W4[0] * Xtilde[3]
+ W4[1] * Xtilde[7] + W4[2] * phi[3]) * Xtilde[7] + W4[1], (W4[0]
* Xtilde[3] + W4[1] * Xtilde[7] + W4[2] * phi[3]) * phi[3]
+ W4[2];
////
s1 = DVDW1.transpose();
s2 = DVDW2.transpose();
s3 = DVDW3.transpose();
s4 = DVDW4.transpose();
Eigen::Matrix3d tmp_Gtilde;
tmp_Gtilde << Gtilde[0] * Gtilde[0], Gtilde[0] * Gtilde[1], Gtilde[0]
* Gtilde[2], Gtilde[1] * Gtilde[0], Gtilde[1] * Gtilde[1], Gtilde[1]
* Gtilde[2], Gtilde[2] * Gtilde[0], Gtilde[2] * Gtilde[1], Gtilde[2]
* Gtilde[2];
//
r1 = -Lbar[0] + 0.25 * DVDX1.dot(tmp_Gtilde * DVDX1)
- DVDX1.dot(Ftilde1);
r2 = -Lbar[1] + 0.25 * DVDX2.dot(tmp_Gtilde * DVDX2)
- DVDX2.dot(Ftilde2);
r3 = -Lbar[2] + 0.25 * DVDX3.dot(tmp_Gtilde * DVDX3)
- DVDX3.dot(Ftilde3);
r4 = -Lbar[3] + 0.25 * DVDX4.dot(tmp_Gtilde * DVDX4)
- DVDX4.dot(Ftilde4);
W1dot = s1.transpose() / (s1.dot(s1)) * r1;
W2dot = s2.transpose() / (s2.dot(s2)) * r2;
W3dot = s3.transpose() / (s3.dot(s3)) * r3;
W4dot = s4.transpose() / (s4.dot(s4)) * r4;
////
ueq1 = 0.5
* (W1[0] * (W1[2] - W1[1]) * Xtilde[0]
+ W1[1] * (W1[2] - W1[1]) * Xtilde[4]
+ W1[2] * (W1[2] - W1[1]) * phi[0]);
ueq2 = 0.5
* (W2[0] * (W2[2] - W2[1]) * Xtilde[1]
+ W2[1] * (W2[2] - W2[1]) * Xtilde[5]
+ W2[2] * (W2[2] - W2[1]) * phi[1]);
ueq3 = 0.5
* (W3[0] * (W3[2] - W3[1]) * Xtilde[2]
+ W3[1] * (W3[2] - W3[1]) * Xtilde[6]
+ W3[2] * (W3[2] - W3[1]) * phi[2]);
ueq4 = 0.5
* (W4[0] * (W4[2] - W4[1]) * Xtilde[3]
+ W4[1] * (W4[2] - W4[1]) * Xtilde[7]
+ W4[2] * (W4[2] - W4[1]) * phi[3]);
//
// Final torque computations
Ueq << ueq1, ueq2, ueq3, ueq4;
Ud = qddd + Md_tmpinverse * Cd_tmp;
Tau = M_tmp * (Ud + Ueq); // Final torque to system.
//
phidot[0] = -fbar[0] - ueq1;
phidot[1] = -fbar[1] - ueq2;
phidot[2] = -fbar[2] - ueq3;
phidot[3] = -fbar[3] - ueq4;
phi = phi + phidot * del;
W1 = W1 + W1dot * del;
W2 = W2 + W2dot * del;
W3 = W3 + W3dot * del;
W4 = W4 + W4dot * del;
torque_tmp[0] = Tau[0];
torque_tmp[1] = Tau[1];
torque_tmp[2] = Tau[2];
torque_tmp[3] = Tau[3];
// torque_tmp[0] = 0;
// torque_tmp[1] = 0;
// torque_tmp[2] = 0;
// torque_tmp[3] = 0;
optslidecontrolOutputValue->setData(&torque_tmp);
}
virtual void Transform(_Out_ frame_view target_frame, _In_ const_frame_view source_frame, _In_ const_frame_view last_frame, float frame_time) override
{
//if (!g_UseVelocity)
//{
// Transform(target_frame, source_frame);
// return;
//}
const auto& blocks = *pBlockArmature;
int pvDim = inputExtractor.GetDimension(*blocks[0]);
RowVectorXd X(g_UseVelocity ? pvDim * 2 : pvDim), Y;
double semga = 1000;
RowVectorXf yf;
std::vector<RowVectorXd> Xabs;
for (auto& block : blocks)
{
//X[0] *= 13;
if (block->Index > 0 && block->ActiveActions.size() > 0)
{
auto& sik = pController->GetStylizedIK(block->Index);
auto& gpr = sik.Gplvm();
auto& joints = block->Joints;
RowVectorXf xf = inputExtractor.Get(*block, source_frame);
RowVectorXf xfl = inputExtractor.Get(*block, last_frame);
yf = outputExtractor.Get(*block, target_frame);
auto xyf = inputExtractor.Get(*block, target_frame);
auto pDecoder = sik.getDecoder();
auto baseRot = target_frame[block->parent()->Joints.back()->ID].GblRotation;
sik.setBaseRotation(baseRot);
sik.setChain(block->Joints, target_frame);
//sik.SetGplvmWeight(block->Wx.cast<double>());
//std::vector<DirectX::Quaternion, XMAllocator> corrrots(joints.size());
//std::vector<DirectX::Quaternion, XMAllocator> rots(joints.size());
//for (int i = 0; i < joints.size(); i++)
//{
// corrrots[i] = target_frame[joints[i]->ID].LclRotation;
//}
//(*pDecoder)(rots.data(), yf.cast<double>());
////outputExtractor.Set(*block, target_frame, yf);
//auto ep = sik.EndPosition(reinterpret_cast<XMFLOAT4A*>(rots.data()));
X.segment(0, pvDim) = xf.cast<double>();
auto Xd = X.segment(0, pvDim);
auto uXd = gpr.uX.segment(0, pvDim);
//auto uXv = block->PdGpr.uX.segment<3>(3);
//Xv = (Xv - uXv).array() * g_NoiseInterpolation.array() + uXv.array();
Xd = (Xd - uXd).array();
double varZ = (Xd.array() * (g_NoiseInterpolation.array() - 1.0)).cwiseAbs2().sum();
// if no noise
varZ = std::max(varZ, 1e-5);
Xd = Xd.array() * g_NoiseInterpolation.array() + uXd.array();
RowVector3d Xld = (xfl.cast<double>() - uXd).array() * g_NoiseInterpolation.array() + uXd.array();
if (g_UseVelocity)
{
auto Xv = X.segment(pvDim, pvDim);
Xv = (Xd - Xld) / (frame_time * g_FrameTimeScaleFactor);
}
xf = X.cast<float>();
SetVisualizeHandle(block, xf);
//m_Xs.row(block->Index) = X;
Xabs.emplace_back(X);
// Beyesian majarnlize over X
//size_t detail = 3;
//MatrixXd Xs(detail*2+1,g_PvDimension), Ys;
//Xs = gaussian_sample(X, X, detail);
//VectorXd Pxs = (Xs - X.replicate(detail * 2 + 1, 1)).cwiseAbs2().rowwise().sum();
//Pxs = (-Pxs.array() / semga).exp();
//VectorXd Py_xs = block->PdGpr.get_expectation_and_likelihood(Xs, &Ys);
//Py_xs = (-Py_xs.array()).exp() * Pxs.array();
//Py_xs /= Py_xs.sum();
//Y = (Ys.array() * Py_xs.replicate(1, Ys.cols()).array()).colwise().sum();
MatrixXd covObsr(g_PvDimension, g_PvDimension);
covObsr.setZero();
covObsr.diagonal() = g_NoiseInterpolation.replicate(1, g_PvDimension / 3).transpose() * varZ;
//block->PdGpr.get_expectation_from_observation(X, covObsr, &Y);
//block->PdGpr.get_expectation(X, &Y);
//auto yc = yf;
//yf = Y.cast<float>();
//yf.array() *= block->Wx.cwiseInverse().array().transpose();
//block->PdStyleIk.SetHint();
if (!g_UseVelocity)
Y = sik.apply(X.transpose(), baseRot).cast<double>();
else
Y = sik.apply(X.segment(0, pvDim).transpose(), Vector3d(X.segment(pvDim, pvDim).transpose()), baseRot).cast<double>();
//block->PdStyleIk.SetGoal(X.leftCols<3>());
//auto scoref = block->PdStyleIk.objective(X, yf.cast<double>());
//auto scorec = block->PdStyleIk.objective(X, yc.cast<double>());
//std::cout << "Gpr score : " << scoref << " ; Cannonical score : " << scorec << endl;
//auto ep = sik.EndPosition(yf.cast<double>());
//Y = yf.cast<double>();
outputExtractor.Set(*block, target_frame, Y.cast<float>());
for (int i = 0; i < block->Joints.size(); i++)
{
target_frame[block->Joints[i]->ID].UpdateGlobalData(target_frame[block->Joints[i]->parent()->ID]);
}
auto ep2 = target_frame[block->Joints.back()->ID].GblTranslation -
target_frame[block->Joints[0]->parent()->ID].GblTranslation;
//break;
}
}
// Fill Xabpv
if (g_EnableDependentControl)
{
RowVectorXd Xabpv;
Xabpv.resize(pController->uXabpv.size());
int i = 0;
for (const auto& xab : Xabs)
{
auto Yi = Xabpv.segment(i, xab.size());
Yi = xab;
i += xab.size();
}
auto _x = (Xabpv.cast<float>() - pController->uXabpv) * pController->XabpvT;
auto _xd = _x.cast<double>().eval();
for (auto& block : blocks)
{
if (block->ActiveActions.size() == 0 && block->SubActiveActions.size() > 0)
{
auto& sik = pController->GetStylizedIK(block->Index);
auto& gpr = sik.Gplvm();
auto lk = gpr.get_expectation_and_likelihood(_xd, &Y);
yf = Y.cast<float>();
//yf *= block->Wx.cwiseInverse().asDiagonal();
outputExtractor.Set(*block, target_frame, yf);
}
}
}
target_frame[0].LclTranslation = source_frame[0].LclTranslation;
target_frame[0].GblTranslation = source_frame[0].GblTranslation;
FrameRebuildGlobal(*m_tArmature, target_frame);
}
void StdCostFunc::impl_gradient(gradient_t& gradient,
const argument_t& x, size_type /* functionId */) const
{
gradient.reserve(robotDatas_[0].pgdata->pbSize());
gradient.setZero();
for(RobotData& rd: robotDatas_)
{
rd.pgdata->x(x);
if(rd.postureScale > 0.)
{
int index = rd.pgdata->qParamsBegin();
const std::vector<std::vector<double>>& q = rd.pgdata->mbc().q;
double coef = 2.*rd.postureScale*scale_;
for(int i = 0; i < rd.pgdata->multibody().nrJoints(); ++i)
{
if(rd.pgdata->multibody().joint(i).params() == 1)
{
gradient.coeffRef(index) += coef*(q[i][0] - rd.tq[i][0]);
}
index += rd.pgdata->mb().joint(i).dof();
}
}
if(rd.forceScale > 0.)
{
int index = rd.pgdata->forceParamsBegin();
for(const auto& fd: rd.pgdata->forceDatas())
{
for(std::size_t i = 0; i < fd.forces.size(); ++i)
{
Eigen::Vector3d forceTmp = fd.forces[i].force()*rd.forceScale*scale_*2.;
gradient.coeffRef(index + 0) += forceTmp.x();
gradient.coeffRef(index + 1) += forceTmp.y();
gradient.coeffRef(index + 2) += forceTmp.z();
index += 3;
}
}
}
for(const ForceContactMinimizationData& fcmd: rd.forceContactsMin)
{
const auto& forceData = rd.pgdata->forceDatas()[fcmd.forcePos];
int index = int(fcmd.gradientPos);
for(std::size_t i = 0; i < forceData.forces.size(); ++i)
{
Eigen::Vector3d forceTmp = forceData.forces[i].force()*fcmd.scale*scale_*2.;
gradient.coeffRef(index + 0) += forceTmp.x();
gradient.coeffRef(index + 1) += forceTmp.y();
gradient.coeffRef(index + 2) += forceTmp.z();
index += 3;
}
}
for(TorqueContactMinimizationData& tcmd: rd.torqueContactsMin)
{
const auto& forceData = rd.pgdata->forceDatas()[tcmd.forcePos];
const sva::PTransformd& X_0_b = rd.pgdata->mbc().bodyPosW[tcmd.bodyIndex];
int forceIndex = int(tcmd.gradientPos);
for(std::size_t pi = 0; pi < tcmd.points.size(); ++pi)
{
Eigen::Vector3d fBody(X_0_b.rotation()*forceData.forces[pi].force());
Eigen::Vector3d lever = tcmd.levers[pi].cross(fBody);
Eigen::Matrix3d crossMat = sva::vector3ToCrossMatrix(tcmd.levers[pi]);
Eigen::RowVector3d dotCrossDiff(tcmd.axis.transpose()*crossMat);
double squareDiff = 2.*tcmd.axis.dot(lever)*tcmd.scale*scale_;
const auto& qDiffX = tcmd.jac.vectorJacobian(rd.pgdata->mb(), rd.pgdata->mbc(),
Eigen::Vector3d::UnitX()).block(3, 0, 3, tcmd.jac.dof());
tcmd.jacMatTmp.row(0) = forceData.forces[pi].force().transpose()*qDiffX;
const auto& qDiffY = tcmd.jac.vectorJacobian(rd.pgdata->mb(), rd.pgdata->mbc(),
Eigen::Vector3d::UnitY()).block(3, 0, 3, tcmd.jac.dof());
tcmd.jacMatTmp.row(1) = forceData.forces[pi].force().transpose()*qDiffY;
const auto& qDiffZ = tcmd.jac.vectorJacobian(rd.pgdata->mb(), rd.pgdata->mbc(),
Eigen::Vector3d::UnitZ()).block(3, 0, 3, tcmd.jac.dof());
tcmd.jacMatTmp.row(2) = forceData.forces[pi].force().transpose()*qDiffZ;
tcmd.jacMat.noalias() = (squareDiff*dotCrossDiff)*tcmd.jacMatTmp;
updateFullJacobianSparse(rd.pgdata->mb(), tcmd.jac, tcmd.jacMat, tcmd.jacMatFull,
{0, rd.pgdata->qParamsBegin()});
gradient += tcmd.jacMatFull.transpose();
Eigen::RowVector3d fDiff;
fDiff = squareDiff*dotCrossDiff*X_0_b.rotation();
gradient.coeffRef(forceIndex + 0) += fDiff.x();
gradient.coeffRef(forceIndex + 1) += fDiff.y();
gradient.coeffRef(forceIndex + 2) += fDiff.z();
forceIndex += 3;
}
}
for(NormalForceTargetData& nft: rd.normalForceTargets)
{
const auto& forceData = rd.pgdata->forceDatas()[nft.forcePos];
const sva::PTransformd& X_0_b = rd.pgdata->mbc().bodyPosW[forceData.bodyIndex];
double nForceSum = 0.;
for(std::size_t pi = 0; pi < forceData.points.size(); ++pi)
{
// force in the point pi frame
sva::PTransformd X_0_pi = forceData.points[pi]*X_0_b;
nForceSum += (X_0_pi.rotation()*forceData.forces[pi].force()).z();
}
double error = nForceSum - nft.target;
int forceIndex = int(nft.gradientPos);
double scale = scale_*nft.scale;
for(std::size_t pi = 0; pi < forceData.points.size(); ++pi)
{
double coef = 2.*scale*error;
sva::PTransformd X_0_pi = forceData.points[pi]*X_0_b;
const auto& jacPointMat =
nft.jac.vectorJacobian(rd.pgdata->mb(),
rd.pgdata->mbc(),
forceData.points[pi].rotation().row(2).transpose())\
.block(3, 0, 3, nft.jac.dof());
nft.jacMat.noalias() = (coef*forceData.forces[pi].force().transpose())*
jacPointMat;
updateFullJacobianSparse(rd.pgdata->mb(), nft.jac, nft.jacMat,
nft.jacMatFull,
{0, rd.pgdata->qParamsBegin()});
gradient += nft.jacMatFull.transpose();
Eigen::RowVector3d fDiff;
fDiff = coef*X_0_pi.rotation().row(2);
gradient.coeffRef(forceIndex + 0) += fDiff.x();
gradient.coeffRef(forceIndex + 1) += fDiff.y();
gradient.coeffRef(forceIndex + 2) += fDiff.z();
forceIndex += 3;
}
}
for(TangentialForceMinimizationData& tfm: rd.tanForceMin)
{
const auto& forceData = rd.pgdata->forceDatas()[tfm.forcePos];
const sva::PTransformd& X_0_b = rd.pgdata->mbc().bodyPosW[forceData.bodyIndex];
int forceIndex = int(tfm.gradientPos);
double scale = scale_*tfm.scale;
for(std::size_t pi = 0; pi < forceData.points.size(); ++pi)
{
sva::PTransformd X_0_pi = forceData.points[pi]*X_0_b;
Eigen::Vector3d fPoint(X_0_pi.rotation()*forceData.forces[pi].force());
double coefT = 2.*scale*fPoint.x();
double coefB = 2.*scale*fPoint.y();
auto fillGradient =
[forceIndex, pi, &X_0_pi, &forceData, &rd, &gradient, &tfm](int row, double coef)
{
const auto& jacPointMat =
tfm.jac.vectorJacobian(rd.pgdata->mb(),
rd.pgdata->mbc(),
forceData.points[pi].rotation().row(row).transpose())\
.block(3, 0, 3, tfm.jac.dof());
tfm.jacMat.noalias() = (coef*forceData.forces[pi].force().transpose())*
jacPointMat;
updateFullJacobianSparse(rd.pgdata->mb(), tfm.jac, tfm.jacMat,
tfm.jacMatFull,
{0, rd.pgdata->qParamsBegin()});
gradient += tfm.jacMatFull.transpose();
Eigen::RowVector3d fDiff;
fDiff = coef*X_0_pi.rotation().row(row);
gradient.coeffRef(forceIndex + 0) += fDiff.x();
gradient.coeffRef(forceIndex + 1) += fDiff.y();
gradient.coeffRef(forceIndex + 2) += fDiff.z();
};
fillGradient(0, coefT);
fillGradient(1, coefB);
forceIndex += 3;
}
}
for(BodyPositionTargetData& bp: rd.bodyPosTargets)
{
Eigen::Vector3d error(rd.pgdata->mbc().bodyPosW[bp.bodyIndex].translation()
- bp.target);
const Eigen::MatrixXd& jacMat = bp.jac.jacobian(rd.pgdata->mb(), rd.pgdata->mbc());
bp.jacMat.noalias() = (scale_*bp.scale*2.*error.transpose())*\
jacMat.block(3, 0, 3, bp.jac.dof());
updateFullJacobianSparse(rd.pgdata->mb(), bp.jac, bp.jacMat, bp.jacMatFull,
{0, rd.pgdata->qParamsBegin()});
gradient += bp.jacMatFull.transpose();
}
for(BodyOrientationTargetData& bo: rd.bodyOriTargets)
{
Eigen::Vector3d error(sva::rotationError(bo.target,
rd.pgdata->mbc().bodyPosW[bo.bodyIndex].rotation(), 1e-7));
const Eigen::MatrixXd& jacMat = bo.jac.jacobian(rd.pgdata->mb(), rd.pgdata->mbc());
bo.jacMat.noalias() = (scale_*bo.scale*2.*error.transpose())*\
jacMat.block(0, 0, 3, bo.jac.dof());
updateFullJacobianSparse(rd.pgdata->mb(), bo.jac, bo.jacMat, bo.jacMatFull,
{0, rd.pgdata->qParamsBegin()});
gradient += bo.jacMatFull.transpose();
}
}
}
void FrictionConeConstr::impl_jacobian(jacobian_t& jac, const argument_t& x) const
{
pgdata_->x(x);
jac.reserve(nrNonZero_);
int index = 0;
for(const PGData::ForceData& fd: pgdata_->forceDatas())
{
const sva::PTransformd& X_0_b = pgdata_->mbc().bodyPosW[fd.bodyIndex];
double muSquare = std::pow(fd.mu, 2);
for(std::size_t i = 0; i < fd.points.size(); ++i)
{
sva::PTransformd X_0_pi = fd.points[i]*X_0_b;
Eigen::Vector3d fBody(X_0_pi.rotation()*fd.forces[i].force());
// Z axis
// dq
// -(mu*forceB.z())^2 => -2*mu^2*forceB.z()*forceW*jacZ
//
// X axis
// dq
// forceB.x()**2 => 2*forceB.x()*forceW*jacX
//
// Y axis
// dq
// forceB.y()**2 => 2*forceB.y()*forceW*jacY
const auto& jacPointVecZMat =
jacPoints_[index].vectorJacobian(pgdata_->mb(),
pgdata_->mbc(),
fd.points[i].rotation().row(2).transpose())\
.block(3, 0, 3, jacPoints_[index].dof());
jacPointsMatTmp_[index].row(0).noalias() =
(-2.*muSquare*fBody.z())*(fd.forces[i].force().transpose()*jacPointVecZMat);
const auto& jacPointVecXMat =
jacPoints_[index].vectorJacobian(pgdata_->mb(),
pgdata_->mbc(),
fd.points[i].rotation().row(0).transpose())\
.block(3, 0, 3, jacPoints_[index].dof());
jacPointsMatTmp_[index].noalias() +=
(2.*fBody.x())*(fd.forces[i].force().transpose()*jacPointVecXMat);
const auto& jacPointVecYMat =
jacPoints_[index].vectorJacobian(pgdata_->mb(),
pgdata_->mbc(),
fd.points[i].rotation().row(1).transpose())\
.block(3, 0, 3, jacPoints_[index].dof());
jacPointsMatTmp_[index].noalias() +=
(2.*fBody.y())*(fd.forces[i].force().transpose()*jacPointVecYMat);
fullJacobianSparse(pgdata_->mb(), jacPoints_[index], jacPointsMatTmp_[index],
jac, {index, pgdata_->qParamsBegin()});
// Z axis
// dforceW
// -(mu*forceB.z())^2 => -2*mu^2*forceB.z()*X_0_pi_rowZ
//
// X axis
// dforceW
// forceB.x()**2 => 2*forceB.x()*X_0_pi_rowX
//
// Y axis
// dforceW
// forceB.y()**2 => 2*forceB.y()*X_0_pi_rowY
Eigen::RowVector3d fDiff = (-2.*muSquare*fBody.z())*X_0_pi.rotation().row(2);
fDiff.noalias() += (2.*fBody.x())*X_0_pi.rotation().row(0);
fDiff.noalias() += (2.*fBody.y())*X_0_pi.rotation().row(1);
int indexCols = pgdata_->forceParamsBegin() + index*3;
jac.insert(index, indexCols + 0) = fDiff(0);
jac.insert(index, indexCols + 1) = fDiff(1);
jac.insert(index, indexCols + 2) = fDiff(2);
++index;
}
}
}