Merge branch 'master' of https://gitlab.iri.upc.edu/humanoides/bioloid_robot

bioloid_control/config/bioloid_ceabot.yaml

@@ -39,92 +39,92 @@ bioloid:
       - j_ankle_roll_l
     gains:
       j_shoulder_l:
-        p: 8.0
-        d: 0.0
-        i: 0.0
-        i_clamp: 0.0
+        punch: 32
+        margin: 1
+        slope: 32
+        max_torque: 1.5
       j_high_arm_l:
-        p: 8.0
-        d: 0.0
-        i: 0.0
-        i_clamp: 0.0
+        punch: 32
+        margin: 1
+        slope: 32
+        max_torque: 1.5
       j_low_arm_l:
-        p: 8.0
-        d: 0.0
-        i: 0.0
-        i_clamp: 0.0
+        punch: 32
+        margin: 1
+        slope: 32
+        max_torque: 1.5
       j_shoulder_r:
-        p: 8.0
-        d: 0.0
-        i: 0.0
-        i_clamp: 0.0
+        punch: 32
+        margin: 1
+        slope: 32
+        max_torque: 1.5
       j_high_arm_r:
-        p: 8.0
-        d: 0.0
-        i: 0.0
-        i_clamp: 0.0
+        punch: 32
+        margin: 1
+        slope: 32
+        max_torque: 1.5
       j_low_arm_r:
-        p: 8.0
-        d: 0.0
-        i: 0.0
-        i_clamp: 0.0
+        punch: 32
+        margin: 1
+        slope: 32
+        max_torque: 1.5
       j_pelvis_yaw_l:
-        p: 8.0
-        d: 0.0
-        i: 0.0
-        i_clamp: 0.0
+        punch: 32
+        margin: 1
+        slope: 32
+        max_torque: 1.5
       j_pelvis_roll_l:
-        p: 8.0
-        d: 0.0
-        i: 0.0
-        i_clamp: 0.0
+        punch: 32
+        margin: 1
+        slope: 32
+        max_torque: 1.5
       j_pelvis_pitch_l:
-        p: 8.0
-        d: 0.0
-        i: 0.0
-        i_clamp: 0.0
+        punch: 32
+        margin: 1
+        slope: 32
+        max_torque: 1.5
       j_knee_l:
-        p: 8.0
-        d: 0.0
-        i: 0.0
-        i_clamp: 0.0
+        punch: 32
+        margin: 1
+        slope: 32
+        max_torque: 1.5
       j_ankle_pitch_l:
-        p: 4.0
-        d: 0.0
-        i: 0.0
-        i_clamp: 0.0
+        punch: 32
+        margin: 1
+        slope: 32
+        max_torque: 1.5
       j_ankle_roll_l:
-        p: 4.0
-        d: 0.0
-        i: 0.0
-        i_clamp: 0.0
+        punch: 32
+        margin: 1
+        slope: 32
+        max_torque: 1.5
       j_pelvis_yaw_r:
-        p: 8.0
-        d: 0.0
-        i: 0.0
-        i_clamp: 0.0
+        punch: 32
+        margin: 1
+        slope: 32
+        max_torque: 1.5
       j_pelvis_roll_r:
-        p: 8.0
-        d: 0.0
-        i: 0.0
-        i_clamp: 0.0
+        punch: 32
+        margin: 1
+        slope: 32
+        max_torque: 1.5
       j_pelvis_pitch_r:
-        p: 8.0
-        d: 0.0
-        i: 0.0
-        i_clamp: 0.0
+        punch: 32
+        margin: 1
+        slope: 32
+        max_torque: 1.5
       j_knee_r:
-        p: 8.0
-        d: 0.0
-        i: 0.0
-        i_clamp: 0.0
+        punch: 32
+        margin: 1
+        slope: 32
+        max_torque: 1.5
       j_ankle_pitch_r:
-        p: 4.0
-        d: 0.0
-        i: 0.0
-        i_clamp: 0.0
+        punch: 32
+        margin: 1
+        slope: 32
+        max_torque: 1.5
       j_ankle_roll_r:
-        p: 4.0
-        d: 0.0
-        i: 0.0
-        i_clamp: 0.0
+        punch: 32
+        margin: 1
+        slope: 32
+        max_torque: 1.5

bioloid_controller_cm510/CMakeLists.txt

