add RBDL and single leg inverse dynamic controller

README.md

@@ -7,10 +7,27 @@ package for quadruped locomotion
 - [ooqp_eigen_interface](https://github.com/HITSZ-LeggedRobotics/dependencies/tree/master/ooqp_eigen_interface-master)
 - [MA27](https://github.com/HITSZ-LeggedRobotics/ma27)
 - [OOQP](https://github.com/HITSZ-LeggedRobotics/OOQP)
+- [RBDL](https://github.com/HITSZ-LeggedRobotics/rbdl)
 - [others](https://github.com/HITSZ-LeggedRobotics/dependencies)
 
 ## Build Dependencies
-  - first install **kindr** and git clone other ROS dependence package AND run `catkin_make`
+  - first build and install **kindr**
+  - build and install **RBDL** follow instructions:
+
+    ```
+    mkdir build
+    cd build
+    cmake -D CMAKE_BUILD_TYPE=Release RBDL_BUILD_ADDON_URDFREADER=TRUE ../
+    make install
+    ```
+    then and .cmake file for cmake to find RBDL
+    ```
+    cd usr/local/lib/cmake
+    sudo mkdir rbdl
+    sudo cp RBDLConfig.cmake /usr/local/lib/cmake/rbdl
+    ```
+  - **Attention:** before make, change the directory in 'quadruped_model/src/quadrupedkinematics,cpp' line 19 with your own directory.
+  - git clone other ROS dependence package AND run `catkin_make`
 
 ## Install
 

balance_controller/CMakeLists.txt

@@ -27,6 +27,7 @@ find_package(catkin REQUIRED COMPONENTS
   realtime_tools
   sim_assiants
   ooqp_eigen_interface
+  single_leg_test
 )
 
 ## System dependencies are found with CMake's conventions

balance_controller/config/controller_gains.yaml

@@ -3,13 +3,13 @@ balance_controller:
     heading:
       kp: 5000
       kd: 5000
-      kff: 1000
+      kff: 10
     lateral:
       kp: 5000
       kd: 4000
-      kff: 1000
+      kff: 10
     vertical:
-      kp: 5000
+      kp: 10000
       kd: 5000
       kff: 100
     roll:
@@ -28,7 +28,7 @@ balance_controller:
     weights:
       force:
         heading: 1
-        lateral: 1
+        lateral: 5
         vertical: 1
       torque:
         roll: 10
@@ -39,3 +39,13 @@ balance_controller:
     constraints:
       friction_coefficient: 0.6
       minimal_normal_force: 10
+single_leg_controller:
+  x_direction:
+    kp: 5000
+    kd: 200
+  y_direction:
+    kp: 5000
+    kd: 200
+  z_direction:
+    kp: 5000
+    kd: 200

balance_controller/include/balance_controller/ros_controler/ros_balance_controller.hpp

@@ -20,6 +20,8 @@
 #include "balance_controller/ros_controler/robot_state_interface.hpp"
 #include "balance_controller/ros_controler/gazebo_state_hardware_interface.hpp"
 
+#include "single_leg_test/model_test_header.hpp"
+
 #include <control_toolbox/pid.h>
 
 #include <pluginlib/class_list_macros.hpp>
@@ -34,6 +36,7 @@
 #include "state_switcher/StateSwitcher.hpp"
 
 #include "std_msgs/Int8MultiArray.h"
+#include "std_msgs/Float64MultiArray.h"
 #include "nav_msgs/Odometry.h"
 #include "Eigen/Dense"
 #include "Eigen/LU"
@@ -45,7 +48,8 @@ namespace balance_controller {
   typedef std::unordered_map<free_gait::LimbEnum, ros::Time, EnumClassHash> LimbDuration;
   typedef std::unordered_map<free_gait::LimbEnum, bool, EnumClassHash> LimbFlag;
   typedef std::unordered_map<free_gait::LimbEnum, double, EnumClassHash> LimbPhase;
-    typedef std::unordered_map<free_gait::LimbEnum, free_gait::Vector, EnumClassHash> LimbVector;
+  typedef std::unordered_map<free_gait::LimbEnum, free_gait::Vector, EnumClassHash> LimbVector;
+
   public:
     RosBalanceController();
     ~RosBalanceController();
@@ -79,6 +83,7 @@ namespace balance_controller {
      */
     realtime_tools::RealtimeBuffer<std::vector<double>> commands_buffer;
     realtime_tools::RealtimeBuffer<Pose> command_pose_buffer;
+    realtime_tools::RealtimeBuffer<LimbVector> command_foot_buffer, command_foot_vel_buffer;
     unsigned int n_joints;
   private:
     /**
@@ -100,10 +105,11 @@ namespace balance_controller {
     std::vector<free_gait::BranchEnum> branches_;
 
     LimbState limbs_state, limbs_desired_state;
+    LimbFlag real_contact_, is_cartisian_motion_;
     LimbDuration t_sw0, t_st0;
     LimbFlag sw_flag, st_flag;
     LimbPhase sw_phase, st_phase;
-
+    LimbVector foot_positions, foot_velocities, foot_accelerations;
     /**
      * @brief contact_distribution_ , pointer to contact force optimaziton
      */
@@ -114,14 +120,16 @@ namespace balance_controller {
 
     std::shared_ptr<VirtualModelController> virtual_model_controller_;
 
+    std::shared_ptr<MyRobotSolver> single_leg_solver_;
+
     std::vector<control_toolbox::Pid> pid_controllers_;       /**< Internal PID controllers. */
 
     std::vector<urdf::JointConstSharedPtr> joint_urdfs_;
     /**
      * @brief baseCommandCallback, ros subscriber callback
      * @param robot_state
      */
-    void baseCommandCallback(const free_gait_msgs::RobotStateConstPtr& robot_state);
+    void baseCommandCallback(const free_gait_msgs::RobotStateConstPtr& robot_state_msg);
     void footContactsCallback(const sim_assiants::FootContactsConstPtr& foot_contacts);
 
     void enforceJointLimits(double &command, unsigned int index);
@@ -134,20 +142,24 @@ namespace balance_controller {
     /**
      * @brief joint_command_pub_, for debug to monitor
      */
-    ros::Publisher joint_command_pub_, base_command_pub_, base_actual_pub_, joint_actual_pub_, leg_state_pub_, contact_desired_pub_;
+    ros::Publisher joint_command_pub_, base_command_pub_, base_actual_pub_, joint_actual_pub_,
+    leg_state_pub_, contact_desired_pub_, leg_phase_pub_, desired_robot_state_pub_, actual_robot_state_pub_;
     std::vector<nav_msgs::Odometry> base_command_pose_, base_actual_pose_;
     std::vector<sensor_msgs::JointState> joint_command_, joint_actual_;
     std::vector<std_msgs::Int8MultiArray> leg_states_;
     std::vector<sim_assiants::FootContacts> foot_desired_contact_;
+    std::vector<std_msgs::Float64MultiArray> leg_phases_;
+    std::vector<free_gait_msgs::RobotState> desired_robot_state_, actual_robot_state_;
     ros::ServiceServer log_data_srv_;
 
     int log_length_, log_index_;
     bool logDataCapture(std_srvs::Empty::Request& req,
                         std_srvs::Empty::Response& res);
-    LimbFlag update_surface_normal_flag;
-    bool store_current_joint_state_flag_;
+    LimbFlag update_surface_normal_flag, store_current_joint_state_flag_;
+//    bool store_current_joint_state_flag_;
     LimbVector surface_normals;
-    std::vector<double> stored_limb_joint_position_;
+//    std::vector<double> stored_limb_joint_position_;
+    free_gait::JointPositions stored_limb_joint_position_;
   };
 
 }

balance_controller/package.xml

@@ -78,6 +78,7 @@
   <depend>realtime_tools</depend>
   <depend>sim_assiants</depend>
   <depend>ooqp_eigen_interface</depend>
+  <depend>single_leg_test</depend>
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>

balance_controller/src/contact_force_distribution/ContactForceDistribution.cpp

@@ -518,7 +518,6 @@ bool ContactForceDistribution::computeJointTorques()
   const LinearAcceleration gravitationalAccelerationInWorldFrame = LinearAcceleration(0.0,0.0,-9.8);//torso_->getProperties().getGravity();
   const LinearAcceleration gravitationalAccelerationInBaseFrame = robot_state_->getOrientationBaseToWorld().inverseRotate(gravitationalAccelerationInWorldFrame);//torso_->getMeasuredState().getOrientationWorldToBase().rotate(gravitationalAccelerationInWorldFrame);
 
-
 //  const int nDofPerLeg = 3; // TODO move to robot commons
 //  const int nDofPerContactPoint = 3; // TODO move to robot commons
 
@@ -553,6 +552,9 @@ bool ContactForceDistribution::computeJointTorques()
 //          jointTorques -= LegBase::JointTorques( link->getTranslationJacobianBaseToCoMInBaseFrame().transpose() * Force(link->getMass() * gravitationalAccelerationInBaseFrame).toImplementation());
 //        }
 
+        free_gait::JointPositionsLeg joint_position_leg = robot_state_->getJointPositionFeedbackForLimb(legInfo.first);
+        jointTorques += robot_state_->getGravityCompensationForLimb(legInfo.first, joint_position_leg, free_gait::Force(gravitationalAccelerationInBaseFrame.toImplementation()));
+
 //        legInfo.first->setDesiredJointTorques(jointTorques);
         robot_state_->setJointEffortsForLimb(legInfo.first, jointTorques);
 //        ROS_INFO("Joint Torque for %d is : \n", static_cast<int>(legInfo.first));

balance_controller/src/ros_controller/gazebo_state_hardware_interface.cpp

@@ -269,7 +269,7 @@ bool SimRobotStateHardwareInterface::initSim(
 void SimRobotStateHardwareInterface::readSim(ros::Time time, ros::Duration period)
 {
   double real_time_factor = base_link_ptr_->GetParentModel()->GetWorld()->GetPhysicsEngine()->GetTargetRealTimeFactor();
-  real_time_factor = 0.4;
+  real_time_factor = 0.55;
   for(unsigned int j=0; j < n_dof_; j++)
   {
     // Gazebo has an interesting API...