Commit 7d795b12 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Added the urdf, xacro and gazebo file for the pan&tilt unit.

Integrated the use of the pan&tilt unit in all CEABOT launch files.
Added the urdf, xacro and gazebo files for the IDS XS camera.
TEmporal chnages to try to redirect the printf output to a virtual serial device.
parent 72c7b0d8
......@@ -2,10 +2,12 @@
<arg name="robot" default="bioloid_ceabot" />
<arg name="mtn_file" default="bio_prm_humanoidtypea_en" />
<arg name="use_pan_tilt" default="false" />
<include file="$(find bioloid_description)/launch/bioloid_cm510_sim.launch">
<arg name="robot" value="$(arg robot)" />
<arg name="mtn_file" value="$(arg mtn_file)" />
<arg name="use_pan_tilt" value="$(arg use_pan_tilt)" />
</include>
<!-- launch the action client node -->
......
......@@ -3,10 +3,12 @@
<arg name="robot" default="bioloid_ceabot" />
<arg name="environment" default="obstacles_env" />
<arg name="mtn_file" default="bio_prm_humanoidtypea_en" />
<arg name="use_pan_tilt" default="false" />
<include file="$(find bioloid_apps)/launch/ceabot/ceabot_base.launch">
<arg name="robot" value="$(arg robot)"/>
<arg name="mtn_file" value="$(arg mtn_file)"/>
<arg name="use_pan_tilt" value="$(arg use_pan_tilt)"/>
</include>
<include file="$(find bioloid_description)/launch/obstacles_env.launch">
......
......@@ -3,10 +3,12 @@
<arg name="robot" default="bioloid_ceabot" />
<arg name="environment" default="stairs_env" />
<arg name="mtn_file" default="bio_prm_humanoidtypea_en" />
<arg name="use_pan_tilt" default="false" />
<include file="$(find bioloid_apps)/launch/ceabot/ceabot_base.launch">
<arg name="robot" value="$(arg robot)"/>
<arg name="mtn_file" value="$(arg mtn_file)"/>
<arg name="use_pan_tilt" value="$(arg use_pan_tilt)"/>
</include>
<include file="$(find bioloid_description)/launch/stairs_env.launch">
......
......@@ -3,10 +3,12 @@
<arg name="robot" default="bioloid_ceabot" />
<arg name="environment" default="vision_env" />
<arg name="mtn_file" default="bio_prm_humanoidtypea_en" />
<arg name="use_pan_tilt" default="false" />
<include file="$(find bioloid_apps)/launch/ceabot/ceabot_base.launch">
<arg name="robot" value="$(arg robot)"/>
<arg name="mtn_file" value="$(arg mtn_file)"/>
<arg name="use_pan_tilt" value="$(arg use_pan_tilt)"/>
</include>
<include file="$(find bioloid_description)/launch/vision_env.launch">
......
......@@ -18,6 +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/ttyS100
joints:
- j_shoulder_r
- j_shoulder_l
......
bioloid:
# Publish all joint states -----------------------------------
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 50
bioloid_cm510_cont:
type: effort_controllers/BioloidControllerCM510
adc2_frame: IR1_link
adc3_frame: gyro_x
adc4_frame: gyro_y
exp_adc1_frame: IR2_link
exp_adc8_frame: IR3_link
exp_board_id: 192
exp_gpio0_frame: left_front_fwd_ir_link
exp_gpio1_frame: left_front_dwn_ir_link
exp_gpio2_frame: left_lateral_dwn_ir_link
exp_gpio3_frame: right_front_fwd_ir_link
exp_gpio4_frame: right_front_dwn_ir_link
exp_gpio5_frame: right_lateral_dwn_ir_link
pan_servo_id: 19
tilt_servo_id: 20
serial_console_device: /dev/pts/19
joints:
- j_shoulder_r
- j_shoulder_l
- j_high_arm_r
- j_high_arm_l
- j_low_arm_r
- j_low_arm_l
- j_pelvis_yaw_r
- j_pelvis_yaw_l
- j_pelvis_roll_r
- j_pelvis_roll_l
- j_pelvis_pitch_r
- j_pelvis_pitch_l
- j_knee_r
- j_knee_l
- j_ankle_pitch_r
- j_ankle_pitch_l
- j_ankle_roll_r
- j_ankle_roll_l
- j_pan
- j_tilt
gains:
j_shoulder_l:
punch: 32
margin: 1
slope: 32
max_torque: 1.5
j_high_arm_l:
punch: 32
margin: 1
slope: 32
max_torque: 1.5
j_low_arm_l:
punch: 32
margin: 1
slope: 32
max_torque: 1.5
j_shoulder_r:
punch: 32
margin: 1
slope: 32
max_torque: 1.5
j_high_arm_r:
punch: 32
margin: 1
slope: 32
max_torque: 1.5
j_low_arm_r:
punch: 32
margin: 1
slope: 32
max_torque: 1.5
j_pelvis_yaw_l:
punch: 32
margin: 1
slope: 32
max_torque: 1.5
j_pelvis_roll_l:
punch: 32
margin: 1
slope: 32
max_torque: 1.5
j_pelvis_pitch_l:
punch: 32
margin: 1
slope: 32
max_torque: 1.5
j_knee_l:
punch: 32
margin: 1
slope: 32
max_torque: 1.5
j_ankle_pitch_l:
punch: 32
margin: 1
slope: 32
max_torque: 1.5
j_ankle_roll_l:
punch: 32
margin: 1
slope: 32
max_torque: 1.5
j_pelvis_yaw_r:
punch: 32
margin: 1
slope: 32
max_torque: 1.5
j_pelvis_roll_r:
punch: 32
margin: 1
slope: 32
max_torque: 1.5
j_pelvis_pitch_r:
punch: 32
margin: 1
slope: 32
max_torque: 1.5
j_knee_r:
punch: 32
margin: 1
slope: 32
max_torque: 1.5
j_ankle_pitch_r:
punch: 32
margin: 1
slope: 32
max_torque: 1.5
j_ankle_roll_r:
punch: 32
margin: 1
slope: 32
max_torque: 1.5
j_pan:
punch: 32
margin: 1
slope: 32
max_torque: 1.5
j_tilt:
punch: 32
margin: 1
slope: 32
max_torque: 1.5
......@@ -2,9 +2,16 @@
<arg name="mtn_file" default="bio_prm_humanoidtypea_en" />
<arg name="robot" default="bioloid_ceabot" />
<arg name="use_pan_tilt" default="false" />
<!-- Load joint controller configurations from YAML file to parameter server -->
<rosparam file="$(find bioloid_control)/config/$(arg robot).yaml" command="load"/>
<group if="$(arg use_pan_tilt)">
<rosparam file="$(find bioloid_control)/config/$(arg robot)_pan_tilt.yaml" command="load"/>
</group>
<group unless="$(arg use_pan_tilt)">
<rosparam file="$(find bioloid_control)/config/$(arg robot).yaml" command="load"/>
</group>
<param name="motions_file" value="$(find bioloid_controller_cm510)/motions/$(arg mtn_file).mtn"/>
......
......@@ -8,9 +8,11 @@
/* firmare headers */
#include "cm510.h"
#include "dynamixel_master.h"
#include "motion_manager.h"
#include "motion_pages.h"
#include "exp_board.h"
#include "pan_tilt.h"
#include "balance.h"
#include "buzzer.h"
#include "gpio.h"
......@@ -54,14 +56,19 @@ extern "C" {
}
// expansion board external variables
extern int exp_board_id;
extern int exp_board_dyn_id;
extern uint16_t exp_adc_values[EXP_NUM_ADC];
extern uint16_t exp_compass_value;
extern uint8_t exp_gpio_values[EXP_NUM_GPIO];
extern int pan_servo_dyn_id;
extern int tilt_servo_dyn_id;
extern "C" {
extern void exp_board_loop(void);
}
// serial console external variables
extern std::string serial_console_device;
namespace bioloid_controller_cm510
{
namespace internal
......@@ -251,9 +258,19 @@ namespace bioloid_controller_cm510
this->init_exp_board_analog_ports(controller_nh);
this->init_exp_board_gpio_ports(controller_nh);
/* read the expansion board identifier */
exp_board_id=-1;
controller_nh_.getParam("exp_board_id", exp_board_id);
ROS_DEBUG_STREAM_NAMED(name_, "Expansion board identifier: " << exp_board_id);
exp_board_dyn_id=-1;
controller_nh_.getParam("exp_board_id", exp_board_dyn_id);
ROS_DEBUG_STREAM_NAMED(name_, "Expansion board identifier: " << exp_board_dyn_id);
pan_servo_dyn_id=-1;
controller_nh_.getParam("pan_servo_id", pan_servo_dyn_id);
ROS_DEBUG_STREAM_NAMED(name_, "Pan servo identifier: " << pan_servo_dyn_id);
tilt_servo_dyn_id=-1;
controller_nh_.getParam("tilt_servo_id", tilt_servo_dyn_id);
ROS_DEBUG_STREAM_NAMED(name_, "Tilt servo identifier: " << tilt_servo_dyn_id);
serial_console_device="";
controller_nh_.getParam("serial_console_device", serial_console_device);
ROS_DEBUG_STREAM_NAMED(name_, "Serial console device: " << serial_console_device);
/* process the motion file parameters */
try{
......@@ -322,10 +339,11 @@ namespace bioloid_controller_cm510
init_buzzer();
init_user_time();
/* call the user initialization function */
user_init();
dyn_master_init();
manager_init(num_servos);
if(exp_board_id!=-1)
exp_board_init(exp_board_id);
if(exp_board_dyn_id!=-1)
exp_board_init(exp_board_dyn_id);
user_init();
action_set_page(31);
action_start_page();
first=false;
......@@ -350,8 +368,8 @@ namespace bioloid_controller_cm510
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);
this->compliances_[i].slope=1<<(manager_servos[i+1].slope&0x0F);
target_angles[i] = ((((manager_servos[i+1].current_value+balance_offsets[i+1]-manager_servos[i+1].center_value)*300.0/1023.0)*3.14159)/180.0);
const double command = this->compliance_control(this->compliances_[i],target_angles[i]-real_angles[i]);
this->joints_[i].setCommand(command);
}
......@@ -450,7 +468,7 @@ namespace bioloid_controller_cm510
adc_values[ADC_PORT_6]=value;
}
/* handle the expansion board analog ports */
if(exp_board_id!=-1)
if(exp_board_dyn_id!=-1)
{
const double Vmax=5.0,Vmin=0.5;
// compute the range value in volts
......@@ -524,7 +542,6 @@ namespace bioloid_controller_cm510
else if(frame_name==this->exp_gpio18_frame)
exp_gpio_values[18]=threshold;
}
std::cout << (int)exp_gpio_values[0] << "," << (int)exp_gpio_values[1] << "," << (int)exp_gpio_values[2] << "," << (int)exp_gpio_values[3] << "," << (int)exp_gpio_values[4] << "," << (int)exp_gpio_values[5] << std::endl;
}
template <class HardwareInterface>
......
......@@ -3,8 +3,5 @@
#include <inttypes.h>
#include <avr/iomxx0_1.h>
#include <stdio.h>
FILE * fdevopen(int(*put)(char, FILE *),int(*get)(FILE *));
#endif
......@@ -2,10 +2,6 @@
#include <inttypes.h>
#include <avr/io.h>
FILE * fdevopen(int(*put)(char, FILE *),int(*get)(FILE *))
{
}
uint8_t PINA;
uint8_t DDRA;
uint8_t PORTA;
......
......@@ -9,7 +9,10 @@
/* external variables for the gazebo controller */
int num_servos;
double real_angles[PAGE_MAX_NUM_SERVOS];
extern int exp_board_id;
int exp_board_dyn_id;
int pan_servo_dyn_id;
int tilt_servo_dyn_id;
extern uint16_t exp_adc_values[EXP_NUM_ADC];
extern uint16_t exp_adc_avg_values[EXP_NUM_ADC];
extern uint16_t exp_compass_value;
......@@ -17,6 +20,7 @@ extern uint16_t exp_compass_avg_value;
extern uint8_t exp_gpio_values[EXP_NUM_GPIO];
/* private variables */
uint8_t dyn_master_initialized=0x00;
uint8_t dyn_master_tx_buffer[DYN_MASTER_MAX_TX_BUFFER_LEN];
uint8_t dyn_master_sent_bytes;
volatile uint8_t dyn_master_sent_done;
......@@ -72,6 +76,12 @@ uint8_t dyn_master_wait_reception(void)
/* public functions */
void dyn_master_init(void)
{
dyn_master_initialized=0x01;
}
uint8_t dyn_master_is_init(void)
{
return dyn_master_initialized;
}
void dyn_master_set_rx_timeout(uint16_t timeout_us)
......@@ -124,7 +134,7 @@ void dyn_master_scan(uint8_t *num,uint8_t *ids)
uint8_t dyn_master_ping(uint8_t id)
{
if((id!=0 && id<=num_servos) || id==(uint8_t)exp_board_id)
if((id!=0 && id<=num_servos) || id==(uint8_t)exp_board_dyn_id)
return DYN_SUCCESS;
else
return DYN_COMM_ERROR;
......@@ -132,10 +142,18 @@ uint8_t dyn_master_ping(uint8_t id)
uint8_t dyn_master_read_byte(uint8_t id,uint16_t address,uint8_t *data)
{
if(id==(uint8_t)exp_board_id)
if(id==(uint8_t)exp_board_dyn_id)
{
if(address==Hardware_available)
*data=0x03;// PWM and DAC present
else if(address==NUM_PWM_SERVOS)
{
*data=0;
if(pan_servo_dyn_id!=0xFF)
(*data)++;
if(tilt_servo_dyn_id!=0xFF)
(*data)++;
}
}
return DYN_SUCCESS;
......@@ -143,7 +161,7 @@ uint8_t dyn_master_read_byte(uint8_t id,uint16_t address,uint8_t *data)
uint8_t dyn_master_read_word(uint8_t id,uint16_t address,uint16_t *data)
{
if(id>=1 && id<=num_servos)
if(id>=1 && id<=18)
{
if(address==MODEL_NUM)
*data=0x000C;
......@@ -156,7 +174,20 @@ uint8_t dyn_master_read_word(uint8_t id,uint16_t address,uint16_t *data)
else
*data=0x0000;
}
else if(id==(uint8_t)exp_board_id)
else if(id==(uint8_t)pan_servo_dyn_id || id==(uint8_t)tilt_servo_dyn_id)
{
if(address==MODEL_NUM)
*data=0x0C00;
else if(address==CURRENT_POS)
*data=((((real_angles[id-1]*180.0)/3.143159)+150.0)*1023.0)/300.0;
else if(address==CW_ANGLE_LIM)
*data=205;
else if(address==CCW_ANGLE_LIM)
*data=818;
else
*data=0x0000;
}
else if(id==(uint8_t)exp_board_dyn_id)
{
if(address==ADC_CHANNEL0_L)
*data=exp_adc_values[0];
......@@ -241,7 +272,7 @@ uint8_t dyn_master_read_table(uint8_t id,uint16_t address,uint16_t length,uint8_
{
unsigned int i=0;
if(id==(uint8_t)exp_board_id)
if(id==(uint8_t)exp_board_dyn_id)
{
data[0]=exp_gpio_values[0];
data[1]=exp_gpio_values[1];
......
#include "motion_pages.h"
#include <stdio.h>
TActionPage pages[MAX_PAGES];
......
#include "comm_cfg.h"
#include "serial_console.h"
#include <stdio.h>
/* private functions */
void serial_console_set_baudrate(uint32_t baudrate)
{
}
int serial_console_putchar(char c,FILE *dev)
{
}
int serial_console_getchar(FILE *dev)
{
return 0x00;
}
/* pubic functions */
void serial_console_init(uint32_t baudrate)
{
}
#include <iostream>
#include <stdarg.h>
#include "comm_cfg.h"
#include "serial_console.h"
#include "commexceptions.h"
#include "rs232.h"
extern "C" int vsprintf (char * s, const char * format, va_list arg );
extern "C" int vfprintf ( FILE * stream, const char * format, va_list arg );
extern FILE *stdout;
// private variables
CRS232 serial_console("bioloid_serial_port");
std::string serial_console_device;
TRS232_config serial_console_config;
bool rs232_enabled;
/* private functions */
void serial_console_set_baudrate(uint32_t baudrate)
{
serial_console_config.baud=baudrate;
try{
serial_console.config(&serial_console_config);
}catch(CCommException &e){
std::cout << e.what() << std::endl;
}
}
int serial_console_getchar(FILE *dev)
{
CEventServer *event_server=CEventServer::instance();
std::list<std::string> events;
unsigned char byte;
events.push_back(serial_console.get_rx_event_id());
event_server->wait_first(events);
serial_console.read(&byte,1);
return byte;
}
/*int printf(const char *fmt, ...)
{
va_list ap;
int i;
std::cout << "printf" << std::endl;
if(rs232_enabled)
{
char out_buff[256];
va_start(ap,fmt);
i=vsprintf(out_buff,fmt,ap);
va_end(ap);
serial_console.write((unsigned char *)out_buff,i);
}
else
{
va_start(ap,fmt);
i = vfprintf(stdout, fmt, ap);
va_end(ap);
}
return i;
}*/
/* pubic functions */
void serial_console_init(uint32_t baudrate)
{
serial_console_config.baud=baudrate;
serial_console_config.num_bits=8;
serial_console_config.parity=none;
serial_console_config.stop_bits=1;
try{
std::cout << serial_console_device << std::endl;
serial_console.open((void *)&serial_console_device);
serial_console.config(&serial_console_config);
rs232_enabled=true;
}catch(CCommException &e){
std::cout << e.what() << std::endl;
rs232_enabled=false;
}
}
<launch>
<arg name="robot" default="bioloid" />
<arg name="use_pan_tilt" default="false" />
<!-- Convert an xacro and put on parameter server -->
<param name="robot_description"
command="$(find xacro)/xacro.py '$(find bioloid_description)/urdf/$(arg robot).xacro'" />
<param name="robot_description" command="$(find xacro)/xacro.py '$(find bioloid_description)/urdf/$(arg robot).xacro' use_pan_tilt:=$(arg use_pan_tilt)"/>
<node pkg="robot_state_publisher" type="state_publisher" name="bioloid_tf_broadcaster">
<param name="tf_prefix" type="string" value="/bioloid"/>
......
......@@ -2,14 +2,17 @@
<arg name="robot" default="bioloid_ceabot" />
<arg name="mtn_file" default="bio_prm_humanoidtypea_en" />
<arg name="use_pan_tilt" default="false" />
<include file="$(find bioloid_gazebo)/launch/bioloid_gazebo.launch">
<arg name="robot" value="$(arg robot)"/>
<arg name="use_pan_tilt" value="$(arg use_pan_tilt)" />
</include>
<include file="$(find bioloid_control)/launch/bioloid_cm510_control.launch">
<arg name="mtn_file" value="$(arg mtn_file)"/>
<arg name="robot" value="$(arg robot)"/>
<arg name="use_pan_tilt" value="$(arg use_pan_tilt)" />
</include>
</launch>
<launch>
<arg name="robot" default="bioloid" />
<arg name="use_pan_tilt" default="false" />
<!-- Convert an xacro and put on parameter server -->
<param name="robot_description"
command="$(find xacro)/xacro.py '$(find bioloid_description)/urdf/$(arg robot).xacro'" />
command="$(find xacro)/xacro.py '$(find bioloid_description)/urdf/$(arg robot).xacro' use_pan_tilt:=$(arg use_pan_tilt)" />
<node pkg="robot_state_publisher" type="state_publisher" name="bioloid_tf_broadcaster">
<param name="tf_prefix" type="string" value="/bioloid"/>
......