From a6fbf76843d4e1c468f0eb92337bb46051fb03da Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Sergi=20Hern=C3=A1ndez?= <shernand@iri.upc.edu>
Date: Tue, 17 May 2016 23:37:18 +0200
Subject: [PATCH] Solved a problem with the ADC Port2 of the CM510 controller.
 Changes in the dynamic parameters of the robot to allow most of the motions
 to work properly. Lateral movements are not yet working.

---
 .../config/bioloid_cm510_control.yaml         | 92 ++++---------------
 bioloid_control/config/bioloid_control.yaml   | 90 ++++--------------
 .../include/bioloid_controller_cm510_impl.h   |  8 +-
 bioloid_description/urdf/bioloid.xacro        | 72 +++++++--------
 bioloid_gazebo/worlds/bioloid.world           |  2 +
 5 files changed, 82 insertions(+), 182 deletions(-)

diff --git a/bioloid_control/config/bioloid_cm510_control.yaml b/bioloid_control/config/bioloid_cm510_control.yaml
index bc3cf29..9e67995 100644
--- a/bioloid_control/config/bioloid_cm510_control.yaml
+++ b/bioloid_control/config/bioloid_cm510_control.yaml
@@ -6,7 +6,7 @@ bioloid:
 
   bioloid_cm510_cont:
     type: effort_controllers/BioloidControllerCM510
-    adc1_frame: IR1_link
+    adc2_frame: IR1_link
     adc3_frame: gyro_x
     adc4_frame: gyro_y
     exp_adc1_frame: IR2_link
@@ -42,143 +42,89 @@ bioloid:
         p: 8.0
         d: 0.0
         i: 0.0
-        proxy:
-          lambda: 3.0
-          vel_limit: 12.0
-          effort_limit: 2.5
+        i_clamp: 0.0
       j_high_arm_l:
         p: 8.0
         d: 0.0
         i: 0.0
-        proxy:
-          lambda: 3.0
-          vel_limit: 12.0
-          effort_limit: 2.5
+        i_clamp: 0.0
       j_low_arm_l:
         p: 8.0
         d: 0.0
         i: 0.0
-        proxy:
-          lambda: 3.0
-          vel_limit: 12.0
-          effort_limit: 2.5
+        i_clamp: 0.0
       j_shoulder_r:
         p: 8.0
         d: 0.0
         i: 0.0
-        proxy:
-          lambda: 3.0
-          vel_limit: 12.0
-          effort_limit: 2.5
+        i_clamp: 0.0
       j_high_arm_r:
         p: 8.0
         d: 0.0
         i: 0.0
-        proxy:
-          lambda: 3.0
-          vel_limit: 12.0
-          effort_limit: 2.5
+        i_clamp: 0.0
       j_low_arm_r:
         p: 8.0
         d: 0.0
         i: 0.0
-        proxy:
-          lambda: 3.0
-          vel_limit: 12.0
-          effort_limit: 2.5
+        i_clamp: 0.0
       j_pelvis_yaw_l:
         p: 8.0
         d: 0.0
         i: 0.0
-        proxy:
-          lambda: 3.0
-          vel_limit: 12.0
-          effort_limit: 2.5
+        i_clamp: 0.0
       j_pelvis_roll_l:
         p: 8.0
         d: 0.0
         i: 0.0
-        proxy:
-          lambda: 3.0
-          vel_limit: 12.0
-          effort_limit: 2.5
+        i_clamp: 0.0
       j_pelvis_pitch_l:
         p: 8.0
         d: 0.0
         i: 0.0
-        proxy:
-          lambda: 3.0
-          vel_limit: 12.0
-          effort_limit: 2.5
+        i_clamp: 0.0
       j_knee_l:
         p: 8.0
         d: 0.0
         i: 0.0
