예제 #1
0
urdf::Pose transformToPose(const sva::PTransformd& X)
{
  urdf::Pose pose;
  pose.position = fromEigen(X.translation());
  pose.rotation = fromEigen(Eigen::Matrix3d(X.rotation().inverse()));
  return pose;
}
예제 #2
0
CylindricalPositionConstr::CylindricalPositionConstr(
  PGData* pgdata, int bodyId,
  const sva::PTransformd& targetFrame,
  const sva::PTransformd& surfaceFrame)
  : roboptim::DifferentiableSparseFunction(pgdata->pbSize(), 3, "FreeGripperPositionContact")
  , pgdata_(pgdata)
  , bodyIndex_(pgdata->multibody().bodyIndexById(bodyId))
  , targetFrame_(targetFrame)
  , surfaceFrame_(surfaceFrame)
  , jac_(pgdata->mb(), bodyId, surfaceFrame.translation())
  , jacMat_(3, jac_.dof())
{}
예제 #3
0
파일: IK.cpp 프로젝트: aescande/RBDyn
bool InverseKinematics::inverseKinematics(const MultiBody& mb, MultiBodyConfig& mbc,
                                          const sva::PTransformd& ef_target)
{
	int iter = 0;
	bool converged = false;
	int dof = 0;
	rbd::forwardKinematics(mb, mbc);
	Eigen::MatrixXd jacMat;
	Eigen::Vector6d v = Eigen::Vector6d::Ones();
	Eigen::Vector3d rotErr;
	Eigen::VectorXd res = Eigen::VectorXd::Zero(3);
	while( ! converged && iter < max_iterations_)
	{
		jacMat = jac_.jacobian(mb, mbc);
		//non-strict zeros in jacobian can be a problem...
		jacMat = jacMat.unaryExpr(CwiseRoundOp(-almost_zero_, almost_zero_));
		svd_.compute(jacMat, Eigen::ComputeThinU | Eigen::ComputeThinV);
		rotErr = sva::rotationError(mbc.bodyPosW[ef_index_].rotation(),
		                            ef_target.rotation());
		v << rotErr, ef_target.translation() - mbc.bodyPosW[ef_index_].translation();
		converged = v.norm() < threshold_;
		res = svd_.solve(v);

		dof = 0;
		for(auto index : jac_.jointsPath())
		{
			std::vector<double>& qi = mbc.q[index];
			for(auto &qv : qi)
			{
				qv += lambda_*res[dof];
				++dof;
			}
		}

		rbd::forwardKinematics(mb, mbc);
		rbd::forwardVelocity(mb, mbc);
		iter++;
	}
	return converged;
}