From 15714b14a7c132cf5989aada975d7adc09e47363 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian Date: Fri, 5 Aug 2022 11:43:47 +0200 Subject: [PATCH 1/2] wrong matrix size --- .../ocs2_centroidal_model/src/CentroidalModelRbdConversions.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ocs2_pinocchio/ocs2_centroidal_model/src/CentroidalModelRbdConversions.cpp b/ocs2_pinocchio/ocs2_centroidal_model/src/CentroidalModelRbdConversions.cpp index 21fdd2119..2d99c9789 100644 --- a/ocs2_pinocchio/ocs2_centroidal_model/src/CentroidalModelRbdConversions.cpp +++ b/ocs2_pinocchio/ocs2_centroidal_model/src/CentroidalModelRbdConversions.cpp @@ -81,7 +81,7 @@ void CentroidalModelRbdConversions::computeBaseKinematicsFromCentroidalModel(con const Vector3 derivativeEulerAnglesZyx = vPinocchio.segment<3>(3); baseVelocity.tail<3>() = getGlobalAngularVelocityFromEulerAnglesZyxDerivatives(baseOrientation, derivativeEulerAnglesZyx); - const Matrix6 Adot = pinocchio::dccrba(model, data, qPinocchio, vPinocchio); + const auto Adot = pinocchio::dccrba(model, data, qPinocchio, vPinocchio); Vector6 centroidalMomentumRate = info.robotMass * getNormalizedCentroidalMomentumRate(pinocchioInterface_, info, input); centroidalMomentumRate.noalias() -= Adot * vPinocchio; centroidalMomentumRate.noalias() -= Aj * jointAccelerations.head(info.actuatedDofNum); From 80092292c72490cf7b1942f316cf10237c8b4489 Mon Sep 17 00:00:00 2001 From: Farbod Farshidian Date: Fri, 5 Aug 2022 11:48:40 +0200 Subject: [PATCH 2/2] preventing asynchronous tracking in debug mode --- .../test/DoubleIntegratorNoRosIntegrationTest.cpp | 2 ++ .../ocs2_mobile_manipulator/test/testDummyMobileManipulator.cpp | 2 ++ 2 files changed, 4 insertions(+) diff --git a/ocs2_robotic_examples/ocs2_double_integrator/test/DoubleIntegratorNoRosIntegrationTest.cpp b/ocs2_robotic_examples/ocs2_double_integrator/test/DoubleIntegratorNoRosIntegrationTest.cpp index 6eaff3204..aa0d941f9 100644 --- a/ocs2_robotic_examples/ocs2_double_integrator/test/DoubleIntegratorNoRosIntegrationTest.cpp +++ b/ocs2_robotic_examples/ocs2_double_integrator/test/DoubleIntegratorNoRosIntegrationTest.cpp @@ -153,6 +153,7 @@ TEST_F(DoubleIntegratorIntegrationTest, coldStartMPC) { ASSERT_NEAR(observation.state(0), goalState(0), tolerance); } +#ifdef NDEBUG TEST_F(DoubleIntegratorIntegrationTest, asynchronousTracking) { auto mpcPtr = getMpc(true); MPC_MRT_Interface mpcInterface(*mpcPtr); @@ -207,3 +208,4 @@ TEST_F(DoubleIntegratorIntegrationTest, asynchronousTracking) { ASSERT_NEAR(observation.state(0), goalState(0), tolerance); } +#endif diff --git a/ocs2_robotic_examples/ocs2_mobile_manipulator/test/testDummyMobileManipulator.cpp b/ocs2_robotic_examples/ocs2_mobile_manipulator/test/testDummyMobileManipulator.cpp index 9fef6927f..f2be4e0c8 100644 --- a/ocs2_robotic_examples/ocs2_mobile_manipulator/test/testDummyMobileManipulator.cpp +++ b/ocs2_robotic_examples/ocs2_mobile_manipulator/test/testDummyMobileManipulator.cpp @@ -199,6 +199,7 @@ TEST_P(DummyMobileManipulatorParametersTests, synchronousTracking) { verifyTrackingQuality(observation.state); } +#ifdef NDEBUG TEST_P(DummyMobileManipulatorParametersTests, asynchronousTracking) { // Obtain mpc initialize(getTaskFile(), getLibFolder(), getUrdfFile()); @@ -258,6 +259,7 @@ TEST_P(DummyMobileManipulatorParametersTests, asynchronousTracking) { verifyTrackingQuality(observation.state); } +#endif /******************************************************************************************************/ /******************************************************************************************************/