-        proxy:
-          lambda: 3.0
-          vel_limit: 12.0
-          effort_limit: 2.5
+        i_clamp: 0.0
       j_ankle_pitch_l:
         p: 4.0
         d: 0.0
         i: 0.0
-        proxy:
-          lambda: 3.0
-          vel_limit: 12.0
-          effort_limit: 2.5
+        i_clamp: 0.0
       j_ankle_roll_l:
         p: 4.0
         d: 0.0
         i: 0.0
-        proxy:
-          lambda: 3.0
-          vel_limit: 12.0
-          effort_limit: 2.5
+        i_clamp: 0.0
       j_pelvis_yaw_r:
         p: 8.0
         d: 0.0
         i: 0.0
-        proxy:
-          lambda: 3.0
-          vel_limit: 12.0
-          effort_limit: 2.5
+        i_clamp: 0.0
       j_pelvis_roll_r:
         p: 8.0
         d: 0.0
         i: 0.0
-        proxy:
-          lambda: 3.0
-          vel_limit: 12.0
-          effort_limit: 2.5
+        i_clamp: 0.0
       j_pelvis_pitch_r:
         p: 8.0
         d: 0.0
         i: 0.0
-        proxy:
-          lambda: 3.0
-          vel_limit: 12.0
-          effort_limit: 2.5
+        i_clamp: 0.0
       j_knee_r:
         p: 8.0
         d: 0.0
         i: 0.0
-        proxy:
-          lambda: 3.0
-          vel_limit: 12.0
-          effort_limit: 2.5
+        i_clamp: 0.0
       j_ankle_pitch_r:
         p: 4.0
         d: 0.0
         i: 0.0
-        proxy:
-          lambda: 3.0
-          vel_limit: 12.0
-          effort_limit: 2.5
+        i_clamp: 0.0
       j_ankle_roll_r:
         p: 4.0
         d: 0.0
         i: 0.0
-        proxy:
-          lambda: 3.0
-          vel_limit: 12.0
-          effort_limit: 2.5
+        i_clamp: 0.0
diff --git a/bioloid_control/config/bioloid_control.yaml b/bioloid_control/config/bioloid_control.yaml
index fffb7b0..fcbcd00 100644
--- a/bioloid_control/config/bioloid_control.yaml
+++ b/bioloid_control/config/bioloid_control.yaml
@@ -30,143 +30,89 @@ bioloid:
         p: 8.0
         d: 0.0
         i: 0.0
-        proxy:
-          lambda: 3.0
-          vel_limit: 6.0
-          effort_limit: 1.5
+        i_clamp: 0.0
       j_high_arm_l:
         p: 8.0
         d: 0.0
         i: 0.0
-        proxy:
-          lambda: 3.0
-          vel_limit: 6.0
-          effort_limit: 1.5
+        i_clamp: 0.0
       j_low_arm_l:
         p: 8.0
         d: 0.0
         i: 0.0
-        proxy:
-          lambda: 3.0
-          vel_limit: 6.0
-          effort_limit: 1.5
+        i_clamp: 0.0
       j_shoulder_r:
         p: 8.0
         d: 0.0
         i: 0.0
-        proxy:
-          lambda: 3.0
-          vel_limit: 6.0
-          effort_limit: 1.5
+        i_clamp: 0.0
       j_high_arm_r:
         p: 8.0
         d: 0.0
         i: 0.0
-        proxy:
-          lambda: 3.0
-          vel_limit: 6.0
-          effort_limit: 1.5
+        i_clamp: 0.0
       j_low_arm_r:
         p: 8.0
         d: 0.0
         i: 0.0
-        proxy:
-          lambda: 3.0
-          vel_limit: 6.0
-          effort_limit: 1.5
+        i_clamp: 0.0
       j_pelvis_yaw_l:
         p: 8.0
         d: 0.0
         i: 0.0
-        proxy:
-          lambda: 3.0
-          vel_limit: 6.0
-          effort_limit: 1.5
+        i_clamp: 0.0
       j_pelvis_roll_l:
         p: 8.0
         d: 0.0
         i: 0.0
