forked from pz4kybsvg/Conception
You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
179 lines
7.0 KiB
179 lines
7.0 KiB
#include "drake/solvers/rotation_constraint.h"
|
|
|
|
#include <algorithm>
|
|
#include <cmath>
|
|
#include <functional>
|
|
#include <limits>
|
|
|
|
#include "drake/common/symbolic/expression.h"
|
|
|
|
using drake::symbolic::Expression;
|
|
using Eigen::MatrixXi;
|
|
using Eigen::VectorXd;
|
|
using std::numeric_limits;
|
|
|
|
namespace drake {
|
|
namespace solvers {
|
|
|
|
MatrixDecisionVariable<3, 3> NewRotationMatrixVars(MathematicalProgram* prog,
|
|
const std::string& name) {
|
|
MatrixDecisionVariable<3, 3> R = prog->NewContinuousVariables<3, 3>(name);
|
|
|
|
// Forall i,j, -1 <= R(i,j) <=1.
|
|
prog->AddBoundingBoxConstraint(-1, 1, R);
|
|
|
|
// -1 <= trace(R) <= 3.
|
|
// Proof sketch:
|
|
// orthonormal => |lambda_i|=1.
|
|
// R is real => eigenvalues either real or appear in complex conj pairs.
|
|
// Case 1: All real (lambda_i \in {-1,1}).
|
|
// det(R)=lambda_1*lambda_2*lambda_3=1 => lambda_1=lambda_2, lambda_3=1.
|
|
// Case 2: Two imaginary, pick conj(lambda_1) = lambda_2.
|
|
// => lambda_1*lambda_2 = 1. => lambda_3 = 1.
|
|
// and also => lambda_1 + lambda_2 = 2*Re(lambda_1) \in [-2,2].
|
|
prog->AddLinearConstraint(Eigen::RowVector3d::Ones(), -1, 3, R.diagonal());
|
|
return R;
|
|
}
|
|
|
|
void AddBoundingBoxConstraintsImpliedByRollPitchYawLimits(
|
|
MathematicalProgram* prog,
|
|
const Eigen::Ref<const MatrixDecisionVariable<3, 3>>& R,
|
|
RollPitchYawLimits limits) {
|
|
// Based on the RPY to Rotation Matrix conversion:
|
|
// [ cp*cy, cy*sp*sr - cr*sy, sr*sy + cr*cy*sp]
|
|
// [ cp*sy, cr*cy + sp*sr*sy, cr*sp*sy - cy*sr]
|
|
// [ -sp, cp*sr, cp*cr]
|
|
// where cz = cos(z) and sz = sin(z), and using
|
|
// kRoll_NegPI_2_to_PI_2 = 1 << 1, // => cos(r)>=0
|
|
// kRoll_0_to_PI = 1 << 2, // => sin(r)>=0
|
|
// kPitch_NegPI_2_to_PI_2 = 1 << 3, // => cos(p)>=0
|
|
// kPitch_0_to_PI = 1 << 4, // => sin(p)>=0
|
|
// kYaw_NegPI_2_to_PI_2 = 1 << 5, // => cos(y)>=0
|
|
// kYaw_0_to_PI = 1 << 6, // => sin(y)>=0
|
|
|
|
if ((limits & kPitch_NegPI_2_to_PI_2) && (limits & kYaw_NegPI_2_to_PI_2))
|
|
prog->AddBoundingBoxConstraint(0, 1, R(0, 0));
|
|
|
|
if ((limits & kPitch_NegPI_2_to_PI_2) && (limits & kYaw_0_to_PI))
|
|
prog->AddBoundingBoxConstraint(0, 1, R(1, 0));
|
|
|
|
if (limits & kPitch_0_to_PI) prog->AddBoundingBoxConstraint(-1, 0, R(2, 0));
|
|
|
|
if ((limits & kRoll_NegPI_2_to_PI_2) && (limits & kYaw_NegPI_2_to_PI_2) &&
|
|
(limits & kPitch_0_to_PI) && (limits & kRoll_0_to_PI) &&
|
|
(limits & kYaw_0_to_PI))
|
|
prog->AddBoundingBoxConstraint(0, 1, R(1, 1));
|
|
|
|
if ((limits & kPitch_NegPI_2_to_PI_2) && (limits & kRoll_0_to_PI))
|
|
prog->AddBoundingBoxConstraint(0, 1, R(2, 1));
|
|
|
|
if ((limits & kRoll_0_to_PI) && (limits & kYaw_0_to_PI) &&
|
|
(limits & kRoll_NegPI_2_to_PI_2) && (limits & kYaw_NegPI_2_to_PI_2) &&
|
|
(limits & kPitch_0_to_PI))
|
|
prog->AddBoundingBoxConstraint(0, 1, R(0, 2));
|
|
|
|
if ((limits & kPitch_NegPI_2_to_PI_2) && (limits & kRoll_NegPI_2_to_PI_2))
|
|
prog->AddBoundingBoxConstraint(0, 1, R(2, 2));
|
|
}
|
|
|
|
void AddBoundingBoxConstraintsImpliedByRollPitchYawLimitsToBinary(
|
|
MathematicalProgram* prog,
|
|
const Eigen::Ref<const MatrixDecisionVariable<3, 3>>& B,
|
|
RollPitchYawLimits limits) {
|
|
if ((limits & kPitch_NegPI_2_to_PI_2) && (limits & kYaw_NegPI_2_to_PI_2))
|
|
prog->AddBoundingBoxConstraint(1, 1, B(0, 0));
|
|
|
|
if ((limits & kPitch_NegPI_2_to_PI_2) && (limits & kYaw_0_to_PI))
|
|
prog->AddBoundingBoxConstraint(1, 1, B(1, 0));
|
|
|
|
if (limits & kPitch_0_to_PI) prog->AddBoundingBoxConstraint(0, 0, B(2, 0));
|
|
|
|
if ((limits & kRoll_NegPI_2_to_PI_2) && (limits & kYaw_NegPI_2_to_PI_2) &&
|
|
(limits & kPitch_0_to_PI) && (limits & kRoll_0_to_PI) &&
|
|
(limits & kYaw_0_to_PI))
|
|
prog->AddBoundingBoxConstraint(1, 1, B(1, 1));
|
|
|
|
if ((limits & kPitch_NegPI_2_to_PI_2) && (limits & kRoll_0_to_PI))
|
|
prog->AddBoundingBoxConstraint(1, 1, B(2, 1));
|
|
|
|
if ((limits & kRoll_0_to_PI) && (limits & kYaw_0_to_PI) &&
|
|
(limits & kRoll_NegPI_2_to_PI_2) && (limits & kYaw_NegPI_2_to_PI_2) &&
|
|
(limits & kPitch_0_to_PI))
|
|
prog->AddBoundingBoxConstraint(1, 1, B(0, 2));
|
|
|
|
if ((limits & kPitch_NegPI_2_to_PI_2) && (limits & kRoll_NegPI_2_to_PI_2))
|
|
prog->AddBoundingBoxConstraint(1, 1, B(2, 2));
|
|
}
|
|
|
|
void AddRotationMatrixSpectrahedralSdpConstraint(
|
|
MathematicalProgram* prog,
|
|
const Eigen::Ref<const MatrixDecisionVariable<3, 3>>& R) {
|
|
// Equation 10 in
|
|
// Semidefinite descriptions of the convex hull of rotation matrices
|
|
// by James Saunderson, Pablo Parrilo and Alan Willsky
|
|
Matrix4<symbolic::Expression> M;
|
|
// clang-format off
|
|
M << 1 - R(0, 0) - R(1, 1) + R(2, 2), R(0, 2) + R(2, 0), R(0, 1) - R(1, 0), R(1, 2) + R(2, 1), // #NOLINT
|
|
R(0, 2) + R(2, 0), 1 + R(0, 0) - R(1, 1) - R(2, 2), R(1, 2) - R(2, 1), R(0, 1) + R(1, 0), // #NOLINT
|
|
R(0, 1) - R(1, 0), R(1, 2) - R(2, 1), 1 + R(0, 0) + R(1, 1) + R(2, 2), R(2, 0) - R(0, 2), // #NOLINT
|
|
R(1, 2) + R(2, 1), R(0, 1) + R(1, 0), R(2, 0) - R(0, 2), 1 - R(0, 0) + R(1, 1) - R(2, 2); // #NOLINT
|
|
// clang-format on
|
|
|
|
prog->AddPositiveSemidefiniteConstraint(M);
|
|
}
|
|
|
|
namespace {
|
|
|
|
void AddOrthogonalConstraint(
|
|
MathematicalProgram* prog,
|
|
const Eigen::Ref<const VectorDecisionVariable<3>>& v1,
|
|
const Eigen::Ref<const VectorDecisionVariable<3>>& v2) {
|
|
// We do this by introducing
|
|
// |v1+v2|^2 = v1'v1 + 2v1'v2 + v2'v2 <= 2
|
|
// |v1-v2|^2 = v1'v1 - 2v1'v2 + v2'v2 <= 2
|
|
// This is tight when v1'v1 = 1 and v2'v2 = 1.
|
|
|
|
// TODO(russt): Consider generalizing this to |v1+alpha*v2|^2 <= 1+alpha^2,
|
|
// for any real-valued alpha. When |R1|<|R2|<=1 or |R2|<|R1|<=1,
|
|
// different alphas represent different constraints.
|
|
|
|
// |v1+v2|^2 <= 2
|
|
// Implemented as a Lorenz cone using z = [ sqrt(2); v1+v2 ].
|
|
Vector4<symbolic::Expression> z;
|
|
z << std::sqrt(2), v1 + v2;
|
|
prog->AddLorentzConeConstraint(z);
|
|
|
|
// |v1-v2|^2 <= 2
|
|
// Implemented as a Lorenz cone using z = [ sqrt(2); v1-v2 ].
|
|
z.tail<3>() = v1 - v2;
|
|
prog->AddLorentzConeConstraint(z);
|
|
}
|
|
|
|
} // namespace
|
|
|
|
void AddRotationMatrixOrthonormalSocpConstraint(
|
|
MathematicalProgram* prog,
|
|
const Eigen::Ref<const MatrixDecisionVariable<3, 3>>& R) {
|
|
// All columns should be unit length (but we can only write Ri'Ri<=1),
|
|
// implemented as a rotated Lorenz cone with z = Ax+b = [1;1;R.col(i)].
|
|
Eigen::Matrix<double, 5, 3> A = Eigen::Matrix<double, 5, 3>::Zero();
|
|
A.bottomRows<3>() = Eigen::Matrix3d::Identity();
|
|
Eigen::Matrix<double, 5, 1> b;
|
|
b << 1, 1, 0, 0, 0;
|
|
for (int i = 0; i < 3; i++) {
|
|
prog->AddRotatedLorentzConeConstraint(A, b, R.col(i));
|
|
prog->AddRotatedLorentzConeConstraint(A, b, R.row(i).transpose());
|
|
}
|
|
|
|
AddOrthogonalConstraint(prog, R.col(0), R.col(1)); // R0'*R1 = 0.
|
|
AddOrthogonalConstraint(prog, R.col(1), R.col(2)); // R1'*R2 = 0.
|
|
AddOrthogonalConstraint(prog, R.col(0), R.col(2)); // R0'*R2 = 0.
|
|
|
|
// Same for the rows
|
|
AddOrthogonalConstraint(prog, R.row(0).transpose(), R.row(1).transpose());
|
|
AddOrthogonalConstraint(prog, R.row(1).transpose(), R.row(2).transpose());
|
|
AddOrthogonalConstraint(prog, R.row(0).transpose(), R.row(2).transpose());
|
|
}
|
|
} // namespace solvers
|
|
} // namespace drake
|