2021/5/23 commit

exoskeleton_control/include/exoskeleton_control/Force_Data_Filter.hpp

@@ -0,0 +1,7 @@
+
+float * Force_Filter(int Force[4])
+{
+    static float Force[4];
+
+    return Force;
+}

exoskeleton_control/src/Admittance_Control/Admittance_control_Hardware.cpp

@@ -1,18 +1,20 @@
 /* * @Author: Beal.MS
    * @Date: 2021-04-21 10:55:36
  * @Last Modified by: Beal.MS
- * @Last Modified time: 2021-04-26 22:19:52
+ * @Last Modified time: 2021-05-23 23:08:51
    * @Description: This is for Hardware to control exoskeleton
 */
 
 #include <iostream>
-#include "exoskeleton_control/Admittance_control_Hardware.hpp"
 #include <unistd.h>
 #include <math.h>
 #include <memory>
 #include <chrono>
 #include <functional>
 
+#include "exoskeleton_control/Admittance_control_Hardware.hpp"
+#include "exoskeleton_control/Force_Data_Filter.hpp"
+
 #include "rclcpp/rclcpp.hpp"
 #include "std_msgs/msg/string.hpp"
 #include "std_msgs/msg/float64_multi_array.hpp"
@@ -51,13 +53,9 @@ class Admittance_Control_Subscription :
         Interaction_force_callback_group_subscriber = this->create_callback_group(
             rclcpp::CallbackGroupType::MutuallyExclusive
         );
+        /* Human Sensor Data Subscribed */
         auto Human_sub_opt = rclcpp::SubscriptionOptions();
         Human_sub_opt.callback_group = Human_callback_group_subscriber;
-        auto Exoskeleton_sub_opt = rclcpp::SubscriptionOptions();
-        Exoskeleton_sub_opt.callback_group = Exoskeleton_callback_group_subscriber;
-        auto Interaction_sub_opt = rclcpp::SubscriptionOptions();
-        Interaction_sub_opt.callback_group = Interaction_force_callback_group_subscriber;
-
         Human_Joint_State_Subscription = this->create_subscription<std_msgs::msg::Float64MultiArray>(
             Human_Sensor_Subscription_Topic,
             rclcpp::QoS(2),
@@ -68,6 +66,9 @@ class Admittance_Control_Subscription :
             ),
             Human_sub_opt
         );
+        /* Exoskeleton Sensor Data Subscribed */
+        auto Exoskeleton_sub_opt = rclcpp::SubscriptionOptions();
+        Exoskeleton_sub_opt.callback_group = Exoskeleton_callback_group_subscriber;
         Exoskeleton_Joint_State_Subscription = this->create_subscription<std_msgs::msg::Float64MultiArray>(
             Exoskeleton_Sensor_Subscription_Topic,
             rclcpp::QoS(2),
@@ -78,6 +79,10 @@ class Admittance_Control_Subscription :
             ),
             Exoskeleton_sub_opt
         );
+        /* Interaction Force Subscribed */
+        auto Interaction_sub_opt = rclcpp::SubscriptionOptions();
+        Interaction_sub_opt.callback_group = Interaction_force_callback_group_subscriber;
+
         Interaction_Force_Subscription = this->create_subscription<std_msgs::msg::Float64MultiArray>(
             Interaction_Force_Subscription_Topic,
             rclcpp::QoS(2),
@@ -88,10 +93,13 @@ class Admittance_Control_Subscription :
             ),
             Interaction_sub_opt
         );
+
         /* Publish Node Initialized */
         Joint_State_Publisher = this->create_publisher<std_msgs::msg::Float64MultiArray>(Joint_State_Publisher_Topic, 1);
         Joint_Error_Publisher = this->create_publisher<std_msgs::msg::Float64MultiArray>(Joint_Error_Publisher_Topic, 1);
 
+
+
         /* Data Preinitialized */
         Expected_Angle_Left << 0.0,0.0,0.0,0.0;
         Expected_Velocity_Left << 0.0,0.0,0.0,0.0;
@@ -187,24 +195,19 @@ class Admittance_Control_Subscription :
         Human_Right_Thigh_Acceleration,
         Human_Right_Calf_Acceleration,
         0.0;
