bool PoseOptimizationObjectiveFunction::getLocalHessian(numopt_common::SparseMatrix& hessian,
const numopt_common::Parameterization& params, bool newParams)
{
// Numerical approach.
// numopt_common::SparseMatrix numericalHessian(params.getLocalSize(), params.getLocalSize());
// NonlinearObjectiveFunction::estimateLocalHessian(numericalHessian, params, 1.0e-6);
// std::cout << "Numerical:\n" << numericalHessian << std::endl;
// Analytical approach.
Eigen::MatrixXd analyticalHessian(params.getLocalSize(), params.getLocalSize());
analyticalHessian.setZero();
const auto& poseParameterization = dynamic_cast<const PoseParameterization&>(params);
const Pose pose = poseParameterization.getPose();
const Eigen::Vector3d& p = pose.getPosition().vector();
const Eigen::Matrix3d p_skew = kindr::getSkewMatrixFromVector(p);
const RotationQuaternion Phi(pose.getRotation());
// Default leg position.
for (const auto& footPosition : stance_) {
const Eigen::Vector3d& f_i = footPosition.second.vector();
const Eigen::Matrix3d f_i_skew = kindr::getSkewMatrixFromVector(f_i);
const Eigen::Vector3d Phi_d_i = pose.getRotation().rotate(nominalStanceInBaseFrame_.at(footPosition.first)).vector();
const Eigen::Matrix3d Phi_d_i_skew = kindr::getSkewMatrixFromVector(Phi_d_i);
analyticalHessian.topLeftCorner(3, 3) += Eigen::Matrix3d::Identity();
analyticalHessian.topRightCorner(3, 3) += -Phi_d_i_skew;
analyticalHessian.bottomLeftCorner(3, 3) += Phi_d_i_skew;
analyticalHessian.bottomRightCorner(3, 3) += 0.5 * (p_skew * Phi_d_i_skew + Phi_d_i_skew * p_skew
-f_i_skew * Phi_d_i_skew - Phi_d_i_skew * f_i_skew);
}
// Center of mass.
const Eigen::Vector3d p_bar(p.x(), p.y(), 0.0); // Projection.
Eigen::Vector3d Phi_r_com = pose.getRotation().rotate(centerOfMassInBaseFrame_).vector();
Phi_r_com.z() = 0.0;
const Eigen::Matrix3d Phi_r_com_skew = kindr::getSkewMatrixFromVector(Phi_r_com);
const grid_map::Position supportPolygonCentroid(supportRegion_.getCentroid());
const Eigen::Vector3d r_centroid(supportPolygonCentroid.x(), supportPolygonCentroid.y(), 0.0);
const Eigen::Matrix3d r_centroid_skew = kindr::getSkewMatrixFromVector(r_centroid);
analyticalHessian.topLeftCorner(3, 3) += comWeight_ * Eigen::Vector3d(1.0, 1.0, 0.0).asDiagonal();
analyticalHessian.topRightCorner(3, 3) += -comWeight_ * Phi_r_com_skew;
analyticalHessian.bottomLeftCorner(3, 3) += comWeight_ * Phi_r_com_skew;
analyticalHessian.bottomRightCorner(3, 3) += 0.5 * comWeight_ * (p_skew * Phi_r_com_skew + Phi_r_com_skew * p_skew
-r_centroid_skew * Phi_r_com_skew - Phi_r_com_skew * r_centroid_skew);
// Factorized with 2.0 (not weight!).
analyticalHessian = 2.0 * analyticalHessian;
// std::cout << "Analytical:\n" << analyticalHessian << std::endl << std::endl;
// Return solution.
// hessian = numericalHessian;
hessian = analyticalHessian.sparseView(1e-10);
return true;
}
void Joint::rotate(){
//not sure if this is how it works...
//local_pose.orientation = local_pose.orientation + rotate;
//PoseEuler temp = PoseEuler(vertex3(), vertex3(0.0, 0.0, angle));
// local_transformation = local_transformation * ( temp.getRotation()* angularVelocity);
Pose currentPose = path->update();
float x, y;
x = currentPose.position.getx();
y = currentPose.position.gety();
angle = -PI/2.0 + (float) (std::atan2(y, x) - std::atan2(1.0, 0.0));
//REALLY MEANS: angle = angularVelocity * dt + angle0;
//angle += angularVelocity;
//std::cout << "x:" << x << ", y:" << y << ", theta:" << angle << std::endl;
Pose temp = Pose(vertex3(), quaternion(angle, 0.0, 0.0));
//Pose temp = Pose(vertex3(), quaternion(0.0, angle, 0.0));
local_transformation = temp.getRotation();
//local_transformation = ( temp.getRotation() * angularVelocity);
//if(!isNull()){
// for(unsigned int i=0; i<children.size(); i++){
// this->children.at(i)->child->rotate(rotate);
// }
//}
}
bool Pose::operator==(const Pose& right) const {
// invalid poses return false for comparison, even against identical invalid poses, like NaN
if (!valid || !right.valid) {
return false;
}
// FIXME add margin of error? Or add an additional withinEpsilon function?
return translation == right.getTranslation() && rotation == right.getRotation() &&
velocity == right.getVelocity() && angularVelocity == right.getAngularVelocity();
}
Transform Pose::transFromPose(const Pose &otherPose) const {
Eigen::Vector3f translationVector =
mVector - otherPose.getVector();
Eigen::Quaternionf rotationQuaternion =
mRotation * otherPose.getRotation().conjugate();
Transform returnValue(translationVector);
return returnValue;
}
bool PoseOptimizationObjectiveFunction::getLocalGradient(numopt_common::Vector& gradient,
const numopt_common::Parameterization& params, bool newParams)
{
// Numercical approach.
// numopt_common::Vector numericalGradient(params.getLocalSize());
// NonlinearObjectiveFunction::estimateLocalGradient(numericalGradient, params, 1.0e-6);
// std::cout << "Numerical: " << numericalGradient.transpose() << std::endl;
// Analytical approach.
numopt_common::Vector analyticalGradient(params.getLocalSize());
analyticalGradient.setZero();
const auto& poseParameterization = dynamic_cast<const PoseParameterization&>(params);
const Pose pose = poseParameterization.getPose();
const Eigen::Vector3d& p = pose.getPosition().vector();
const RotationQuaternion Phi(pose.getRotation());
// Default leg position.
for (const auto& footPosition : stance_) {
const Eigen::Vector3d& f_i = footPosition.second.vector();
const Eigen::Vector3d Phi_d_i = Phi.rotate(nominalStanceInBaseFrame_.at(footPosition.first)).vector();
const Eigen::Matrix3d Phi_d_i_skew = kindr::getSkewMatrixFromVector(Phi_d_i);
analyticalGradient.head(3) += p + Phi_d_i - f_i;
analyticalGradient.tail(3) += Phi_d_i_skew * p - Phi_d_i_skew * f_i;
}
// Center of mass.
const Eigen::Vector3d p_bar(p.x(), p.y(), 0.0); // Projection.
Eigen::Vector3d Phi_r_com = Phi.rotate(centerOfMassInBaseFrame_).vector();
Phi_r_com.z() = 0.0;
const Eigen::Matrix3d Phi_r_com_skew = kindr::getSkewMatrixFromVector(Phi_r_com);
const grid_map::Position supportPolygonCentroid(supportRegion_.getCentroid());
const Eigen::Vector3d r_centroid(supportPolygonCentroid.x(), supportPolygonCentroid.y(), 0.0);
analyticalGradient.head(3) += comWeight_ * (p_bar - r_centroid + Phi_r_com);
analyticalGradient.tail(3) += comWeight_ * (Phi_r_com_skew * p_bar - Phi_r_com_skew * r_centroid);
// Factorized with 2.0 (not weight!).
analyticalGradient = 2.0 * analyticalGradient;
// std::cout << "Analytical: " << analyticalGradient.transpose() << std::endl << std::endl;
// Return solution.
// gradient = numericalGradient;
gradient = analyticalGradient;
return true;
}
bool PoseOptimizationObjectiveFunction::computeValue(numopt_common::Scalar& value,
const numopt_common::Parameterization& params,
bool newParams)
{
const auto& poseParameterization = dynamic_cast<const PoseParameterization&>(params);
const Pose pose = poseParameterization.getPose();
value = 0.0;
// Cost for default leg positions.
for (const auto& footPosition : stance_) {
const Position nominalFootPositionInWorld = pose.getPosition()
+ pose.getRotation().rotate(nominalStanceInBaseFrame_.at(footPosition.first)); // d_i
value += (nominalFootPositionInWorld - footPosition.second).squaredNorm();
}
// Cost for foot below hip.
// for (const auto& foot : stance_) {
// const Position footPosition(foot.second);
// const Position hipInWorld = pose.getPosition() + pose.getRotation().rotate(positionsBaseToHipInBaseFrame_.at(foot.first));
// value += (footPosition - hipInWorld).vector().head(2).squaredNorm();
// }
// // Cost for deviation from initial position.
// Vector positionDifference(state.getPositionWorldToBaseInWorldFrame() - initialPose_.getPosition());
// value += 0.1 * positionDifference.vector().squaredNorm();
// Cost for deviation from initial orientation.
// RotationQuaternion rotationDifference(pose.getRotation() * initialPose_.getRotation().inverted());
// const double rotationDifferenceNorm = rotationDifference.norm();
// value += 10.0 * rotationDifferenceNorm * rotationDifferenceNorm;
// Cost for deviation from horizontal pose.
// RotationVector rotationDifference(pose.getRotation());
// rotationDifference.toImplementation().z() = 0.0;
// const double rotationDifferenceNorm = rotationDifference.vector().norm();
// value += 1.0 * rotationDifferenceNorm;
// Cost for CoM.
const Position centerOfMassInWorldFrame = pose.getPosition() + pose.getRotation().rotate(centerOfMassInBaseFrame_);
value += comWeight_ * (supportRegion_.getCentroid().head(2) - centerOfMassInWorldFrame.vector().head(2)).squaredNorm();
// // Cost for torques.
//// state_.setPoseBaseToWorld(pose);
//// adapter_.setInternalDataFromState(state_); // To guide IK.
////
//// for (const auto& foot : stance_) {
//// const Position footPositionInBase(
//// adapter_.transformPosition(adapter_.getWorldFrameId(), adapter_.getBaseFrameId(), foot.second));
//// JointPositionsLeg jointPositions;
//// const bool success = adapter_.getLimbJointPositionsFromPositionBaseToFootInBaseFrame(footPositionInBase, foot.first, jointPositions);
//// if (success) state_.setJointPositionsForLimb(foot.first, jointPositions);
//// }
//
//// adapter_.setInternalDataFromState(state_);
//
//// adapter_.
//
// for (const auto& foot : stance_) {
// const Position footPosition(foot.second);
// const Position baseToFootInBase = pose.getRotation().inverseRotate(footPosition - pose.getPosition());
// Eigen::Vector3d jointPositions;
// // https://ashwinnarayan.blogspot.ch/2014/07/inverse-kinematics-for-2dof-arm.html
// const double l1 = 1.0;
// const double l1_2 = l1 * l1;
// const double l2 = 1.0;
// const double l2_2 = l2 * l2;
//
// const double x = -baseToFootInBase(2);
// const double x2 = x * x;
// const double y = baseToFootInBase(0);
// const double y2 = y * y;
// const double k = (x2 + y2 - l1_2 - l2_2) / (2 * l1 * l2);
// jointPositions(0) = atan2(sqrt(1 - k * k), k);
//
// const double k1 = l1 + l2 * cos(jointPositions(1));
// const double k2 = l2 * sin(jointPositions(1));
// const double gamma = atan2(k2, k1);
// jointPositions(1) = atan2(y, x) - gamma;
// value += 0.1 * jointPositions.array().sin().matrix().squaredNorm();
//// JointPositionsLeg jointPositions;
//// adapter_.getLimbJointPositionsFromPositionBaseToFootInBaseFrame(baseToFootInBase, foot.first, jointPositions);
//// value += 0.01 * jointPositions.vector().array().sin().matrix().squaredNorm();
// }
return true;
}
inline static Translation inverseTransform(const Pose & pose, const Translation & position){
return pose.getRotation().inverseRotate((position-pose.getPosition()));
}
inline static Translation transform(const Pose & pose, const Translation & position){
return pose.getRotation().rotate(position) + pose.getPosition();
}
