The Parallel transport direction in computing Jacobian #1126

huweiATgithub · 2023-04-17T03:18:31Z

huweiATgithub
Apr 17, 2023

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

crocoddyl/include/crocoddyl/core/integrator/euler.hxx

Lines 76 to 115 in 0500e39

    
           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!

cmastalli · 2023-04-17T13:27:48Z

cmastalli
Apr 17, 2023
Maintainer

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?

2 replies

huweiATgithub Apr 17, 2023
Author

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$

crocoddyl/include/crocoddyl/multibody/states/multibody.hxx

Lines 211 to 229 in 0500e39

    
           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.

cmastalli Apr 17, 2023
Maintainer

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

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Uh oh!

The Parallel transport direction in computing Jacobian #1126

Uh oh!

{{title}}

Uh oh!

Replies: 1 comment 2 replies

Uh oh!

{{title}}

Uh oh!

Uh oh!

{{title}}

Uh oh!

Uh oh!

{{title}}

Uh oh!

Select a reply

Uh oh!

The Parallel transport direction in computing Jacobian #1126

Uh oh!

huweiATgithub Apr 17, 2023

Replies: 1 comment · 2 replies

Uh oh!

cmastalli Apr 17, 2023 Maintainer

Uh oh!

huweiATgithub Apr 17, 2023 Author

Uh oh!

cmastalli Apr 17, 2023 Maintainer

huweiATgithub
Apr 17, 2023

Replies: 1 comment 2 replies

cmastalli
Apr 17, 2023
Maintainer

huweiATgithub Apr 17, 2023
Author

cmastalli Apr 17, 2023
Maintainer