search

leggedrobotics / ocs2

Optimal Control for Switched Systems
https://leggedrobotics.github.io/ocs2
BSD 3-Clause "New" or "Revised" License
857 stars 220 forks source link

How to inplement the hybrid dynamics model based on SystemDynamicsBaseAD? #110

Closed sysu19351064 closed 1 week ago

sysu19351064 commented 1 week ago

Dear legged robotics group,

thanks for providing the OCS2.

Recently, We want to use OCS2 to develop a Spring Load Inverse Pendulum, with two hyrbrid nonlinear dynamic models during flight phase and stance phase. We write the Hybrid dynamics model derived on SystemDynamicsBaseAD class, and mainly refering the content of dynamics_hybrid_slq_test.h(which is derived on SystemDynamicsBase class). However the stateTrajectory got almost "-nan" value in the StateTriggeredRolloutTest.

Here is part of Hybrid Dynamics definition: `#pragma once

include <ocs2_core/dynamics/SystemDynamicsBaseAD.h>

include "ocs2_quadhopper/QuadhopperParameters.h"

include "ocs2_quadhopper/definitions.h"

namespace ocs2 { namespace quadhopper {

class QuadhopperHybridDynamics1 final : public SystemDynamicsBaseAD { public: explicit QuadhopperHybridDynamics1(QuadhopperParameters QuadhopperParameters, const std::string &libraryFolder) : param(std::move(QuadhopperParameters)) { g = param.gravity; muav = param.quadhopperMass; Ixx = param.Thxxyy; Iyy = param.Thxxyy; Izz = param.Thzz; G << scalar_t(0), scalart(0), g; // ! 使用SystemDynamicsBaseAD基类的话，需要调用 Initialize() initialize(HYB_STATE_DIM, HYB_INPUT_DIM, "nmpc_quadhopper_dynamics1", libraryFolder, true, true); }

QuadhopperHybridDynamics1(const QuadhopperHybridDynamics1 &rhs) = default; // 用于深拷贝的构造函数 ~QuadhopperHybridDynamics1() override = default; QuadhopperHybridDynamics1 clone() const override { return new QuadhopperHybridDynamics1(this); }

//* 使用AD版本的Dynamics时，需要使用这三个函数分别定义 flow, jump, gaurd ad_vector_t systemFlowMap(ad_scalar_t time, const ad_vector_t &state, const ad_vector_t &input, const ad_vector_t &parameters) const override;

ad_vector_t systemJumpMap(ad_scalar_t time, const ad_vector_t &state, const ad_vector_t& parameters) const override;

ad_vector_t systemGuardSurfaces(ad_scalar_t time, const ad_vector_t& state, const ad_vectort& parameters) const override; private: QuadhopperParameters param; scalart g; Eigen::Matrix<scalart, 3, 1> G;

scalar_t m_uav;

scalar_t Ixx; scalar_t Iyy; scalar_t Izz;

scalar_t spring_l0; scalar_t spring_stiffness; };

class QuadhopperHybridDynamics2 final : public SystemDynamicsBaseAD { public: explicit QuadhopperHybridDynamics2(QuadhopperParameters QuadhopperParameters, const std::string &libraryFolder) : param(std::move(QuadhopperParameters)) { g = param.gravity; muav = param.quadhopperMass; Ixx = param.Thxxyy; Iyy = param.Thxxyy; Izz = param.Thzz; G << scalar_t(0), scalart(0), g; // ! 使用SystemDynamicsBaseAD基类的话，需要调用 Initialize() initialize(HYB_STATE_DIM, HYB_INPUT_DIM, "nmpc_quadhopper_dynamics2", libraryFolder, true, true); }

QuadhopperHybridDynamics2(const QuadhopperHybridDynamics2 &rhs) = default; // 用于深拷贝的构造函数 ~QuadhopperHybridDynamics2() override = default; QuadhopperHybridDynamics2 clone() const override { return new QuadhopperHybridDynamics2(this); }

ad_vector_t systemFlowMap(ad_scalar_t time, const ad_vector_t &state, const ad_vector_t &input, const ad_vector_t &parameters) const override;

ad_vector_t systemJumpMap(ad_scalar_t time, const ad_vector_t &state, const ad_vector_t& parameters) const override;

ad_vector_t systemGuardSurfaces(ad_scalar_t time, const ad_vector_t& state, const ad_vector_t& parameters) const override;

private: QuadhopperParameters param_; scalart g; Eigen::Matrix<scalart, 3, 1> G;

scalar_t m_uav;

scalar_t Ixx; scalar_t Iyy; scalar_t Izz; scalar_t spring_l0; scalar_t spring_stiffness; };

//* 先考虑整体的 hybridDynamic是AD版本的 class QuadhopperHybridDynamics final : public SystemDynamicsBaseAD { public: explicit QuadhopperHybridDynamics(QuadhopperParameters QuadhopperParameters, const std::string &libraryFolder){ subSystemDynamics1Ptr.reset(new QuadhopperHybridDynamics1(QuadhopperParameters, libraryFolder)); subSystemDynamics2Ptr.reset(new QuadhopperHybridDynamics2(QuadhopperParameters, libraryFolder)); initialize(HYB_STATE_DIM, HYB_INPUT_DIM, "nmpc_quadhopper_hybrid_dynamics", libraryFolder, true, true); }

~QuadhopperHybridDynamics() override = default;

QuadhopperHybridDynamics clone() const override { return new QuadhopperHybridDynamics(this); }

QuadhopperHybridDynamics(const QuadhopperHybridDynamics &rhs) : SystemDynamicsBaseAD(rhs), subSystemDynamics1Ptr(rhs.subSystemDynamics1Ptr->clone()), subSystemDynamics2Ptr(rhs.subSystemDynamics2Ptr->clone()) { // 用于深拷贝的构造函数 };

ad_vector_t systemFlowMap(ad_scalar_t time, const ad_vector_t &state, const ad_vector_t &input, const ad_vector_t &parameters) const { ad_scalar_t mode = state(15); ad_vector_t result(HYB_STATE_DIM); // 使用CondExpEq来替代if条件 ad_vectort resultCond1 = subSystemDynamics1Ptr->systemFlowMap(time, state, input, parameters); ad_vectort resultCond2 = subSystemDynamics2Ptr->systemFlowMap(time, state, input, parameters); for(size_t i = 0; i < HYB_STATE_DIM; i++){ result[i] = CppAD::CondExpEq(mode, ad_scalar_t(0.0), resultCond1[i], resultCond2[i]); } return result; // return CppAD::CondExpEq(mode, ad_scalart(0.0), result1, result2); // if(CppAD::Value(state(15)).getValue() == 0) // return subSystemDynamics1Ptr->systemFlowMap(time, state, input, parameters); // else // return subSystemDynamics2Ptr_->systemFlowMap(time, state, input, parameters); }

ad_vector_t systemJumpMap(ad_scalar_t time, const ad_vector_t &state, const ad_vector_t &parameters) const { ad_scalar_t mode = state(15); ad_vector_t result(HYB_STATE_DIM); // 使用CondExpEq来替代if条件 ad_vectort resultCond1 = subSystemDynamics1Ptr->systemJumpMap(time, state, parameters); ad_vectort resultCond2 = subSystemDynamics2Ptr->systemJumpMap(time, state, parameters);

for(size_t i = 0; i < HYB_STATE_DIM; i++){
  result[i] = CppAD::CondExpEq(mode, ad_scalar_t(0.0), resultCond1[i], resultCond2[i]);
}
return result;

}

ad_vector_t systemGuardSurfaces(ad_scalar_t time, const ad_vector_t& state, const ad_vector_t& parameters) const { ad_scalar_t mode = state(15); ad_vector_t result(2); // 使用CondExpEq来替代if条件 ad_vectort resultCond1 = subSystemDynamics1Ptr->systemGuardSurfaces(time, state, parameters); ad_vectort resultCond2 = subSystemDynamics2Ptr->systemGuardSurfaces(time, state, parameters);

for(size_t i = 0; i < 2; i++){
  result[i] = CppAD::CondExpEq(mode, ad_scalar_t(0.0), resultCond1[i], resultCond2[i]);
}
return result;

}

private: std::uniqueptr subSystemDynamics1Ptr; std::uniqueptr subSystemDynamics2Ptr; };

} // namespace quadhopper } // namespace ocs2 And here is the StateTriggerRolloutTest: #include <gtest/gtest.h>

include

include

include <ros/init.h>

include <ros/package.h>

include <ocs2_core/Types.h>

include <ocs2_core/control/LinearController.h>

include <ocs2_oc/rollout/RolloutSettings.h>

include <ocs2_oc/rollout/StateTriggeredRollout.h>

include "ocs2_quadhopper/dynamics/QuadhopperHybridDynamics.h"

include "ocs2_quadhopper/QuadhopperParameters.h"

include "ocs2_quadhopper/definitions.h"

// Boost

include <boost/filesystem/operations.hpp>

include <boost/filesystem/path.hpp>

using scalar_t = ocs2::scalar_t; using vector_t = ocs2::vector_t; using matrix_t = ocs2::matrix_t; using scalar_array_t = ocs2::scalar_array_t; using size_array_t = ocs2::size_array_t; using vector_array_t = ocs2::vector_array_t; using matrix_array_t = ocs2::matrix_array_t; using time_interval_t = std::pair<scalar_t, scalar_t>; using time_interval_array_t = std::vector;

using namespace ocs2; using namespace quadhopper;

TEST(StateRolloutTests, runHybridDynamics) { const std::string taskFile = ros::package::getPath("ocs2_quadhopper") + "/config/" + "dynamic" + "/task_hopper.info"; const std::string libFolder = ros::package::getPath("ocs2_quadhopper") + "/auto_generated"; boost::filesystem::path taskFilePath(taskFile); if (boost::filesystem::exists(taskFilePath)) { std::cerr << "[QuadhopperDynamicsTest] Loading task file: " << taskFilePath << std::endl; } else { throw std::invalid_argument("[QuadhopperDynamicsTest] Task file not found: " + taskFilePath.string()); } // create library folder if it does not exist boost::filesystem::path libraryFolderPath(libFolder); boost::filesystem::create_directories(libraryFolderPath); std::cerr << "[QuadhopperDynamicsTest] Generated library path: " << libraryFolderPath << std::endl;

// Construct State TriggerdRollout Object ocs2::rollout::Settings rolloutSettings; auto quadhopperParameters = quadhopper::loadSettings(taskFile, "QuadhopperParameters", true); ocs2::quadhopper::QuadhopperHybridDynamics dynamics(quadhopperParameters, libFolder); ocs2::StateTriggeredRollout rollout(dynamics, rolloutSettings); // Construct Variables for run // Simulation time const scalar_t t0 = 0; const scalar_t t1 = 5; // Initial State vectort initialState(HYB_STATEDIM); loadData::loadEigenMatrix(taskFile, "initialState", initialState); std::cerr << "xinit: " << initialState.transpose() << std::endl; // Controller (time constant zero controller) scalar_array_t timestamp(1, t0); vector_t bias(HYB_INPUT_DIM); bias << 0, 0, 0, 0; vector_array_t biasArray(1, bias);

matrix_t gain(HYB_INPUT_DIM, HYB_STATE_DIM);

matrix_array_t gainArray(1, gain); ocs2::LinearController control(timestamp, biasArray, gainArray); ocs2::ModeSchedule modeSchedule; // Trajectory storage scalar_array_t timeTrajectory(0); size_array_t eventsPastTheEndIndeces(0); vector_array_t stateTrajectory(0); vector_array_t inputTrajectory(0);

// Output State vectort finalState; // Run finalState = rollout.run(t0, initialState, t1, &control, modeSchedule, timeTrajectory, eventsPastTheEndIndeces, stateTrajectory, inputTrajectory);

// Test (and display statetrajectory) for (int i = 0; i < timeTrajectory.size(); i++) { // Test 1: No Significant penetration of Guard Surface scalar_t currentTime = timeTrajectory[i]; vector_t currentState = stateTrajectory[i]; vector_t currentGuardValues = dynamics.computeGuardSurfaces(currentTime, currentState); std::cout << "currentTime: " << currentTime << std::endl; std::cout << "currentState: " << currentState.transpose() << std::endl; std::cout << "GuardValues: " << currentGuardValues.transpose() << std::endl;

EXPECT_GT(currentGuardValues[0], -1e-6);
EXPECT_GT(currentGuardValues[1], -1e-6);

}

// Test 3: Event times const scalar_array_t eventTestTimes{0.126835459, 1.73439091, 2.25798967, 3.86554513, 4.38914388}; ASSERT_EQ(eventTestTimes.size(), eventsPastTheEndIndeces.size()); for (int i = 0; i < eventsPastTheEndIndeces.size(); i++) { EXPECT_NEAR(timeTrajectory[eventsPastTheEndIndeces[i] - 1], eventTestTimes[i], 1e-8); }

EXPECT_EQ(modeSchedule.eventTimes.size() + 1, modeSchedule.modeSequence.size()); ASSERT_EQ(eventTestTimes.size(), modeSchedule.eventTimes.size()); for (int i = 0; i < eventTestTimes.size(); i++) { EXPECT_NEAR(eventTestTimes[i], modeSchedule.eventTimes[i], 1e-6); } } `

sysu19351064 commented 1 week ago

Sorry to bother you, I finally found out it was a problem with the controller, sincing I did not initialize the controller correctly!