const typename
Type<Scalar>::MatrixX& HumanoidLipComJerkMinimizationObjective<Scalar>::getHessian()
{
assert(feetSupervisor_.getNbSamples() == lipModel_.getNbSamples());;
int N = lipModel_.getNbSamples();
int M = feetSupervisor_.getNbPreviewedSteps();
int nb = feetSupervisor_.getNbOfCallsBeforeNextSample() - 1;
const LinearDynamic<Scalar>& dynCopX = lipModel_.getCopXLinearDynamic(nb);
const LinearDynamic<Scalar>& dynCopY = lipModel_.getCopYLinearDynamic(nb);
MatrixX tmp = MatrixX::Zero(2*N, 2*N + 2*M);
tmp.block(0, 0, 2*N, 2*N) = feetSupervisor_.getRotationMatrixT();
tmp.block(0, 2*N, N, M) = feetSupervisor_.getFeetPosLinearDynamic().U;
tmp.block(N, 2*N + M, N, M) = feetSupervisor_.getFeetPosLinearDynamic().U;
MatrixX tmp2 = MatrixX::Zero(2*N, 2*N);
const MatrixX& weight = feetSupervisor_.getSampleWeightMatrix();
tmp2.block(0, 0, N, N) = dynCopX.UTinv*weight*dynCopX.Uinv;
tmp2.block(N, N, N, N) = dynCopY.UTinv*weight*dynCopY.Uinv;
hessian_.noalias() = tmp.transpose()*tmp2*tmp;
return hessian_;
}
void run_test(int dim, int num_elements)
{
using std::abs;
typedef typename internal::traits<MatrixType>::Scalar Scalar;
typedef Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> MatrixX;
typedef Matrix<Scalar, Eigen::Dynamic, 1> VectorX;
// MUST be positive because in any other case det(cR_t) may become negative for
// odd dimensions!
const Scalar c = abs(internal::random<Scalar>());
MatrixX R = randMatrixSpecialUnitary<Scalar>(dim);
VectorX t = Scalar(50)*VectorX::Random(dim,1);
MatrixX cR_t = MatrixX::Identity(dim+1,dim+1);
cR_t.block(0,0,dim,dim) = c*R;
cR_t.block(0,dim,dim,1) = t;
MatrixX src = MatrixX::Random(dim+1, num_elements);
src.row(dim) = Matrix<Scalar, 1, Dynamic>::Constant(num_elements, Scalar(1));
MatrixX dst = cR_t*src;
MatrixX cR_t_umeyama = umeyama(src.block(0,0,dim,num_elements), dst.block(0,0,dim,num_elements));
const Scalar error = ( cR_t_umeyama*src - dst ).norm() / dst.norm();
VERIFY(error < Scalar(40)*std::numeric_limits<Scalar>::epsilon());
}
const typename Type<Scalar>::VectorX&
HumanoidLipComJerkMinimizationObjective<Scalar>::getGradient(const VectorX& x0)
{
assert(x0.rows() == 2*lipModel_.getNbSamples() + 2*feetSupervisor_.getNbPreviewedSteps());
int N = lipModel_.getNbSamples();
int M = feetSupervisor_.getNbPreviewedSteps();
int nb = feetSupervisor_.getNbOfCallsBeforeNextSample() - 1;
const LinearDynamic<Scalar>& dynCopX = lipModel_.getCopXLinearDynamic(nb);
const LinearDynamic<Scalar>& dynCopY = lipModel_.getCopYLinearDynamic(nb);
const LinearDynamic<Scalar>& dynFeetPos = feetSupervisor_.getFeetPosLinearDynamic();
const MatrixX& weight = feetSupervisor_.getSampleWeightMatrix();
//TODO: sparse matrices?
// Ill change these tmp matrices after calculus optimization
// I may change some function to avoid copies
MatrixX tmp = MatrixX::Zero(2*N + 2*M, 2*N);
tmp.block(0, 0, 2*N, 2*N) = feetSupervisor_.getRotationMatrix();
tmp.block(2*N, 0, M, N) = feetSupervisor_.getFeetPosLinearDynamic().UT;
tmp.block(2*N + M, N, M, N) = feetSupervisor_.getFeetPosLinearDynamic().UT;
VectorX tmp2 = VectorX::Zero(2*N);
tmp2.segment(0, N) = dynCopX.UTinv*weight*dynCopX.Uinv*dynFeetPos.S
*feetSupervisor_.getSupportFootStateX()(0)
- dynCopX.UTinv*weight*dynCopX.Uinv
*(dynCopX.S*lipModel_.getStateX() + dynCopX.K);
tmp2.segment(N, N) = dynCopY.UTinv*weight*dynCopY.Uinv*dynFeetPos.S
*feetSupervisor_.getSupportFootStateY()(0)
- dynCopY.UTinv*weight*dynCopY.Uinv
*(dynCopY.S*lipModel_.getStateY() + dynCopY.K);
gradient_ = tmp*tmp2;
gradient_ += getHessian()*x0;
return gradient_;
}
