Derivative of Jacobian in drake

Question

I am currently trying to implement feedback linearization and for that, I was looking to compute Coriolis and gravitational terms,mass matrix, Jacobian and Jacobian Derivative.

In Drake I found functions for mass matrix

plant.CalcMassMatrix(*plant_context,*M)

I can also calculate the gravity term by using the

Eigen::VectorXd G = plant.CalcGravityGeneralizedForces(*plant_context). 

However, I am not sure if there is a command to calculate Coriolis term or better jointly gravitational and Coriolis term together.

I can think of an alternative by

plant.CalcMassMatrix(*plant_context, M);
h = plant_.CalcGravityGeneralizedForces(*plant_context);
multibody::MultibodyForces<double> external_forces(plant_);
external_forces.SetZero();
C = plant_.CalcInverseDynamics(*plant_context, ZeroQddot, external_forces);

For Jacobian, I did the following

end_effector_link_ = &plant_.GetBodyByName("ee_name");

/* End-effector frame E */
const multibody::Frame<double>& frame_E = (plant_.get_body(end_effector_link_->index())).body_frame();
const multibody::Frame<double>& frame_W = plant_.world_frame();

Eigen::MatrixXd p_EoEi_E(3, 1);
p_EoEi_E.col(0) << 0.0, 0.0, 0.00;

 const int num_positions = plant_.num_positions();
 MatrixXd Jq(3, num_positions);

plant_.CalcJacobianTranslationalVelocity(*plant_context,
                                            multibody::JacobianWrtVariable::kQDot,
                                            frame_E,
                                            p_EoEi_E,
                                            frame_W,
                                            frame_W,
                                            &Jq);

However, I was not able to find any update regarding jacobian derivative. I tried looking autodiffxd as mentioned in very old question but it do not provide enough information.

Any lead will be highly appreciated.

Answer 1

Do you really need J̇ (the time-derivative of the Jacobian) or could you want J̇q̇ or J̇v ? If you are looking for J̇*v, you may want to consider the method:

SpatialAcceleration MultibodyPlant::CalcBiasSpatialAcceleration()

Or, if you just want the translational part, use the method: MultibodyPlant::CalcBiasTranslationalAcceleration()

Below is the documentation for CalcBiasSpatialAcceleration().

  /// For one point Bp affixed/welded to a frame B, calculates ABias_ABp, Bp's
  /// spatial acceleration bias in frame A with respect to "speeds" ,
  /// where  is either q̇ (time-derivatives of generalized positions) or v
  /// (generalized velocities).  ABias_ABp is the term in A_ABp (Bp's
  /// spatial acceleration in A) that does not include ̇, i.e.,
  /// ABias_ABp is Bi's translational acceleration in A when ̇ = 0. <pre>
  ///   A_ABp =  J_V_ABp ⋅ ̇  +  J̇_V_ABp ⋅    ( = q̇ or  = v), hence
  ///   ABias_ABp = J̇_V_ABp ⋅ 
  /// </pre>
  /// where J_V_ABp is Bp's spatial velocity Jacobian in frame A for speeds s
  /// (see CalcJacobianSpatialVelocity() for details on J_V_ABp).
  /// @param[in] context The state of the multibody system.
  /// @param[in] with_respect_to Enum equal to JacobianWrtVariable::kQDot or
  /// JacobianWrtVariable::kV, indicating whether the spatial accceleration bias
  /// is with respect to  = q̇ or  = v.
  /// @param[in] frame_B The frame on which point Bp is affixed/welded.
  /// @param[in] p_BoBp_B Position vector from Bo (frame_B's origin) to point Bp
  /// (regarded as affixed/welded to B), expressed in frame_B.
  /// @param[in] frame_A The frame that measures ABias_ABp.
  /// @param[in] frame_E The frame in which ABias_ABp is expressed on output.
  /// @returns ABias_ABp_E Point Bp's spatial acceleration bias in frame A
  /// with respect to speeds  ( = q̇ or  = v), expressed in frame E.
  /// @note Shown below, ABias_ABp_E = J̇_V_ABp ⋅   is quadratic in . <pre>
  ///  V_ABp =  J_V_ABp ⋅         which upon vector differentiation in A gives
  ///  A_ABp =  J_V_ABp ⋅ ̇  +  J̇_V_ABp ⋅    Since J̇_V_ABp is linear in ,
  ///  ABias_ABp = J̇_V_ABp ⋅                              is quadratic in .
  /// </pre>
  /// @see CalcJacobianSpatialVelocity() to compute J_V_ABp, point Bp's
  /// translational velocity Jacobian in frame A with respect to .
  /// @throws std::exception if with_respect_to is not JacobianWrtVariable::kV