@@ -91,7 +91,7 @@ catkin_package(
 ###########
 
 SET(BIOLOID_FW_PATH ~/humanoids/cm510_controller_fw)
-SET(ProjectPath ~/Desktop/new_fw/sensors)
+SET(ProjectPath ~/humanoids/cm510_controller_fw/examples/stairs)
 
 ## Specify additional locations of header files
 ## Your package locations should be listed before other locations
@@ -105,6 +105,7 @@ include_directories(${iriutils_INCLUDE_DIR})
 INCLUDE_DIRECTORIES(${robotis_bin_parser_INCLUDE_DIR})
 INCLUDE_DIRECTORIES(${robotis_mtn_parser_INCLUDE_DIR})
 INCLUDE_DIRECTORIES(${robotis_mtn_INCLUDE_DIR})
+INCLUDE_DIRECTORIES(${ProjectPath}/../movements/)
 
 ## Declare a cpp library
 # add_library(bioloid_controller
@@ -126,8 +127,8 @@ add_library(${PROJECT_NAME} src/bioloid_controller_cm510.cpp
                             ${BIOLOID_FW_PATH}/communications/src/dynamixel.c
                             ${BIOLOID_FW_PATH}/communications/src/serial_console.c
                             # main application module
-                           
-		            ${ProjectPath}/main.c
+		            ${ProjectPath}/stairs.c
+		            ${ProjectPath}/../movements/mtn_library.c
                             # AVR simulation modules
                             src/sim/avr_delay.c
                             src/sim/avr_registers.c

bioloid_controller_cm510/include/bioloid_controller_cm510.h

@@ -54,7 +54,6 @@
 // ros_controls
 #include <controller_interface/controller.h>
 #include <hardware_interface/joint_command_interface.h>
-#include <control_toolbox/pid.h>
 
 #include "mutex.h"
 #include "threadserver.h"
@@ -62,6 +61,12 @@
 
 namespace bioloid_controller_cm510
 {
+  typedef struct{
+    int punch;
+    int margin;
+    int slope;
+    double max_torque;
+  }TCompliance;
   /**
    * \brief 
    *
@@ -83,15 +88,15 @@ namespace bioloid_controller_cm510
       void init_controller_analog_ports(ros::NodeHandle &nh);
       void init_exp_board_analog_ports(ros::NodeHandle &nh);
       void init_exp_board_gpio_ports(ros::NodeHandle &nh);
+      double compliance_control(TCompliance &comp,double error);
 
     private:
       typedef typename HardwareInterface::ResourceHandleType JointHandle;
-      typedef boost::shared_ptr<control_toolbox::Pid> PidPtr;
 
       std::string name_;               ///< Controller name.
       std::vector<JointHandle> joints_;             ///< Handles to controlled joints.
       std::vector<std::string> joint_names_;        ///< Controlled joint names.
-      std::vector<PidPtr> pids_;
+      std::vector<TCompliance> compliances_;
 
       /* configuration parameters */
       /* frame associated to each of the ADC of the CM510 controller */

bioloid_controller_cm510/include/bioloid_controller_cm510_impl.h

@@ -154,7 +154,6 @@ namespace bioloid_controller_cm510
     {
       for(unsigned int i=0;i<this->joints_.size();++i)
       {
-        pids_[i]->reset();
         this->joints_[i].setCommand(0.0);
       }
       /* initialize the buttons state */
@@ -213,7 +212,7 @@ namespace bioloid_controller_cm510
       num_servos=n_joints;
       // Initialize members
       this->joints_.resize(n_joints);
-      this->pids_.resize(n_joints);
+      this->compliances_.resize(n_joints);
       for (unsigned int i=0;i<n_joints;++i)
       {
 	// Joint handle
@@ -222,18 +221,20 @@ namespace bioloid_controller_cm510
         }catch(...){
 	  ROS_ERROR_STREAM_NAMED(name_, "Could not find joint '" << joint_names_[i] << "' in '" <<
 	      this->getHardwareInterfaceType() << "'.");
-	  return false;
+ 	  return false;
 	}
         // Node handle to PID gains
         ros::NodeHandle joint_nh(this->controller_nh_,std::string("gains/")+joint_names_[i]);
 
-        // Init PID gains from ROS parameter server
-        this->pids_[i].reset(new control_toolbox::Pid());
-        if(!this->pids_[i]->init(joint_nh))
-        {
-          ROS_WARN_STREAM("Failed to initialize PID gains from ROS parameter server.");
-          return false;
-        }
+        // init the compliance controllers
+        this->compliances_[i].punch=32;      
+        joint_nh.getParam("punch",this->compliances_[i].punch);
+        this->compliances_[i].margin=1;      
+        joint_nh.getParam("margin",this->compliances_[i].margin);
+        this->compliances_[i].slope=32;      
+        joint_nh.getParam("slope",this->compliances_[i].slope);
+        this->compliances_[i].max_torque=2.5;      
+        joint_nh.getParam("max_torque",this->compliances_[i].max_torque);
 
 	// Whether a joint is continuous (ie. has angle wraparound)
 	const std::string not_if=urdf_joints[i]->type==urdf::Joint::CONTINUOUS ? "" : "non-";
@@ -307,6 +308,7 @@ namespace bioloid_controller_cm510
     void BioloidControllerCM510<HardwareInterface>::update(const ros::Time& time, const ros::Duration& period)
     {
       static bool first=true;
+      static ros::Time last_time=ros::Time(0,0);
       ros::Time current_time=ros::Time::now();
       std::vector<double> target_angles(joints_.size());
 
@@ -330,24 +332,55 @@ 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();
-      pushbuttons_loop();
-      adc_loop();
-      exp_board_loop();
-      exp_board_sim_loop();
-      buzzer_loop();
-      TIFR4|=(1<<OCF4A);
-      manager_loop();
-      user_time_loop();
-      user_loop();
+      if((current_time-last_time).toSec()>0.0078)
+      {
+        pushbuttons_loop();
+        adc_loop();
+        exp_board_loop();
+        exp_board_sim_loop();
+        buzzer_loop();
+        TIFR4|=(1<<OCF4A);
+        manager_loop();
+        user_time_loop();
+        user_loop();
+        last_time=current_time;
+      }
 
       for (unsigned int i = 0; i < this->joints_.size(); ++i)
       {
+        this->compliances_[i].slope=1<<(manager_servos[i].cc_slope&0x0F);
         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);
+        const double command = this->compliance_control(this->compliances_[i],target_angles[i]-real_angles[i]);
         this->joints_[i].setCommand(command);
       }
     }
 
+  template <class HardwareInterface>
+    double BioloidControllerCM510<HardwareInterface>::compliance_control(TCompliance &comp,double error)
+    {
+      double p,cmd;
+
+      if(fabs(error)<comp.margin*300.0*3.14159/(1024.0*180.0))
+        return 0.0;
+      else
+      {
+        p=comp.max_torque*(1024.0-comp.punch)*180.0/(comp.slope*300.0*3.14159);
+        if(error<0.0)
+        {
+          cmd=p*error-comp.max_torque*comp.punch/1024.0;
+          if(cmd<-comp.max_torque)
+            cmd=-comp.max_torque;
+        }
+        else
+        {
+          cmd=p*error+comp.max_torque*comp.punch/1024.0;
+          if(cmd>comp.max_torque)
+            cmd=comp.max_torque;
+        }
+        return cmd;
+      }
+    }
+
   template <class HardwareInterface>
     void BioloidControllerCM510<HardwareInterface>::imu_callback(const sensor_msgs::Imu::ConstPtr& msg)
     {

bioloid_controller_cm510/motions/ceabot_motions.mtn

@@ -2554,54 +2554,55 @@ step=512 235 788 279 744 462 561 358 666 501 510 342 681 242 782 646 377 501 510
 step=512 235 788 279 744 462 561 358 666 507 516 341 682 240 783 647 376 507 516 512 512 512 512 512 512 512 0.000 0.080
 page_end
 page_begin
-name=stairs_up_1
+name=stairs_up1
 compliance=5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5
 play_param=0 0 1 1.0 32
-step=512 231 800 274 737 460 557 361 664 490 497 448 583 438 611 549 458 484 490 512 512 512 512 512 512 512 0.000 0.296
-step=512 231 800 274 737 460 557 372 673 464 471 451 583 438 614 549 459 461 465 512 512 512 512 512 512 512 0.000 0.296
-step=512 229 800 272 736 458 557 371 671 457 461 451 582 435 618 548 458 450 451 512 512 512 512 512 512 512 0.000 0.296
-step=512 228 800 313 736 457 557 371 670 432 505 252 582 66 618 712 458 448 450 512 512 512 512 512 512 512 0.000 0.600
+step=512 231 800 274 737 460 557 361 664 490 497 448 583 438 611 549 458 484 490 512 512 512 512 512 512 512 0.000 0.600
+step=512 231 800 274 737 460 557 372 673 464 471 451 583 438 614 549 459 461 465 512 512 512 512 512 512 512 0.000 0.600
+step=512 229 800 272 663 458 557 371 671 457 461 451 582 435 618 548 458 450 451 512 512 512 512 512 512 512 0.000 0.600
+step=512 228 800 313 663 457 557 371 670 432 505 252 582 66 618 712 458 448 450 512 512 512 512 512 512 512 0.000 0.600
+step=512 228 800 313 663 457 557 426 734 432 505 252 540 66 618 712 458 448 450 512 512 512 512 512 512 512 0.000 0.592
+step=512 228 799 310 663 455 555 417 727 499 510 274 557 303 616 512 428 481 489 512 512 512 512 512 512 512 0.000 0.600
 page_end
 page_begin
-name=stairs_up_2
+name=stairs_up2
 compliance=5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5
 play_param=0 0 1 1.0 32
-step=512 228 800 313 736 457 557 426 734 432 505 252 540 66 618 712 458 448 450 512 512 512 512 512 512 512 0.000 0.496
-step=512 228 799 310 737 455 555 417 727 499 510 274 557 303 616 512 428 481 489 512 512 512 512 512 512 512 0.000 0.600
+step=512 228 622 311 736 457 555 408 698 500 510 221 574 268 608 532 417 486 490 512 512 512 512 512 512 512 0.000 1.000
+step=512 228 622 311 736 457 555 405 702 500 514 253 493 159 609 621 384 505 514 512 512 512 512 512 512 512 0.000 1.000
+step=512 233 622 354 737 369 555 403 713 443 443 172 486 49 705 724 351 546 529 512 512 512 512 512 512 512 0.000 1.000
+step=512 233 622 354 737 369 555 403 696 440 446 172 584 49 893 724 243 546 536 512 512 512 512 512 512 512 0.000 1.000
 page_end
 page_begin
-name=stairs_up_3
+name=stairs_up3
 compliance=5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5
 play_param=0 0 1 1.0 32
-step=512 228 794 311 736 457 555 408 698 500 510 221 574 268 608 532 417 486 490 512 512 512 512 512 512 512 0.000 1.000
-step=512 228 794 311 736 457 555 405 702 500 514 253 493 159 609 621 384 505 514 512 512 512 512 512 512 512 0.000 1.000
-step=512 233 792 354 737 369 555 403 713 443 443 172 486 49 705 724 351 546 529 512 512 512 512 512 512 512 0.000 1.000
-step=512 233 792 354 737 369 555 403 696 440 446 172 584 49 893 724 243 546 536 512 512 512 512 512 512 512 0.000 1.000
+step=512 233 792 354 737 369 555 353 649 440 466 172 804 49 981 724 251 559 534 512 512 512 512 512 512 512 0.000 0.600
+step=512 233 792 354 737 369 555 353 653 440 461 172 783 49 902 724 311 546 538 512 512 512 512 512 512 512 0.000 0.600
+step=512 232 792 353 737 369 555 340 653 493 497 234 764 77 912 729 305 530 521 512 512 512 512 512 512 512 0.000 0.800
+step=512 231 801 273 737 460 557 361 664 511 523 450 583 440 609 550 460 509 514 512 512 512 512 512 512 512 0.000 1.520
 page_end
 page_begin
-name=stairs_up_4
+name=Init
 compliance=5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5
 play_param=0 0 1 1.0 32
-step=512 233 792 354 737 369 555 353 649 440 466 172 804 49 981 724 251 559 534 512 512 512 512 512 512 512 0.000 0.600
-step=512 233 792 354 737 369 555 353 653 440 461 172 783 49 902 724 311 546 538 512 512 512 512 512 512 512 0.000 0.600
-step=512 232 792 353 737 369 555 340 653 493 497 234 764 77 912 729 305 530 521 512 512 512 512 512 512 512 0.000 0.800
-step=512 231 801 273 737 460 557 361 664 511 523 450 583 440 609 550 460 509 514 512 512 512 512 512 512 512 0.000 0.800
+step=512 231 801 274 737 460 557 361 664 511 523 450 584 440 610 551 460 509 514 512 512 512 512 512 512 512 0.000 1.000
 page_end
 page_begin
 name=stairs_down1
 compliance=5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5
-play_param=229 0 1 1.0 32
+play_param=0 0 1 1.0 32
 step=512 229 804 271 735 460 556 367 665 541 552 370 694 220 874 679 311 498 504 512 512 512 512 512 512 512 0.000 1.000
 step=512 229 1023 271 735 460 556 367 665 543 543 370 702 217 906 690 278 458 456 512 512 512 512 512 512 512 0.000 1.000
 step=512 229 1023 271 735 460 556 356 665 507 543 300 702 56 905 751 278 458 456 512 512 512 512 512 512 512 0.000 1.000
-step=512 229 1023 271 735 460 556 361 665 511 543 213 702 100 905 594 278 481 456 512 512 512 512 512 512 512 0.000 1.000
-step=512 35 1023 271 735 460 556 361 665 511 543 213 702 100 905 594 278 481 456 512 512 512 512 512 512 512 0.000 1.000
-step=512 35 809 271 735 460 556 372 665 510 543 376 702 428 905 456 278 490 456 512 512 512 512 512 512 512 0.000 1.000
+step=512 229 1023 271 735 460 556 361 665 511 543 213 602 100 905 594 278 481 456 512 512 512 512 512 512 512 0.000 1.000
+step=512 35 1023 271 735 460 556 361 665 511 543 213 602 100 905 594 278 481 456 512 512 512 512 512 512 512 0.000 1.000
+step=512 35 809 271 735 460 556 372 665 510 543 376 602 428 905 456 278 490 456 512 512 512 512 512 512 512 0.000 1.000
 page_end
 page_begin
 name=stairs_down2
 compliance=5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5 5
-play_param=230 0 1 1.0 32
+play_param=0 0 1 1.0 32
 step=512 227 833 271 735 459 555 383 684 501 490 330 712 446 894 449 270 490 476 512 512 512 512 512 512 512 0.000 1.000
 step=512 227 832 271 735 459 555 381 687 500 489 323 635 324 864 549 251 486 478 512 512 512 512 512 512 512 0.000 1.000
 step=512 227 837 271 735 459 555 420 717 497 506 331 567 212 811 656 239 510 514 512 512 512 512 512 512 512 0.000 1.000

bioloid_description/urdf/bioloid.gazebo

@@ -243,8 +243,8 @@
     <dampingFactor>0.2</dampingFactor>
     <maxVel>0.0</maxVel>
     <minDepth>0.0</minDepth>
-    <mu1>100.0</mu1>
-    <mu2>100.0</mu2>
+    <mu1>1.0</mu1>
+    <mu2>1.0</mu2>
     <kp>0.0</kp>
     <kd>0.0</kd>
   </gazebo>
@@ -321,8 +321,8 @@
     <dampingFactor>0.2</dampingFactor>
     <maxVel>0.0</maxVel>
     <minDepth>0.0</minDepth>
-    <mu1>100.0</mu1>
-    <mu2>100.0</mu2>
+    <mu1>1.0</mu1>
+    <mu2>1.0</mu2>
     <kp>0.0</kp>
     <kd>0.0</kd>
   </gazebo>
@@ -438,7 +438,7 @@
   <gazebo>
     <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
       <robotNamespace>/bioloid</robotNamespace>
-      <controlPeriod>0.0078</controlPeriod>
+      <controlPeriod>0.001</controlPeriod>
     </plugin>
   </gazebo>
 

bioloid_description/urdf/bioloid.xacro

@@ -445,7 +445,7 @@
     <origin xyz="-0.07188 0.0 0" rpy="-1.5707 0 0" />
     <axis xyz="-1 0 0" />
     <limit effort="1.5" velocity="12.0" lower="-3.14" upper="3.14" />
-    <dynamics damping="0.15"/>
+    <dynamics damping="0.2"/>
   </joint>
 
   <transmission name="tran_shoulder_r">
@@ -465,7 +465,7 @@
     <origin xyz="0 -0.0145 0.0" rpy="0 0 -1.5707" />
     <axis xyz="0 0 -1" />
     <limit effort="1.5" velocity="12.0" lower="-3.14" upper="3.14" />
-    <dynamics damping="0.15"/>
+    <dynamics damping="0.2"/>
   </joint>
 
   <transmission name="tran_high_arm_r">
@@ -485,7 +485,7 @@
     <origin xyz="0.0 -0.0675 0.0" rpy="0 0 0" />
     <axis xyz="0 0 -1" />
     <limit effort="1.5" velocity="12.0" lower="-3.14" upper="3.14" />
-    <dynamics damping="0.15"/>
+    <dynamics damping="0.2"/>
   </joint>
 
   <transmission name="tran_low_arm_r">
@@ -505,7 +505,7 @@
     <origin xyz="0.07188 0.0 0" rpy="-1.5707 0 0" />
     <axis xyz="1 0 0" />
     <limit effort="1.5" velocity="12.0" lower="-3.14" upper="3.14" />
-    <dynamics damping="0.15"/>
+    <dynamics damping="0.2"/>
   </joint>
 
   <transmission name="tran_shoulder_l">
@@ -525,7 +525,7 @@
     <origin xyz="0 -0.0145 0.0" rpy="0 0 1.5707" />
     <axis xyz="0 0 -1" />
     <limit effort="1.5" velocity="12.0" lower="-3.14" upper="3.14" />
-    <dynamics damping="0.15"/>
+    <dynamics damping="0.2"/>
   </joint>
 
   <transmission name="tran_high_arm_l">
@@ -545,7 +545,7 @@
     <origin xyz="0.0 -0.0675 0.0" rpy="0 0 0" />
     <axis xyz="0 0 -1" />
     <limit effort="1.5" velocity="12.0" lower="-3.14" upper="3.14" />
-    <dynamics damping="0.15"/>
+    <dynamics damping="0.2"/>
   </joint>
 
   <transmission name="tran_low_arm_l">
@@ -565,7 +565,7 @@
     <origin xyz="-0.0385 -0.12037 0.0" rpy="0 -0.7854 0" />
     <axis xyz="0 -1 0" />
     <limit effort="1.5" velocity="12.0" lower="-3.14" upper="3.14" />
-    <dynamics damping="0.15"/>
+    <dynamics damping="0.2"/>
   </joint>
 
   <transmission name="tran_pelvis_yaw_r">
@@ -585,7 +585,7 @@
     <origin xyz="0 0 0" rpy="0 0 0" />
     <axis xyz="0 0 1" />
     <limit effort="1.5" velocity="12.0" lower="-3.14" upper="3.14" />
-    <dynamics damping="0.15"/>
+    <dynamics damping="0.2"/>
   </joint>
 
   <transmission name="tran_pelvis_roll_r">
@@ -605,7 +605,7 @@
     <origin xyz="0 0 0" rpy="0 0 0" />
     <axis xyz="1 0 0" />
     <limit effort="1.5" velocity="12.0" lower="-3.14" upper="3.14" />
-    <dynamics damping="0.15"/>
+    <dynamics damping="0.2"/>
   </joint>
 
   <transmission name="tran_pelvis_pitch_r">
@@ -625,7 +625,7 @@
     <origin xyz="0 -0.075 -0.01488" rpy="0 0 0" />
     <axis xyz="-1 0 0" />
     <limit effort="1.5" velocity="12.0" lower="-3.14" upper="3.14" />
-    <dynamics damping="0.15"/>
+    <dynamics damping="0.2"/>
   </joint>
 
   <transmission name="tran_knee_r">
@@ -645,7 +645,7 @@
     <origin xyz="0 -0.075 0.01488" rpy="0 0 0" />
     <axis xyz="-1 0 0" />
     <limit effort="1.5" velocity="12.0" lower="-3.14" upper="3.14" />
-    <dynamics damping="0.15"/>
+    <dynamics damping="0.2"/>
   </joint>
 
   <transmission name="tran_ankle_pitch_r">
@@ -665,7 +665,7 @@
     <origin xyz="0 0 0" rpy="0 0 0" />
     <axis xyz="0 0 -1" />
     <limit effort="1.5" velocity="12.0" lower="-3.14" upper="3.14" />
-    <dynamics damping="0.15"/>
+    <dynamics damping="0.2"/>
   </joint>
 
   <transmission name="tran_ankle_roll_r">
@@ -685,7 +685,7 @@
     <origin xyz="0.0385 -0.12037 0.0" rpy="0 0.7853 0" />
     <axis xyz="0 -1 0" />
     <limit effort="1.5" velocity="12.0" lower="-3.14" upper="3.14" />
-    <dynamics damping="0.15"/>
+    <dynamics damping="0.2"/>
   </joint>
 
   <transmission name="tran_pelvis_yaw_l">
@@ -705,7 +705,7 @@
     <origin xyz="0 0 0" rpy="0 0 0" />
     <axis xyz="0 0 1" />
     <limit effort="1.5" velocity="12.0" lower="-3.14" upper="3.14" />
-    <dynamics damping="0.15"/>
+    <dynamics damping="0.2"/>
   </joint>
 
   <transmission name="tran_pelvis_roll_l">
@@ -725,7 +725,7 @@
     <origin xyz="0 0 0" rpy="0 0 0" />
     <axis xyz="-1 0 0" />
     <limit effort="1.5" velocity="12.0" lower="-3.14" upper="3.14" />
-    <dynamics damping="0.15"/>
+    <dynamics damping="0.2"/>
   </joint>
 
   <transmission name="tran_pelvis_pitch_l">
@@ -745,7 +745,7 @@
     <origin xyz="0 -0.075 -0.01488" rpy="0 0 0" />
     <axis xyz="1 0 0" />
     <limit effort="1.5" velocity="12.0" lower="-3.14" upper="3.14" />
-    <dynamics damping="0.15"/>
+    <dynamics damping="0.2"/>
   </joint>
 
   <transmission name="tran_knee_l">
@@ -765,7 +765,7 @@
     <origin xyz="0 -0.075 0.01488" rpy="0 0 0" />
     <axis xyz="1 0 0" />
     <limit effort="1.5" velocity="12.0" lower="-3.14" upper="3.14" />
-    <dynamics damping="0.15"/>
+    <dynamics damping="0.2"/>
   </joint>
 
   <transmission name="tran_ankle_pitch_l">
@@ -785,7 +785,7 @@
     <origin xyz="0 0 0" rpy="0 0 0" />
     <axis xyz="0 0 -1" />
     <limit effort="1.5" velocity="12.0" lower="-3.14" upper="3.14" />
-    <dynamics damping="0.15"/>
+    <dynamics damping="0.2"/>
   </joint>
 
   <transmission name="tran_ankle_roll_l">

bioloid_description/urdf/bioloid_ceabot.xacro

@@ -14,7 +14,7 @@
   <xacro:sharp_ir name="IR3" parent="base_link" update_rate="20" fov="0.05" min_range="0.1" max_range="0.8">
     <origin xyz="0 0.05 0.035" rpy="-1.5707 -1.5707 0" />
   </xacro:sharp_ir>
-<!--  <xacro:feet_ir left_parent="left_leg_ankle_roll" right_parent="right_leg_ankle_roll" update_rate="20" range="0.01"/>-->
+  <xacro:feet_ir left_parent="left_leg_ankle_roll" right_parent="right_leg_ankle_roll" update_rate="20" range="0.01"/>
 
   <link name="battery_pack">
     <collision>

bioloid_description/urdf/ceabot/obstacle_base.xacro

@@ -5,10 +5,15 @@
   <xacro:macro name="obstacle_base" params="name">
   <!-- IR distance sensors -->
     <link name="${name}_link">
-      <inertial>
+<!--      <inertial>
         <mass value="50"/>
         <origin xyz="1.01500000 1.26500000 0.04510729" rpy="0 0 0"/>
         <inertia ixx="32.72577613" ixy="0.0" ixz="0.0" iyy="22.18978101" iyz="0.0" izz="53.20566219" />
+      </inertial>-->
+      <inertial>
+        <mass value="1"/>
+        <origin xyz="1.01500000 1.26500000 0.04510729" rpy="0 0 0"/>
+        <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0" />
       </inertial>
       <visual>
         <origin xyz="0 0 0" rpy="0 0 0"/>
@@ -24,8 +29,18 @@
       </collision>
     </link>
 
-    <gazebo>
-      <static>1</static>
+    <gazebo reference="${name}_link">
+      <gravity>true</gravity>
+      <selfCollide>false</selfCollide>
+      <maxContacts>10</maxContacts>
+      <dampingFactor>0.2</dampingFactor>
+      <maxVel>0.01</maxVel>
+      <minDepth>0.0</minDepth>
+      <fdir1>0 0 0</fdir1>
+      <mu1>1.0</mu1>
+      <mu2>1.0</mu2>
+      <kp>1000000000.0</kp>
+      <kd>1.0</kd>
     </gazebo>
   </xacro:macro>
 </root>

bioloid_description/urdf/ceabot/stairs.xacro

@@ -4,34 +4,123 @@
 
   <xacro:macro name="stairs" params="name parent orientation">
   <!-- obstacle -->
-    <link name="${name}_link">
+    <link name="${name}_step1_link">
       <inertial>
-        <mass value="50"/>
-        <origin xyz="0.0 0.01666667 0.03611111" rpy="0 0 0"/>
-        <inertia ixx="4.76591049" ixy="0.0" ixz="0.0" iyy="4.19646605" iyz="0.00740741" izz="8.90277778" />
+        <mass value="1"/>
+        <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+        <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0" />
       </inertial>
       <visual>
-        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <origin xyz="0 0 0.015" rpy="0 0 0"/>
         <geometry>
-          <mesh filename="package://bioloid_description/meshes/ceabot/stairs.stl"/>
-      </geometry>
+          <box size="1.0 1.3 0.03"/>
+        </geometry>
       </visual>
       <collision>
-        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <origin xyz="0 0 0.015" rpy="0 0 0"/>
         <geometry>
-          <mesh filename="package://bioloid_description/meshes/ceabot/stairs.stl"/>
+          <box size="1.0 1.3 0.03"/>
         </geometry>
       </collision>
     </link>
 
-    <joint name="${name}_joint" type="fixed">
-      <origin xyz="1.0 1.25 0" rpy="0 0 ${orientation}"/>
+    <joint name="${name}_step1_joint" type="fixed">
+      <origin xyz="1.0 1.25 0" rpy="0 0 0"/>
       <parent link="${parent}_link"/>
-      <child link="${name}_link"/>
+      <child link="${name}_step1_link"/>
+    </joint>
+
+    <gazebo reference="${name}_step1_link">
+      <gravity>true</gravity>
+      <selfCollide>false</selfCollide>
+      <maxContacts>10</maxContacts>
+      <dampingFactor>0.2</dampingFactor>
+      <maxVel>1.0</maxVel>
+      <minDepth>0.0</minDepth>
+      <mu1>1.0</mu1>
+      <mu2>1.0</mu2>
+      <kp>1000000.0</kp>
+      <kd>1000.0</kd>
+    </gazebo>
+
+    <link name="${name}_step2_link">
+      <inertial>
+        <mass value="1"/>
+        <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+        <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0" />
+      </inertial>
+      <visual>
+        <origin xyz="0 0 0.015" rpy="0 0 0"/>
+        <geometry>
+          <box size="1.0 0.9 0.03"/>
+        </geometry>
+      </visual>
+      <collision>
+        <origin xyz="0 0 0.015" rpy="0 0 0"/>
+        <geometry>
+          <box size="1.0 0.9 0.03"/>
+        </geometry>
+      </collision>
+    </link>
+
+    <joint name="${name}_step2_joint" type="fixed">
+      <origin xyz="0.0 0.05 0.03" rpy="0 0 ${orientation}"/>
+      <parent link="${name}_step1_link"/>
+      <child link="${name}_step2_link"/>
     </joint>
 
-    <gazebo reference="${name}_link">
+    <gazebo reference="${name}_step2_link">
+      <gravity>true</gravity>
+      <selfCollide>false</selfCollide>
+      <maxContacts>10</maxContacts>
+      <dampingFactor>0.2</dampingFactor>
+      <maxVel>1.0</maxVel>
+      <minDepth>0.0</minDepth>
+      <mu1>1.0</mu1>
+      <mu2>1.0</mu2>
+      <kp>1000000.0</kp>
+      <kd>1000.0</kd>
     </gazebo>
+
+    <link name="${name}_step3_link">
+      <inertial>
+        <mass value="1"/>
+        <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+        <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0" />
+      </inertial>
+      <visual>
+        <origin xyz="0 0 0.015" rpy="0 0 0"/>
+        <geometry>
+          <box size="1.0 0.5 0.03"/>
+        </geometry>
+      </visual>
+      <collision>
+        <origin xyz="0 0 0.015" rpy="0 0 0"/>
+        <geometry>
+          <box size="1.0 0.5 0.03"/>
+        </geometry>
+      </collision>
+    </link>
+
+    <joint name="${name}_step3_joint" type="fixed">
+      <origin xyz="0.0 0.0 0.06" rpy="0 0 0"/>
+      <parent link="${name}_step1_link"/>
+      <child link="${name}_step3_link"/>
+    </joint>
+
+    <gazebo reference="${name}_step3_link">
+      <gravity>true</gravity>
+      <selfCollide>false</selfCollide>
+      <maxContacts>10</maxContacts>
+      <dampingFactor>0.2</dampingFactor>
+      <maxVel>1.0</maxVel>
+      <minDepth>0.0</minDepth>
+      <mu1>1.0</mu1>
+      <mu2>1.0</mu2>
+      <kp>1000000.0</kp>
+      <kd>1000.0</kd>
+    </gazebo>
+
   </xacro:macro>
 </root>
 

bioloid_gazebo/worlds/bioloid.world

@@ -13,6 +13,7 @@
           <type>quick</type>
           <min_step_size>0.0001</min_step_size>
           <iters>100</iters>
+          <sor>0.7</sor>
         </solver>
       </ode>
     </physics>