-        proxy:
-          lambda: 3.0
-          vel_limit: 6.0
-          effort_limit: 1.5
+        i_clamp: 0.0
       j_pelvis_pitch_l:
         p: 8.0
         d: 0.0
         i: 0.0
-        proxy:
-          lambda: 3.0
-          vel_limit: 6.0
-          effort_limit: 1.5
+        i_clamp: 0.0
       j_knee_l:
         p: 8.0
         d: 0.0
         i: 0.0
-        proxy:
-          lambda: 3.0
-          vel_limit: 6.0
-          effort_limit: 1.5
+        i_clamp: 0.0
       j_ankle_pitch_l:
         p: 8.0
         d: 0.0
         i: 0.0
-        proxy:
-          lambda: 3.0
-          vel_limit: 6.0
-          effort_limit: 1.5
+        i_clamp: 0.0
       j_ankle_roll_l:
         p: 8.0
         d: 0.0
         i: 0.0
-        proxy:
-          lambda: 3.0
-          vel_limit: 6.0
-          effort_limit: 1.5
+        i_clamp: 0.0
       j_pelvis_yaw_r:
         p: 8.0
         d: 0.0
         i: 0.0
-        proxy:
-          lambda: 3.0
-          vel_limit: 6.0
-          effort_limit: 1.5
+        i_clamp: 0.0
       j_pelvis_roll_r:
         p: 8.0
         d: 0.0
         i: 0.0
-        proxy:
-          lambda: 3.0
-          vel_limit: 6.0
-          effort_limit: 1.5
+        i_clamp: 0.0
       j_pelvis_pitch_r:
         p: 8.0
         d: 0.0
         i: 0.0
-        proxy:
-          lambda: 3.0
-          vel_limit: 6.0
-          effort_limit: 1.5
+        i_clamp: 0.0
       j_knee_r:
         p: 8.0
         d: 0.0
         i: 0.0
-        proxy:
-          lambda: 3.0
-          vel_limit: 6.0
-          effort_limit: 1.5
+        i_clamp: 0.0
       j_ankle_pitch_r:
         p: 8.0
         d: 0.0
         i: 0.0
-        proxy:
-          lambda: 3.0
-          vel_limit: 6.0
-          effort_limit: 1.5
+        i_clamp: 0.0
       j_ankle_roll_r:
         p: 8.0
         d: 0.0
         i: 0.0
-        proxy:
-          lambda: 3.0
-          vel_limit: 6.0
-          effort_limit: 1.5
+        i_clamp: 0.0
diff --git a/bioloid_controller_cm510/include/bioloid_controller_cm510_impl.h b/bioloid_controller_cm510/include/bioloid_controller_cm510_impl.h
index 1d0f8fa..7e1da9b 100644
--- a/bioloid_controller_cm510/include/bioloid_controller_cm510_impl.h
+++ b/bioloid_controller_cm510/include/bioloid_controller_cm510_impl.h
@@ -329,7 +329,11 @@ namespace bioloid_controller_cm510
 
       /* get the actual simulation angles */
       for(unsigned int i=0;i<this->joints_.size();++i)
+      {
         real_angles[i]=joints_[i].getPosition();
+        std::cout << real_angles[i] << ",";
+      }
+      std::cout << std::endl;
       pushbuttons_loop();
       adc_loop();
       exp_board_loop();
@@ -344,8 +348,10 @@ namespace bioloid_controller_cm510
       {
         target_angles[i] = ((((manager_servos[i].current_value+balance_offsets[i]-manager_servos[i].center_value)*300.0/1023.0)*3.14159)/180.0);
         const double command = this->pids_[i]->computeCommand(target_angles[i]-real_angles[i],period);
+        std::cout << target_angles[i] << ",";
         this->joints_[i].setCommand(command);
       }
