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;
}
void GeneralizedStateStarlETH::setOrientationWorldToBase(const RotationQuaternion& orientationWorldToBase) {
generalizedCoordinates_(3) = orientationWorldToBase.w();
generalizedCoordinates_(4) = orientationWorldToBase.x();
generalizedCoordinates_(5) = orientationWorldToBase.y();
generalizedCoordinates_(6) = orientationWorldToBase.z();
}
bool TorsoControlStaticGait::advance(double dt) {
comControl_->advance(dt);
// Get measured orientation
const RotationQuaternion orientationWorldToControl = torso_->getMeasuredState().getOrientationWorldToControl(); // --> current heading orientation
/********************************************************************************************************
* Set desired base position in world frame
********************************************************************************************************/
// this is the position we need to compute:
Position positionControlToTargetBaseInControlFrame;
/* Compute the horizontal component of the desired position in world frame.
*
* evaluate desired CoM position in control frame
*/
Position positionWorldToDesiredHorizontalBaseInWorldFrame = comControl_->getPositionWorldToDesiredCoMInWorldFrame();
// this is the desired location of the base location relative to the origin of the control frame projected on the x-y plane of the world frame and expressed in the world frame
Position positionHorizontalControlToHorizontalBaseInWorldFrame = positionWorldToDesiredHorizontalBaseInWorldFrame
- torso_->getMeasuredState().getPositionWorldToControlInWorldFrame();
positionHorizontalControlToHorizontalBaseInWorldFrame.z() = 0.0;
/*************************************************************************
* Method I - NEW
*
* The desired base position has to lie on the line:
* W_r_C_B* = W_r_C_Bh* + lambda*W_e_z^W
* and on the plane parallel to the surface (with normal W_n) with
* the desired height above ground (h*):
* C_n * (C_r_C_B* - C_n * h*) = 0
*
* The intersection point is given by:
* C_n * ( C_n * h* - C_r_C_Bh*)
* C_r_C_B* = C_r_C_Bh* + ----------------------------- * C_e_z^W
* C_n * C_e_z^W
*
* We compute it in control frame instead of world frame.
*
*************************************************************************/
// // this is the surface normal
// loco::Vector surfaceNormalInWorldFrame;
// terrain_->getNormal(loco::Position::Zero(), surfaceNormalInWorldFrame);
// loco::Vector surfaceNormalInControlFrame = orientationWorldToControl.rotate(surfaceNormalInWorldFrame);
//
// const loco::Vector verticalAxisOfWorldFrameInWorldFrame = loco::Vector::UnitZ();
// const loco::Vector verticalAxisOfWorldFrameInControlFrame = orientationWorldToControl.rotate(verticalAxisOfWorldFrameInWorldFrame);
//
//
// const Position positionHorizontalControlToHorizontalBaseInControlFrame = orientationWorldToControl.rotate(positionHorizontalControlToHorizontalBaseInWorldFrame);
//
// Position temp2 = (desiredTorsoCoMHeightAboveGroundInControlFrameOffset_*Position(surfaceNormalInControlFrame))
// - positionHorizontalControlToHorizontalBaseInControlFrame;
// double scaleNumerator = Position(surfaceNormalInControlFrame).dot(temp2);
// double scaleDenominator = Position(surfaceNormalInControlFrame).dot(verticalAxisOfWorldFrameInControlFrame);
// double scale = scaleNumerator/scaleDenominator;
//
// positionControlToTargetBaseInControlFrame = orientationWorldToControl.rotate(positionHorizontalControlToHorizontalBaseInWorldFrame)
// + Position(verticalAxisOfWorldFrameInControlFrame*scale);
/*************************************************************************
* Method II - OLD
*
* The desired base position has to lie on the line:
* W_r_C_B* = W_r_C_Bh* + lambda*W_e_z^W
*
* and the vertical component in world frame has to be equal to the sum of
* the height of the terrain where this line goes through and the desired
* height of the torso above ground.
*
*************************************************************************/
RotationQuaternion orientationWorldToTerrain = getOrientationWorldToHeadingOnTerrainSurface(RotationQuaternion());
Position positionWorldToDesiredHeightAboveTerrainInTerrainFrame(0.0, 0.0, desiredTorsoCoMHeightAboveGroundInControlFrameOffset_);
Position positionWorldToDesiredHeightAboveTerrainInWorldFrame = orientationWorldToTerrain.inverseRotate(positionWorldToDesiredHeightAboveTerrainInTerrainFrame);
loco::Vector surfaceNormalInWorldFrame;
terrain_->getNormal(loco::Position::Zero(), surfaceNormalInWorldFrame);
double heightOverTerrain = positionWorldToDesiredHeightAboveTerrainInWorldFrame.dot(surfaceNormalInWorldFrame);
heightOverTerrain /= surfaceNormalInWorldFrame.z();
double heightOfTerrainInWorldFrame = 0.0;
terrain_->getHeight(positionWorldToDesiredHorizontalBaseInWorldFrame, heightOfTerrainInWorldFrame);
// Position positionWorldToHorizontalBaseInWorldFrame_temp = positionWorldToDesiredHorizontalBaseInWorldFrame + heightOfTerrainInWorldFrame*Position::UnitZ();
Position positionControlToTargetBaseInWorldFrame = positionHorizontalControlToHorizontalBaseInWorldFrame
+ (heightOfTerrainInWorldFrame + heightOverTerrain)*Position::UnitZ();
positionControlToTargetBaseInControlFrame = orientationWorldToControl.rotate(positionControlToTargetBaseInWorldFrame + desiredPositionOffsetInWorldFrame_);
/********************************************************************************************************
* End set desired CoM position in world frame *
********************************************************************************************************/
/********************************************************************************************************
* Set the desired orientation of the base frame with respect to the control frame
********************************************************************************************************/
// this is the orientation we need to compute
RotationQuaternion orientationControlToDesiredBase;
/*************************************************************************
* Method I - NEW
*
* Assumes that the control frame is aligned with the terrain surface.
* If the orientation of the body should be fully adapted to the terrain,
* we don't have to do anything.
*
* The desired orientation will be given by a composition of rotations:
* --> rotation from world to current heading (starting point for the composition).
* --> rotation from current heading to desired heading. This can be due to the body yaw induced by the foot holds.
* --> rotation from desired heading to desired base. This is an adaption to the terrain's estimated pitch and roll angles.
*
*************************************************************************/
RotationQuaternion orientationCurrentHeadingToDesiredHeading = getOrientationHeadingToDesiredHeadingBasedOnFeetLocations(positionWorldToDesiredHorizontalBaseInWorldFrame);
//--- Compose rotations
orientationControlToDesiredBase = desiredOrientationOffset_*orientationCurrentHeadingToDesiredHeading;
//---
/*************************************************************************
* Method II - OLD
*
*************************************************************************/
// RotationQuaternion orientationCurrentHeadingToDesiredHeading = getOrientationHeadingToDesiredHeadingBasedOnFeetLocations(positionWorldToDesiredHorizontalBaseInWorldFrame);
// RotationQuaternion orientationWorldToDesiredBase = getOrientationWorldToHeadingOnTerrainSurface(orientationCurrentHeadingToDesiredHeading*getOrientationWorldToHeadingBasedOnHipLocations());
// orientationControlToDesiredBase = orientationWorldToDesiredBase*orientationWorldToControl.inverted();
/*******************************
* End set desired orientation *
*******************************/
torso_->getDesiredState().setPositionControlToBaseInControlFrame(positionControlToTargetBaseInControlFrame);
torso_->getDesiredState().setOrientationControlToBase(orientationControlToDesiredBase);
// EulerAnglesZyx torsoAttitude = EulerAnglesZyx(orientationControlToDesiredBase).getUnique();
// std::cout << "*******" << std::endl;
// std::cout << "Desired torso position in control frame: " << std::endl << positionControlToTargetBaseInControlFrame << std::endl;
// std::cout << "Desired torso attitude in control frame: " << std::endl << torsoAttitude.roll() << " "
// << torsoAttitude.pitch() << " "
// << torsoAttitude.yaw() << std::endl;
// std::cout << "*******" << std::endl << std::endl;
/*************************************************************************
* Method II - OLD
* The input torso_->getDesiredState().setLinearVelocityBaseInControlFrame is actually not in control frame!!!!
* We need to correct this with a hack (fixme in future)
*
* velocities are given in frame HeadingOnTerrain
*
*************************************************************************/
// RotationQuaternion orientationWorldToHeadingOnTerrain = getOrientationWorldToHeadingOnTerrainSurface(getOrientationWorldToHeadingBasedOnHipLocations());
// RotationQuaternion orientationControlToHeadingOnTerrain = orientationWorldToControl*orientationWorldToHeadingOnTerrain.inverted();
// LinearVelocity linearVelocityBaseInControlFrame = orientationControlToHeadingOnTerrain.rotate(torso_->getDesiredState().getLinearVelocityBaseInControlFrame());
// torso_->getDesiredState().setLinearVelocityBaseInControlFrame(linearVelocityBaseInControlFrame);
LinearVelocity linearVelocityBaseInControlFrame;
LocalAngularVelocity angularVelocityBaseInControlFrame;
torso_->getDesiredState().setLinearVelocityBaseInControlFrame(linearVelocityBaseInControlFrame);
torso_->getDesiredState().setAngularVelocityBaseInControlFrame(angularVelocityBaseInControlFrame);
return true;
}
inline static EulerAnglesXyz<DestPrimType_> convert(const RotationQuaternion<SourcePrimType_>& q) {
Eigen::Matrix<DestPrimType_, 3, 1> vec = q.toImplementation().toRotationMatrix().eulerAngles(0, 1, 2).template cast<DestPrimType_>();
return EulerAnglesXyz<DestPrimType_>(vec(0), vec(1), vec(2));
}
