diff --git a/bioloid_control/config/bioloid_cm510_control.yaml b/bioloid_control/config/bioloid_cm510_control.yaml
index 0e1b647a50c64737cc85b37d6d07861be6ce8b02..b327a9f52a792943c44379742090ed1fdc0fa72f 100644
--- a/bioloid_control/config/bioloid_cm510_control.yaml
+++ b/bioloid_control/config/bioloid_cm510_control.yaml
@@ -11,6 +11,12 @@ bioloid:
adc4_frame: gyro_y
exp_adc1_frame: IR2_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
joints:
- j_shoulder_r
- j_shoulder_l
diff --git a/bioloid_controller_cm510/include/bioloid_controller_cm510.h b/bioloid_controller_cm510/include/bioloid_controller_cm510.h
index 93f10c5bcc4e73d324f99f40f10f024ae0635d06..e2dfe328cbab4d4a86ecb66f98394decdba36e27 100644
--- a/bioloid_controller_cm510/include/bioloid_controller_cm510.h
+++ b/bioloid_controller_cm510/include/bioloid_controller_cm510.h
@@ -82,6 +82,7 @@ namespace bioloid_controller_cm510
void convert_page_info(void);
void init_controller_analog_ports(ros::NodeHandle &nh);
void init_exp_board_analog_ports(ros::NodeHandle &nh);
+ void init_exp_board_gpio_ports(ros::NodeHandle &nh);
private:
typedef typename HardwareInterface::ResourceHandleType JointHandle;
@@ -110,6 +111,26 @@ namespace bioloid_controller_cm510
std::string exp_adc6_frame;
std::string exp_adc7_frame;
std::string exp_adc8_frame;
+ /* frame associated to each of the GPIO of the expansion board */
+ std::string exp_gpio0_frame;
+ std::string exp_gpio1_frame;
+ std::string exp_gpio2_frame;
+ std::string exp_gpio3_frame;
+ std::string exp_gpio4_frame;
+ std::string exp_gpio5_frame;
+ std::string exp_gpio6_frame;
+ std::string exp_gpio7_frame;
+ std::string exp_gpio8_frame;
+ std::string exp_gpio9_frame;
+ std::string exp_gpio10_frame;
+ std::string exp_gpio11_frame;
+ std::string exp_gpio12_frame;
+ std::string exp_gpio13_frame;
+ std::string exp_gpio14_frame;
+ std::string exp_gpio15_frame;
+ std::string exp_gpio16_frame;
+ std::string exp_gpio17_frame;
+ std::string exp_gpio18_frame;
/* motion file handling */
CRobotisMtn motions;
std::string motions_file_name;
diff --git a/bioloid_controller_cm510/include/bioloid_controller_cm510_impl.h b/bioloid_controller_cm510/include/bioloid_controller_cm510_impl.h
index 6468d9a9568fc67401e38085eb186e7c3992d7a1..6d46dfe1e3283babaef725a9ee025bb32895816c 100644
--- a/bioloid_controller_cm510/include/bioloid_controller_cm510_impl.h
+++ b/bioloid_controller_cm510/include/bioloid_controller_cm510_impl.h
@@ -57,6 +57,7 @@ extern "C" {
extern int exp_board_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];
namespace bioloid_controller_cm510
{
@@ -244,6 +245,7 @@ namespace bioloid_controller_cm510
/* read the configuration */
this->init_controller_analog_ports(controller_nh);
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);
@@ -354,12 +356,12 @@ namespace bioloid_controller_cm510
fb_imu_value=300.0;
else if(fb_imu_value<-300.0)
fb_imu_value=-300.0;
- adc_values[BALANCE_GYRO_X_CHANNEL]=((fb_imu_value*512)/300.0)+512;
+ adc_values[BALANCE_GYRO_X_CHANNEL]=((fb_imu_value*256)/300.0)+256;
if(lr_imu_value>300.0)
lr_imu_value=300.0;
else if(lr_imu_value<-300.0)
lr_imu_value=-300.0;
- adc_values[BALANCE_GYRO_Y_CHANNEL]=((lr_imu_value*512)/300.0)+512;
+ adc_values[BALANCE_GYRO_Y_CHANNEL]=((lr_imu_value*256)/300.0)+256;
}
else
{
@@ -378,7 +380,15 @@ namespace bioloid_controller_cm510
unsigned int i=0;
unsigned short int value;
double B,C,voltage;
-
+ std::string frame_name;
+ unsigned char threshold;
+
+ if(msg->header.frame_id.find("/")==std::string::npos)
+ frame_name=msg->header.frame_id;
+ else
+ frame_name=msg->header.frame_id.substr(msg->header.frame_id.find("/")+1);
+
+ /* handle the controller analog ports */
for(i=0;i<NUM_ADC;i++)
{
const double Vmax=3.3,Vmin=0.5;
@@ -387,19 +397,20 @@ namespace bioloid_controller_cm510
C=(100.0-Vmax*msg->min_range*B)/Vmax;
voltage=100.0/(B*msg->range+C);// apply the approximation of the transfer function of the sensor
value=voltage*1023.0/Vmax;// transform it to a digital value
- if(msg->header.frame_id==this->adc1_frame)
+ if(frame_name==this->adc1_frame)
adc_values[ADC_PORT_1]=value;
- else if(msg->header.frame_id==this->adc2_frame)
+ else if(frame_name==this->adc2_frame)
adc_values[ADC_PORT_2]=value;
- else if(msg->header.frame_id==this->adc3_frame)
+ else if(frame_name==this->adc3_frame)
adc_values[ADC_PORT_3]=value;
- else if(msg->header.frame_id==this->adc4_frame)
+ else if(frame_name==this->adc4_frame)
adc_values[ADC_PORT_4]=value;
- else if(msg->header.frame_id==this->adc5_frame)
+ else if(frame_name==this->adc5_frame)
adc_values[ADC_PORT_5]=value;
- else if(msg->header.frame_id==this->adc6_frame)
+ else if(frame_name==this->adc6_frame)
adc_values[ADC_PORT_6]=value;
}
+ /* handle the expansion board analog ports */
if(exp_board_id!=-1)
{
const double Vmax=5.0,Vmin=0.5;
@@ -410,24 +421,70 @@ namespace bioloid_controller_cm510
value=voltage*1023.0/Vmax;// transform it to a digital value
for(i=0;i<EXP_NUM_ADC;i++)
{
- if(msg->header.frame_id==this->exp_adc1_frame)
+ if(frame_name==this->exp_adc1_frame)
exp_adc_values[0]=value;
- else if(msg->header.frame_id==this->exp_adc2_frame)
+ else if(frame_name==this->exp_adc2_frame)
exp_adc_values[1]=value;
- else if(msg->header.frame_id==this->exp_adc3_frame)
+ else if(frame_name==this->exp_adc3_frame)
exp_adc_values[2]=value;
- else if(msg->header.frame_id==this->exp_adc4_frame)
+ else if(frame_name==this->exp_adc4_frame)
exp_adc_values[3]=value;
- else if(msg->header.frame_id==this->exp_adc5_frame)
+ else if(frame_name==this->exp_adc5_frame)
exp_adc_values[4]=value;
- else if(msg->header.frame_id==this->exp_adc6_frame)
+ else if(frame_name==this->exp_adc6_frame)
exp_adc_values[5]=value;
- else if(msg->header.frame_id==this->exp_adc7_frame)
+ else if(frame_name==this->exp_adc7_frame)
exp_adc_values[6]=value;
- else if(msg->header.frame_id==this->exp_adc8_frame)
+ else if(frame_name==this->exp_adc8_frame)
exp_adc_values[7]=value;
}
}
+ /* handle the feet sensors */
+ for(i=0;i<EXP_NUM_GPIO;i++)
+ {
+ if(msg->range<(msg->max_range/2.0))
+ threshold=0x01;
+ else
+ threshold=0x00;
+ if(frame_name==this->exp_gpio0_frame)
+ exp_gpio_values[0]=threshold;
+ else if(frame_name==this->exp_gpio1_frame)
+ exp_gpio_values[1]=threshold;
+ else if(frame_name==this->exp_gpio2_frame)
+ exp_gpio_values[2]=threshold;
+ else if(frame_name==this->exp_gpio3_frame)
+ exp_gpio_values[3]=threshold;
+ else if(frame_name==this->exp_gpio4_frame)
+ exp_gpio_values[4]=threshold;
+ else if(frame_name==this->exp_gpio5_frame)
+ exp_gpio_values[5]=threshold;
+ else if(frame_name==this->exp_gpio6_frame)
+ exp_gpio_values[6]=threshold;
+ else if(frame_name==this->exp_gpio7_frame)
+ exp_gpio_values[7]=threshold;
+ else if(frame_name==this->exp_gpio8_frame)
+ exp_gpio_values[8]=threshold;
+ else if(frame_name==this->exp_gpio9_frame)
+ exp_gpio_values[9]=threshold;
+ else if(frame_name==this->exp_gpio10_frame)
+ exp_gpio_values[10]=threshold;
+ else if(frame_name==this->exp_gpio11_frame)
+ exp_gpio_values[11]=threshold;
+ else if(frame_name==this->exp_gpio12_frame)
+ exp_gpio_values[12]=threshold;
+ else if(frame_name==this->exp_gpio13_frame)
+ exp_gpio_values[13]=threshold;
+ else if(frame_name==this->exp_gpio14_frame)
+ exp_gpio_values[14]=threshold;
+ else if(frame_name==this->exp_gpio15_frame)
+ exp_gpio_values[15]=threshold;
+ else if(frame_name==this->exp_gpio16_frame)
+ exp_gpio_values[16]=threshold;
+ else if(frame_name==this->exp_gpio17_frame)
+ exp_gpio_values[17]=threshold;
+ else if(frame_name==this->exp_gpio18_frame)
+ exp_gpio_values[18]=threshold;
+ }
}
template <class HardwareInterface>
@@ -584,6 +641,67 @@ namespace bioloid_controller_cm510
nh.getParam("exp_adc8_frame", this->exp_adc8_frame);
ROS_DEBUG_STREAM_NAMED(name_, "Expansion board ADC8 associated to sensor: " << this->exp_adc8_frame);
}
+ template <class HardwareInterface>
+ void BioloidControllerCM510<HardwareInterface>::init_exp_board_gpio_ports(ros::NodeHandle &nh)
+ {
+ this->exp_gpio0_frame="none";
+ nh.getParam("exp_gpio0_frame", this->exp_gpio0_frame);
+ ROS_DEBUG_STREAM_NAMED(name_, "Expansion board GPIO0 associated to sensor: " << this->exp_gpio0_frame);
+ this->exp_gpio1_frame="none";
+ nh.getParam("exp_gpio1_frame", this->exp_gpio1_frame);
+ ROS_DEBUG_STREAM_NAMED(name_, "Expansion board GPIO1 associated to sensor: " << this->exp_gpio1_frame);
+ this->exp_gpio2_frame="none";
+ nh.getParam("exp_gpio2_frame", this->exp_gpio2_frame);
+ ROS_DEBUG_STREAM_NAMED(name_, "Expansion board GPIO2 associated to sensor: " << this->exp_gpio2_frame);
+ this->exp_gpio3_frame="none";
+ nh.getParam("exp_gpio3_frame", this->exp_gpio3_frame);
+ ROS_DEBUG_STREAM_NAMED(name_, "Expansion board GPIO3 associated to sensor: " << this->exp_gpio3_frame);
+ this->exp_gpio4_frame="none";
+ nh.getParam("exp_gpio4_frame", this->exp_gpio4_frame);
+ ROS_DEBUG_STREAM_NAMED(name_, "Expansion board GPIO4 associated to sensor: " << this->exp_gpio4_frame);
+ this->exp_gpio5_frame="none";
+ nh.getParam("exp_gpio5_frame", this->exp_gpio5_frame);
+ ROS_DEBUG_STREAM_NAMED(name_, "Expansion board GPIO5 associated to sensor: " << this->exp_gpio5_frame);
+ this->exp_gpio6_frame="none";
+ nh.getParam("exp_gpio6_frame", this->exp_gpio6_frame);
+ ROS_DEBUG_STREAM_NAMED(name_, "Expansion board GPIO6 associated to sensor: " << this->exp_gpio6_frame);
+ this->exp_gpio7_frame="none";
+ nh.getParam("exp_gpio7_frame", this->exp_gpio7_frame);
+ ROS_DEBUG_STREAM_NAMED(name_, "Expansion board GPIO7 associated to sensor: " << this->exp_gpio7_frame);
+ this->exp_gpio8_frame="none";
+ nh.getParam("exp_gpio8_frame", this->exp_gpio8_frame);
+ ROS_DEBUG_STREAM_NAMED(name_, "Expansion board GPIO8 associated to sensor: " << this->exp_gpio8_frame);
+ this->exp_gpio9_frame="none";
+ nh.getParam("exp_gpio9_frame", this->exp_gpio9_frame);
+ ROS_DEBUG_STREAM_NAMED(name_, "Expansion board GPIO9 associated to sensor: " << this->exp_gpio9_frame);
+ this->exp_gpio10_frame="none";
+ nh.getParam("exp_gpio10_frame", this->exp_gpio10_frame);
+ ROS_DEBUG_STREAM_NAMED(name_, "Expansion board GPIO10 associated to sensor: " << this->exp_gpio10_frame);
+ this->exp_gpio11_frame="none";
+ nh.getParam("exp_gpio11_frame", this->exp_gpio11_frame);
+ ROS_DEBUG_STREAM_NAMED(name_, "Expansion board GPIO11 associated to sensor: " << this->exp_gpio11_frame);
+ this->exp_gpio12_frame="none";
+ nh.getParam("exp_gpio12_frame", this->exp_gpio12_frame);
+ ROS_DEBUG_STREAM_NAMED(name_, "Expansion board GPIO12 associated to sensor: " << this->exp_gpio12_frame);
+ this->exp_gpio13_frame="none";
+ nh.getParam("exp_gpio13_frame", this->exp_gpio13_frame);
+ ROS_DEBUG_STREAM_NAMED(name_, "Expansion board GPIO13 associated to sensor: " << this->exp_gpio13_frame);
+ this->exp_gpio14_frame="none";
+ nh.getParam("exp_gpio14_frame", this->exp_gpio14_frame);
+ ROS_DEBUG_STREAM_NAMED(name_, "Expansion board GPIO14 associated to sensor: " << this->exp_gpio14_frame);
+ this->exp_gpio15_frame="none";
+ nh.getParam("exp_gpio15_frame", this->exp_gpio15_frame);
+ ROS_DEBUG_STREAM_NAMED(name_, "Expansion board GPIO15 associated to sensor: " << this->exp_gpio15_frame);
+ this->exp_gpio16_frame="none";
+ nh.getParam("exp_gpio16_frame", this->exp_gpio16_frame);
+ ROS_DEBUG_STREAM_NAMED(name_, "Expansion board GPIO16 associated to sensor: " << this->exp_gpio16_frame);
+ this->exp_gpio17_frame="none";
+ nh.getParam("exp_gpio17_frame", this->exp_gpio17_frame);
+ ROS_DEBUG_STREAM_NAMED(name_, "Expansion board GPIO17 associated to sensor: " << this->exp_gpio17_frame);
+ this->exp_gpio18_frame="none";
+ nh.getParam("exp_gpio18_frame", this->exp_gpio18_frame);
+ ROS_DEBUG_STREAM_NAMED(name_, "Expansion board GPIO18 associated to sensor: " << this->exp_gpio18_frame);
+ }
} // namespace
#endif // header guard
diff --git a/bioloid_controller_cm510/include/sim/exp_board_sim.h b/bioloid_controller_cm510/include/sim/exp_board_sim.h
index d654a278c02e1728ab28bce064498f31257e84da..45a262484c7485e5df83e291cab82eaddd91b7c7 100644
--- a/bioloid_controller_cm510/include/sim/exp_board_sim.h
+++ b/bioloid_controller_cm510/include/sim/exp_board_sim.h
@@ -8,6 +8,7 @@ extern "C" {
#include <avr/io.h>
#define EXP_NUM_ADC 8
+#define EXP_NUM_GPIO 19
void exp_board_sim_init(void);
void exp_board_sim_loop(void);
diff --git a/bioloid_controller_cm510/src/sim/adc.c b/bioloid_controller_cm510/src/sim/adc.c
index 29200fdc9af419b2d9bf0133df23b96d57b07f26..c5562c1c175c822ac6bd81f14475c0c9f265a4b1 100644
--- a/bioloid_controller_cm510/src/sim/adc.c
+++ b/bioloid_controller_cm510/src/sim/adc.c
@@ -58,10 +58,10 @@ void init_adc(void)
for(i=0;i<NUM_ADC;i++)
{
- adc_values[i]=512;
- adc_avg_values[i]=512;
+ adc_values[i]=256;
+ adc_avg_values[i]=256;
for(j=0;j<ADC_MAX_NUM_SAMPLES;j++)
- adc_ch_data[i][j]=512;
+ adc_ch_data[i][j]=256;
}
adc_current_sample=0;
}
diff --git a/bioloid_controller_cm510/src/sim/dynamixel_master.c b/bioloid_controller_cm510/src/sim/dynamixel_master.c
index 94982057f68d76b83c5bb84852879b49bc654356..c5dcac98dff2aec4390c69666ad619345c165232 100644
--- a/bioloid_controller_cm510/src/sim/dynamixel_master.c
+++ b/bioloid_controller_cm510/src/sim/dynamixel_master.c
@@ -14,6 +14,7 @@ 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;
extern uint16_t exp_compass_avg_value;
+extern uint8_t exp_gpio_values[EXP_NUM_GPIO];
/* private variables */
uint8_t dyn_master_tx_buffer[DYN_MASTER_MAX_TX_BUFFER_LEN];
@@ -193,6 +194,44 @@ uint8_t dyn_master_read_word(uint8_t id,uint16_t address,uint16_t *data)
*data=exp_compass_value;
else if(address==COMPASS_AVG_HEADING_VAL_L)
*data=exp_compass_avg_value;
+ else if(address==GPIO0_data)
+ *data=exp_gpio_values[0];
+ else if(address==GPIO1_data)
+ *data=exp_gpio_values[1];
+ else if(address==GPIO2_data)
+ *data=exp_gpio_values[2];
+ else if(address==GPIO3_data)
+ *data=exp_gpio_values[3];
+ else if(address==GPIO4_data)
+ *data=exp_gpio_values[4];
+ else if(address==GPIO5_data)
+ *data=exp_gpio_values[5];
+ else if(address==GPIO6_data)
+ *data=exp_gpio_values[6];
+ else if(address==GPIO7_data)
+ *data=exp_gpio_values[7];
+ else if(address==GPIO8_data)
+ *data=exp_gpio_values[8];
+ else if(address==GPIO9_data)
+ *data=exp_gpio_values[9];
+ else if(address==GPIO10_data)
+ *data=exp_gpio_values[10];
+ else if(address==GPIO11_data)
+ *data=exp_gpio_values[11];
+ else if(address==GPIO12_data)
+ *data=exp_gpio_values[12];
+ else if(address==GPIO13_data)
+ *data=exp_gpio_values[13];
+ else if(address==GPIO14_data)
+ *data=exp_gpio_values[14];
+ else if(address==GPIO15_data)
+ *data=exp_gpio_values[15];
+ else if(address==GPIO16_data)
+ *data=exp_gpio_values[16];
+ else if(address==GPIO17_data)
+ *data=exp_gpio_values[17];
+ else if(address==GPIO18_data)
+ *data=exp_gpio_values[18];
}
return DYN_SUCCESS;
diff --git a/bioloid_controller_cm510/src/sim/exp_board_sim.c b/bioloid_controller_cm510/src/sim/exp_board_sim.c
index 7527156f4b3d9f9174e925d8dfc1bef1189ce540..7959fb412eb47651052485a02435fec6105dc380 100644
--- a/bioloid_controller_cm510/src/sim/exp_board_sim.c
+++ b/bioloid_controller_cm510/src/sim/exp_board_sim.c
@@ -13,6 +13,8 @@ uint16_t exp_compass_value;
uint16_t exp_compass_avg_value;
uint8_t exp_compass_current_sample;
+uint8_t exp_gpio_values[EXP_NUM_GPIO];
+
void exp_board_compass_avrg(uint16_t heading)
{
static unsigned long int new_average,first_value;
@@ -62,6 +64,9 @@ void exp_board_sim_init(void)
exp_compass_value=0;
exp_compass_avg_value=0;
exp_compass_current_sample=0;
+ /* initialize the expansion board gpio data */
+ for(i=0;i<EXP_NUM_GPIO;i++)
+ exp_gpio_values[i]=0x00;
}
void exp_board_sim_loop(void)
diff --git a/bioloid_description/meshes/bounding_boxes/cny70_bb.stl b/bioloid_description/meshes/bounding_boxes/cny70_bb.stl
new file mode 100644
index 0000000000000000000000000000000000000000..e3ef1eb5536cbc62de80c30f3b56d6c132d32e75
Binary files /dev/null and b/bioloid_description/meshes/bounding_boxes/cny70_bb.stl differ
diff --git a/bioloid_description/meshes/cny70.stl b/bioloid_description/meshes/cny70.stl
new file mode 100644
index 0000000000000000000000000000000000000000..069b736f6734e8ae653bd8bfc6451426628da060
Binary files /dev/null and b/bioloid_description/meshes/cny70.stl differ
diff --git a/bioloid_description/meshes/left_foot_board.stl b/bioloid_description/meshes/left_foot_board.stl
new file mode 100644
index 0000000000000000000000000000000000000000..54942f0efd0396079f68821d4a7f869a8c41d6e1
Binary files /dev/null and b/bioloid_description/meshes/left_foot_board.stl differ
diff --git a/bioloid_description/meshes/right_foot_board.stl b/bioloid_description/meshes/right_foot_board.stl
new file mode 100644
index 0000000000000000000000000000000000000000..567d6bb539f501c27a133637d071f0c4a9060b51
Binary files /dev/null and b/bioloid_description/meshes/right_foot_board.stl differ
diff --git a/bioloid_description/urdf/bioloid_ceabot.xacro b/bioloid_description/urdf/bioloid_ceabot.xacro
index 474d40ea7587dff40a8c60509dd859a74ad83e93..82db14f1ed39e5e1777e3688b18b6a8cc7e229b3 100755
--- a/bioloid_description/urdf/bioloid_ceabot.xacro
+++ b/bioloid_description/urdf/bioloid_ceabot.xacro
@@ -2,12 +2,14 @@
<xacro:include filename="$(find bioloid_description)/urdf/bioloid.xacro" />
<xacro:include filename="$(find bioloid_description)/urdf/sharp_ir.xacro" />
+ <xacro:include filename="$(find bioloid_description)/urdf/feet_ir.xacro" />
- <xacro:sharp_ir name="IR1" parent="base_link" update_rate="100" fov="0.05" min_range="0.1" max_range="0.8">
+ <xacro:sharp_ir name="IR1" parent="base_link" update_rate="20" fov="0.05" min_range="0.1" max_range="0.8">
<origin xyz="0 0.0 0.035" rpy="-1.5707 -1.5707 0" />
</xacro:sharp_ir>
- <xacro:sharp_ir name="IR2" parent="base_link" update_rate="100" fov="0.05" min_range="0.1" max_range="0.8">
+ <xacro:sharp_ir name="IR2" parent="base_link" update_rate="20" fov="0.05" min_range="0.1" max_range="0.8">
<origin xyz="0 -0.05 0.035" rpy="-1.5707 -1.5707 0" />
</xacro:sharp_ir>
+ <xacro:feet_ir left_parent="left_leg_ankle_roll" right_parent="right_leg_ankle_roll" update_rate="20" range="0.01"/>
</robot>
diff --git a/bioloid_description/urdf/cny70_ir.gazebo b/bioloid_description/urdf/cny70_ir.gazebo
new file mode 100644
index 0000000000000000000000000000000000000000..ed1b1f68bc35457fda2ad3346c01f6068727f8fc
--- /dev/null
+++ b/bioloid_description/urdf/cny70_ir.gazebo
@@ -0,0 +1,55 @@
+<?xml version="1.0"?>
+
+<root xmlns:xacro="http://ros.org/wiki/xacro">
+
+ <xacro:macro name="cny70_ir_gazebo" params="name update_rate fov min_range max_range">
+ <gazebo reference="${name}_ir_link">
+ <material>Gazebo/Black</material>
+ <gravity>true</gravity>
+ <self_collide>false</self_collide>
+ <mu1>0.200000</mu1>
+ <mu2>0.200000</mu2>
+ </gazebo>
+
+ <gazebo reference="${name}_ir_joint">
+ <implicitSpringDamper>true</implicitSpringDamper>
+ </gazebo>
+
+ <gazebo reference="${name}_ir_link">
+ <sensor type="ray" name="${name}">
+ <always_on>true</always_on>
+ <update_rate>${update_rate}</update_rate>
+ <visualize>true</visualize>
+ <ray>
+ <scan>
+ <horizontal>
+ <samples>1</samples>
+ <resolution>1</resolution>
+ <min_angle>-${fov/2}</min_angle>
+ <max_angle>${fov/2}</max_angle>
+ </horizontal>
+ <vertical>
+ <samples>1</samples>
+ <resolution>1</resolution>
+ <min_angle>-${fov/2}</min_angle>
+ <max_angle>${fov/2}</max_angle>
+ </vertical>
+ </scan>
+ <range>
+ <min>${min_range}</min>
+ <max>${max_range}</max>
+ <resolution>0.001</resolution>
+ </range>
+ </ray>
+ <plugin name="${name}_range_sensor" filename="libgazebo_ros_range.so">
+ <frameName>/bioloid/${name}_ir_link</frameName>
+ <topicName>/bioloid/sensors/range</topicName>
+ <radiation>infrared</radiation>
+ <fov>${fov}</fov>
+ <gaussianNoise>0.0005</gaussianNoise>
+ <updateRate>${update_rate}</updateRate>
+ </plugin>
+ </sensor>
+ </gazebo>
+ </xacro:macro>
+</root>
diff --git a/bioloid_description/urdf/cny70_ir.xacro b/bioloid_description/urdf/cny70_ir.xacro
new file mode 100644
index 0000000000000000000000000000000000000000..a566bef1c022be53e944cf52100f9bdc77084960
--- /dev/null
+++ b/bioloid_description/urdf/cny70_ir.xacro
@@ -0,0 +1,38 @@
+<?xml version="1.0"?>
+
+<root xmlns:xacro="http://ros.org/wiki/xacro">
+
+ <xacro:include filename="$(find bioloid_description)/urdf/cny70_ir.gazebo" />
+
+ <xacro:macro name="cny70_ir" params="name parent *origin update_rate fov min_range max_range">
+ <!-- IR distance sensors -->
+ <link name="${name}_ir_link">
+ <collision>
+ <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+ <geometry>
+ <mesh filename="package://bioloid_description/meshes/bounding_boxes/cny70_bb.stl" />
+ </geometry>
+ </collision>
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0"/>
+ <geometry>
+ <mesh filename="package://bioloid_description/meshes/cny70.stl"/>
+ </geometry>
+ <material name="black"/>
+ </visual>
+ <inertial>
+ <mass value="0.0007"/>
+ <origin xyz="-0.00307736 0.0000000 0.00000000" rpy="0 0 0"/>
+ <inertia ixx="0.000000001" ixy="0.00000000" ixz="0.000000" iyy="0.000000001" iyz="0.00000000" izz="0.000000001" />
+ </inertial>
+ </link>
+ <joint name="${name}_ir_joint" type="fixed">
+ <xacro:insert_block name="origin" />
+ <parent link="${parent}"/>
+ <child link="${name}_ir_link"/>
+ </joint>
+
+ <xacro:cny70_ir_gazebo name="${name}" update_rate="${update_rate}" fov="${fov}" min_range="${min_range}" max_range="${max_range}"/>
+ </xacro:macro>
+</root>
+
diff --git a/bioloid_description/urdf/feet_ir.xacro b/bioloid_description/urdf/feet_ir.xacro
new file mode 100644
index 0000000000000000000000000000000000000000..93ca7bc818300af66643425bfb5576a9f4d4a0cd
--- /dev/null
+++ b/bioloid_description/urdf/feet_ir.xacro
@@ -0,0 +1,91 @@
+<?xml version="1.0"?>
+
+<root xmlns:xacro="http://ros.org/wiki/xacro">
+
+ <xacro:include filename="$(find bioloid_description)/urdf/cny70_ir.xacro" />
+
+ <xacro:macro name="feet_ir" params="left_parent right_parent update_rate range">
+ <!-- IR distance sensors -->
+ <link name="left_board_link">
+ <collision>
+ <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+ <geometry>
+ <mesh filename="package://bioloid_description/meshes/left_foot_board.stl" />
+ </geometry>
+ </collision>
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0"/>
+ <geometry>
+ <mesh filename="package://bioloid_description/meshes/left_foot_board.stl"/>
+ </geometry>
+ <material name="green"/>
+ </visual>
+ <inertial>
+ <mass value="0.011" />
+ <origin xyz="0.02939017 -0.00080000 0.01656892" rpy="0 0 0"/>
+ <inertia ixx="0.00000152" ixy="0.00000000" ixz="-0.00000061" iyy="0.00000465" iyz="0.00000000" izz="0.00000313" />
+ </inertial>
+ </link>
+
+ <joint name="left_board_joint" type="fixed">
+ <origin xyz="-0.024 -0.0179 0.024" rpy="0 0 0"/>
+ <parent link="${left_parent}"/>
+ <child link="left_board_link"/>
+ </joint>
+
+ <!-- sensor links -->
+ <xacro:cny70_ir name="left_front_fwd" parent="left_board_link" update_rate="${update_rate}" fov="0.05" min_range="0.001" max_range="${range}">
+ <origin xyz="0.0085 -0.0055 0.038" rpy="-1.5707 -1.5707 0" />
+ </xacro:cny70_ir>
+
+ <xacro:cny70_ir name="left_front_dwn" parent="left_board_link" update_rate="${update_rate}" fov="0.05" min_range="0.001" max_range="${range}">
+ <origin xyz="0.0225 -0.012 0.0345" rpy="0 0 -1.5707" />
+ </xacro:cny70_ir>
+
+ <xacro:cny70_ir name="left_lateral_dwn" parent="left_board_link" update_rate="${update_rate}" fov="0.05" min_range="0.001" max_range="${range}">
+ <origin xyz="0.0575 -0.012 0.0065" rpy="0 0 -1.5707" />
+ </xacro:cny70_ir>
+
+ <link name="right_board_link">
+ <collision>
+ <origin xyz="0.0 0.0 0.0" rpy="0 0 0"/>
+ <geometry>
+ <mesh filename="package://bioloid_description/meshes/right_foot_board.stl" />
+ </geometry>
+ </collision>
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0"/>
+ <geometry>
+ <mesh filename="package://bioloid_description/meshes/right_foot_board.stl"/>
+ </geometry>
+ <material name="green"/>
+ </visual>
+ <inertial>
+ <mass value="0.011" />
+ <origin xyz="-0.02939017 -0.00080000 0.01656892" rpy="0 0 0"/>
+ <inertia ixx="0.00000152" ixy="0.00000000" ixz="0.00000061" iyy="0.00000465" iyz="0.00000000" izz="0.00000313" />
+ </inertial>
+ </link>
+
+ <joint name="right_board_joint" type="fixed">
+ <origin xyz="0.024 -0.0179 0.024" rpy="0 0 0"/>
+ <parent link="${right_parent}"/>
+ <child link="right_board_link"/>
+ </joint>
+
+ <!-- sensor links -->
+ <xacro:cny70_ir name="right_front_fwd" parent="right_board_link" update_rate="${update_rate}" fov="0.05" min_range="0.001" max_range="${range}">
+ <origin xyz="-0.0085 -0.0055 0.038" rpy="-1.5707 -1.5707 0" />
+ </xacro:cny70_ir>
+
+ <xacro:cny70_ir name="right_front_dwn" parent="right_board_link" update_rate="${update_rate}" fov="0.05" min_range="0.001" max_range="${range}">
+ <origin xyz="-0.0225 -0.012 0.0345" rpy="0 0 -1.5707" />
+ </xacro:cny70_ir>
+
+ <xacro:cny70_ir name="right_lateral_dwn" parent="right_board_link" update_rate="${update_rate}" fov="0.05" min_range="0.001" max_range="${range}">
+ <origin xyz="-0.0575 -0.012 0.0065" rpy="0 0 -1.5707" />
+ </xacro:cny70_ir>
+
+ </xacro:macro>
+</root>
+
diff --git a/bioloid_description/urdf/sharp_ir.gazebo b/bioloid_description/urdf/sharp_ir.gazebo
index b068a9344a098c50387c15cb88019c0de9e2ab2f..117d9f436704c21a813ccbb6c66ef5cd104d48fd 100644
--- a/bioloid_description/urdf/sharp_ir.gazebo
+++ b/bioloid_description/urdf/sharp_ir.gazebo
@@ -42,7 +42,7 @@
</range>
</ray>
<plugin name="${name}_range_sensor" filename="libgazebo_ros_range.so">
- <frameName>${name}_link</frameName>
+ <frameName>/bioloid/${name}_link</frameName>
<topicName>/bioloid/sensors/range</topicName>
<radiation>infrared</radiation>
<fov>${fov}</fov>