From a93045cfc2219af1399baecef7dbeac855e38e0e Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Sergi=20Hern=C3=A1ndez?= <shernand@iri.upc.edu>
Date: Sun, 12 Jun 2016 22:31:40 +0200
Subject: [PATCH] Changed the PID controller of the servos with a compliance
controller similar to the one in the real servos. Changed the control period
of the simulated model to 0.001s instead of 0.0078s. Modified the up and down
stairs motions. Updted the stairs environment to use standard box instead of
the stl model.
---
bioloid_control/config/bioloid_ceabot.yaml | 144 +++++++++---------
.../include/bioloid_controller_cm510.h | 11 +-
.../include/bioloid_controller_cm510_impl.h | 73 ++++++---
.../motions/ceabot_motions.mtn | 47 +++---
bioloid_description/urdf/bioloid.gazebo | 10 +-
bioloid_description/urdf/bioloid.xacro | 36 ++---
bioloid_description/urdf/bioloid_ceabot.xacro | 2 +-
.../urdf/ceabot/obstacle_base.xacro | 21 ++-
bioloid_description/urdf/ceabot/stairs.xacro | 115 ++++++++++++--
bioloid_gazebo/worlds/bioloid.world | 1 +
10 files changed, 302 insertions(+), 158 deletions(-)
diff --git a/bioloid_control/config/bioloid_ceabot.yaml b/bioloid_control/config/bioloid_ceabot.yaml
index 9e67995..a75de36 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/include/bioloid_controller_cm510.h b/bioloid_controller_cm510/include/bioloid_controller_cm510.h
index e2dfe32..2874583 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 b8d3b12..f23b201 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 b86baf9..b5a1215 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/urdf/bioloid.gazebo b/bioloid_description/urdf/bioloid.gazebo
index 634f329..f3354d4 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 6d15735..3a2e47e 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 a8a845f..87c2a30 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 dc2d4e5..d105437 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 48fb844..ccb322d 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 49da872..40544c8 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>
--
GitLab