diff --git a/bioloid_control/config/bioloid_control.yaml b/bioloid_control/config/bioloid_control.yaml
index fcbcd0035676bd8a6612b44ac9f7fa92cf7a7510..4ca8f3ea6c13cc5e742bf0280057a69c1bbbb90b 100644
--- a/bioloid_control/config/bioloid_control.yaml
+++ b/bioloid_control/config/bioloid_control.yaml
@@ -27,92 +27,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: 8.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: 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_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: 8.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: 8.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/include/bioloid_controller.h b/bioloid_controller/include/bioloid_controller.h
index 404d73085b1d41cd200e27e602f9cb9471b83bf2..e7b454ed45dd0d4a6cabc495dee41c588b258ec0 100644
--- a/bioloid_controller/include/bioloid_controller.h
+++ b/bioloid_controller/include/bioloid_controller.h
@@ -53,7 +53,6 @@
// ros_controls
#include <controller_interface/controller.h>
#include <hardware_interface/joint_command_interface.h>
-#include <control_toolbox/pid.h>
#include <iri_action_server/iri_action_server.h>
#include <humanoid_common_msgs/humanoid_motionAction.h>
@@ -65,6 +64,13 @@
namespace bioloid_controller
{
+ typedef struct{
+ int punch;
+ int margin;
+ int slope;
+ double max_torque;
+ }TCompliance;
+
/**
* \brief
*
@@ -80,15 +86,17 @@ namespace bioloid_controller
void starting(const ros::Time& time);
void stopping(const ros::Time& time);
void update(const ros::Time& time, const ros::Duration& period);
+
+ protected:
+ 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_;
// motion action subscriber
IriActionServer<humanoid_common_msgs::humanoid_motionAction> *motion_action_aserver_;
diff --git a/bioloid_controller/include/bioloid_controller_impl.h b/bioloid_controller/include/bioloid_controller_impl.h
index d4d6ce7b0796719a485f222fe1f98c7e0d741427..322d52ee0dac1fab599b61dba24e8a55800db03f 100644
--- a/bioloid_controller/include/bioloid_controller_impl.h
+++ b/bioloid_controller/include/bioloid_controller_impl.h
@@ -151,7 +151,6 @@ namespace bioloid_controller
{
for(unsigned int i=0;i<this->joints_.size();++i)
{
- pids_[i]->reset();
this->joints_[i].setCommand(0.0);
}
}
@@ -219,7 +218,7 @@ namespace bioloid_controller
num_servos=n_joints;// set the number of servos to the number of joint of the urdf model
// 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
@@ -233,13 +232,15 @@ namespace bioloid_controller
// 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=1.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-";
@@ -269,6 +270,7 @@ namespace bioloid_controller
void BioloidController<HardwareInterface>::update(const ros::Time& time, const ros::Duration& period)
{
static bool gyro_calibrated=false;
+ static ros::Time last_time=ros::Time(0,0);
ros::Time current_time=ros::Time::now();
std::vector<double> target_angles(joints_.size());
@@ -283,20 +285,56 @@ namespace bioloid_controller
/* get the actual simulation angles */
for(unsigned int i=0;i<this->joints_.size();++i)
+ {
real_angles[i]=joints_[i].getPosition();
- __HAL_TIM_SET_FLAG(TIM3,TIM_FLAG_CC1);
- __HAL_TIM_SET_IT_SOURCE(TIM3,TIM_IT_CC1);
- TIM3_IRQHandler();
+ std::cout << real_angles[i] << ",";
+ }
+ std::cout << std::endl;
+ if((current_time-last_time).toSec()>0.0078)
+ {
+ __HAL_TIM_SET_FLAG(TIM3,TIM_FLAG_CC1);
+ __HAL_TIM_SET_IT_SOURCE(TIM3,TIM_IT_CC1);
+ TIM3_IRQHandler();
+ last_time=current_time;
+ }
for (unsigned int i = 0; i < this->joints_.size(); ++i)
{
target_angles[i] = ((manager_current_angles[i+1]*3.14159)/180.0)/65536.0;
- const double command = 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]);
+ std::cout << target_angles[i] << ",";
this->joints_[i].setCommand(command);
}
+ std::cout << std::endl;
if(is_walking() && ((current_time-this->walk_timeout).toSec()>1.0))
walking_stop();
}
+ template <class HardwareInterface>
+ double BioloidController<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 BioloidController<HardwareInterface>::motion_action_startCallback(const humanoid_common_msgs::humanoid_motionGoalConstPtr& goal)
{
@@ -461,8 +499,7 @@ namespace bioloid_controller
ROS_INFO("Start walking");
walking_start();
}
- else
- this->walk_timeout=ros::Time::now();
+ this->walk_timeout=ros::Time::now();
this->walk_access.exit();
}
diff --git a/bioloid_description/urdf/bioloid.gazebo b/bioloid_description/urdf/bioloid.gazebo
index 56ae3d5295784dc297d8764cb3cf947bbd9742a5..28017bb6b0c520cd70ac42bf0f152adaad6513f1 100644
--- a/bioloid_description/urdf/bioloid.gazebo
+++ b/bioloid_description/urdf/bioloid.gazebo
@@ -438,7 +438,7 @@
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<robotNamespace>/bioloid</robotNamespace>
- <controlPeriod>0.001</controlPeriod>
+<!-- <controlPeriod>0.001</controlPeriod>-->
</plugin>
</gazebo>
diff --git a/bioloid_gazebo/worlds/bioloid.world b/bioloid_gazebo/worlds/bioloid.world
index 40544c87db3858d15a167f211191a35499f6cd13..d92c5cce2485d7c80e0fb5a742dd9f9ce7000039 100644
--- a/bioloid_gazebo/worlds/bioloid.world
+++ b/bioloid_gazebo/worlds/bioloid.world
@@ -2,17 +2,17 @@
<sdf version="1.4">
<world name="default">
<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>-->
+<!-- <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>
<solver>
<type>quick</type>
- <min_step_size>0.0001</min_step_size>
- <iters>100</iters>
+ <min_step_size>0.00025</min_step_size>
+ <iters>200</iters>
<sor>0.7</sor>
</solver>
</ode>