问题描述
我使用以下代码生成离散动态梯度矩阵。多次运行相同的 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 (将#修改为@)