diff --git a/bioloid_controller_cm510/include/bioloid_controller_cm510_impl.h b/bioloid_controller_cm510/include/bioloid_controller_cm510_impl.h
index 6468d9a9568fc67401e38085eb186e7c3992d7a1..7ccba10f1da5fed0adedf775f7ee002529df07c1 100644
--- a/bioloid_controller_cm510/include/bioloid_controller_cm510_impl.h
+++ b/bioloid_controller_cm510/include/bioloid_controller_cm510_impl.h
@@ -354,12 +354,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
{
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..2c1d9250d50aeead5ddf97ae3af2eaee352f5d76 100755
--- a/bioloid_description/urdf/bioloid_ceabot.xacro
+++ b/bioloid_description/urdf/bioloid_ceabot.xacro
@@ -2,6 +2,7 @@
<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">
<origin xyz="0 0.0 0.035" rpy="-1.5707 -1.5707 0" />
@@ -9,5 +10,6 @@
<xacro:sharp_ir name="IR2" parent="base_link" update_rate="100" 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="100" range="0.005"/>
</robot>
diff --git a/bioloid_description/urdf/cny70_ir.gazebo b/bioloid_description/urdf/cny70_ir.gazebo
new file mode 100644
index 0000000000000000000000000000000000000000..a0b2b85226c528dc74460d06aa32162150a75dc4
--- /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>false</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>${name}_ir_link</frameName>
+ <topicName>/bioloid/sensors/range</topicName>
+ <radiation>infrared</radiation>
+ <fov>${fov}</fov>
+ <gaussianNoise>0.01</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..1798773abe0caa84408e4adfc69b448a585a00b7
--- /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..04dc1afc9242775b041d2388bf1cf9c021bfecf7
--- /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="${range}" 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="${range}" max_range="${range}">
+ <origin xyz="0.0225 -0.012 0.038" 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="${range}" 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="${range}" 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="${range}" max_range="${range}">
+ <origin xyz="-0.0225 -0.012 0.038" 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="${range}" max_range="${range}">
+ <origin xyz="-0.0575 -0.012 0.0065" rpy="0 0 -1.5707" />
+ </xacro:cny70_ir>
+
+ </xacro:macro>
+</root>
+