Fix bugs with diff-drive dynamics (#115)

Author: pockerman

src/rlenvs/dynamics/diff_drive_dynamics.cpp

@@ -20,7 +20,7 @@ DiffDriveDynamics::integrate_state_v1(const SysState<3>& state, real_t tol, real
     if(std::fabs(w) < tol){
 
         /// assume zero angular velocity
-       auto distance = 0.5*v*dt;
+       auto distance = 0.5 * v * dt;
        auto xincrement = (distance + errors[0])*std::cos(values[2]  + errors[1]);
        auto yincrement = (distance + errors[0])*std::sin(values[2]  + errors[1]);
 
@@ -29,7 +29,7 @@ DiffDriveDynamics::integrate_state_v1(const SysState<3>& state, real_t tol, real
     }
     else{
 
-        other[2] += w*dt + errors[1];
+        other[2] += w * dt + errors[1];
 
         /// clip the value
         if(std::fabs(state[2]) > rlenvscpp::consts::maths::PI){
@@ -215,7 +215,7 @@ DiffDriveDynamics::initialize_matrices(const DiffDriveDynamics::input_type& inpu
   set_matrix_update_flag(true);
 
   if(!this->has_matrix("F")){
-    matrix_type F = matrix_type::Zero(3, 2); //(3,3, 0.0);
+    matrix_type F = matrix_type::Zero(3, 3); //(3,3, 0.0);
     this->set_matrix("F", F);
   }
 
@@ -235,8 +235,8 @@ DiffDriveDynamics::update_matrices(const DiffDriveDynamics::input_type& input){
    auto v = rlenvscpp::utils::template resolve<real_t>("v", input);
    auto errors = rlenvscpp::utils::template resolve<std::array<real_t, 2>>("errors", input);
 
-   auto distance = 0.5*v*get_time_step();
-   auto orientation = w*get_time_step();
+   auto distance = 0.5 * v * get_time_step();
+   auto orientation = w * get_time_step();
    auto values = this->state_.get_values();
 
    if(std::fabs(w) < tol_){

src/rlenvs/dynamics/system_state.h

@@ -123,8 +123,13 @@ class SysState
     /// \brief Set the values of state variables
     /// container must be of size dim.
     ///
-    template<typename Container>
-    void set(const Container& container);
+    void add(const DynVec<real_t>& container);
+	
+	///
+    /// \brief Set the values of state variables
+    /// container must be of size dim.
+    ///
+	void add(const std::vector<real_t>& container);
 
     ///
     /// \brief Returns the size of the system
@@ -196,14 +201,6 @@ class SysState
     ///
     const std::string as_string()const;
 
-    ///
-    /// \brief increment the analogous entries of the
-    /// state with the entries in the container
-    /// The container must have same size as the state
-    ///
-    template<typename Container>
-    void add(const Container& container);
-
     ///
     /// \brief Scale the values of the state
     ///
@@ -579,9 +576,23 @@ SysState<dim>::as_string()const{
 }
 
 template<int dim>
-template<typename Container>
 void
-SysState<dim>::add(const Container& container){
+SysState<dim>::add(const DynVec<real_t>& container){
+    if(container.size() != dim){
+        throw std::logic_error("Invalid container size for update. "+
+                               std::to_string(container.size())+
+                               " should be"+
+                               std::to_string(dim));
+    }
+
+    for(uint_t i=0; i<dim; ++i){
+        values_[i].second += container[i];
+    }
+}
+
+template<int dim>
+void
+SysState<dim>::add(const std::vector<real_t>& container){
     if(container.size() != dim){
         throw std::logic_error("Invalid container size for update. "+
                                std::to_string(container.size())+