add the code explanation

balance_controller/include/balance_controller/ros_controler/single_leg_controller.hpp

@@ -29,6 +29,7 @@
 #include "geometry_msgs/Wrench.h"
 
 #include "algorithm"
+#include "new_quadruped_model_kp/joint_state.h"
 
 
 namespace balance_controller {

control_panel/include/rqt_control_panel_plugin/twst_plugin_ui.cpp

@@ -0,0 +1,102 @@
+#include "twst_plugin_ui.h"
+
+twst_plugin_ui::twst_plugin_ui(QObject *parent)
+    : QAbstractItemModel(parent)
+{
+}
+
+QVariant twst_plugin_ui::headerData(int section, Qt::Orientation orientation, int role) const
+{
+    // FIXME: Implement me!
+}
+
+bool twst_plugin_ui::setHeaderData(int section, Qt::Orientation orientation, const QVariant &value, int role)
+{
+    if (value != headerData(section, orientation, role)) {
+        // FIXME: Implement me!
+        emit headerDataChanged(orientation, section, section);
+        return true;
+    }
+    return false;
+}
+
+QModelIndex twst_plugin_ui::index(int row, int column, const QModelIndex &parent) const
+{
+    // FIXME: Implement me!
+}
+
+QModelIndex twst_plugin_ui::parent(const QModelIndex &index) const
+{
+    // FIXME: Implement me!
+}
+
+int twst_plugin_ui::rowCount(const QModelIndex &parent) const
+{
+    if (!parent.isValid())
+        return 0;
+
+    // FIXME: Implement me!
+}
+
+int twst_plugin_ui::columnCount(const QModelIndex &parent) const
+{
+    if (!parent.isValid())
+        return 0;
+
+    // FIXME: Implement me!
+}
+
+QVariant twst_plugin_ui::data(const QModelIndex &index, int role) const
+{
+    if (!index.isValid())
+        return QVariant();
+
+    // FIXME: Implement me!
+    return QVariant();
+}
+
+bool twst_plugin_ui::setData(const QModelIndex &index, const QVariant &value, int role)
+{
+    if (data(index, role) != value) {
+        // FIXME: Implement me!
+        emit dataChanged(index, index, QVector<int>() << role);
+        return true;
+    }
+    return false;
+}
+
+Qt::ItemFlags twst_plugin_ui::flags(const QModelIndex &index) const
+{
+    if (!index.isValid())
+        return Qt::NoItemFlags;
+
+    return Qt::ItemIsEditable; // FIXME: Implement me!
+}
+
+bool twst_plugin_ui::insertRows(int row, int count, const QModelIndex &parent)
+{
+    beginInsertRows(parent, row, row + count - 1);
+    // FIXME: Implement me!
+    endInsertRows();
+}
+
+bool twst_plugin_ui::insertColumns(int column, int count, const QModelIndex &parent)
+{
+    beginInsertColumns(parent, column, column + count - 1);
+    // FIXME: Implement me!
+    endInsertColumns();
+}
+
+bool twst_plugin_ui::removeRows(int row, int count, const QModelIndex &parent)
+{
+    beginRemoveRows(parent, row, row + count - 1);
+    // FIXME: Implement me!
+    endRemoveRows();
+}
+
+bool twst_plugin_ui::removeColumns(int column, int count, const QModelIndex &parent)
+{
+    beginRemoveColumns(parent, column, column + count - 1);
+    // FIXME: Implement me!
+    endRemoveColumns();
+}

control_panel/include/rqt_control_panel_plugin/twst_plugin_ui.h

@@ -0,0 +1,45 @@
+#ifndef TWST_PLUGIN_UI_H
+#define TWST_PLUGIN_UI_H
+
+#include <QAbstractItemModel>
+
+class twst_plugin_ui : public QAbstractItemModel
+{
+    Q_OBJECT
+
+public:
+    explicit twst_plugin_ui(QObject *parent = nullptr);
+
+    // Header:
+    QVariant headerData(int section, Qt::Orientation orientation, int role = Qt::DisplayRole) const override;
+
+    bool setHeaderData(int section, Qt::Orientation orientation, const QVariant &value, int role = Qt::EditRole) override;
+
+    // Basic functionality:
+    QModelIndex index(int row, int column,
+                      const QModelIndex &parent = QModelIndex()) const override;
+    QModelIndex parent(const QModelIndex &index) const override;
+
+    int rowCount(const QModelIndex &parent = QModelIndex()) const override;
+    int columnCount(const QModelIndex &parent = QModelIndex()) const override;
+
+    QVariant data(const QModelIndex &index, int role = Qt::DisplayRole) const override;
+
+    // Editable:
+    bool setData(const QModelIndex &index, const QVariant &value,
+                 int role = Qt::EditRole) override;
+
+    Qt::ItemFlags flags(const QModelIndex& index) const override;
+
+    // Add data:
+    bool insertRows(int row, int count, const QModelIndex &parent = QModelIndex()) override;
+    bool insertColumns(int column, int count, const QModelIndex &parent = QModelIndex()) override;
+
+    // Remove data:
+    bool removeRows(int row, int count, const QModelIndex &parent = QModelIndex()) override;
+    bool removeColumns(int column, int count, const QModelIndex &parent = QModelIndex()) override;
+
+private:
+};
+
+#endif // TWST_PLUGIN_UI_H

control_panel/src/rqt_control_panel_plugin/rqt_control_panel_plugin.cpp

@@ -16,8 +16,6 @@ namespace rqt_control_panel_plugin {
   {
     widget = new rqt_control_panel_plugin_widget(getNodeHandle());
     context.addWidget(widget);
-
-
   }
 
   void rqt_control_panel_plugin::control_panel_plugin::shutdownPlugin()

control_panel/src/rqt_control_panel_plugin/rqt_control_panel_plugin_widget.cpp

@@ -24,7 +24,7 @@ rqt_control_panel_plugin_widget::rqt_control_panel_plugin_widget(const ros::Node
 
   trotSwitchClient_ = nodehandle_.serviceClient<std_srvs::SetBool>("/gait_generate_switch", false);
 
-  paceSwitchClient_ = nodehandle_.serviceClient<std_srvs::SetBool>("pace_switch",false);
+  paceSwitchClient_ = nodehandle_.serviceClient<std_srvs::SetBool>("/pace_switch",false);
 
   eStopPublisher_ = nodehandle_.advertise<std_msgs::Bool>("/e_stop", 1);
 
@@ -175,17 +175,25 @@ void rqt_control_panel_plugin_widget::on_Controllers_currentChanged(int index)
       e_stop_msg.data = false;
       eStopPublisher_.publish(e_stop_msg);
       std::cout<<"Balance"<<std::endl;
-      controller_switch.request.start_controllers.push_back("base_balance_controller");
-      controller_switch.request.stop_controllers.push_back(current_controller.c_str());
+      controller_switch.request.start_controllers.push_back("base_balance_controller");//the name to start
+      controller_switch.request.stop_controllers.push_back(current_controller.c_str());//the name to stop
       controller_switch.request.strictness = controller_switch.request.STRICT;
+      /**
+         the strictness (BEST_EFFORT or STRICT)
+        #    * STRICT means that switching will fail if anything goes wrong (an invalid
+        #      controller name, a controller that failed to start, etc. )
+        #    * BEST_EFFORT means that even when something goes wrong with on controller,
+        #      the service will still try to start/stop the remaining controllers
+
+        */
       switchControllerClient_.call(controller_switch.request, controller_switch.response);
 
-      control_method.request.configure = tab_name.toStdString();
-      switchControlMethodClient_.call(control_method.request, control_method.response);
+      control_method.request.configure = tab_name.toStdString();//Qstring to std::string, balance means what?
+      switchControlMethodClient_.call(control_method.request, control_method.response);//SwitchControllerMethod, balance
 
       if(controller_switch.response.ok && control_method.response.result)
         {
-          control_method_ = BALANCE;
+          control_method_ = BALANCE;// the whole body controller method
           displayOutputInfos("green", "Switch to Balance Controller");
         }
 

free_gait_core/CMakeLists.txt

@@ -107,28 +107,37 @@ add_library(${PROJECT_NAME}
 )
 
 target_link_libraries(${PROJECT_NAME}
-  ${catkin_LIBRARIES}
-)
+    ${PROJECT_NAME}
+    ${catkin_LIBRARIES})
+
+add_executable(test_kp_node
+    test/test_kp.cpp
+    )
+target_link_libraries(test_kp_node
+    ${PROJECT_NAME}
+    ${catkin_LIBRARIES}
+    )
 
 #############
 ## Testing ##
 #############
 
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread")
 # Add gtest based cpp test target and link libraries
-catkin_add_gtest(${PROJECT_NAME}-test
-  test/test_free_gait_core.cpp
-  test/AdapterDummy.cpp
-  test/StepTest.cpp
-  test/FootstepTest.cpp
+#catkin_add_gtest(${PROJECT_NAME}-test
+#  test/test_free_gait_core.cpp
+#  test/AdapterDummy.cpp
+#  test/StepTest.cpp
+#  test/FootstepTest.cpp
+#  test/test_kp.cpp
 #  test/PoseOptimizationQpTest.cpp
 #  test/PoseOptimizationSQPTest.cpp
-)
-if(TARGET ${PROJECT_NAME}-test)
-  target_link_libraries(${PROJECT_NAME}-test
-    ${PROJECT_NAME}
-  )
-endif()
+#)
+#if(TARGET ${PROJECT_NAME}-test)
+#  target_link_libraries(${PROJECT_NAME}-test
+#    ${PROJECT_NAME}
+#  )
+#endif()
 
 #############
 ## Install ##

free_gait_core/include/free_gait_core/TypeDefs.hpp

@@ -51,7 +51,7 @@ using romo::RotationVector;
 using romo::EulerAnglesXyz;
 using romo::EulerAnglesXyzDiff;
 using romo::Position;
-using Position2 = kindr::Position<double, 2>;
+using Position2 = kindr::Position<double, 2>;//2D position
 using romo::LinearVelocity;
 using romo::LocalAngularVelocity;
 using romo::EulerAnglesZyxDiff;
@@ -88,7 +88,7 @@ const std::vector<LimbEnum> limbEnumCounterClockWiseOrder = { LimbEnum::LF_LEG,
                                                               LimbEnum::RF_LEG };
 
 typedef std::unordered_map<ControlLevel, bool, EnumClassHash> ControlSetup;
-typedef std::unordered_map<LimbEnum, Position, EnumClassHash> Stance;
+typedef std::unordered_map<LimbEnum, Position, EnumClassHash> Stance;//leg and corresponding position
 typedef std::unordered_map<LimbEnum, Position2, EnumClassHash> PlanarStance;
 
 struct CompareByCounterClockwiseOrder;

free_gait_core/include/free_gait_core/base_motion/BaseAuto.hpp

@@ -63,7 +63,7 @@ class BaseAuto : public BaseMotionBase
 
   const std::string& getFrameId(const ControlLevel& controlLevel) const;
 
-  void setNominalStanceInBaseFrame(const PlanarStance& nominalPlanarStanceInBaseFrame);
+  void setNominalStanceInBaseFrame(const PlanarStance& nominalPlanarStanceInBaseFrame);//what is that?
   void setHeight(const double height);
   double getHeight() const;
   void setAverageLinearVelocity(const double averageLinearVelocity);

free_gait_core/include/free_gait_core/base_motion/BaseMotionBase.hpp

@@ -56,7 +56,7 @@ class BaseMotionBase
    */
   BaseMotionBase::Type getType() const;
 
-  virtual const ControlSetup getControlSetup() const;
+  virtual const ControlSetup getControlSetup() const;//control level typedef std::unordered_map<ControlLevel, bool, EnumClassHash> ControlSetup;
 
   /*!
    * Update the trajectory with the base start pose.

free_gait_core/include/free_gait_core/base_motion/BaseTarget.hpp

@@ -20,8 +20,8 @@ namespace free_gait {
 class BaseTarget : public BaseMotionBase
 {
  public:
-  typedef typename curves::CubicHermiteSE3Curve::ValueType ValueType;
-  typedef typename curves::Time Time;
+  typedef typename curves::CubicHermiteSE3Curve::ValueType ValueType;// the value type of CubicHermiteSE3Curve
+  typedef typename curves::Time Time;// typedef double time
 
   BaseTarget();
   virtual ~BaseTarget();
@@ -55,7 +55,7 @@ class BaseTarget : public BaseMotionBase
   double getDuration() const;
 
   /*!
-   * Returns the frame id base motion.
+   * Returns the frame id base motion, the motion of base is relative to....
    * @return the frame id.
    */
   const std::string& getFrameId(const ControlLevel& controlLevel) const;

free_gait_core/include/free_gait_core/executor/Executor.hpp

@@ -18,6 +18,7 @@
 // Robot utils
 #include <std_utils/timers/ChronoTimer.hpp>
 
+
 // STD
 #include <memory>
 #include <string>
@@ -76,6 +77,7 @@ class Executor
   const State& getState() const;
   const AdapterBase& getAdapter() const;
 
+  //youxian QUAN
   enum class PreemptionType {
     PREEMPT_IMMEDIATE,
     PREEMPT_STEP,

free_gait_core/include/free_gait_core/executor/State.hpp

@@ -43,6 +43,10 @@ class State : public quadruped_model::QuadrupedState
 
   bool isSupportLeg(const LimbEnum& limb) const;
   void setSupportLeg(const LimbEnum& limb, bool isSupportLeg);
+  /**
+   * @brief getNumberOfSupportLegs
+   * @return how many support legs?
+   */
   unsigned int getNumberOfSupportLegs() const;
 
   bool isIgnoreContact(const LimbEnum& limb) const;
@@ -92,24 +96,24 @@ class State : public quadruped_model::QuadrupedState
  private:
   LocalAngularVelocity angularVelocityBaseInWorldFrame_;//dimension 3
   JointEfforts jointEfforts_;//dimension 12
-  JointAccelerations jointAccelerations_;
-  LinearAcceleration linearAccelerationBaseInWorldFrame_;
-  AngularAcceleration angularAccelerationBaseInBaseFrame_;
+  JointAccelerations jointAccelerations_;//dimension 12
+  LinearAcceleration linearAccelerationBaseInWorldFrame_;//3D
+  AngularAcceleration angularAccelerationBaseInBaseFrame_;//3D
 
   // Free gait specific.
-  std::unordered_map<BranchEnum, ControlSetup, EnumClassHash> controlSetups_;
-  Force netForceOnBaseInBaseFrame_;
-  Torque netTorqueOnBaseInBaseFrame_;
+  std::unordered_map<BranchEnum, ControlSetup, EnumClassHash> controlSetups_;//BranchEnum->control_level
+  Force netForceOnBaseInBaseFrame_;//3D
+  Torque netTorqueOnBaseInBaseFrame_;//3D
   std::unordered_map<LimbEnum, bool, EnumClassHash> isSupportLegs_;
   std::unordered_map<LimbEnum, bool, EnumClassHash> ignoreContact_;
-  std::unordered_map<LimbEnum, bool, EnumClassHash> ignoreForPoseAdaptation_;
-  std::unordered_map<LimbEnum, Vector, EnumClassHash> surfaceNormals_;
+  std::unordered_map<LimbEnum, bool, EnumClassHash> ignoreForPoseAdaptation_;//???Why need the ignore th e pose adaptation?
+  std::unordered_map<LimbEnum, Vector, EnumClassHash> surfaceNormals_;//get the corresponding the leg footholds surfacenormals?
   bool robotExecutionStatus_;
   std::string stepId_; // empty if undefined.
 
-  Stance footHoldInSupport_;
+  Stance footHoldInSupport_;//typedef std::unordered_map<LimbEnum, Position, EnumClassHash> Stance
 
-  Pose footholds_plane_pose_;
+  Pose footholds_plane_pose_;//position and rotation
 };
 
 } /* namespace */

free_gait_core/include/free_gait_core/leg_motion/JointTrajectory.hpp

@@ -39,6 +39,12 @@ class JointTrajectory : public JointMotionBase
    */
   std::unique_ptr<LegMotionBase> clone() const;
 
+  /**
+   * @brief setTrajectory
+   * @param times at the specific controllevel with a specific time and values, passed points
+   * @param values
+   * @param jointNodeEnums_
+   */
   void setTrajectory(
       const std::unordered_map<ControlLevel, std::vector<Time>, EnumClassHash>& times,
       const std::unordered_map<ControlLevel, std::vector<std::vector<ValueType>>, EnumClassHash>& values,
@@ -101,6 +107,7 @@ class JointTrajectory : public JointMotionBase
   ControlSetup controlSetup_;
 
   //! Knots.
+  //! typedef std::unordered_map<ControlLevel, bool, EnumClassHash> ControlSetup;
   std::unordered_map<ControlLevel, std::vector<Time>, EnumClassHash> times_;
   std::unordered_map<ControlLevel, std::vector<std::vector<ValueType>>, EnumClassHash> values_;
   std::vector<JointNodeEnum> jointNodeEnums_;