Added the necessary files for the simulation of the charge environment.

darwin_apps/launch/charge/charge_sim.launch

+<launch>
+
+  <arg name="robot" default="darwin" />
+  <arg name="environment" default="charge_env" />
+
+  <include file="$(find darwin_description)/launch/darwin_sim.launch">
+    <arg name="robot" value="$(arg robot)" />
+  </include>
+
+  <!-- launch dynamic reconfigure -->
+  <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" respawn="false"
+    output="screen"/>
+
+  <include file="$(find darwin_description)/launch/charge_env.launch">
+    <arg name="environment" value="$(arg environment)" />
+  </include>
+
+</launch>
+

darwin_controller/CMakeLists.txt

@@ -119,6 +119,7 @@ add_library(${PROJECT_NAME} src/darwin_controller.cpp
                             ${DARWIN_FW_PATH}/src/joint_motion.c
                             ${DARWIN_FW_PATH}/src/head_tracking.c
                             ${DARWIN_FW_PATH}/src/motion_manager.c
+                            ${DARWIN_FW_PATH}/src/smart_charger.c
                             ${DARWIN_FW_PATH}/src/grippers.c
                             src/sim/stm32_hal.c
                             src/sim/stm32_cortex.c

darwin_description/launch/charge_env.launch

+<launch>
+  <arg name="environment" default="charge_env" />
+
+  <!-- Convert an xacro and put on parameter server -->
+  <param name="charge_environment"
+         command="$(find xacro)/xacro.py '$(find darwin_description)/urdf/charge/$(arg environment).xacro'" />
+
+  <node name="spawn_charge_station" pkg="gazebo_ros" type="spawn_model" args="-param charge_environment -urdf -model charge_station -x 1.0 -y 0.25 -z 0.03 -R 0 -P 0 -Y 3.14159"/>
+
+</launch>

darwin_description/urdf/charge/charge_env.xacro

+<robot name="bioloid" xmlns:xacro="http://www.ros.org/wiki/xacro">
+ 
+  <xacro:include filename="$(find bioloid_description)/urdf/ceabot/obstacle_base.xacro" />
+  <xacro:include filename="$(find darwin_description)/urdf/charge/charge_station.xacro" />
+
+  <xacro:obstacle_base name="obstacle_base"/>
+  <xacro:charge_station name="station" parent="obstacle_base">
+    <origin xyz="0.5 0.5 0.0" rpy="0 0 0" />
+  </xacro:charge_station>
+
+</robot>

darwin_description/urdf/charge/charge_station.xacro

+<?xml version="1.0"?>
+
+<root xmlns:xacro="http://ros.org/wiki/xacro">
+
+  <xacro:include filename="$(find bioloid_description)/urdf/qrcodes/qrcode.xacro" />
+
+  <xacro:macro name="charge_station" params="name parent *origin">
+    <!-- charge station support -->
+    <link name="${name}_support_link">
+      <inertial>
+        <mass value="0.20000000"/>
+        <origin xyz="0.00000000 0.00000000 0.25000000" rpy="0 0 0"/>
+        <inertia ixx="0.00417484" ixy="0.0" ixz="0.0" iyy="0.00417484" iyz="0.0" izz="0.00001634" />
+      </inertial>
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <mesh filename="package://darwin_description/meshes/charge/charge_support.stl"/>
+        </geometry>
+      </visual>
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <mesh filename="package://darwin_description/meshes/charge/charge_support.stl"/>
+        </geometry>
+      </collision>
+    </link>
+
+    <joint name="${name}_support_joint" type="fixed">
+      <xacro:insert_block name="origin" />
+      <parent link="${parent}_link"/>
+      <child link="${name}_support_link"/>
+    </joint>
+
+    <!-- charge station far QR -->
+    <link name="${name}_far_qr_link">
+      <inertial>
+        <mass value="0.10000000"/>
+        <origin xyz="0.00000000 0.00000000 0.000000" rpy="0 0 0"/>
+        <inertia ixx="0.00017000" ixy="0.0" ixz="0.0" iyy="0.00017000" iyz="0.0" izz="0.00017333" />
+      </inertial>
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <mesh filename="package://darwin_description/meshes/charge/far_qr.stl"/>
+        </geometry>
+      </visual>
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <mesh filename="package://darwin_description/meshes/charge/far_qr.stl"/>
+        </geometry>
+      </collision>
+    </link>
+
+    <joint name="${name}_far_qr_joint" type="fixed">
+      <origin xyz="0.0 0.0 0.4" rpy="0 0 0"/>
+      <parent link="${name}_support_link"/>
+      <child link="${name}_far_qr_link"/>
+    </joint>
+
+    <xacro:qrcode name="${name}_north" parent="${name}_far_qr" code="N1" size="0.07">
+      <origin xyz="0.0501 0.0 0.0" rpy="-1.5707 0 1.5707" />
+    </xacro:qrcode>
+
+    <xacro:qrcode name="${name}_south" parent="${name}_far_qr" code="S1" size="0.07">
+      <origin xyz="-0.0501 0.0 0.0" rpy="-1.5707 0 -1.5707" />
+    </xacro:qrcode>
+
+    <xacro:qrcode name="${name}_west" parent="${name}_far_qr" code="W1" size="0.07">
+      <origin xyz="0.0 0.0501 0.0" rpy="1.5707 3.14159 0" />
+    </xacro:qrcode>
+
+    <xacro:qrcode name="${name}_east" parent="${name}_far_qr" code="E1" size="0.07">
+      <origin xyz="0.0 -0.0501 0.0" rpy="1.5707 3.14159 3.14159 " />
+    </xacro:qrcode>
+
+    <!-- charge station connector -->
+    <link name="${name}_connector_link">
+      <inertial>
+        <mass value="0.10000000"/>
+        <origin xyz="0.00000000 0.00000000 0.000000" rpy="0 0 0"/>
+        <inertia ixx="0.00014464" ixy="0.0" ixz="0.0" iyy="0.00014464" iyz="0.0" izz="0.00028761" />
+      </inertial>
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <mesh filename="package://darwin_description/meshes/charge/connector.stl"/>
+        </geometry>
+      </visual>
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <mesh filename="package://darwin_description/meshes/charge/connector.stl"/>
+        </geometry>
+      </collision>
+    </link>
+
+    <joint name="${name}_connector_joint" type="fixed">
+      <origin xyz="0.0 0.0 0.2" rpy="0 0 0"/>
+      <parent link="${name}_support_link"/>
+      <child link="${name}_connector_link"/>
+    </joint>
+
+    <!-- charge station near_qr -->
+    <link name="${name}_near_qr_link">
+      <inertial>
+        <mass value="0.10000000"/>
+        <origin xyz="0.00000000 0.00000000 0.000000" rpy="0 0 0"/>
+        <inertia ixx="0.00008564" ixy="0.0" ixz="0.0" iyy="0.00008564" iyz="0.0" izz="0.00016968" />
+      </inertial>
+      <visual>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <mesh filename="package://darwin_description/meshes/charge/near_qr.stl"/>
+        </geometry>
+      </visual>
+      <collision>
+        <origin xyz="0 0 0" rpy="0 0 0"/>
+        <geometry>
+          <mesh filename="package://darwin_description/meshes/charge/near_qr.stl"/>
+        </geometry>
+      </collision>
+    </link>
+
+    <joint name="${name}_near_qr_joint" type="fixed">
+      <origin xyz="0.0 0.0 0.3" rpy="0 0 0"/>
+      <parent link="${name}_support_link"/>
+      <child link="${name}_near_qr_link"/>
+    </joint>
+
+    <gazebo reference="${name}_support_link">
+<!--      <gravity>true</gravity>
+      <selfCollide>false</selfCollide>
+      <maxContacts>10</maxContacts>
+      <dampingFactor>0.2</dampingFactor>
+      <maxVel>0.0</maxVel>
+      <minDepth>0.0</minDepth>
+      <mu1>0.200000</mu1>
+      <mu2>0.200000</mu2>
+      <kp>0.0</kp>
+      <kd>0.0</kd>-->
+    </gazebo>
+
+<!--    <gazebo reference="${name}_support_joint">
+      <implicitSpringDamper>true</implicitSpringDamper>
+      <stopCfm>0.0</stopCfm>
+      <stopErp>0.0</stopErp>
+    </gazebo>-->
+
+  </xacro:macro>
+</root>