Commit 72c7b0d8 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Substituted the servo PID's controllers for compliance controllers, more...

Substituted the servo PID's controllers for compliance controllers, more similar to the real servos.
The walk timeout is updated each time a new cmd_vel is received, whatever the current state.
parent 38554ea0
......@@ -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
......@@ -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_;
......
......@@ -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();
}
......
......@@ -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>
......
......@@ -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>
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment