Merge pull request #23 from ori-drs/tuning/kinematics

Tuning/kinematics

Author: mcamurri

filter_tools/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
   <name>filter_tools</name>
-  <version>0.0.0</version>
+  <version>0.1.0</version>
   <description>Description</description>
   <maintainer email="mfallon@robots.ox.ac.uk">Maurice Fallon</maintainer>
   <author email="mfallon@robots.ox.ac.uk">Maurice Fallon</author>

pronto_biped_core/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>pronto_biped_core</name>
-  <version>0.0.1</version>
+  <version>0.1.0</version>
 
   <description>Description</description>
   <maintainer email="mfallon@robots.ox.ac.uk">Maurice Fallon</maintainer>

pronto_biped_ros/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>pronto_biped_ros</name>
-  <version>0.0.0</version>
+  <version>0.1.0</version>
   <description>The pronto_biped_ros package</description>
 
   <!-- One maintainer tag required, multiple allowed, one person per tag -->

pronto_core/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
   <name>pronto_core</name>
-  <version>0.0.1</version>
+  <version>0.1.0</version>
   <description>
     The pronto_core package contains the EKF and the main 
     modules to perform state estimation, including: inertial, gps, optical

pronto_msgs/CMakeLists.txt

@@ -1,47 +1,8 @@
 cmake_minimum_required(VERSION 3.0.2)
 project(pronto_msgs)
 
-set(CMAKE_CXX_STANDARD 14)
-
-## Find catkin macros and libraries
-## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
-## is used, also find other catkin packages
 find_package(catkin REQUIRED roscpp rospy std_msgs geometry_msgs message_generation)
 
-## System dependencies are found with CMake's conventions
-# find_package(Boost REQUIRED COMPONENTS system)
-
-
-## Uncomment this if the package has a setup.py. This macro ensures
-## modules and global scripts declared therein get installed
-## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
-# catkin_python_setup()
-
-################################################
-## Declare ROS messages, services and actions ##
-################################################
-
-## To declare and build messages, services or actions from within this
-## package, follow these steps:
-## * Let MSG_DEP_SET be the set of packages whose message types you use in
-##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
-## * In the file package.xml:
-##   * add a build_depend tag for "message_generation"
-##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
-##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
-##     but can be declared for certainty nonetheless:
-##     * add a exec_depend tag for "message_runtime"
-## * In this file (CMakeLists.txt):
-##   * add "message_generation" and every package in MSG_DEP_SET to
-##     find_package(catkin REQUIRED COMPONENTS ...)
-##   * add "message_runtime" and every package in MSG_DEP_SET to
-##     catkin_package(CATKIN_DEPENDS ...)
-##   * uncomment the add_*_files sections below as needed
-##     and list every .msg/.srv/.action file to be processed
-##   * uncomment the generate_messages entry below
-##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
-
-## Generate messages in the 'msg' folder
 add_message_files(FILES  VisualOdometryUpdate.msg
                          FilterState.msg
                          GPSData.msg
@@ -50,146 +11,9 @@ add_message_files(FILES  VisualOdometryUpdate.msg
 			 LidarOdometryUpdate.msg
                          ControllerFootContact.msg
                          BipedForceTorqueSensors.msg
-                         QuadrupedForceTorqueSensors.msg)
-
-## Generate services in the 'srv' folder
-# add_service_files(
-#   FILES
-#   Service1.srv
-#   Service2.srv
-# )
+                         QuadrupedForceTorqueSensors.msg
+                         VelocityWithSigmaBounds.msg)
 
-## Generate actions in the 'action' folder
-# add_action_files(
-#   FILES
-#   Action1.action
-#   Action2.action
-# )
-
-## Generate added messages and services with any dependencies listed here
 generate_messages(DEPENDENCIES std_msgs geometry_msgs)
 
-################################################
-## Declare ROS dynamic reconfigure parameters ##
-################################################
-
-## To declare and build dynamic reconfigure parameters within this
-## package, follow these steps:
-## * In the file package.xml:
-##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
-## * In this file (CMakeLists.txt):
-##   * add "dynamic_reconfigure" to
-##     find_package(catkin REQUIRED COMPONENTS ...)
-##   * uncomment the "generate_dynamic_reconfigure_options" section below
-##     and list every .cfg file to be processed
-
-## Generate dynamic reconfigure parameters in the 'cfg' folder
-# generate_dynamic_reconfigure_options(
-#   cfg/DynReconf1.cfg
-#   cfg/DynReconf2.cfg
-# )
-
-###################################
-## catkin specific configuration ##
-###################################
-## The catkin_package macro generates cmake config files for your package
-## Declare things to be passed to dependent projects
-## INCLUDE_DIRS: uncomment this if your package contains header files
-## LIBRARIES: libraries you create in this project that dependent projects also need
-## CATKIN_DEPENDS: catkin_packages dependent projects also need
-## DEPENDS: system dependencies of this project that dependent projects also need
-catkin_package(
-#  INCLUDE_DIRS include
-#  LIBRARIES pronto_msgs
-  CATKIN_DEPENDS message_runtime std_msgs geometry_msgs
-#  DEPENDS system_lib
-)
-
-###########
-## Build ##
-###########
-
-## Specify additional locations of header files
-## Your package locations should be listed before other locations
-include_directories(
-# include
-# ${catkin_INCLUDE_DIRS}
-)
-
-## Declare a C++ library
-# add_library(${PROJECT_NAME}
-#   src/${PROJECT_NAME}/pronto_msgs.cpp
-# )
-
-## Add cmake target dependencies of the library
-## as an example, code may need to be generated before libraries
-## either from message generation or dynamic reconfigure
-# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Declare a C++ executable
-## With catkin_make all packages are built within a single CMake context
-## The recommended prefix ensures that target names across packages don't collide
-# add_executable(${PROJECT_NAME}_node src/pronto_msgs_node.cpp)
-
-## Rename C++ executable without prefix
-## The above recommended prefix causes long target names, the following renames the
-## target back to the shorter version for ease of user use
-## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
-# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
-
-## Add cmake target dependencies of the executable
-## same as for the library above
-# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
-## Specify libraries to link a library or executable target against
-# target_link_libraries(${PROJECT_NAME}_node
-#   ${catkin_LIBRARIES}
-# )
-
-#############
-## Install ##
-#############
-
-# all install targets should use catkin DESTINATION variables
-# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
-
-## Mark executable scripts (Python etc.) for installation
-## in contrast to setup.py, you can choose the destination
-# install(PROGRAMS
-#   scripts/my_python_script
-#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark executables and/or libraries for installation
-# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
-#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
-#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-# )
-
-## Mark cpp header files for installation
-# install(DIRECTORY include/${PROJECT_NAME}/
-#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
-#   FILES_MATCHING PATTERN "*.h"
-#   PATTERN ".svn" EXCLUDE
-# )
-
-## Mark other files for installation (e.g. launch and bag files, etc.)
-# install(FILES
-#   # myfile1
-#   # myfile2
-#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
-# )
-
-#############
-## Testing ##
-#############
-
-## Add gtest based cpp test target and link libraries
-# catkin_add_gtest(${PROJECT_NAME}-test test/test_pronto_msgs.cpp)
-# if(TARGET ${PROJECT_NAME}-test)
-#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
-# endif()
-
-## Add folders to be run by python nosetests
-# catkin_add_nosetests(test)
+catkin_package(CATKIN_DEPENDS message_runtime std_msgs geometry_msgs)

pronto_msgs/msg/VelocityWithSigmaBounds.msg

@@ -0,0 +1,8 @@
+# convenience message encoding the linear velocity
+# one sigma bounds (plus, minus) and the sigma alone
+
+Header header
+geometry_msgs/Vector3 velocity_plus_one_sigma
+geometry_msgs/Vector3 velocity_minus_one_sigma
+geometry_msgs/Vector3 plus_one_sigma
+

pronto_msgs/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>pronto_msgs</name>
-  <version>0.0.0</version>
+  <version>0.1.0</version>
 
   <description>
 	  The pronto_msgs package contains common message types used by the

pronto_quadruped/include/pronto_quadruped/DynamicStanceEstimator.hpp

@@ -1,4 +1,4 @@
-/* Copyright (c) 2015-2019
+/* Copyright (c) 2015-2021
  * Istituto Italiano di Tecnologia (IIT), University of Oxford
  * All rights reserved.
  *
@@ -36,14 +36,9 @@
 namespace pronto {
 namespace quadruped {
 
-typedef typename pronto::quadruped::FeetJacobians FeetJacobians;
-typedef typename pronto::quadruped::FeetContactForces FeetContactForces;
-typedef typename pronto::quadruped::InverseDynamicsBase InverseDynamics;
-typedef typename pronto::quadruped::JSIMBase JSIM;
-typedef typename pronto::quadruped::LegDataMap<Eigen::Vector3d> LegVector3Map;
-typedef typename pronto::quadruped::LegDataMap<double> LegScalarMap;
-typedef typename pronto::quadruped::ForwardKinematics ForwardKinematics;
-
+using InverseDynamics = InverseDynamicsBase;
+using JSIM = JSIMBase ;
+using LegScalarMap = LegDataMap<double>;
 using Wrench = iit::rbd::ForceVector;
 
 /**

pronto_quadruped/include/pronto_quadruped/LegOdometer.hpp

@@ -1,4 +1,4 @@
-/* Copyright (c) 2015-2019
+/* Copyright (c) 2015-2021
  * Istituto Italiano di Tecnologia (IIT), University of Oxford
  * All rights reserved.
  *
@@ -36,6 +36,7 @@
 
 // iit-commons
 #include <pronto_quadruped_commons/leg_data_map.h>
+#include <pronto_quadruped_commons/leg_vector_map.h>
 #include <pronto_quadruped_commons/forward_kinematics.h>
 #include <pronto_quadruped_commons/feet_jacobians.h>
 
@@ -45,11 +46,6 @@
 namespace pronto {
 namespace quadruped {
 
-using pronto::quadruped::LF;
-using pronto::quadruped::RF;
-using pronto::quadruped::LH;
-using pronto::quadruped::RH;
-
 /**
  * @brief The LegOdometer class computes the velocity of a floating base
  * legged robot given its joint state, the stance feet, and other data.
@@ -59,12 +55,6 @@ class LegOdometer  : public LegOdometerBase {
     template <class T>
     using LegDataMap = pronto::quadruped::LegDataMap<T>;
 
-    typedef typename pronto::quadruped::FeetJacobians FeetJacobians;
-    typedef typename pronto::quadruped::ForwardKinematics ForwardKinematics;
-    typedef typename pronto::quadruped::JointState JointState;
-    typedef typename pronto::quadruped::LegBoolMap LegBoolMap;
-    typedef typename pronto::quadruped::LegDataMap<Eigen::Vector3d> LegVector3Map;
-
     enum class SigmaMode {STATIC_SIGMA = 0,/*!< use constant covariance */
                           VAR_SIGMA, /*!< compute covariance from stance legs */
                           IMPACT_SIGMA, /*!< compute covariance from impact information */
@@ -84,16 +74,16 @@ class LegOdometer  : public LegOdometerBase {
 
 
 public:
-    inline bool estimatePose(const uint64_t utime,
+    bool estimatePose(const uint64_t utime,
                       const JointState& q,
                       const LegBoolMap& stance_legs,
                       const LegScalarMap& stance_prob,
                       Vector3d& position,
                       Matrix3d& pos_covariance,
                       Quaterniond& orientation,
                       Matrix3d& orient_covariance) override {
-        std::cerr << "Function not implemented yet!" << std::endl;
-	return false;
+      std::cerr << "Function not implemented yet!" << std::endl;
+      return false;
     }
 
     bool estimateVelocity(const uint64_t utime,
@@ -105,16 +95,16 @@ class LegOdometer  : public LegOdometerBase {
                           Vector3d& velocity,
                           Matrix3d& covariance) override;
 
-    inline void getVelocity(Vector3d& velocity, Matrix3d& covariance) override
+    void getVelocity(Vector3d& velocity, Matrix3d& covariance) override
     {
         velocity = xd_b_;
         covariance = vel_cov_;
     }
-    inline void getPosition(Vector3d& position, Matrix3d& covariance) override
+    void getPosition(Vector3d& position, Matrix3d& covariance) override
     {
         std::cerr << "Function not implemented yet!" << std::endl;
     }
-    inline void getOrientation(Quaterniond& orientation, Matrix3d& covariance) override
+    void getOrientation(Quaterniond& orientation, Matrix3d& covariance) override
     {
         std::cerr << "Function not implemented yet!" << std::endl;
     }
@@ -128,9 +118,9 @@ class LegOdometer  : public LegOdometerBase {
     virtual void setGrfDelta(const LegScalarMap& grf_delta);
 
     // Debugging methods
-    void getVelocitiesFromLegs(LegVector3Map & vd) override;
-    void getFeetPositions(LegVector3Map & jd) override;
-    virtual LegVector3Map getFootPos();
+    void getVelocitiesFromLegs(LegVectorMap & vd) override;
+    void getFeetPositions(LegVectorMap & jd) override;
+    virtual LegVectorMap getFootPos();
 
 
     void setInitVelocityCov(const Matrix3d& vel_cov) override;
@@ -140,6 +130,9 @@ class LegOdometer  : public LegOdometerBase {
 
     // Configuration methods
     virtual void setMode(const SigmaMode  s_mode, const AverageMode a_mode);
+    virtual void getMode(SigmaMode& s_mode, AverageMode& a_mode);
+    virtual std::string printMode();
+    void setSpeedLimit(const double& limit) override;
 
 protected:
     FeetJacobians& feet_jacobians_;
@@ -165,14 +158,15 @@ class LegOdometer  : public LegOdometerBase {
      * kinematics.
      * <b>NOTE</b>: these are NOT the velocity of the legs.
      */
-    LegVector3Map base_vel_leg_;
-    LegVector3Map foot_pos_;
+    LegVectorMap base_vel_leg_;
+    LegVectorMap foot_pos_;
 
     Eigen::Vector3d xd_b_; // estimated velocity, base frame
-    Eigen::Vector3d old_xd_b_; // previous estimated velocity, base frame
 
     Eigen::Array4d grf_delta_;
     Eigen::Array4d grf_;
+
+    double speed_limit_; // upper limit of the absolute norm of the linear velocity
 };
 } // namespace quadruped
 } // namespace pronto