diff --git a/bioloid_control/config/bioloid_ceabot.yaml b/bioloid_control/config/bioloid_ceabot.yaml
index 9e6799581118fad867026101e00b6320dde62e01..a75de36e6b69e6483e54b32977d76012d7e31727 100644
--- a/bioloid_control/config/bioloid_ceabot.yaml
+++ b/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
diff --git a/bioloid_controller_cm510/CMakeLists.txt b/bioloid_controller_cm510/CMakeLists.txt
index 97703550d7d121ba041e9eb8c27f6d46ec25e077..f9c36474af76b798e69b90d8001cbf25b60466ab 100644
--- a/bioloid_controller_cm510/CMakeLists.txt
+++ b/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
diff --git a/bioloid_controller_cm510/include/bioloid_controller_cm510.h b/bioloid_controller_cm510/include/bioloid_controller_cm510.h
index e2dfe328cbab4d4a86ecb66f98394decdba36e27..287458382333fa2a96c517add2a75e10a7ea10e2 100644
--- a/bioloid_controller_cm510/include/bioloid_controller_cm510.h
+++ b/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 */
diff --git a/bioloid_controller_cm510/include/bioloid_controller_cm510_impl.h b/bioloid_controller_cm510/include/bioloid_controller_cm510_impl.h
index b8d3b127e0bf610712f81f608b710e2a4d87f247..f23b2011649345879883815e44f943ed4ca405ac 100644
--- a/bioloid_controller_cm510/include/bioloid_controller_cm510_impl.h
+++ b/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)
{
diff --git a/bioloid_controller_cm510/motions/ceabot_motions.mtn b/bioloid_controller_cm510/motions/ceabot_motions.mtn
index b86baf950058a126c567fb60da65a588abe94ba8..b5a1215ce109816953c67f55d5440eb31bd14849 100755
--- a/bioloid_controller_cm510/motions/ceabot_motions.mtn
+++ b/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
diff --git a/bioloid_description/meshes/ceabot/stairs.stl b/bioloid_description/meshes/ceabot/stairs.stl
deleted file mode 100644
index db45f435c99a1859e87f9b24f32ff9504ba34e36..0000000000000000000000000000000000000000
Binary files a/bioloid_description/meshes/ceabot/stairs.stl and /dev/null differ
diff --git a/bioloid_description/urdf/bioloid.gazebo b/bioloid_description/urdf/bioloid.gazebo
index 634f329f7f37380ca1eaa7bb15cbcb17e6499cdd..f3354d45db563b44d6c7e975e5727091398c18bb 100644
--- a/bioloid_description/urdf/bioloid.gazebo
+++ b/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>
diff --git a/bioloid_description/urdf/bioloid.xacro b/bioloid_description/urdf/bioloid.xacro
index 6d15735afca3f1020779a6106cace6533e5470fb..3a2e47ea01d5bd37cb62e257fb447275ecd3e59b 100755
--- a/bioloid_description/urdf/bioloid.xacro
+++ b/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">
diff --git a/bioloid_description/urdf/bioloid_ceabot.xacro b/bioloid_description/urdf/bioloid_ceabot.xacro
index a8a845f7ca5f355164094502a0aac3d6dcc56083..87c2a303f5b671bd07757aa7d376e3413fd497dc 100755
--- a/bioloid_description/urdf/bioloid_ceabot.xacro
+++ b/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>
diff --git a/bioloid_description/urdf/ceabot/obstacle_base.xacro b/bioloid_description/urdf/ceabot/obstacle_base.xacro
index dc2d4e56d6f33864394a23bba460b0971f16083e..d1054374db03b3dfd9708ecb743df4bc6056ab95 100644
--- a/bioloid_description/urdf/ceabot/obstacle_base.xacro
+++ b/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>
diff --git a/bioloid_description/urdf/ceabot/stairs.xacro b/bioloid_description/urdf/ceabot/stairs.xacro
index 48fb844eb0658bf653828f47e248b9471bbc6fc0..ccb322df46ec219cb5b10f675503c02a256cc5df 100644
--- a/bioloid_description/urdf/ceabot/stairs.xacro
+++ b/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>
diff --git a/bioloid_gazebo/worlds/bioloid.world b/bioloid_gazebo/worlds/bioloid.world
index 49da872082b7ec71012b484d3ce509dfcc3ce41c..40544c87db3858d15a167f211191a35499f6cd13 100644
--- a/bioloid_gazebo/worlds/bioloid.world
+++ b/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>