-        // Interaction Force
-        float K_T_F = 1.0;
-        float K_T_B = 1.0;
-
-        float K_C_F = 1.0;
-        float K_C_B = 1.0;
 
-        float Left_Thigh_Force = Left_Thigh_Back_Force*K_T_B - Left_Thigh_Front_Force*K_T_F;
-        float Left_Calf_Force  = Left_Calf_Back_Force*K_C_B  - Left_Calf_Front_Force*K_C_F;
-        float Right_Thigh_Force = Right_Thigh_Back_Force*K_T_B - Right_Thigh_Front_Force*K_T_F;
-        float Right_Calf_Force  = Right_Calf_Back_Force*K_C_B  - Right_Calf_Front_Force*K_C_F;
+        // Interaction Force
+        float Left_Thigh_Force  = Left_Thigh_Back_Force  - Left_Thigh_Front_Force;
+        float Left_Calf_Force   = Left_Calf_Back_Force   - Left_Calf_Front_Force;
+        float Right_Thigh_Force = Right_Thigh_Back_Force - Right_Thigh_Front_Force;
+        float Right_Calf_Force  = Right_Calf_Back_Force  - Right_Calf_Front_Force;
 
         float K = 0.0005;
-        Force_Left  << 0.0, Left_Thigh_Force*K, -Left_Calf_Force*K, 0.0;
+        Force_Left  << 0.0, Left_Thigh_Force*K ,  Left_Calf_Force*K, 0.0;
         Force_Right << 0.0, Right_Thigh_Force*K, Right_Calf_Force*K, 0.0;
 
-        std::cout<<"Force_Left"<<Force_Left<<std::endl;
-        std::cout<<"Force_Right"<<Force_Right<<std::endl;
+        Force_Left  = Force_Filter(Force_Left);
+        Force_Right = Force_Filter(Force_Right);
         //Angle Test
 
 
@@ -217,6 +220,7 @@ class Admittance_Control_Subscription :
             Expected_Acceleration_Left,
             Force_Left
             );
