diff --git a/bioloid_control/config/bioloid_cm510_control.yaml b/bioloid_control/config/bioloid_cm510_control.yaml
index bc3cf29bd95286ce0e58d202727bdf8138280e21..9e6799581118fad867026101e00b6320dde62e01 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 fffb7b0566e5582fc845b23502122151b1612917..fcbcd0035676bd8a6612b44ac9f7fa92cf7a7510 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 1d0f8fabadf8ae9065afdc422548f3b936e83d6a..7e1da9b9481fe5f4e71b6aa3ff21d44ebd27af57 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 1b580fef6db09a03f1376687233bc11ecc1bc0bc..6d15735afca3f1020779a6106cace6533e5470fb 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 4c285a608f4d349029cc5f1e0e29bfb559c1e6c8..49da872082b7ec71012b484d3ce509dfcc3ce41c 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>