diff --git a/bioloid_control/config/bioloid_cm510_control.yaml b/bioloid_control/config/bioloid_ceabot.yaml similarity index 100% rename from bioloid_control/config/bioloid_cm510_control.yaml rename to bioloid_control/config/bioloid_ceabot.yaml diff --git a/bioloid_control/launch/bioloid_cm510_control.launch b/bioloid_control/launch/bioloid_cm510_control.launch index 40e308bb40e142956e802b5da50e83cae5fda928..a8d343c940237e3af0f40e5ac2741d49dc89dc91 100644 --- a/bioloid_control/launch/bioloid_cm510_control.launch +++ b/bioloid_control/launch/bioloid_cm510_control.launch @@ -1,9 +1,10 @@ <launch> <arg name="mtn_file" default="bio_prm_humanoidtypea_en" /> + <arg name="robot" default="bioloid_ceabot" /> <!-- Load joint controller configurations from YAML file to parameter server --> - <rosparam file="$(find bioloid_control)/config/bioloid_cm510_control.yaml" command="load"/> + <rosparam file="$(find bioloid_control)/config/$(arg robot).yaml" command="load"/> <param name="motions_file" value="$(find bioloid_controller_cm510)/motions/$(arg mtn_file).mtn"/> diff --git a/bioloid_description/launch/bioloid_cm510_sim.launch b/bioloid_description/launch/bioloid_cm510_sim.launch index 286663bf584c8a550edd8b7e45a7c55562522348..b0bfa7b2610218ca46d389f6b7138ce23e352bb9 100644 --- a/bioloid_description/launch/bioloid_cm510_sim.launch +++ b/bioloid_description/launch/bioloid_cm510_sim.launch @@ -9,6 +9,7 @@ <include file="$(find bioloid_control)/launch/bioloid_cm510_control.launch"> <arg name="mtn_file" value="$(arg mtn_file)"/> + <arg name="robot" value="$(arg robot)"/> </include> </launch> diff --git a/bioloid_description/urdf/ceabot/obstacle.xacro b/bioloid_description/urdf/ceabot/obstacle.xacro index e16aea1bb245f94872790eb6c515fc5af9ac78dd..d6b8eb28b81aa47ddb58bb65b1e2e789e47b3836 100644 --- a/bioloid_description/urdf/ceabot/obstacle.xacro +++ b/bioloid_description/urdf/ceabot/obstacle.xacro @@ -26,7 +26,7 @@ </collision> </link> - <joint name="${name}_joint" type="fixed"> + <joint name="${name}_joint" type="floating"> <origin xyz="${grid_x*0.25+0.13} ${grid_y*0.25+0.63} 0" rpy="1.5707 0 0"/> <parent link="${parent}_link"/> <child link="${name}_link"/> @@ -46,10 +46,27 @@ <xacro:qrcode name="${name}_east" parent="${name}" code="${east_code}"> <origin xyz="-0.126 0.375 0.0" rpy="3.14159 0 -1.5707" /> - </xacro:qrcode>--> + </xacro:qrcode> <gazebo reference="${name}_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}_joint"> + <implicitSpringDamper>true</implicitSpringDamper> + <stopCfm>0.0</stopCfm> + <stopErp>0.0</stopErp> </gazebo> + </xacro:macro> </root> diff --git a/bioloid_description/urdf/ceabot/vision_env.xacro b/bioloid_description/urdf/ceabot/vision_env.xacro index f792a684a39500c00bf195a20b40bbd8b56f622a..39b8d7b45dddfa2df08bcaef579c0eaf12ac4695 100755 --- a/bioloid_description/urdf/ceabot/vision_env.xacro +++ b/bioloid_description/urdf/ceabot/vision_env.xacro @@ -10,7 +10,7 @@ <xacro:obstacle_vis name="obstacle1" parent="obstacle_base" center_x="1.015" center_y="1.265" distance="0.75" angle="0" cosine="1" sine="0" code="turn_right_45"/> <xacro:obstacle_vis name="obstacle2" parent="obstacle_base" center_x="1.015" center_y="1.265" distance="0.75" angle="0.79" cosine="0.707" sine="0.707" code="turn_left_180"/> <xacro:obstacle_vis name="obstacle3" parent="obstacle_base" center_x="1.015" center_y="1.265" distance="0.75" angle="3.14159" cosine="-1" sine="0" code="turn_right_45"/> - <xacro:obstacle_vis name="obstacle4" parent="obstacle_base" center_x="1.015" center_y="1.265" distance="0.75" angle="3.93" cosine="-0.707" sine="0.707" code="turn_right_90"/> + <xacro:obstacle_vis name="obstacle4" parent="obstacle_base" center_x="1.015" center_y="1.265" distance="0.75" angle="3.93" cosine="-0.707" sine="-0.707" code="turn_right_90"/> <xacro:obstacle_vis name="obstacle5" parent="obstacle_base" center_x="1.015" center_y="1.265" distance="0.75" angle="4.71" cosine="0" sine="-1" code="turn_right_180"/> </robot>