Commit 15e6878d authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Solved a problem with the balance feature. The sign of the forward/backeard compensation was wrong.

Used the low resolution model for the basic Bioloid CEABOT robot.
parent 1688f96b
......@@ -18,7 +18,7 @@ bioloid:
exp_gpio3_frame: right_front_fwd_ir_link
exp_gpio4_frame: right_front_dwn_ir_link
exp_gpio5_frame: right_lateral_dwn_ir_link
serial_console_device: /dev/pts/18
serial_console_device: /dev/pts/3
joints:
- j_shoulder_r
- j_shoulder_l
......
......@@ -20,7 +20,7 @@ bioloid:
exp_gpio5_frame: right_lateral_dwn_ir_link
pan_servo_id: 19
tilt_servo_id: 20
serial_console_device: /dev/pts/18
serial_console_device: /dev/pts/3
joints:
- j_shoulder_r
- j_shoulder_l
......
......@@ -190,7 +190,7 @@ namespace bioloid_controller
this->get_walk_params_server_=root_nh.advertiseService("robot/get_walk_params", &BioloidController<HardwareInterface>::get_walk_paramsCallback, this);
// initialize topics
this->imu_sub=controller_nh.subscribe("raw_imu", 1, &BioloidController<HardwareInterface>::imu_callback,this);
this->imu_sub=root_nh.subscribe("sensors/raw_imu", 1, &BioloidController<HardwareInterface>::imu_callback,this);
this->cmd_vel_sub=root_nh.subscribe("robot/cmd_vel", 1, &BioloidController<HardwareInterface>::cmd_vel_callback,this);
// Controller name
......@@ -285,11 +285,7 @@ namespace bioloid_controller
/* get the actual simulation angles */
for(unsigned int i=0;i<this->joints_.size();++i)
{
real_angles[i]=joints_[i].getPosition();
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);
......@@ -301,10 +297,8 @@ namespace bioloid_controller
{
target_angles[i] = ((manager_current_angles[i+1]*3.14159)/180.0)/65536.0;
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();
}
......@@ -507,7 +501,7 @@ namespace bioloid_controller
void BioloidController<HardwareInterface>::imu_callback(const sensor_msgs::Imu::ConstPtr& msg)
{
double fb_imu_value=-(msg->angular_velocity.x*180.0)/PI;
double lr_imu_value=-(msg->angular_velocity.z*180.0)/PI;
double lr_imu_value=(msg->angular_velocity.z*180.0)/PI;
if(fb_imu_value>300.0)
fb_imu_value=300.0;
......
......@@ -404,7 +404,7 @@ namespace bioloid_controller_cm510
template <class HardwareInterface>
void BioloidControllerCM510<HardwareInterface>::imu_callback(const sensor_msgs::Imu::ConstPtr& msg)
{
double fb_imu_value=-(msg->angular_velocity.x*180.0)/3.14159;
double fb_imu_value=(msg->angular_velocity.x*180.0)/3.14159;
double lr_imu_value=(msg->angular_velocity.z*180.0)/3.14159;
if(this->found_gyro_x && this->found_gyro_y)
......
<robot name="bioloid" xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:include filename="$(find bioloid_description)/urdf/bioloid.xacro" />
<xacro:include filename="$(find bioloid_description)/urdf/bioloid_low_res.xacro" />
<xacro:include filename="$(find bioloid_description)/urdf/bioloid_ceabot.gazebo" />
<xacro:include filename="$(find bioloid_description)/urdf/sensors/sharp_ir.xacro" />
<xacro:include filename="$(find bioloid_description)/urdf/sensors/feet_ir.xacro" />
......
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