Solved a problem with the obstacles in the obstacles environment.

The obstacles can now be moved by the robot.

bioloid_controller_cm510/CMakeLists.txt

@@ -10,6 +10,7 @@ find_package(catkin REQUIRED cmake_modules roscpp urdf controller_interface hard
 # find_package(Boost REQUIRED COMPONENTS system)
 
 FIND_PACKAGE(iriutils REQUIRED)
+FIND_PACKAGE(comm REQUIRED)
 FIND_PACKAGE(robotis_bin_parser REQUIRED)
 FIND_PACKAGE(robotis_mtn REQUIRED)
 FIND_PACKAGE(robotis_mtn_parser REQUIRED)
@@ -91,7 +92,7 @@ catkin_package(
 ###########
 
 SET(BIOLOID_FW_PATH ~/humanoids/cm510_controller_fw)
-SET(ProjectPath ~/humanoids/cm510_controller_fw/examples/dexter/dexter_robot/proves/stairs)
+SET(ProjectPath ~/humanoids/cm510_controller_fw/examples/pan_tilt)
 
 ## Specify additional locations of header files
 ## Your package locations should be listed before other locations
@@ -102,10 +103,11 @@ include_directories(${BIOLOID_FW_PATH}/motion/include)
 include_directories(${BIOLOID_FW_PATH}/communications/include)
 include_directories(${BIOLOID_FW_PATH}/dyn_devices/include)
 include_directories(${iriutils_INCLUDE_DIR})
+include_directories(${comm_INCLUDE_DIR})
 INCLUDE_DIRECTORIES(${robotis_bin_parser_INCLUDE_DIR})
 INCLUDE_DIRECTORIES(${robotis_mtn_parser_INCLUDE_DIR})
 INCLUDE_DIRECTORIES(${robotis_mtn_INCLUDE_DIR})
-INCLUDE_DIRECTORIES(${ProjectPath}/../movements/)
+#INCLUDE_DIRECTORIES(${ProjectPath}/../movements/)
 
 ## Declare a cpp library
 # add_library(bioloid_controller
@@ -116,7 +118,8 @@ add_library(${PROJECT_NAME} src/bioloid_controller_cm510.cpp
                             ${BIOLOID_FW_PATH}/motion/src/motion_manager.c
                             ${BIOLOID_FW_PATH}/motion/src/action.c
                             ${BIOLOID_FW_PATH}/motion/src/balance.c
-                            ${BIOLOID_FW_PATH}/motion/src/mtn_library.c
+                            ${BIOLOID_FW_PATH}/motion/src/pan_tilt.c
+#                            ${BIOLOID_FW_PATH}/motion/src/mtn_library.c
                             # dynamixel devices modules
                             ${BIOLOID_FW_PATH}/dyn_devices/src/dyn_servos/dyn_servos.c
                             ${BIOLOID_FW_PATH}/dyn_devices/src/exp_board/exp_board.c
@@ -126,9 +129,8 @@ add_library(${PROJECT_NAME} src/bioloid_controller_cm510.cpp
                             ${BIOLOID_FW_PATH}/controllers/src/buzzer.c
                             # communications modules
                             ${BIOLOID_FW_PATH}/communications/src/dynamixel.c
-                            ${BIOLOID_FW_PATH}/communications/src/serial_console.c
                             # main application module
-		            						${ProjectPath}/main_2.c
+                            ${ProjectPath}/pan_tilt.c
                             # AVR simulation modules
                             src/sim/avr_delay.c
                             src/sim/avr_registers.c
@@ -137,6 +139,7 @@ add_library(${PROJECT_NAME} src/bioloid_controller_cm510.cpp
                             src/sim/adc.c
                             src/sim/user_time.c
                             src/sim/exp_board_sim.c
+                            src/sim/serial_console.cpp
                             include/bioloid_controller_cm510.h
                             include/bioloid_controller_cm510_impl.h)
 
@@ -152,6 +155,7 @@ add_library(${PROJECT_NAME} src/bioloid_controller_cm510.cpp
 # )
 target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
 target_link_libraries(${PROJECT_NAME} ${iriutils_LIBRARY})
+target_link_libraries(${PROJECT_NAME} ${comm_LIBRARY})
 TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${robotis_bin_parser_LIBRARY})
 TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${robotis_mtn_parser_LIBRARY})
 TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${robotis_mtn_LIBRARY})

bioloid_description/urdf/ceabot/obstacle.xacro

@@ -49,7 +49,7 @@
     </xacro:qrcode>
 
     <gazebo reference="${name}_link">
-      <gravity>true</gravity>
+<!--      <gravity>true</gravity>
       <selfCollide>false</selfCollide>
       <maxContacts>10</maxContacts>
       <dampingFactor>0.2</dampingFactor>
@@ -58,14 +58,14 @@
       <mu1>0.200000</mu1>
       <mu2>0.200000</mu2>
       <kp>0.0</kp>
-      <kd>0.0</kd>
+      <kd>0.0</kd>-->
     </gazebo>
 
-    <gazebo reference="${name}_joint">
+<!--    <gazebo reference="${name}_joint">
       <implicitSpringDamper>true</implicitSpringDamper>
       <stopCfm>0.0</stopCfm>
       <stopErp>0.0</stopErp>
-    </gazebo>
+    </gazebo>-->
 
   </xacro:macro>
 </root>

bioloid_description/urdf/ceabot/obstacle_base.xacro

@@ -5,11 +5,11 @@
   <xacro:macro name="obstacle_base" params="name">
   <!-- IR distance sensors -->
     <link name="${name}_link">
-<!--      <inertial>
+      <inertial>
         <mass value="50"/>
         <origin xyz="1.01500000 1.26500000 0.04510729" rpy="0 0 0"/>
         <inertia ixx="32.72577613" ixy="0.0" ixz="0.0" iyy="22.18978101" iyz="0.0" izz="53.20566219" />
-      </inertial>-->
+      </inertial>
       <inertial>
         <mass value="1"/>
         <origin xyz="1.01500000 1.26500000 0.04510729" rpy="0 0 0"/>