+      std::cout << std::endl;
     }
 
   template <class HardwareInterface>
@@ -581,7 +587,7 @@ namespace bioloid_controller_cm510
       else if(!this->found_gyro_y && this->adc1_frame=="gyro_y")
         this->found_gyro_y=true;
       this->adc2_frame="none";
-      nh.getParam("adc1_frame", this->adc2_frame);
+      nh.getParam("adc2_frame", this->adc2_frame);
       ROS_DEBUG_STREAM_NAMED(name_, "CM510 ADC2 associated to sensor: " << this->adc2_frame);
       if(!this->found_gyro_x && this->adc2_frame=="gyro_x")
         this->found_gyro_x=true;
diff --git a/bioloid_description/urdf/bioloid.xacro b/bioloid_description/urdf/bioloid.xacro
index 1b580fe..6d15735 100755
--- a/bioloid_description/urdf/bioloid.xacro
+++ b/bioloid_description/urdf/bioloid.xacro
@@ -444,8 +444,8 @@
     <child link="right_shoulder"/>
     <origin xyz="-0.07188 0.0 0" rpy="-1.5707 0 0" />
     <axis xyz="-1 0 0" />
-    <limit effort="2.5" velocity="12.0" lower="-3.14" upper="3.14" />
-    <dynamics damping="0.2"/>
+    <limit effort="1.5" velocity="12.0" lower="-3.14" upper="3.14" />
+    <dynamics damping="0.15"/>
   </joint>
 
   <transmission name="tran_shoulder_r">
@@ -464,8 +464,8 @@
     <child link="right_arm_high"/>
     <origin xyz="0 -0.0145 0.0" rpy="0 0 -1.5707" />
     <axis xyz="0 0 -1" />
-    <limit effort="2.5" velocity="12.0" lower="-3.14" upper="3.14" />
-    <dynamics damping="0.2"/>
+    <limit effort="1.5" velocity="12.0" lower="-3.14" upper="3.14" />
+    <dynamics damping="0.15"/>
   </joint>
 
   <transmission name="tran_high_arm_r">
@@ -484,8 +484,8 @@
     <child link="right_arm_low"/>
     <origin xyz="0.0 -0.0675 0.0" rpy="0 0 0" />
     <axis xyz="0 0 -1" />
-    <limit effort="2.5" velocity="12.0" lower="-3.14" upper="3.14" />
-    <dynamics damping="0.2"/>
+    <limit effort="1.5" velocity="12.0" lower="-3.14" upper="3.14" />
+    <dynamics damping="0.15"/>
   </joint>
 
   <transmission name="tran_low_arm_r">
@@ -504,8 +504,8 @@
     <child link="left_shoulder"/>
     <origin xyz="0.07188 0.0 0" rpy="-1.5707 0 0" />
     <axis xyz="1 0 0" />
-    <limit effort="2.5" velocity="12.0" lower="-3.14" upper="3.14" />
-    <dynamics damping="0.2"/>
+    <limit effort="1.5" velocity="12.0" lower="-3.14" upper="3.14" />
+    <dynamics damping="0.15"/>
   </joint>
 
   <transmission name="tran_shoulder_l">
@@ -524,8 +524,8 @@
     <child link="left_arm_high"/>
     <origin xyz="0 -0.0145 0.0" rpy="0 0 1.5707" />
     <axis xyz="0 0 -1" />
-    <limit effort="2.5" velocity="12.0" lower="-3.14" upper="3.14" />
-    <dynamics damping="0.2"/>
+    <limit effort="1.5" velocity="12.0" lower="-3.14" upper="3.14" />
+    <dynamics damping="0.15"/>
   </joint>
 
   <transmission name="tran_high_arm_l">
@@ -544,8 +544,8 @@
     <child link="left_arm_low"/>
     <origin xyz="0.0 -0.0675 0.0" rpy="0 0 0" />
     <axis xyz="0 0 -1" />
