Skip to content

Navigation Menu

Sign in
Sign up

The Parallel transport direction in computing Jacobian #1126

Unanswered
huweiATgithub asked this question in Q&A
Discussion options

In semi-explicit Euler integration, I am confused about the Jtransport part of calcDiff:

template <typename Scalar>
void IntegratedActionModelEulerTpl<Scalar>::calcDiff(const boost::shared_ptr<ActionDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x,
const Eigen::Ref<const VectorXs>& u) {
if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
throw_pretty("Invalid argument: "
<< "x has wrong dimension (it should be " + std::to_string(state_->get_nx()) + ")");
}
if (static_cast<std::size_t>(u.size()) != nu_) {
throw_pretty("Invalid argument: "
<< "u has wrong dimension (it should be " + std::to_string(nu_) + ")");
}
const std::size_t nv = state_->get_nv();
Data* d = static_cast<Data*>(data.get());
control_->calc(d->control, 0., u);
differential_->calcDiff(d->differential, x, d->control->w);
const MatrixXs& da_dx = d->differential->Fx;
const MatrixXs& da_du = d->differential->Fu;
control_->multiplyByJacobian(d->control, da_du, d->da_du);
d->Fx.topRows(nv).noalias() = da_dx * time_step2_;
d->Fx.bottomRows(nv).noalias() = da_dx * time_step_;
d->Fx.topRightCorner(nv, nv).diagonal().array() += Scalar(time_step_);
d->Fu.topRows(nv).noalias() = time_step2_ * d->da_du;
d->Fu.bottomRows(nv).noalias() = time_step_ * d->da_du;
state_->JintegrateTransport(x, d->dx, d->Fx, second);
state_->Jintegrate(x, d->dx, d->Fx, d->Fx, first, addto);
state_->JintegrateTransport(x, d->dx, d->Fu, second);
d->Lx.noalias() = time_step_ * d->differential->Lx;
control_->multiplyJacobianTransposeBy(d->control, d->differential->Lu, d->Lu);
d->Lu *= time_step_;
d->Lxx.noalias() = time_step_ * d->differential->Lxx;
control_->multiplyByJacobian(d->control, d->differential->Lxu, d->Lxu);
d->Lxu *= time_step_;
control_->multiplyByJacobian(d->control, d->differential->Luu, d->Lwu);
control_->multiplyJacobianTransposeBy(d->control, d->Lwu, d->Luu);
d->Luu *= time_step_;
}

Let us take $F_u$ as an example. The code is computing $\frac{dx_{n+1}}{du_n} =\frac{\partial x_{n+1}}{\partial \delta x} \frac{\partial \delta x}{\partial u_n} $.
I understand the first part of the code is computing $\frac{\partial \delta x}{\partial u_n} $, and then call JintegrateTransport to do the transportation, i.e. left multiply $\frac{\partial x_{n+1}}{\partial \delta x} $:

state_->JintegrateTransport(x, d->dx, d->Fu, second); 

If I understand correctly, $\frac{\partial \delta x}{\partial u_n}$ has column vectors in tangent space at $x_n$. The desired quantity $\frac{dx_{n+1}}{du_n}$ has column vectors in tangent space at $x_{n+1}$.
Therefore, we are transporting vectors from tangent space at $x_n$ to that at $x_{n+1}=x_{n}\oplus\delta x$.

But in the documentation of JintegrateTransport, it says:

This function performs the parallel transportation of an input matrix whose columns are expressed in the tangent space at $x\oplus\delta x$ to the tangent space at $x$ point.

It is the reverse of my argument above.
What is wrong with my arguments?

Thanks!

You must be logged in to vote

Replies: 1 comment 2 replies

Comment options

But in the documentation of JintegrateTransport, it says:

This function performs the parallel transportation of an input matrix whose columns are expressed in the tangent space at x⊕δx to the tangent space at x point.

It is the reverse of my argument above. What is wrong with my arguments?

Thanks for raising this question. Indeed, the documentation is wrong, it should be the other way around. Could you help us creating a PR that fixes this issue?

You must be logged in to vote
2 replies
Comment options

Sure, no problem!
BTW, in the implementation of JintegrateTransport of StateMultibodyTpl, it calls Pinocchio's pinocchio::dIntegrateTransport which says

this functions transforms a tangent vector expressed at $q\oplus v$ to a tangent vector expressed at $q$

template <typename Scalar>
void StateMultibodyTpl<Scalar>::JintegrateTransport(const Eigen::Ref<const VectorXs>& x,
const Eigen::Ref<const VectorXs>& dx, Eigen::Ref<MatrixXs> Jin,
const Jcomponent firstsecond) const {
assert_pretty(is_a_Jcomponent(firstsecond), ("firstsecond must be one of the Jcomponent {both, first, second}"));
switch (firstsecond) {
case first:
pinocchio::dIntegrateTransport(*pinocchio_.get(), x.head(nq_), dx.head(nv_), Jin.topRows(nv_), pinocchio::ARG0);
break;
case second:
pinocchio::dIntegrateTransport(*pinocchio_.get(), x.head(nq_), dx.head(nv_), Jin.topRows(nv_), pinocchio::ARG1);
break;
default:
throw_pretty(
"Invalid argument: firstsecond must be either first or second. both not supported for this operation.");
break;
}
}

I don't know whether Pinocchio's documentation is wrong or if I should call Pinocchio reversely.

Comment options

Pinocchio documentation seems to be wrong, our code is correct and tested.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Category
Q&A
Labels
None yet

AltStyle によって変換されたページ (->オリジナル) /