时间戳

首页 > 解决方案 > 德雷克雅可比矩阵的导数

drake - 德雷克雅可比矩阵的导数

问题描述

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

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

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

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

标签： drake

解决方案

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

SpatialAcceleration MultibodyPlant::CalcBiasSpatialAcceleration()

或者，如果您只想要平移部分，请使用方法：MultibodyPlant::CalcBiasTranslationalAcceleration()

以下是 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

推荐阅读