When testing the legged_robot in your examples by using full centroidal dynamics(FCD) method, I find that it will crash. #11
Description
Describe the bug
When testing the legged_robot in your examples by using full centroidal dynamics(FCD) method, I find that it will crash.
To Reproduce
Steps to reproduce the behavior:
- build this project ocs2
- source devel/setup.bash
- change the config file in legged robot
ocs2/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info
I change the "centroidalModelType" to "0" - roslaunch ocs2_legged_robot legged_robot.launch
Expected behavior
This info i get
SUMMARY
PARAMETERS
- /legged_robot_description: <?xml version="1....
- /rosdistro: noetic
- /rosversion: 1.15.11
NODES
/
legged_robot_dummy (ocs2_legged_robot/legged_robot_dummy)
legged_robot_gait_command (ocs2_legged_robot/legged_robot_gait_command)
legged_robot_mpc (ocs2_legged_robot/legged_robot_mpc)
legged_robot_target (ocs2_legged_robot/legged_robot_target)
rviz (rviz/rviz)
auto-starting new master
process[master]: started with pid [41295]
ROS_MASTER_URI=http://localhost:11311
setting /run_id to ffeea4d0-0256-11ec-ae58-db1db3bbff06
WARNING: Package name "JYPro" does not follow the naming conventions. It should start with a lower case letter and only contain lower case letters, digits, underscores, and dashes.
process[rosout-1]: started with pid [41324]
started core service [/rosout]
process[rviz-2]: started with pid [41328]
process[legged_robot_mpc-3]: started with pid [41332]
process[legged_robot_dummy-4]: started with pid [41333]
process[legged_robot_target-5]: started with pid [41335]
process[legged_robot_gait_command-6]: started with pid [41336]
[ WARN] [1629533323.482678414]: link 'imu_link' material 'orange' undefined.
[ WARN] [1629533323.484238724]: link 'imu_link' material 'orange' undefined.
Loading task file: /ocs2_ws/src/ocs2/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info
'display'................................................................0
Legged Robot Model Settings:
=============================================================================
'positionErrorGain'......................................................0
'phaseTransitionStanceTime'............................................0.4
'verboseCppAd'...........................................................1
'recompileLibrariesCppAd'................................................1
'modelFolderCppAd'.............................................../tmp/ocs2
=============================================================================
DDP Settings:
=============================================================================
'algorithm'............................................................SLQ
'nThreads'...............................................................3
'threadPriority'........................................................50
'maxNumIterations'......................................................10
'minRelCost'...........................................................0.1
'constraintTolerance'................................................0.005
'displayInfo'............................................................0
'displayShortSummary'....................................................0
'checkNumericalStability'................................................0
'debugPrintRollout'......................................................0
'debugCaching'...........................................................0
'AbsTolODE'..........................................................1e-05
'RelTolODE'..........................................................0.001
'maxNumStepsPerSecond'...............................................10000
'timeStep'............................................................0.01
'backwardPassIntegratorType'.........................................ODE45
'constraintPenaltyInitialValue'.........................................20
'constraintPenaltyIncreaseRate'..........................................2
'inequalityConstraintMu'...............................................0.1
'inequalityConstraintDelta'..............................................5
'preComputeRiccatiTerms'.................................................1
'useNominalTimeForBackwardPass'..........................................1
'useFeedbackPolicy'......................................................0
'riskSensitiveCoeff'.....................................................0 (default)
'strategy'.....................................................LINE_SEARCH
LINE_SEARCH Settings: {
'minStepLength'.....................................................0.0001
'maxStepLength'..........................................................1
'contractionRate'......................................................0.5 (default)
'armijoCoefficient'.................................................0.0001 (default)
'hessianCorrectionStrategy'.................................DIAGONAL_SHIFT
'hessianCorrectionMultiple'..........................................1e-05
}
=============================================================================
MPC Settings:
=============================================================================
'timeHorizon'............................................................1
'numPartitions'..........................................................2
'solutionTimeWindow'....................................................-1
'debugPrint'.............................................................0
'coldStart'..............................................................0
'initMaxNumIterations'..................................................10
'initMinStepLength'...................................................0.01
'initMaxStepLength'......................................................1
'runtimeMaxNumIterations'................................................1
'runtimeMinStepLength'................................................0.01
'runtimeMaxStepLength'...................................................1
'mpcDesiredFrequency'...................................................50
'mrtDesiredFrequency'..................................................400
=============================================================================
Rollout Settings:
=============================================================================
'AbsTolODE'..........................................................1e-05
'RelTolODE'..........................................................0.001
'maxNumStepsPerSecond'...............................................10000
'timeStep'............................................................0.01
'integratorType'.....................................................ODE45 (default)
'checkNumericalStability'................................................0
'reconstructInputTrajectory'.............................................1 (default)
'rootFindingAlgorithm'...................................................0 (default)
'maxSingleEventIterations'..............................................10 (default)
'useTrajectorySpreadingController'.......................................0 (default)
=============================================================================
Swing Trajectory Config:
=============================================================================
'liftOffVelocity'......................................................0.2
'touchDownVelocity'...................................................-0.4
'swingHeight'..........................................................0.1
'swingTimeScale'......................................................0.15
=============================================================================
Initial Modes Schedule:
event times: {0.5}
mode sequence: {15, 15}
Default Modes Sequence Template:
Template switching times: {0, 1}
Template mode sequence: {15}
[CppAdInterface] Compiling Shared Library: /tmp/ocs2/dynamics_systemFlowMap/cppad_generated/dynamics_systemFlowMap_libcppadcg_tmp65741332.so
[ INFO] [1629533323.634868827]: rviz version 1.14.8
[ INFO] [1629533323.635052159]: compiled against Qt version 5.12.8
[ INFO] [1629533323.635091688]: compiled against OGRE version 1.9.0 (Ghadamon)
[ INFO] [1629533323.643808641]: Forcing OpenGl version 0.
[legged_robot_dummy-4] process has finished cleanly
log file: .ros/log/ffeea4d0-0256-11ec-ae58-db1db3bbff06/legged_robot_dummy-4*.log
[legged_robot_target-5] process has finished cleanly
log file: .ros/log/ffeea4d0-0256-11ec-ae58-db1db3bbff06/legged_robot_target-5*.log
[legged_robot_gait_command-6] process has finished cleanly
log file: .ros/log/ffeea4d0-0256-11ec-ae58-db1db3bbff06/legged_robot_gait_command-6*.log
[ INFO] [1629533324.329155885]: Stereo is NOT SUPPORTED
[ INFO] [1629533324.329341591]: OpenGL device: GeForce RTX 3070 Laptop GPU/PCIe/SSE2
[ INFO] [1629533324.329368270]: OpenGl version: 4.6 (GLSL 4.6).
[ WARN] [1629533324.510231347]: link 'imu_link' material 'orange' undefined.
[ WARN] [1629533324.510271435]: link 'imu_link' material 'orange' undefined.
[CppAdInterface] Renaming /tmp/ocs2/dynamics_systemFlowMap/cppad_generated/dynamics_systemFlowMap_libcppadcg_tmp65741332.so to /tmp/ocs2/dynamics_systemFlowMap/cppad_generated/dynamics_systemFlowMap_lib.so
WARNING: Loaded at least one default value in matrix: "Q"
WARNING: Loaded at least one default value in matrix: "R"
[CppAdInterface] Compiling Shared Library: /tmp/ocs2/LF_FOOT_position/cppad_generated/LF_FOOT_position_libcppadcg_tmp46441332.so
[CppAdInterface] Renaming /tmp/ocs2/LF_FOOT_position/cppad_generated/LF_FOOT_position_libcppadcg_tmp46441332.so to /tmp/ocs2/LF_FOOT_position/cppad_generated/LF_FOOT_position_lib.so
[CppAdInterface] Compiling Shared Library: /tmp/ocs2/LF_FOOT_velocity/cppad_generated/LF_FOOT_velocity_libcppadcg_tmp47441332.so
[CppAdInterface] Renaming /tmp/ocs2/LF_FOOT_velocity/cppad_generated/LF_FOOT_velocity_libcppadcg_tmp47441332.so to /tmp/ocs2/LF_FOOT_velocity/cppad_generated/LF_FOOT_velocity_lib.so
[CppAdInterface] Compiling Shared Library: /tmp/ocs2/LF_FOOT_orientation/cppad_generated/LF_FOOT_orientation_libcppadcg_tmp13141332.so
[CppAdInterface] Renaming /tmp/ocs2/LF_FOOT_orientation/cppad_generated/LF_FOOT_orientation_libcppadcg_tmp13141332.so to /tmp/ocs2/LF_FOOT_orientation/cppad_generated/LF_FOOT_orientation_lib.so
[CppAdInterface] Compiling Shared Library: /tmp/ocs2/RF_FOOT_position/cppad_generated/RF_FOOT_position_libcppadcg_tmp26941332.so
[CppAdInterface] Renaming /tmp/ocs2/RF_FOOT_position/cppad_generated/RF_FOOT_position_libcppadcg_tmp26941332.so to /tmp/ocs2/RF_FOOT_position/cppad_generated/RF_FOOT_position_lib.so
[CppAdInterface] Compiling Shared Library: /tmp/ocs2/RF_FOOT_velocity/cppad_generated/RF_FOOT_velocity_libcppadcg_tmp71941332.so
[CppAdInterface] Renaming /tmp/ocs2/RF_FOOT_velocity/cppad_generated/RF_FOOT_velocity_libcppadcg_tmp71941332.so to /tmp/ocs2/RF_FOOT_velocity/cppad_generated/RF_FOOT_velocity_lib.so
[CppAdInterface] Compiling Shared Library: /tmp/ocs2/RF_FOOT_orientation/cppad_generated/RF_FOOT_orientation_libcppadcg_tmp58341332.so
[CppAdInterface] Renaming /tmp/ocs2/RF_FOOT_orientation/cppad_generated/RF_FOOT_orientation_libcppadcg_tmp58341332.so to /tmp/ocs2/RF_FOOT_orientation/cppad_generated/RF_FOOT_orientation_lib.so
[CppAdInterface] Compiling Shared Library: /tmp/ocs2/LH_FOOT_position/cppad_generated/LH_FOOT_position_libcppadcg_tmp4541332.so
[CppAdInterface] Renaming /tmp/ocs2/LH_FOOT_position/cppad_generated/LH_FOOT_position_libcppadcg_tmp4541332.so to /tmp/ocs2/LH_FOOT_position/cppad_generated/LH_FOOT_position_lib.so
[CppAdInterface] Compiling Shared Library: /tmp/ocs2/LH_FOOT_velocity/cppad_generated/LH_FOOT_velocity_libcppadcg_tmp28641332.so
[CppAdInterface] Renaming /tmp/ocs2/LH_FOOT_velocity/cppad_generated/LH_FOOT_velocity_libcppadcg_tmp28641332.so to /tmp/ocs2/LH_FOOT_velocity/cppad_generated/LH_FOOT_velocity_lib.so
[CppAdInterface] Compiling Shared Library: /tmp/ocs2/LH_FOOT_orientation/cppad_generated/LH_FOOT_orientation_libcppadcg_tmp59141332.so
[CppAdInterface] Renaming /tmp/ocs2/LH_FOOT_orientation/cppad_generated/LH_FOOT_orientation_libcppadcg_tmp59141332.so to /tmp/ocs2/LH_FOOT_orientation/cppad_generated/LH_FOOT_orientation_lib.so
[CppAdInterface] Compiling Shared Library: /tmp/ocs2/RH_FOOT_position/cppad_generated/RH_FOOT_position_libcppadcg_tmp52641332.so
[CppAdInterface] Renaming /tmp/ocs2/RH_FOOT_position/cppad_generated/RH_FOOT_position_libcppadcg_tmp52641332.so to /tmp/ocs2/RH_FOOT_position/cppad_generated/RH_FOOT_position_lib.so
[CppAdInterface] Compiling Shared Library: /tmp/ocs2/RH_FOOT_velocity/cppad_generated/RH_FOOT_velocity_libcppadcg_tmp13841332.so
[CppAdInterface] Renaming /tmp/ocs2/RH_FOOT_velocity/cppad_generated/RH_FOOT_velocity_libcppadcg_tmp13841332.so to /tmp/ocs2/RH_FOOT_velocity/cppad_generated/RH_FOOT_velocity_lib.so
[CppAdInterface] Compiling Shared Library: /tmp/ocs2/RH_FOOT_orientation/cppad_generated/RH_FOOT_orientation_libcppadcg_tmp51341332.so
[CppAdInterface] Renaming /tmp/ocs2/RH_FOOT_orientation/cppad_generated/RH_FOOT_orientation_libcppadcg_tmp51341332.so to /tmp/ocs2/RH_FOOT_orientation/cppad_generated/RH_FOOT_orientation_lib.so
WARNING: Failed to set threads priority (one possible reason could be that the user and the group permissions are not set properly.)
WARNING: Failed to set threads priority (one possible reason could be that the user and the group permissions are not set properly.)
[ INFO] [1629533400.919630067]: MPC node is setting up ...
[ INFO] [1629533400.927968340]: Publishing SLQ-MPC messages on a separate thread.
[ INFO] [1629533400.928009266]: MPC node is ready.
[ INFO] [1629533400.928021349]: Start spinning now ...
#####################################################
#####################################################
################# MPC is reset. ###################
#####################################################
#####################################################
terminate called after throwing an instance of 'std::runtime_error'
what(): DDP controller does not generate a stable rollout.
[legged_robot_mpc-3] process has died [pid 41332, exit code -6, cmd
ocs2_ws/devel/lib/ocs2_legged_robot/legged_robot_mpc legged_robot mpc ocs2_ws/src/ocs2/ocs2_robotic_examples/ocs2_legged_robot/config/command/targetTrajectories.info legged_robot_description __name:=legged_robot_mpc __log:=/home/usrname/.ros/log/ffeea4d0-0256-11ec-ae58-db1db3bbff06/legged_robot_mpc-3.log].
log file: .ros/log/ffeea4d0-0256-11ec-ae58-db1db3bbff06/legged_robot_mpc-3*.log
Would you plz give us a detail info about how to use full centroidal dynamics(FCD)? or are there some config i havent changed for FCD?