drake中Jacobian的导数

Question

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

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

对于Jacobian，我做了以下

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，正如在一个非常古老的问题中提到的，但它没有提供足够的信息。

任何线索都将不胜感激。

Answer 1

你真的需要J̇(雅可比阶的时间导数(吗？或者你想要JṈqṇ或J \7751v吗？如果你正在寻找J̇*v，你可能想考虑的方法：

空间加速度多体植物：：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