+
         Right_Angle = main(
             Feedback_Angle_Right,
             Feedback_Velocity_Left,

exoskeleton_control/src/Admittance_Control/Admittance_control_Software.cpp

@@ -24,17 +24,19 @@ using namespace std::chrono_literals;
 
 Eigen::Matrix<float,1,4> Feedback_Angle_;
 Eigen::Matrix<float,1,4> Expected_Angle_;
+Eigen::Matrix<float,1,4> Expected_Left_Ang;
+Eigen::Matrix<float,1,4> Expected_Right_Ang;
 Eigen::Matrix<float,1,4> Expected_Velocity_;
 Eigen::Matrix<float,1,4> Expected_Acceleration_;
 
 bool flag_;
 
 #define Joint_Subscription_Topic                "joint_states"
-#define Sensor_Subscription_Topic               "Sensor"
+#define Sensor_Subscription_Topic               "Sensor_Human"
 #define Interaction_Force_Subscription_Topic    "Interaction_Force"
 
 #define Joint_Thigh_Offset          -0.7046042369967704
-#define JOint_Calf_Offset           -0.7046042369967704+0.4744191163880517
+#define Joint_Calf_Offset           -0.7046042369967704+0.4744191163880517
 
 class Admittance_Control_Subscription :
     public rclcpp::Node,
@@ -78,28 +80,32 @@ class Admittance_Control_Subscription :
 
         Feedback_Angle_ << -position[0], -position[1], -position[2], -position[3];
         // Expected_Angle_(0,1) = ((Angle_Thigh)/180)*PI-Joint_Thigh_Offset;
-        // Expected_Angle_(0,2) = ((Angle_Calf - Angle_Thigh )/180)*PI-JOint_Calf_Offset;
-
-        // Force_ << 0.0, 0.0, 0.0, 0.0, 0.0, 0.0;
-
-        Left_Angle = main(
-            Feedback_Angle_,
-            Expected_Angle_,
-            Expected_Velocity_,
-            Expected_Acceleration_,
-            Force_
-            );
-        Right_Angle = main(
-            Feedback_Angle_,
-            Expected_Angle_,
-            Expected_Velocity_,
-            Expected_Acceleration_,
-            Force_
-            );
+        // Expected_Angle_(0,2) = ((Angle_Calf - Angle_Thigh )/180)*PI-Joint_Calf_Offset;
+        Expected_Left_Ang(0,1) = ((Left_Thigh_Angle)/180)*PI+Joint_Thigh_Offset;
+        Expected_Left_Ang(0,2) = ((Left_Calf_Angle - Left_Thigh_Angle )/180)*PI+Joint_Calf_Offset-1.1;
+
+        Expected_Right_Ang(0,1) = ((Right_Thigh_Angle)/180)*PI+Joint_Thigh_Offset+Joint_Thigh_Offset+0.2;
+        Expected_Right_Ang(0,2) = ((Right_Calf_Angle - Right_Thigh_Angle )/180)*PI+Joint_Calf_Offset-0.2;
+        Force_ << 0.0, 0.0, 0.0, 0.0;
+
+        // Left_Angle = main(
+        //     Feedback_Angle_,
+        //     Expected_Left_Ang,
+        //     Expected_Velocity_,
+        //     Expected_Acceleration_,
+        //     Force_
+        //     );
+        // Right_Angle = main(
+        //     Feedback_Angle_,
+        //     Expected_Right_Ang,
+        //     Expected_Velocity_,
+        //     Expected_Acceleration_,
+        //     Force_
+        //     );
 
         auto message = std_msgs::msg::Float64MultiArray();
-        message.data = {  -Left_Angle(0,0),  -Left_Angle(1,0),  -Left_Angle(2,0),  -Left_Angle(3,0),
-                        -Right_Angle(0,0), -Right_Angle(1,0), -Right_Angle(2,0), -Right_Angle(3,0) };
+        message.data = {  -Expected_Left_Ang(0,0),  Expected_Left_Ang(0,1),  Expected_Left_Ang(0,2),  -Expected_Left_Ang(0,3),
+                        -Expected_Right_Ang(0,0), Expected_Right_Ang(0,1), Expected_Right_Ang(0,2), -Expected_Right_Ang(0,3) };
 
         Joint_Publisher->publish(message);
 
@@ -113,15 +119,13 @@ class Admittance_Control_Subscription :
     void Sensor_Callback(const std_msgs::msg::Float64MultiArray::SharedPtr msg)
     {
         auto Sensor_Data = msg->data;
-        RCLCPP_INFO(this->get_logger(), "Piezoelectric: '%f','%f','%f'",
-        Sensor_Data[0], Sensor_Data[1], Sensor_Data[2],
-        "Angle_Thigh: '%f','%f','%f'",
-        Sensor_Data[3], Sensor_Data[4], Sensor_Data[5],
-        "Angle_Calf: '%f','%f','%f'",
-        Sensor_Data[6], Sensor_Data[7], Sensor_Data[8]
-        );
-        Angle_Thigh = Sensor_Data[3];
-        Angle_Calf  = Sensor_Data[6];
+        // Angle_Thigh = Sensor_Data[3];
+        // Angle_Calf  = Sensor_Data[6];
+
+        Left_Thigh_Angle = Sensor_Data[0];
+        Left_Calf_Angle  = Sensor_Data[1];
+        Right_Thigh_Angle = Sensor_Data[2];
+        Right_Calf_Angle = Sensor_Data[3];
     }
 
     void Interaction_Force_Callback(const std_msgs::msg::Float64MultiArray::SharedPtr msg)
@@ -147,12 +151,21 @@ class Admittance_Control_Subscription :
 
     float Angle_Thigh;
     float Angle_Calf;
+
+    float Left_Thigh_Angle;
+    float Left_Calf_Angle;
+    float Right_Thigh_Angle;
+    float Right_Calf_Angle;
 };
 
 int main(int argc, char * argv[])
 {
 
     Expected_Angle_ << 0.0,0.0,0.0,0.0;
+
+    Expected_Left_Ang << 0.0,0.0,0.0,0.0;
+    Expected_Right_Ang << 0.0,0.0,0.0,0.0;
+
     Expected_Velocity_ << 0.0,0.0,0.0,0.0;
     Expected_Acceleration_ << 0.0,0.0,0.0,0.0;