diff --git a/bioloid_control/config/bioloid_ceabot.yaml b/bioloid_control/config/bioloid_ceabot.yaml index 3dd9fb51c94595a36f568e6603ababbf7e0ec4ef..c8b0fa55068f71b77ed99825ec6fdb45dc95d23c 100644 --- a/bioloid_control/config/bioloid_ceabot.yaml +++ b/bioloid_control/config/bioloid_ceabot.yaml @@ -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 diff --git a/bioloid_control/config/bioloid_ceabot_pan_tilt.yaml b/bioloid_control/config/bioloid_ceabot_pan_tilt.yaml index c69bdd309482667419835c6e6fe225e060964333..db5f385e5c7fdff01d3ef24fcc40c5e4aa32f679 100644 --- a/bioloid_control/config/bioloid_ceabot_pan_tilt.yaml +++ b/bioloid_control/config/bioloid_ceabot_pan_tilt.yaml @@ -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 diff --git a/bioloid_controller/include/bioloid_controller_impl.h b/bioloid_controller/include/bioloid_controller_impl.h index 322d52ee0dac1fab599b61dba24e8a55800db03f..8113a9153f6290177dfd5c5437e5b52e2450f460 100644 --- a/bioloid_controller/include/bioloid_controller_impl.h +++ b/bioloid_controller/include/bioloid_controller_impl.h @@ -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; diff --git a/bioloid_controller_cm510/include/bioloid_controller_cm510_impl.h b/bioloid_controller_cm510/include/bioloid_controller_cm510_impl.h index f80dd5b4b1585110d260779cc2b1b8fe4a35e0df..27d1a06a44f545ac1e36d610d59376a9badbdd91 100644 --- a/bioloid_controller_cm510/include/bioloid_controller_cm510_impl.h +++ b/bioloid_controller_cm510/include/bioloid_controller_cm510_impl.h @@ -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) diff --git a/bioloid_description/urdf/bioloid_ceabot.xacro b/bioloid_description/urdf/bioloid_ceabot.xacro index a108862677d8e3ecb61f6fe3bf030f8b03596ce3..7134cd2d809067acc99147e3941d5fbedffa4879 100755 --- a/bioloid_description/urdf/bioloid_ceabot.xacro +++ b/bioloid_description/urdf/bioloid_ceabot.xacro @@ -1,6 +1,6 @@ <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" />