Example #1
0
  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;
    }

  }
Example #2
0
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);
	}
Example #5
0
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();
    }
  }
}
Example #6
0
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;
    }
  }
}