M1 demo with ROS

Author: ywen3

launch/m1_real.launch

@@ -0,0 +1,14 @@
+<?xml version="1.0"?>
+<launch>
+
+    <arg name="robot_name_1" default="m1_x"/>
+    <arg name="name_spaces" default="[$(arg robot_name_1)]"/>
+
+    <!-- CORC M1_X -->
+    <node name="$(arg robot_name_1)" pkg="CORC" type="MultiM1Machine_APP" output="screen" args="-can can0"/>
+
+    <!-- rqt -->
+    <node name="rqt" pkg="CORC" type="rqt.sh">
+    </node>
+
+</launch>

src/apps/MultiM1Machine/states/MultiControllerState.cpp

@@ -42,7 +42,9 @@ void MultiControllerState::entry(void) {
     digitalInValue_ = 0;
     digitalOutValue_ = 0; //0
     robot_->setDigitalOut(digitalOutValue_);
+
 }
+
 void MultiControllerState::during(void) {
 
     //Compute some basic time values
@@ -54,7 +56,6 @@ void MultiControllerState::during(void) {
     dt = now - lastTime;
     lastTime = now;
 
-    //
     tick_count = tick_count + 1;
     if(controller_mode_ == 0){  // Homing
         if(cali_stage == 1){
@@ -78,8 +79,7 @@ void MultiControllerState::during(void) {
                 robot_->printJointStatus();
             }
         }
-        else if(cali_stage == 2)
-        {
+        else if(cali_stage == 2){
             // set position control: 16 is vertical for #2; 45 is neutral position for random trajectory
             q(0) = 45; //16
             if(robot_->setJointPos(q) != SUCCESS){
@@ -106,7 +106,6 @@ void MultiControllerState::during(void) {
     }
     else if(controller_mode_ == 4){ // virtual spring - torque mode
         tau = robot_->getJointTor();
-        //tau_s = (robot_->getJointTor_s()+tau_s)/2.0;
         dq = robot_->getJointVel();
 
         // filter q
@@ -117,14 +116,13 @@ void MultiControllerState::during(void) {
         q = robot_->getJointPos();
         q_filtered = q(0);
 
-//         filter torque signal
+        // filter torque signal
         tau_s = robot_->getJointTor_s();
         tau_raw = tau_s(0);
         alpha_tau = (2*M_PI*dt*cut_off)/(2*M_PI*dt*cut_off+1);
         robot_->filter_tau(alpha_tau);
         tau_s = robot_->getJointTor_s();
         tau_filtered = tau_s(0);
-//        std::cout << "Pre :" << tau_raw << "; Post :" << tau_filtered << std::endl;
 
         // get interaction torque from virtual spring
         spring_tor = -multiM1MachineRos_->interactionTorqueCommand_(0);
@@ -137,7 +135,6 @@ void MultiControllerState::during(void) {
         integral_error = integral_error + error/control_freq;
         tau_cmd(0) = error*kp_ + delta_error*kd_ + integral_error*ki_;  // tau_cmd = P*error + D*delta_error; 1 and 0.001
         torque_error_last_time_step = error;
-//        std::cout << "spring_tor:" << spring_tor  << "; sensor_tor: " << tau_s(0) << "; cmd_tor: " << tau_cmd(0) << "; motor_tor: " << tau(0) << std::endl;
         robot_->setJointTor_comp(tau_cmd, tau_s, ffRatio_);
 
         // reset integral_error every 1 mins, to be decided
@@ -159,7 +156,7 @@ void MultiControllerState::during(void) {
         q = robot_->getJointPos();
         q_filtered = q(0);
 
-//         filter torque signal
+        // filter torque signal
         tau_s = robot_->getJointTor_s();
         tau_raw = tau_s(0);
         alpha_tau = (2*M_PI*dt*cut_off)/(2*M_PI*dt*cut_off+1);
@@ -183,7 +180,6 @@ void MultiControllerState::during(void) {
     }
     else if (controller_mode_ == 11){ // SEND HIGH
 //        std::cout<<"SET HIGH"<<std::endl;
-
         double time = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::steady_clock::now() - time0).count()/1000.0;
 
         if(robot_->getRobotName() == "m1_y"){
@@ -204,7 +200,6 @@ void MultiControllerState::during(void) {
     }
     else if (controller_mode_ == 12){ // SEND LOW
 //        std::cout<<"SET LOW"<<std::endl;
-
         double time = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::steady_clock::now() - time0).count()/1000.0;
 
         if(robot_->getRobotName() == "m1_y"){
@@ -253,7 +248,7 @@ void MultiControllerState::dynReconfCallback(CORC::dynamic_paramsConfig &config,
         tick_count = 0;
     }
 
-//    controller_mode_ = config.controller_mode;
+    // change controller mode
     if(controller_mode_!=config.controller_mode)
     {
         controller_mode_ = config.controller_mode;