From d73a880820154f4099af62d8f444880348e0e403 Mon Sep 17 00:00:00 2001 From: AmirSamanMirjalili Date: Sun, 19 May 2024 20:00:02 +0330 Subject: [PATCH 1/9] Jacobian types fixed plus the need for casting shared ptr avoided --- .gitignore | 1 + .idea/.gitignore | 1 + .idea/.name | 1 + libmbse/include/mbse/factors/FactorConstraints.h | 3 ++- libmbse/include/mbse/factors/FactorConstraintsIndep.h | 4 ++-- libmbse/src/factors/FactorConstraints.cpp | 5 ++--- libmbse/src/factors/FactorConstraintsIndep.cpp | 8 +++++++- 7 files changed, 16 insertions(+), 7 deletions(-) create mode 100644 .idea/.gitignore create mode 100644 .idea/.name diff --git a/.gitignore b/.gitignore index 3c6f4e4..5ae78d1 100644 --- a/.gitignore +++ b/.gitignore @@ -11,3 +11,4 @@ doc/source/mainpage.md doc/html experiments/*.txt .vscode +/cmake-build-debug/ diff --git a/.idea/.gitignore b/.idea/.gitignore new file mode 100644 index 0000000..6722cd9 --- /dev/null +++ b/.idea/.gitignore @@ -0,0 +1 @@ +*.xml diff --git a/.idea/.name b/.idea/.name new file mode 100644 index 0000000..8351b1d --- /dev/null +++ b/.idea/.name @@ -0,0 +1 @@ +MBSE \ No newline at end of file diff --git a/libmbse/include/mbse/factors/FactorConstraints.h b/libmbse/include/mbse/factors/FactorConstraints.h index 0999806..f6b9d77 100644 --- a/libmbse/include/mbse/factors/FactorConstraints.h +++ b/libmbse/include/mbse/factors/FactorConstraints.h @@ -13,6 +13,7 @@ #include #include #include +#include namespace mbse { @@ -61,7 +62,7 @@ class FactorConstraints : public gtsam::NoiseModelFactor1 /** vector of errors */ gtsam::Vector evaluateError( const state_t& q_k, - gtsam::OptionalMatrixType H1 = OptionalNone) const override; + boost::optional H1 ) const override; /** numberof variable attached to this factor */ std::size_t size() const { return 1; } diff --git a/libmbse/include/mbse/factors/FactorConstraintsIndep.h b/libmbse/include/mbse/factors/FactorConstraintsIndep.h index 9498b69..878414e 100644 --- a/libmbse/include/mbse/factors/FactorConstraintsIndep.h +++ b/libmbse/include/mbse/factors/FactorConstraintsIndep.h @@ -63,8 +63,8 @@ class FactorConstraintsIndep /** vector of errors */ gtsam::Vector evaluateError( const state_t& z_k, const state_t& q_k, - gtsam::OptionalMatrixType de_dz = OptionalNone, - gtsam::OptionalMatrixType de_dq = OptionalNone) const override; + boost::optional de_dz = boost::none, + boost::optional de_dq = boost::none) const override; /** numberof variable attached to this factor */ std::size_t size() const { return 2; } diff --git a/libmbse/src/factors/FactorConstraints.cpp b/libmbse/src/factors/FactorConstraints.cpp index fb054c1..f36e6e7 100644 --- a/libmbse/src/factors/FactorConstraints.cpp +++ b/libmbse/src/factors/FactorConstraints.cpp @@ -17,8 +17,7 @@ FactorConstraints::~FactorConstraints() = default; gtsam::NonlinearFactor::shared_ptr FactorConstraints::clone() const { - return std::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); + return gtsam::NonlinearFactor::shared_ptr(new This(*this)); } void FactorConstraints::print( @@ -37,7 +36,7 @@ bool FactorConstraints::equals( } gtsam::Vector FactorConstraints::evaluateError( - const state_t& q_k, gtsam::OptionalMatrixType H1) const + const state_t& q_k, boost::optional H1) const { MRPT_START diff --git a/libmbse/src/factors/FactorConstraintsIndep.cpp b/libmbse/src/factors/FactorConstraintsIndep.cpp index 4b70e47..9288bed 100644 --- a/libmbse/src/factors/FactorConstraintsIndep.cpp +++ b/libmbse/src/factors/FactorConstraintsIndep.cpp @@ -26,6 +26,8 @@ FactorConstraintsIndep::FactorConstraintsIndep( { } + + FactorConstraintsIndep::~FactorConstraintsIndep() = default; gtsam::NonlinearFactor::shared_ptr FactorConstraintsIndep::clone() const @@ -34,6 +36,7 @@ gtsam::NonlinearFactor::shared_ptr FactorConstraintsIndep::clone() const gtsam::NonlinearFactor::shared_ptr(new This(*this))); } + void FactorConstraintsIndep::print( const std::string& s, const gtsam::KeyFormatter& keyFormatter) const { @@ -43,6 +46,8 @@ void FactorConstraintsIndep::print( noiseModel_->print(" noise model: "); } + + bool FactorConstraintsIndep::equals( const gtsam::NonlinearFactor& expected, double tol) const { @@ -50,8 +55,9 @@ bool FactorConstraintsIndep::equals( return e != nullptr && Base::equals(*e, tol); } + gtsam::Vector FactorConstraintsIndep::evaluateError( - const state_t& z_k, const state_t& q_k, gtsam::OptionalMatrixType de_dz, + const state_t& z_k, const state_t& q_k, gtsam::Optional de_dz, gtsam::OptionalMatrixType de_dq) const { MRPT_START From b6353d7971ddd62dc42aeac29676b3c575c0aef4 Mon Sep 17 00:00:00 2001 From: AmirSamanMirjalili Date: Sun, 19 May 2024 20:36:19 +0330 Subject: [PATCH 2/9] GTSAM v4.2 type compatibility fixed --- libmbse/include/mbse/factors/FactorConstraintsAccIndep.h | 8 ++++---- libmbse/include/mbse/factors/FactorConstraintsVel.h | 4 ++-- libmbse/include/mbse/factors/FactorConstraintsVelIndep.h | 6 +++--- libmbse/include/mbse/factors/FactorDynamics.h | 6 +++--- libmbse/include/mbse/factors/FactorDynamicsIndep.h | 6 +++--- libmbse/include/mbse/factors/FactorEulerInt.h | 6 +++--- libmbse/include/mbse/factors/FactorGyroscope.h | 4 ++-- libmbse/include/mbse/factors/FactorInverseDynamics.h | 2 +- libmbse/include/mbse/factors/FactorTrapInt.h | 8 ++++---- libmbse/src/factors/FactorConstraintsAccIndep.cpp | 9 ++++----- libmbse/src/factors/FactorConstraintsIndep.cpp | 7 +++---- libmbse/src/factors/FactorConstraintsVel.cpp | 7 +++---- libmbse/src/factors/FactorConstraintsVelIndep.cpp | 7 +++---- libmbse/src/factors/FactorDynamics.cpp | 7 +++---- libmbse/src/factors/FactorDynamicsIndep.cpp | 7 +++---- libmbse/src/factors/FactorEulerInt.cpp | 7 +++---- libmbse/src/factors/FactorGyroscope.cpp | 7 +++---- libmbse/src/factors/FactorInverseDynamics.cpp | 5 ++--- libmbse/src/factors/FactorTrapInt.cpp | 9 ++++----- 19 files changed, 56 insertions(+), 66 deletions(-) diff --git a/libmbse/include/mbse/factors/FactorConstraintsAccIndep.h b/libmbse/include/mbse/factors/FactorConstraintsAccIndep.h index f9b4e15..233ff03 100644 --- a/libmbse/include/mbse/factors/FactorConstraintsAccIndep.h +++ b/libmbse/include/mbse/factors/FactorConstraintsAccIndep.h @@ -67,10 +67,10 @@ class FactorConstraintsAccIndep /** vector of errors */ gtsam::Vector evaluateError( const state_t& q_k, const state_t& dotq_k, const state_t& ddotq_k, - const state_t& ddotz_k, gtsam::OptionalMatrixType de_dq = OptionalNone, - gtsam::OptionalMatrixType de_dqp = OptionalNone, - gtsam::OptionalMatrixType de_dqpp = OptionalNone, - gtsam::OptionalMatrixType de_dzpp = OptionalNone) const override; + const state_t& ddotz_k, boost::optional de_dq = boost::none, + boost::optional de_dqp = boost::none, + boost::optional de_dqpp = boost::none, + boost::optional de_dzpp = boost::none) const override; /** numberof variable attached to this factor */ std::size_t size() const { return 4; } diff --git a/libmbse/include/mbse/factors/FactorConstraintsVel.h b/libmbse/include/mbse/factors/FactorConstraintsVel.h index 7ce36f3..4b1f315 100644 --- a/libmbse/include/mbse/factors/FactorConstraintsVel.h +++ b/libmbse/include/mbse/factors/FactorConstraintsVel.h @@ -64,8 +64,8 @@ class FactorConstraintsVel : public gtsam::NoiseModelFactor2 /** vector of errors */ gtsam::Vector evaluateError( const state_t& q_k, const state_t& dotq_k, - gtsam::OptionalMatrixType H1 = OptionalNone, - gtsam::OptionalMatrixType H2 = OptionalNone) const override; + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const override; /** numberof variable attached to this factor */ std::size_t size() const { return 2; } diff --git a/libmbse/include/mbse/factors/FactorConstraintsVelIndep.h b/libmbse/include/mbse/factors/FactorConstraintsVelIndep.h index e17af71..73f463b 100644 --- a/libmbse/include/mbse/factors/FactorConstraintsVelIndep.h +++ b/libmbse/include/mbse/factors/FactorConstraintsVelIndep.h @@ -67,9 +67,9 @@ class FactorConstraintsVelIndep /** vector of errors */ gtsam::Vector evaluateError( const state_t& q_k, const state_t& dotq_k, const state_t& dotz_k, - gtsam::OptionalMatrixType de_dq = OptionalNone, - gtsam::OptionalMatrixType de_dqp = OptionalNone, - gtsam::OptionalMatrixType de_dzp = OptionalNone) const override; + boost::optional de_dq = boost::none, + boost::optional de_dqp = boost::none, + boost::optional de_dzp = boost::none) const override; /** numberof variable attached to this factor */ std::size_t size() const { return 3; } diff --git a/libmbse/include/mbse/factors/FactorDynamics.h b/libmbse/include/mbse/factors/FactorDynamics.h index 41c6563..41a63d6 100644 --- a/libmbse/include/mbse/factors/FactorDynamics.h +++ b/libmbse/include/mbse/factors/FactorDynamics.h @@ -75,9 +75,9 @@ class FactorDynamics /** vector of errors */ gtsam::Vector evaluateError( const state_t& q_k, const state_t& dq_k, const state_t& ddq_k, - gtsam::OptionalMatrixType H1 = OptionalNone, - gtsam::OptionalMatrixType H2 = OptionalNone, - gtsam::OptionalMatrixType H3 = OptionalNone) const override; + boost::optional H1 = boost::none, + boost::optional H2 = boost::none, + boost::optional H3 = boost::none) const override; /** number of variables attached to this factor */ std::size_t size() const { return 3; } diff --git a/libmbse/include/mbse/factors/FactorDynamicsIndep.h b/libmbse/include/mbse/factors/FactorDynamicsIndep.h index 847beec..670d889 100644 --- a/libmbse/include/mbse/factors/FactorDynamicsIndep.h +++ b/libmbse/include/mbse/factors/FactorDynamicsIndep.h @@ -90,9 +90,9 @@ class FactorDynamicsIndep /** vector of errors */ gtsam::Vector evaluateError( const state_t& z_k, const state_t& dz_k, const state_t& ddz_k, - gtsam::OptionalMatrixType de_dz = OptionalNone, - gtsam::OptionalMatrixType de_dzp = OptionalNone, - gtsam::OptionalMatrixType de_dzpp = OptionalNone) const override; + boost::optional de_dz = boost::none, + boost::optional de_dzp = boost::none, + boost::optional de_dzpp = boost::none) const override; /** number of variables attached to this factor */ std::size_t size() const { return 3; } diff --git a/libmbse/include/mbse/factors/FactorEulerInt.h b/libmbse/include/mbse/factors/FactorEulerInt.h index 4d4b9a8..a7f6958 100644 --- a/libmbse/include/mbse/factors/FactorEulerInt.h +++ b/libmbse/include/mbse/factors/FactorEulerInt.h @@ -71,9 +71,9 @@ class FactorEulerInt /** vector of errors */ gtsam::Vector evaluateError( const state_t& x_k, const state_t& x_kp1, const state_t& v_k, - gtsam::OptionalMatrixType H1 = OptionalNone, - gtsam::OptionalMatrixType H2 = OptionalNone, - gtsam::OptionalMatrixType H3 = OptionalNone) const override; + boost::optional H1 = boost::none, + boost::optional H2 = boost::none, + boost::optional H3 = boost::none) const override; /** number of variables attached to this factor */ std::size_t size() const { return 3; } diff --git a/libmbse/include/mbse/factors/FactorGyroscope.h b/libmbse/include/mbse/factors/FactorGyroscope.h index 902736c..3a50eb6 100644 --- a/libmbse/include/mbse/factors/FactorGyroscope.h +++ b/libmbse/include/mbse/factors/FactorGyroscope.h @@ -71,8 +71,8 @@ class FactorGyroscope : public gtsam::NoiseModelFactor2 /** vector of errors */ gtsam::Vector evaluateError( const state_t& q_k, const state_t& dq_k, - gtsam::OptionalMatrixType H1 = OptionalNone, - gtsam::OptionalMatrixType H2 = OptionalNone) const override; + boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const override; /** number of variables attached to this factor */ std::size_t size() const { return 2; } diff --git a/libmbse/include/mbse/factors/FactorInverseDynamics.h b/libmbse/include/mbse/factors/FactorInverseDynamics.h index b6aaedb..4677def 100644 --- a/libmbse/include/mbse/factors/FactorInverseDynamics.h +++ b/libmbse/include/mbse/factors/FactorInverseDynamics.h @@ -95,7 +95,7 @@ class FactorInverseDynamics : public gtsam::NoiseModelFactor1 /** vector of errors */ gtsam::Vector evaluateError( const state_t& Q_k, - gtsam::OptionalMatrixType d_e_Q = OptionalNone) const override; + boost::optional d_e_Q = boost::none) const override; /** number of variables attached to this factor */ std::size_t size() const { return 2; } diff --git a/libmbse/include/mbse/factors/FactorTrapInt.h b/libmbse/include/mbse/factors/FactorTrapInt.h index 5defa0a..adeb2b9 100644 --- a/libmbse/include/mbse/factors/FactorTrapInt.h +++ b/libmbse/include/mbse/factors/FactorTrapInt.h @@ -75,10 +75,10 @@ class FactorTrapInt /** vector of errors */ gtsam::Vector evaluateError( const state_t& x_k, const state_t& x_kp1, const state_t& v_k, - const state_t& v_kp1, gtsam::OptionalMatrixType H1 = OptionalNone, - gtsam::OptionalMatrixType H2 = OptionalNone, - gtsam::OptionalMatrixType H3 = OptionalNone, - gtsam::OptionalMatrixType H4 = OptionalNone) const override; + const state_t& v_kp1, boost::optional H1 = boost::none, + boost::optional H2 = boost::none, + boost::optional H3 = boost::none, + boost::optional H4 = boost::none) const override; /** number of variables attached to this factor */ std::size_t size() const { return 4; } diff --git a/libmbse/src/factors/FactorConstraintsAccIndep.cpp b/libmbse/src/factors/FactorConstraintsAccIndep.cpp index ed440dc..7d7f5da 100644 --- a/libmbse/src/factors/FactorConstraintsAccIndep.cpp +++ b/libmbse/src/factors/FactorConstraintsAccIndep.cpp @@ -31,8 +31,7 @@ FactorConstraintsAccIndep::~FactorConstraintsAccIndep() = default; gtsam::NonlinearFactor::shared_ptr FactorConstraintsAccIndep::clone() const { - return std::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); +return gtsam::NonlinearFactor::shared_ptr(new This(*this)); } void FactorConstraintsAccIndep::print( @@ -53,9 +52,9 @@ bool FactorConstraintsAccIndep::equals( gtsam::Vector FactorConstraintsAccIndep::evaluateError( const state_t& q_k, const state_t& dotq_k, const state_t& ddotq_k, - const state_t& ddotz_k, gtsam::OptionalMatrixType de_dq, - gtsam::OptionalMatrixType de_dqp, gtsam::OptionalMatrixType de_dqpp, - gtsam::OptionalMatrixType de_dzpp) const + const state_t& ddotz_k, boost::optional de_dq, + boost::optional de_dqp, boost::optional de_dqpp, + boost::optional de_dzpp) const { MRPT_START diff --git a/libmbse/src/factors/FactorConstraintsIndep.cpp b/libmbse/src/factors/FactorConstraintsIndep.cpp index 9288bed..f85b640 100644 --- a/libmbse/src/factors/FactorConstraintsIndep.cpp +++ b/libmbse/src/factors/FactorConstraintsIndep.cpp @@ -32,8 +32,7 @@ FactorConstraintsIndep::~FactorConstraintsIndep() = default; gtsam::NonlinearFactor::shared_ptr FactorConstraintsIndep::clone() const { - return std::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); + return gtsam::NonlinearFactor::shared_ptr(new This(*this)); } @@ -57,8 +56,8 @@ bool FactorConstraintsIndep::equals( gtsam::Vector FactorConstraintsIndep::evaluateError( - const state_t& z_k, const state_t& q_k, gtsam::Optional de_dz, - gtsam::OptionalMatrixType de_dq) const + const state_t& z_k, const state_t& q_k, boost::optional de_dz, + boost::optional de_dq) const { MRPT_START diff --git a/libmbse/src/factors/FactorConstraintsVel.cpp b/libmbse/src/factors/FactorConstraintsVel.cpp index 489e97b..bcaa01c 100644 --- a/libmbse/src/factors/FactorConstraintsVel.cpp +++ b/libmbse/src/factors/FactorConstraintsVel.cpp @@ -27,8 +27,7 @@ FactorConstraintsVel::~FactorConstraintsVel() = default; gtsam::NonlinearFactor::shared_ptr FactorConstraintsVel::clone() const { - return std::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); +return gtsam::NonlinearFactor::shared_ptr(new This(*this)); } void FactorConstraintsVel::print( @@ -85,8 +84,8 @@ static void num_err_wrt_dq( #endif gtsam::Vector FactorConstraintsVel::evaluateError( - const state_t& q_k, const state_t& dotq_k, gtsam::OptionalMatrixType H1, - gtsam::OptionalMatrixType H2) const + const state_t& q_k, const state_t& dotq_k, boost::optional H1, + boost::optional H2) const { MRPT_START diff --git a/libmbse/src/factors/FactorConstraintsVelIndep.cpp b/libmbse/src/factors/FactorConstraintsVelIndep.cpp index d0a31dc..922c49f 100644 --- a/libmbse/src/factors/FactorConstraintsVelIndep.cpp +++ b/libmbse/src/factors/FactorConstraintsVelIndep.cpp @@ -31,8 +31,7 @@ FactorConstraintsVelIndep::~FactorConstraintsVelIndep() = default; gtsam::NonlinearFactor::shared_ptr FactorConstraintsVelIndep::clone() const { - return std::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); +return gtsam::NonlinearFactor::shared_ptr(new This(*this)); } void FactorConstraintsVelIndep::print( @@ -53,8 +52,8 @@ bool FactorConstraintsVelIndep::equals( gtsam::Vector FactorConstraintsVelIndep::evaluateError( const state_t& q_k, const state_t& dotq_k, const state_t& dotz_k, - gtsam::OptionalMatrixType de_dq, gtsam::OptionalMatrixType de_dqp, - gtsam::OptionalMatrixType de_dzp) const + boost::optional de_dq, boost::optional de_dqp, + boost::optional de_dzp) const { MRPT_START diff --git a/libmbse/src/factors/FactorDynamics.cpp b/libmbse/src/factors/FactorDynamics.cpp index 6f9c95c..684746e 100644 --- a/libmbse/src/factors/FactorDynamics.cpp +++ b/libmbse/src/factors/FactorDynamics.cpp @@ -33,8 +33,7 @@ FactorDynamics::~FactorDynamics() = default; gtsam::NonlinearFactor::shared_ptr FactorDynamics::clone() const { - return std::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); + return gtsam::NonlinearFactor::shared_ptr(new This(*this)); } void FactorDynamics::print( @@ -97,8 +96,8 @@ bool FactorDynamics::equals( gtsam::Vector FactorDynamics::evaluateError( const state_t& q_k, const state_t& dq_k, const state_t& ddq_k, - gtsam::OptionalMatrixType H1, gtsam::OptionalMatrixType H2, - gtsam::OptionalMatrixType H3) const + boost::optional H1 , boost::optional H2, + boost::optional H3) const { MRPT_START diff --git a/libmbse/src/factors/FactorDynamicsIndep.cpp b/libmbse/src/factors/FactorDynamicsIndep.cpp index 200ff22..26c2e08 100644 --- a/libmbse/src/factors/FactorDynamicsIndep.cpp +++ b/libmbse/src/factors/FactorDynamicsIndep.cpp @@ -26,8 +26,7 @@ FactorDynamicsIndep::~FactorDynamicsIndep() = default; gtsam::NonlinearFactor::shared_ptr FactorDynamicsIndep::clone() const { - return std::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); +return gtsam::NonlinearFactor::shared_ptr(new This(*this)); } void FactorDynamicsIndep::print( @@ -117,8 +116,8 @@ bool FactorDynamicsIndep::equals( gtsam::Vector FactorDynamicsIndep::evaluateError( const state_t& z_k, const state_t& dz_k, const state_t& ddz_k, - gtsam::OptionalMatrixType de_dz, gtsam::OptionalMatrixType de_dzp, - gtsam::OptionalMatrixType de_dzpp) const + boost::optional de_dz, boost::optional de_dzp, + boost::optional de_dzpp) const { MRPT_START diff --git a/libmbse/src/factors/FactorEulerInt.cpp b/libmbse/src/factors/FactorEulerInt.cpp index 620bb1a..5cf0d23 100644 --- a/libmbse/src/factors/FactorEulerInt.cpp +++ b/libmbse/src/factors/FactorEulerInt.cpp @@ -17,8 +17,7 @@ FactorEulerInt::~FactorEulerInt() = default; gtsam::NonlinearFactor::shared_ptr FactorEulerInt::clone() const { - return std::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); + return gtsam::NonlinearFactor::shared_ptr(new This(*this)); } void FactorEulerInt::print( @@ -41,8 +40,8 @@ bool FactorEulerInt::equals( gtsam::Vector FactorEulerInt::evaluateError( const state_t& x_k, const state_t& x_kp1, const state_t& v_k, - gtsam::OptionalMatrixType H1, gtsam::OptionalMatrixType H2, - gtsam::OptionalMatrixType H3) const + boost::optional H1, boost::optional H2, + boost::optional H3) const { const auto n = x_k.size(); diff --git a/libmbse/src/factors/FactorGyroscope.cpp b/libmbse/src/factors/FactorGyroscope.cpp index 897bcd5..0446507 100644 --- a/libmbse/src/factors/FactorGyroscope.cpp +++ b/libmbse/src/factors/FactorGyroscope.cpp @@ -22,8 +22,7 @@ using namespace mrpt::math; gtsam::NonlinearFactor::shared_ptr FactorGyroscope::clone() const { - return std::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); +return gtsam::NonlinearFactor::shared_ptr(new This(*this)); } void FactorGyroscope::print( @@ -44,8 +43,8 @@ bool FactorGyroscope::equals( } gtsam::Vector FactorGyroscope::evaluateError( - const state_t& q_k, const state_t& dq_k, gtsam::OptionalMatrixType H1, - gtsam::OptionalMatrixType H2) const + const state_t& q_k, const state_t& dq_k, boost::optional H1, + boost::optional H2) const { const auto n = q_k.size(); if (dq_k.size() != n) diff --git a/libmbse/src/factors/FactorInverseDynamics.cpp b/libmbse/src/factors/FactorInverseDynamics.cpp index 1e8b50a..41781e3 100644 --- a/libmbse/src/factors/FactorInverseDynamics.cpp +++ b/libmbse/src/factors/FactorInverseDynamics.cpp @@ -24,8 +24,7 @@ FactorInverseDynamics::~FactorInverseDynamics() = default; gtsam::NonlinearFactor::shared_ptr FactorInverseDynamics::clone() const { - return std::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); +return gtsam::NonlinearFactor::shared_ptr(new This(*this)); } void FactorInverseDynamics::print( @@ -110,7 +109,7 @@ bool FactorInverseDynamics::equals( } gtsam::Vector FactorInverseDynamics::evaluateError( - const state_t& Q_k, gtsam::OptionalMatrixType d_e_Q) const + const state_t& Q_k, boost::optional d_e_Q) const { MRPT_START diff --git a/libmbse/src/factors/FactorTrapInt.cpp b/libmbse/src/factors/FactorTrapInt.cpp index a134561..d451fb0 100644 --- a/libmbse/src/factors/FactorTrapInt.cpp +++ b/libmbse/src/factors/FactorTrapInt.cpp @@ -17,8 +17,7 @@ FactorTrapInt::~FactorTrapInt() = default; gtsam::NonlinearFactor::shared_ptr FactorTrapInt::clone() const { - return std::static_pointer_cast( - gtsam::NonlinearFactor::shared_ptr(new This(*this))); +return gtsam::NonlinearFactor::shared_ptr(new This(*this)); } // Build function print defined in the header FactorTrapInt.h @@ -45,9 +44,9 @@ bool FactorTrapInt::equals( gtsam::Vector FactorTrapInt::evaluateError( const state_t& x_k, const state_t& x_kp1, const state_t& v_k, - const state_t& v_kp1, gtsam::OptionalMatrixType H1, - gtsam::OptionalMatrixType H2, gtsam::OptionalMatrixType H3, - gtsam::OptionalMatrixType H4) const + const state_t& v_kp1, boost::optional H1, + boost::optional H2, boost::optional H3, + boost::optional H4) const { const auto n = x_k.size(); From c53d811ff61acabedff94b3667cc45d3af589d32 Mon Sep 17 00:00:00 2001 From: AmirSamanMirjalili Date: Mon, 20 May 2024 17:59:19 +0330 Subject: [PATCH 3/9] Eigen issue solved --- FourBar.YAML | 1 + 1 file changed, 1 insertion(+) create mode 100644 FourBar.YAML diff --git a/FourBar.YAML b/FourBar.YAML new file mode 100644 index 0000000..950eeda --- /dev/null +++ b/FourBar.YAML @@ -0,0 +1 @@ +# A four bars mecahnism# Degrees of freedom in q=[x1 y1 x2 y2]^T# Modeled in Natural coordinates:## 2:(q3,q4)# +--------- o# | (xb,yb)# |# |# o--------- + 1:(q1,q2)# (xa,ya)# parameters: L: 1.0 # length [m] xb: 4.0 points:# 0 (="A")- {x: 0, y: 0, fixed: true}# 1- { x: L, y: 0 }# 2- { x: L, y: 2∗L }# 3 (="B")- { x: xb, y: 0, fixed: true } planar_bodies:# 0- points: [0 , 1] length: L mass: 1.0 I0: (1/3)∗mass∗length^2 cog: [0.5∗length , 0.0]# 1- points: [1 , 2] length: 2∗L mass: 2.0 I0: (1/3)∗mass∗length^2 cog: [0.5∗length , 0.0]# 3- points: [2 , 3] length: auto mass: 4.0 I0: (1/3)∗mass∗length^2 cog: [0.5∗length , 0.0] \ No newline at end of file From e7d35ed744e8b4e3aee51e55666004a003e69288 Mon Sep 17 00:00:00 2001 From: AmirSamanMirjalili Date: Mon, 20 May 2024 18:18:52 +0330 Subject: [PATCH 4/9] FourBar mechanism which is outlined in paper --- FourBar.YAML | 42 +++++++++++++++++++++++++++++++++++++++++- 1 file changed, 41 insertions(+), 1 deletion(-) diff --git a/FourBar.YAML b/FourBar.YAML index 950eeda..2d60bf3 100644 --- a/FourBar.YAML +++ b/FourBar.YAML @@ -1 +1,41 @@ -# A four bars mecahnism# Degrees of freedom in q=[x1 y1 x2 y2]^T# Modeled in Natural coordinates:## 2:(q3,q4)# +--------- o# | (xb,yb)# |# |# o--------- + 1:(q1,q2)# (xa,ya)# parameters: L: 1.0 # length [m] xb: 4.0 points:# 0 (="A")- {x: 0, y: 0, fixed: true}# 1- { x: L, y: 0 }# 2- { x: L, y: 2∗L }# 3 (="B")- { x: xb, y: 0, fixed: true } planar_bodies:# 0- points: [0 , 1] length: L mass: 1.0 I0: (1/3)∗mass∗length^2 cog: [0.5∗length , 0.0]# 1- points: [1 , 2] length: 2∗L mass: 2.0 I0: (1/3)∗mass∗length^2 cog: [0.5∗length , 0.0]# 3- points: [2 , 3] length: auto mass: 4.0 I0: (1/3)∗mass∗length^2 cog: [0.5∗length , 0.0] \ No newline at end of file +# A four bars mechanism +# Degrees of freedom in q=[x1 y1 x2 y2]^T +# Modeled in Natural coordinates: +# 2:(q3,q4) +# +--------- o +# | (xb,yb) +# | +# | +# o--------- + 1:(q1,q2) +# (xa,ya) + +parameters: + L: 1.0 # length [m] + xb: 4.0 + +points: + #0: + - {x: 0, y: 0, fixed: true} # A + - { x: L, y: 0 } + - { x: L, y: 2*L } + - { x: xb, y: 0, fixed: true } # B + +planar_bodies: +- + points: [0, 1] + length: L + mass: 1.0 + I0: (1/3)*mass*length^2 + cog: [0.5*length, 0.0] +- + points: [1, 2] + length: 2*L + mass: 2.0 + I0: (1/3)*mass*length^2 + cog: [0.5*length, 0.0] +- + points: [2, 3] + length: auto + mass: 4.0 + I0: (1/3)*mass*length^2 + cog: [0.5*length, 0.0] \ No newline at end of file From 2f497bd9e6fc506fafe5d089edadfb3df8747d58 Mon Sep 17 00:00:00 2001 From: AmirSamanMirjalili Date: Mon, 20 May 2024 18:19:42 +0330 Subject: [PATCH 5/9] GTSAM TBB check commented, since it doesnt affect simulation --- libmbse/src/factors/FactorDynamics.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/libmbse/src/factors/FactorDynamics.cpp b/libmbse/src/factors/FactorDynamics.cpp index 684746e..35057bb 100644 --- a/libmbse/src/factors/FactorDynamics.cpp +++ b/libmbse/src/factors/FactorDynamics.cpp @@ -14,9 +14,9 @@ #include #include -#if defined(GTSAM_USE_TBB) -#error "So far, MBSE is incompatible with GTSAM+TBB!" -#endif +//#if defined(GTSAM_USE_TBB) +//#error "So far, MBSE is incompatible with GTSAM+TBB!" +//#endif MRPT_TODO( "**IMPORTANT** Refactor AssembledRigidModel to separate state and model " "data to avoid multithread errors using GTSAM+TBB") From f845a69bc56e93048d8a1c5dd4be5db479e9f620 Mon Sep 17 00:00:00 2001 From: Amir Saman Mirjalili <57065409+AmirSamanMirjalili@users.noreply.github.com> Date: Mon, 20 May 2024 18:31:14 +0330 Subject: [PATCH 6/9] Update README.md --- README.md | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) diff --git a/README.md b/README.md index ec16c7e..1cbf73f 100644 --- a/README.md +++ b/README.md @@ -32,19 +32,19 @@ For the theory behinds this work, refer to : ![ScreenShot](https://raw.githubusercontent.com/MBDS/multibody-state-estimation/master/doc/source/_static/mbde-pf-screenshot.jpg) -## Compiling +## My config for running this project Dependencies: * A C++ compiler * CMake * SuiteSparse * [MRPT](https://github.com/mrpt/mrpt/) (>=2.0.0) * Ubuntu: Use [this PPA](https://launchpad.net/~joseluisblancoc/+archive/ubuntu/mrpt) - * [GTSAM](https://github.com/borglab/gtsam) >=4.2 - * Ubuntu: Use [this PPA](https://launchpad.net/~borglab/+archive/ubuntu/gtsam-release-4.1) + * [GTSAM](https://github.com/borglab/gtsam) =4.2 + * Ubuntu: built by using system's default Eigen v3.4 with command : `cmake -DGTSAM_USE_SYSTEM_EIGEN=ON ..` In Ubuntu, install dependencies with: - sudo apt-get install build-essential cmake libsuitesparse-dev libmrpt-dev libgtsam-dev + sudo apt-get install build-essential cmake libsuitesparse-dev To build: @@ -57,6 +57,12 @@ To build: You should also be able to compile this project under Windows and Visual Studio. +To Run the FourBar.YAML example do the following in terminal + + ```sh + /bin/mbse-dynamic-simulation --mechanism [other_optional_arguments] + ``` + ## Using mbse as a library in a user program In your CMake project, add: From c7662a5140264652fa899ab82ad4485d66583d3e Mon Sep 17 00:00:00 2001 From: Amir Saman Mirjalili <57065409+AmirSamanMirjalili@users.noreply.github.com> Date: Mon, 20 May 2024 18:32:19 +0330 Subject: [PATCH 7/9] Update README.md --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 1cbf73f..260b6fa 100644 --- a/README.md +++ b/README.md @@ -40,7 +40,7 @@ Dependencies: * [MRPT](https://github.com/mrpt/mrpt/) (>=2.0.0) * Ubuntu: Use [this PPA](https://launchpad.net/~joseluisblancoc/+archive/ubuntu/mrpt) * [GTSAM](https://github.com/borglab/gtsam) =4.2 - * Ubuntu: built by using system's default Eigen v3.4 with command : `cmake -DGTSAM_USE_SYSTEM_EIGEN=ON ..` + * Ubuntu: gtsam was built by using system's default Eigen v3.4 with command : `cmake -DGTSAM_USE_SYSTEM_EIGEN=ON ..` In Ubuntu, install dependencies with: @@ -59,7 +59,7 @@ You should also be able to compile this project under Windows and Visual Studio. To Run the FourBar.YAML example do the following in terminal - ```sh + ```bash /bin/mbse-dynamic-simulation --mechanism [other_optional_arguments] ``` From 2a4f52763053a462e6784ed982e0c4100746d894 Mon Sep 17 00:00:00 2001 From: Amir Saman Mirjalili <57065409+AmirSamanMirjalili@users.noreply.github.com> Date: Mon, 20 May 2024 18:32:52 +0330 Subject: [PATCH 8/9] Update README.md --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index 260b6fa..ce6664a 100644 --- a/README.md +++ b/README.md @@ -59,8 +59,8 @@ You should also be able to compile this project under Windows and Visual Studio. To Run the FourBar.YAML example do the following in terminal - ```bash - /bin/mbse-dynamic-simulation --mechanism [other_optional_arguments] + ``` + /bin/mbse-dynamic-simulation --mechanism [other_optional_arguments] ``` ## Using mbse as a library in a user program From fd4c005836149ac5721c2c6bccbb8addbaaf7965 Mon Sep 17 00:00:00 2001 From: Amir Saman Mirjalili <57065409+AmirSamanMirjalili@users.noreply.github.com> Date: Mon, 20 May 2024 18:33:10 +0330 Subject: [PATCH 9/9] Update README.md --- README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/README.md b/README.md index ce6664a..8331a80 100644 --- a/README.md +++ b/README.md @@ -59,9 +59,9 @@ You should also be able to compile this project under Windows and Visual Studio. To Run the FourBar.YAML example do the following in terminal - ``` + /bin/mbse-dynamic-simulation --mechanism [other_optional_arguments] - ``` + ## Using mbse as a library in a user program