-    <limit effort="2.5" velocity="12.0" lower="-3.14" upper="3.14" />
-    <dynamics damping="0.2"/>
+    <limit effort="1.5" velocity="12.0" lower="-3.14" upper="3.14" />
+    <dynamics damping="0.15"/>
   </joint>
 
   <transmission name="tran_low_arm_l">
@@ -564,8 +564,8 @@
     <child link="right_leg_pelvis_yaw"/>
     <origin xyz="-0.0385 -0.12037 0.0" rpy="0 -0.7854 0" />
     <axis xyz="0 -1 0" />
-    <limit effort="2.5" velocity="12.0" lower="-3.14" upper="3.14" />
-    <dynamics damping="0.2"/>
+    <limit effort="1.5" velocity="12.0" lower="-3.14" upper="3.14" />
+    <dynamics damping="0.15"/>
   </joint>
 
   <transmission name="tran_pelvis_yaw_r">
@@ -584,8 +584,8 @@
     <child link="right_leg_pelvis_roll"/>
     <origin xyz="0 0 0" rpy="0 0 0" />
     <axis xyz="0 0 1" />
-    <limit effort="2.5" velocity="12.0" lower="-3.14" upper="3.14" />
-    <dynamics damping="0.2"/>
+    <limit effort="1.5" velocity="12.0" lower="-3.14" upper="3.14" />
+    <dynamics damping="0.15"/>
   </joint>
 
   <transmission name="tran_pelvis_roll_r">
@@ -604,8 +604,8 @@
     <child link="right_leg_pelvis_pitch"/>
     <origin xyz="0 0 0" rpy="0 0 0" />
     <axis xyz="1 0 0" />
-    <limit effort="2.5" velocity="12.0" lower="-3.14" upper="3.14" />
-    <dynamics damping="0.2"/>
+    <limit effort="1.5" velocity="12.0" lower="-3.14" upper="3.14" />
+    <dynamics damping="0.15"/>
   </joint>
 
   <transmission name="tran_pelvis_pitch_r">
@@ -624,8 +624,8 @@
     <child link="right_leg_knee"/>
     <origin xyz="0 -0.075 -0.01488" rpy="0 0 0" />
     <axis xyz="-1 0 0" />
-    <limit effort="2.5" velocity="12.0" lower="-3.14" upper="3.14" />
-    <dynamics damping="0.2"/>
+    <limit effort="1.5" velocity="12.0" lower="-3.14" upper="3.14" />
+    <dynamics damping="0.15"/>
   </joint>
 
   <transmission name="tran_knee_r">
@@ -644,8 +644,8 @@
     <child link="right_leg_ankle_pitch"/>
     <origin xyz="0 -0.075 0.01488" rpy="0 0 0" />
     <axis xyz="-1 0 0" />
-    <limit effort="2.5" velocity="12.0" lower="-3.14" upper="3.14" />
-    <dynamics damping="0.2"/>
+    <limit effort="1.5" velocity="12.0" lower="-3.14" upper="3.14" />
+    <dynamics damping="0.15"/>
   </joint>
 
   <transmission name="tran_ankle_pitch_r">
@@ -664,8 +664,8 @@
     <child link="right_leg_ankle_roll"/>
     <origin xyz="0 0 0" rpy="0 0 0" />
     <axis xyz="0 0 -1" />
-    <limit effort="2.5" velocity="12.0" lower="-3.14" upper="3.14" />
-    <dynamics damping="0.2"/>
+    <limit effort="1.5" velocity="12.0" lower="-3.14" upper="3.14" />
+    <dynamics damping="0.15"/>
   </joint>
 
   <transmission name="tran_ankle_roll_r">
@@ -684,8 +684,8 @@
     <child link="left_leg_pelvis_yaw"/>
     <origin xyz="0.0385 -0.12037 0.0" rpy="0 0.7853 0" />
     <axis xyz="0 -1 0" />
