如何解决使用 autoDiffToGradientMatrix 时出了点问题
我使用以下代码生成离散动态梯度矩阵。多次运行相同的 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,u_valp);
MatrixXd AB3 = get_dynamics_gradient2(plant_ad,u_valp);
MatrixXd AB4 = get_dynamics_gradient2(plant_ad,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
版权声明:本文内容由互联网用户自发贡献,该文观点与技术仅代表作者本人。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如发现本站有涉嫌侵权/违法违规的内容, 请发送邮件至 dio@foxmail.com 举报,一经查实,本站将立刻删除。