德雷克雅可比的导数

问题描述

我目前正在尝试实现反馈线性化，为此，我希望计算科里奥利和引力项、质量矩阵、雅可比和雅可比导数。

在 Drake 中我找到了质量矩阵的函数

plant.CalcMassMatrix(*plant_context,*M)

我也可以使用

来计算重力项

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

但是，我不确定是否有命令可以计算科里奥利项或更好地将引力和科里奥利项一起计算。

我可以想到一个替代方案

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);

对于雅可比，我做了以下

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,&Jq);

但是，我找不到有关雅可比派生的任何更新。我尝试按照非常古老的问题中提到的方式查找 autodiffxd，但它没有提供足够的信息。

任何线索都将受到高度赞赏。

解决方法

你真的需要 J̇（雅可比矩阵的时间导数）还是你想要 J̇q̇ 或 J̇v ？ 如果你在找J̇*v，你可能要考虑下方法：

SpatialAcceleration MultibodyPlant::CalcBiasSpatialAcceleration()

或者，如果您只想要翻译部分，请使用以下方法： MultibodyPlant::CalcBiasTranslationalAcceleration()

以下是 CalcBiasSpatialAcceleration() 的文档。

  /// For one point Bp affixed/welded to a frame B,calculates A?Bias_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).  A?Bias_ABp is the term in A_ABp (Bp's
  /// spatial acceleration in A) that does not include ?̇,i.e.,/// A?Bias_ABp is Bi's translational acceleration in A when ?̇ = 0. <pre>
  ///   A_ABp =  J?_V_ABp ⋅ ?̇  +  J̇?_V_ABp ⋅ ?   (? = q̇ or ? = v),hence
  ///   A?Bias_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 A?Bias_ABp.
  /// @param[in] frame_E The frame in which A?Bias_ABp is expressed on output.
  /// @returns A?Bias_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,A?Bias_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 ?,///  A?Bias_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