-    <limit effort="2.5" velocity="12.0" lower="-3.14" upper="3.14" />
-    <dynamics damping="0.2"/>
+    <limit effort="1.5" velocity="12.0" lower="-3.14" upper="3.14" />
+    <dynamics damping="0.15"/>
   </joint>
 
   <transmission name="tran_pelvis_yaw_l">
@@ -704,8 +704,8 @@
     <child link="left_leg_pelvis_roll"/>
     <origin xyz="0 0 0" rpy="0 0 0" />
     <axis xyz="0 0 1" />
-    <limit effort="2.5" velocity="12.0" lower="-3.14" upper="3.14" />
-    <dynamics damping="0.2"/>
+    <limit effort="1.5" velocity="12.0" lower="-3.14" upper="3.14" />
+    <dynamics damping="0.15"/>
   </joint>
 
   <transmission name="tran_pelvis_roll_l">
@@ -724,8 +724,8 @@
     <child link="left_leg_pelvis_pitch"/>
     <origin xyz="0 0 0" rpy="0 0 0" />
     <axis xyz="-1 0 0" />
-    <limit effort="2.5" velocity="12.0" lower="-3.14" upper="3.14" />
-    <dynamics damping="0.2"/>
+    <limit effort="1.5" velocity="12.0" lower="-3.14" upper="3.14" />
+    <dynamics damping="0.15"/>
   </joint>
 
   <transmission name="tran_pelvis_pitch_l">
@@ -744,8 +744,8 @@
     <child link="left_leg_knee"/>
     <origin xyz="0 -0.075 -0.01488" rpy="0 0 0" />
     <axis xyz="1 0 0" />
-    <limit effort="2.5" velocity="12.0" lower="-3.14" upper="3.14" />
-    <dynamics damping="0.2"/>
+    <limit effort="1.5" velocity="12.0" lower="-3.14" upper="3.14" />
+    <dynamics damping="0.15"/>
   </joint>
 
   <transmission name="tran_knee_l">
@@ -764,8 +764,8 @@
     <child link="left_leg_ankle_pitch"/>
     <origin xyz="0 -0.075 0.01488" rpy="0 0 0" />
     <axis xyz="1 0 0" />
-    <limit effort="2.5" velocity="12.0" lower="-3.14" upper="3.14" />
-    <dynamics damping="0.2"/>
+    <limit effort="1.5" velocity="12.0" lower="-3.14" upper="3.14" />
+    <dynamics damping="0.15"/>
   </joint>
 
   <transmission name="tran_ankle_pitch_l">
@@ -784,8 +784,8 @@
     <child link="left_leg_ankle_roll"/>
     <origin xyz="0 0 0" rpy="0 0 0" />
     <axis xyz="0 0 -1" />
-    <limit effort="2.5" velocity="12.0" lower="-3.14" upper="3.14" />
-    <dynamics damping="0.2"/>
+    <limit effort="1.5" velocity="12.0" lower="-3.14" upper="3.14" />
+    <dynamics damping="0.15"/>
   </joint>
 
   <transmission name="tran_ankle_roll_l">
diff --git a/bioloid_gazebo/worlds/bioloid.world b/bioloid_gazebo/worlds/bioloid.world
index 4c285a6..49da872 100644
--- a/bioloid_gazebo/worlds/bioloid.world
+++ b/bioloid_gazebo/worlds/bioloid.world
@@ -4,6 +4,8 @@
     <physics type="ode">
       <real_time_factor>1.0</real_time_factor>
       <real_time_update_rate>1000</real_time_update_rate>
+<!--      <real_time_factor>0.1</real_time_factor>
+      <real_time_update_rate>100</real_time_update_rate>-->
       <max_step_size>0.001</max_step_size>
       <gravity>0 0 -9.8</gravity>
       <ode>
-- 
GitLab