drake - 使用 autoDiffToGradientMatrix 的一些问题

Question

我使用以下代码生成离散动态梯度矩阵。当多次运行相同的 get_dynamics_gradient2() 时，似乎在某些矩阵元素中生成不同的值。我从许多测试中找不到任何明显的错误。你能告诉我如何让它正确吗？

#include "drake/common/find_resource.h"
#include "drake/multibody/parsing/parser.h"
#include "drake/multibody/plant/multibody_plant.h"
#include "drake/systems/framework/diagram_builder.h"
#include <Eigen/Dense>
#include "drake/math/autodiff.h"
#include "drake/math/autodiff_gradient.h"
#include "iostream"


using namespace std;
using drake::FindResourceOrThrow;
using drake::multibody::MultibodyPlant;
using drake::multibody::Parser;
using drake::systems::Context;
using drake::systems::InputPort;
using drake::systems::DiagramBuilder;
using Eigen::VectorXd;
using drake::AutoDiffVecXd;
using Eigen::MatrixXd;
using drake::AutoDiffXd;
using drake::math::initializeAutoDiff;
using drake::math::autoDiffToGradientMatrix;
using drake::math::autoDiffToValueMatrix;

MatrixXd get_dynamics_gradient2(std::unique_ptr<MultibodyPlant<AutoDiffXd>>& plant_ad,std::unique_ptr<Context<AutoDiffXd>>& context_ad,const VectorXd& x_val,const VectorXd& u_val) {
  
  AutoDiffVecXd x_ad = initializeAutoDiff(x_val);
  context_ad -> SetContinuousState(x_ad);
  AutoDiffVecXd u_ad = initializeAutoDiff(u_val);
  const InputPort<Eigen::AutoDiffScalar<Eigen::VectorXd>>& actuation_port = plant_ad -> get_actuation_input_port();
  actuation_port.FixValue(context_ad.get(), u_ad);
  auto derivatives = plant_ad -> AllocateTimeDerivatives();
  plant_ad -> CalcTimeDerivatives(*context_ad, derivatives.get());
    
  AutoDiffVecXd xdot_ad = derivatives -> get_vector().CopyToVector();
  AutoDiffVecXd x_next_ad = xdot_ad * 0.1 + x_ad;
    
  MatrixXd x_next = autoDiffToValueMatrix(x_next_ad);
    
  AutoDiffVecXd  x_next_ad_t = x_next_ad.transpose();
  AutoDiffVecXd  u_ad_t = u_ad.transpose();
    
  AutoDiffVecXd xu_next_ad_t(x_next_ad_t.rows()+u_ad_t.rows(),x_next_ad_t.cols());
  
  xu_next_ad_t << x_next_ad_t,u_ad_t;
 
  MatrixXd AB = autoDiffToGradientMatrix(xu_next_ad_t.transpose());
    
  return AB;
}



int main(int argc, char* argv[]) {

    const double time_step = 0;
    DiagramBuilder<double> builder;
    const std::string relative_name = "drake/wc/ll0/acrobot.sdf";
    const std::string full_name = FindResourceOrThrow(relative_name);
    MultibodyPlant<double>& acrobot = *builder.AddSystem<MultibodyPlant>(time_step);
   
    Parser parser(&acrobot);
    parser.AddModelFromFile(full_name);

    acrobot.Finalize();
    
    std::unique_ptr<MultibodyPlant<AutoDiffXd>> plant_ad = MultibodyPlant<double>::ToAutoDiffXd(acrobot);
    std::unique_ptr<Context<AutoDiffXd>> context_ad = plant_ad -> CreateDefaultContext();

    VectorXd x_valp = VectorXd(4);
    x_valp << 1.3, 5.8, 1.5, 0.02;

    VectorXd u_valp= VectorXd(1);
    u_valp << 0.01;


    MatrixXd AB1 = get_dynamics_gradient2(plant_ad, context_ad, x_valp, u_valp);
    MatrixXd AB2 = get_dynamics_gradient2(plant_ad, context_ad, x_valp, u_valp);
    MatrixXd AB3 = get_dynamics_gradient2(plant_ad, context_ad, x_valp, u_valp);
    MatrixXd AB4 = get_dynamics_gradient2(plant_ad, context_ad, x_valp, u_valp);

    cout <<"AB2-AB1" << endl << AB2-AB1 <<endl;
    cout <<"AB3-AB1" << endl << AB3-AB1 <<endl;
    cout <<"AB4-AB1" << endl << AB4-AB1 <<endl;

  return 0;
}

结果如下：

AB2-AB1
          0           0           0           0
          0           0           0           0
          0           0    0.735961    0.377986
          0           0    -1.47292   -0.756483
          0 1.73059e-77     1.96356           0
AB3-AB1
          0           0           0           0
          0           0           0           0
          0           0    -0.35439    -0.35439
          0           0     0.70926     0.70926
          0 1.73059e-77    0.963558           0
AB4-AB1
           0            0            0            0
           0            0            0            0
           0            0      1.28067      1.50475
           0            0     -2.56308     -3.01153
           0 2.90227e-157     0.963558            0

标签： drake