Exemple #1
0
void check_values(const std::vector<SimpleFrame*>& targets,
                  const std::vector<SimpleFrame*>& followers,
                  const Frame* relativeTo,
                  const Frame* inCoordinatesOf,
                  double tolerance)
{
  for(std::size_t i=0; i<targets.size(); ++i)
  {
    Frame* T = targets[i];
    Frame* F = followers[i];

    const Eigen::Isometry3d& tf_error =
        T->getTransform(relativeTo)*F->getTransform(relativeTo).inverse();
    Eigen::Vector6d error;
    Eigen::AngleAxisd rot_error(tf_error.rotation());
    error.block<3,1>(0,0) = rot_error.angle()*rot_error.axis();
    error.block<3,1>(3,0) = tf_error.translation();

    EXPECT_TRUE( error.norm() < tolerance );

    EXPECT_TRUE( equals(T->getSpatialVelocity(relativeTo, inCoordinatesOf),
                        F->getSpatialVelocity(relativeTo, inCoordinatesOf),
                        tolerance) );

    EXPECT_TRUE( equals(T->getLinearVelocity(relativeTo, inCoordinatesOf),
                        F->getLinearVelocity(relativeTo, inCoordinatesOf),
                        tolerance) );

    EXPECT_TRUE( equals(T->getAngularVelocity(relativeTo, inCoordinatesOf),
                        F->getAngularVelocity(relativeTo, inCoordinatesOf),
                        tolerance) );

    EXPECT_TRUE( equals(T->getSpatialAcceleration(relativeTo, inCoordinatesOf),
                        F->getSpatialAcceleration(relativeTo, inCoordinatesOf),
                        tolerance) );

    EXPECT_TRUE( equals(T->getLinearAcceleration(relativeTo, inCoordinatesOf),
                        F->getLinearAcceleration(relativeTo, inCoordinatesOf),
                        tolerance) );

    EXPECT_TRUE( equals(T->getAngularAcceleration(relativeTo, inCoordinatesOf),
                        F->getAngularAcceleration(relativeTo, inCoordinatesOf),
                        tolerance) );
  }
}
Exemple #2
0
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;
}