記事一覧

4脚ロボットでmoveit -> gazeboをやってみた(進捗報告)

4脚ロボットでmoveit -> gazeboをやってみた(進捗報告)



こんにちはRoboTakaoです。
前回、練習として3軸のロボットでROSのMoveitからgazeboに繋げてシミュレーションしましたが
今回は以前作った4脚ロボットで試しました。ROS controllerの設定にかなり苦労しました。
今回はわかりやすくするためにworldに固定しているため宙に浮いた状態です。


NX15_ROS_04.jpg


ちなみに静止状態でgazeboでシミュレーションすると、流れて移動してしまう現象が発生
いろいろ試行錯誤していますがまだうまくいっていません。今回は途中経過となります。


Moveitで生成したファイルから以下を変更及び追加しています


nx15
+- launch
+- nx15_simulation.launch
+- urdf
+- nx15.urdf
+- controllers.yaml

nx15_moveit_config
+- config
+- controllers.yaml
+- launch
+- moveit_rviz.launch
+- nx15_all.launch
+- nx15_moveit_controller_manager.launch.xml


以下で実行
roslaunch nx15_moveit_config nx15_all.launch




以下ソースコード

nx15/launch/nx15_simulation.launch

<?xml version="1.0" ?>
<launch>
<param name="robot_description" textfile="$(find nx15)/urdf/nx15.urdf" />
<include file="$(find gazebo_ros)/launch/empty_world.launch" />
<node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model nx15" />
<rosparam file="$(find nx15)/controllers.yaml" command="load"/>
<node name="controller_spawner" pkg="controller_manager" type="spawner" args="leg_lf_controller leg_lr_controller leg_rf_controller leg_rr_controller"/>
<node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>
</launch>


nx15/urdf/nx15urdf
xacro使ってなくてちょっと恥ずかしい…

<?xml version="1.0" ?>
<robot name="nx15">
<material name="blue">
<color rgba="0.0 0.0 1.0 2.0" />
</material>
<material name="red">
<color rgba="1.0 0.0 0.0 2.0" />
</material>
<material name="green">
<color rgba="0.0 1.0 0.0 2.0" />
</material>
<material name="yellow">
<color rgba="1.0 1.0 0.0 2.0" />
</material>
<material name="white">
<color rgba="1.0 1.0 1.0 2.0" />
</material>
<link name="world" />
<joint name="base_joint" type="fixed">
<parent link="world" />
<child link="Body" />
<origin xyz="0 0 0.150" rpy="0 0 0" />
</joint>
<link name="Body">
<visual>
<geometry>
<mesh filename="package://nx15/stl/Body.stl" />
</geometry>
<origin xyz="0 0 0" rpy="0 0 0" />
<material name="blue" />
</visual>
<collision>
<geometry>
<box size="0.076 0.115 0.048" />
</geometry>
<origin xyz="0 0.0575 0.009" rpy="0 0 0" />
<material name="blue" />
</collision>
<inertial>
<origin xyz="0 0.052 0.008" rpy="0 0 0" />
<mass value="0.346" />
<inertia ixx="0.000441846" ixy="0" ixz="0" iyx="0" iyy="0.000227303" iyz="0" izx="0" izy="0" izz="0.00054726" />
</inertial>
</link>
<joint name="joint_LF_1" type="revolute">
<parent link="Body" />
<child link="Leg_Upper_LF" />
<origin xyz="0.023 -0.0355 0" rpy="0 0 0" />
<axis xyz="0 1 0" />
<limit lower="-1.57" upper="1.57" effort="30" velocity="3.142" />
<dybamics damping="10000.0" friction="10000.0" />
</joint>
<link name="Leg_Upper_LF">
<visual>
<geometry>
<mesh filename="package://nx15/stl/Leg_Upper_LH.stl" />
</geometry>
<origin xyz="-0.007 0.035 -0.022" rpy="0 0 0" />
<material name="red" />
</visual>
<collision>
<geometry>
<box size="0.018 0.032 0.0422" />
</geometry>
<origin xyz="0.002 0.019 -0.0009" rpy="0 0 0" />
<material name="red" />
</collision>
<inertial>
<origin xyz="0.007 0.00203 0.0003" rpy="0 0 0" />
<mass value="0.035143" />
<inertia ixx="0.000008214" ixy="0" ixz="0" iyx="0" iyy="0.000006164" iyz="0" izx="0" izy="0" izz="0.000003948" />
</inertial>
</link>
<joint name="joint_LF_2" type="revolute">
<parent link="Leg_Upper_LF" />
<child link="Leg_Mid_LF" />
<origin xyz="0.013 0.021 -0.0093" rpy="0.785398163397 0 0" />
<axis xyz="1 0 0" />
<limit lower="-1.57" upper="1.57" effort="30" velocity="3.142" />
<dybamics damping="10000.0" friction="10000.0" />
</joint>
<link name="Leg_Mid_LF">
<visual>
<geometry>
<mesh filename="package://nx15/stl/Leg_Mid_LH.stl" />
</geometry>
<origin xyz="0.035 0.007 -0.0557" rpy="0 0 0" />
<material name="green" />
</visual>
<collision>
<geometry>
<box size="0.032 0.014 0.0714" />
</geometry>
<origin xyz="0.019 0 -0.020" rpy="0 0 0" />
<material name="green" />
</collision>
<inertial>
<origin xyz="0.0166 0 -0.0229" rpy="0 0 0" />
<mass value="0.039138" />
<inertia ixx="0.000017266" ixy="0" ixz="0" iyx="0" iyy="0.000019967" iyz="0" izx="0" izy="0" izz="0.000003979" />
</inertial>
</link>
<joint name="joint_LF_3" type="revolute">
<parent link="Leg_Mid_LF" />
<child link="Leg_bottom_LF" />
<origin xyz="0.022 0 -0.0707" rpy="-0.785398163397 0 0" />
<axis xyz="1 0 0" />
<limit lower="-1.57" upper="1.57" effort="30" velocity="3.142" />
<dybamics damping="10000.0" friction="10000.0" />
</joint>
<link name="Leg_bottom_LF">
<visual>
<geometry>
<mesh filename="package://nx15/stl/Leg_bottom_LH.stl" />
</geometry>
<origin xyz="0.0045 0 0" rpy="0 0 0" />
<material name="yellow" />
</visual>
<collision>
<geometry>
<mesh filename="package://nx15/stl/Leg_bottom_LH_C.stl" />
</geometry>
<origin xyz="0.0045 0 0" rpy="0 0 0" />
<material name="yellow" />
</collision>
<inertial>
<origin xyz="0.0049 0.0072 -0.0156" rpy="0 0 0" />
<mass value="0.00816" />
<inertia ixx="0.000006633" ixy="0.000000024" ixz="-0.000000094" iyx="0.000000024" iyy="0.000005740" iyz="0.000001963" izx="-0.000000094" izy="0.000001963" izz="0.000000997" />
</inertial>
</link>
<joint name="joint_LF_4" type="revolute">
<parent link="Leg_bottom_LF" />
<child link="Rubber_LF" />
<origin xyz="0.0023 -0.030 -0.063" rpy="0 0 0" />
<axis xyz="1 0 0" />
<limit lower="-1.57" upper="1.57" effort="30" velocity="3.142" />
<dybamics damping="10000.0" friction="10000.0" />
</joint>
<link name="Rubber_LF">
<visual>
<geometry>
<mesh filename="package://nx15/stl/Rubber.stl" />
</geometry>
<origin xyz="0.0002 0 0" rpy="0 0 0" />
<material name="white" />
</visual>
<collision>
<geometry>
<sphere radius="0.0075" />
</geometry>
<origin xyz="0.0022 0 0" rpy="0 0 0" />
<material name="white" />
</collision>
<inertial>
<origin xyz="0.0022 0 0" rpy="0 0 0" />
<mass value="0.0004" />
<inertia ixx="0.000000011" ixy="0" ixz="0" iyx="0" iyy="0.000000006" iyz="0" izx="0" izy="0" izz="0.000000006" />
</inertial>
</link>
<joint name="control_point_joint_LF" type="revolute">
<parent link="Rubber_LF" />
<child link="control_point_LF" />
<origin xyz="0.0022 0 0" rpy="0 0 0" />
<axis xyz="0 1 0" />
<limit lower="-1.57" upper="1.57" effort="30" velocity="3.142" />
</joint>
<link name="control_point_LF">
<visual>
<geometry>
<sphere radius="0.004" />
</geometry>
<origin xyz="0 0 0" rpy="0 0 0" />
<material name="white" />
</visual>
<collision>
<geometry>
<sphere radius="0.004" />
</geometry>
<origin xyz="0 0 0" rpy="0 0 0" />
<material name="white" />
</collision>
<inertial>
<origin xyz="0 0 0" rpy="0 0 0" />
<mass value="0.0001" />
<inertia ixx="0.000000003" ixy="0" ixz="0" iyx="0" iyy="0.000000003" iyz="0" izx="0" izy="0" izz="0.000000003" />
</inertial>
</link>

<joint name="joint_LR_1" type="revolute">
<parent link="Body" />
<child link="Leg_Upper_LR" />
<origin xyz="0.023 0.110 0" rpy="0 0 0" />
<axis xyz="0 1 0" />
<limit lower="-1.57" upper="1.57" effort="30" velocity="3.142" />
<dybamics damping="10000.0" friction="10000.0" />
</joint>
<link name="Leg_Upper_LR">
<visual>
<geometry>
<mesh filename="package://nx15/stl/Leg_Upper_LH.stl" />
</geometry>
<origin xyz="-0.007 0.035 -0.022" rpy="0 0 0" />
<material name="red" />
</visual>
<collision>
<geometry>
<box size="0.018 0.032 0.0422" />
</geometry>
<origin xyz="0.002 0.019 -0.0009" rpy="0 0 0" />
<material name="red" />
</collision>
<inertial>
<origin xyz="0.007 0.00203 0.0003" rpy="0 0 0" />
<mass value="0.035143" />
<inertia ixx="0.000008214" ixy="0" ixz="0" iyx="0" iyy="0.000006164" iyz="0" izx="0" izy="0" izz="0.000003948" />
</inertial>
</link>
<joint name="joint_LR_2" type="revolute">
<parent link="Leg_Upper_LR" />
<child link="Leg_Mid_LR" />
<origin xyz="0.013 0.021 -0.0093" rpy="0.785398163397 0 0" />
<axis xyz="1 0 0" />
<limit lower="-1.57" upper="1.57" effort="30" velocity="3.142" />
<dybamics damping="10000.0" friction="10000.0" />
</joint>
<link name="Leg_Mid_LR">
<visual>
<geometry>
<mesh filename="package://nx15/stl/Leg_Mid_LH.stl" />
</geometry>
<origin xyz="0.035 0.007 -0.0557" rpy="0 0 0" />
<material name="green" />
</visual>
<collision>
<geometry>
<box size="0.032 0.014 0.0714" />
</geometry>
<origin xyz="0.019 0 -0.020" rpy="0 0 0" />
<material name="green" />
</collision>
<inertial>
<origin xyz="0.0166 0 -0.0229" rpy="0 0 0" />
<mass value="0.039138" />
<inertia ixx="0.000017266" ixy="0" ixz="0" iyx="0" iyy="0.000019967" iyz="0" izx="0" izy="0" izz="0.000003979" />
</inertial>
</link>
<joint name="joint_LR_3" type="revolute">
<parent link="Leg_Mid_LR" />
<child link="Leg_bottom_LR" />
<origin xyz="0.022 0 -0.0707" rpy="-0.785398163397 0 0" />
<axis xyz="1 0 0" />
<limit lower="-1.57" upper="1.57" effort="30" velocity="3.142" />
<dybamics damping="10000.0" friction="10000.0" />
</joint>
<link name="Leg_bottom_LR">
<visual>
<geometry>
<mesh filename="package://nx15/stl/Leg_bottom_LH.stl" />
</geometry>
<origin xyz="0.0045 0 0" rpy="0 0 0" />
<material name="yellow" />
</visual>
<collision>
<geometry>
<mesh filename="package://nx15/stl/Leg_bottom_LH_C.stl" />
</geometry>
<origin xyz="0.0045 0 0" rpy="0 0 0" />
<material name="yellow" />
</collision>
<inertial>
<origin xyz="0.0049 0.0072 -0.0156" rpy="0 0 0" />
<mass value="0.00816" />
<inertia ixx="0.000006633" ixy="0.000000024" ixz="-0.000000094" iyx="0.000000024" iyy="0.000005740" iyz="0.000001963" izx="-0.000000094" izy="0.000001963" izz="0.000000997" />
</inertial>
</link>
<joint name="joint_LR_4" type="revolute">
<parent link="Leg_bottom_LR" />
<child link="Rubber_LR" />
<origin xyz="0.0023 -0.030 -0.063" rpy="0 0 0" />
<axis xyz="1 0 0" />
<limit lower="-1.57" upper="1.57" effort="30" velocity="3.142" />
<dybamics damping="10000.0" friction="10000.0" />
</joint>
<link name="Rubber_LR">
<visual>
<geometry>
<mesh filename="package://nx15/stl/Rubber.stl" />
</geometry>
<origin xyz="0.0002 0 0" rpy="0 0 0" />
<material name="white" />
</visual>
<collision>
<geometry>
<sphere radius="0.0075" />
</geometry>
<origin xyz="0.0022 0 0" rpy="0 0 0" />
<material name="white" />
</collision>
<inertial>
<origin xyz="0.0022 0 0" rpy="0 0 0" />
<mass value="0.0004" />
<inertia ixx="0.000000011" ixy="0" ixz="0" iyx="0" iyy="0.000000006" iyz="0" izx="0" izy="0" izz="0.000000006" />
</inertial>
</link>
<joint name="control_point_joint_LR" type="revolute">
<parent link="Rubber_LR" />
<child link="control_point_LR" />
<origin xyz="0.0022 0 0" rpy="0 0 0" />
<axis xyz="0 1 0" />
<limit lower="-1.57" upper="1.57" effort="30" velocity="3.142" />
</joint>
<link name="control_point_LR">
<visual>
<geometry>
<sphere radius="0.004" />
</geometry>
<origin xyz="0 0 0" rpy="0 0 0" />
<material name="white" />
</visual>
<collision>
<geometry>
<sphere radius="0.004" />
</geometry>
<origin xyz="0 0 0" rpy="0 0 0" />
<material name="white" />
</collision>
<inertial>
<origin xyz="0 0 0" rpy="0 0 0" />
<mass value="0.0001" />
<inertia ixx="0.000000003" ixy="0" ixz="0" iyx="0" iyy="0.000000003" iyz="0" izx="0" izy="0" izz="0.000000003" />
</inertial>
</link>

<joint name="joint_RF_1" type="revolute">
<parent link="Body" />
<child link="Leg_Upper_RF" />
<origin xyz="-0.023 -0.0355 0" rpy="0 0 0" />
<axis xyz="0 1 0" />
<limit lower="-1.57" upper="1.57" effort="30" velocity="3.142" />
<dybamics damping="10000.0" friction="10000.0" />
</joint>
<link name="Leg_Upper_RF">
<visual>
<geometry>
<mesh filename="package://nx15/stl/Leg_Upper_RH.stl" />
</geometry>
<origin xyz="0.007 0.035 -0.022" rpy="0 0 0" />
<material name="red" />
</visual>
<collision>
<geometry>
<box size="0.018 0.032 0.0422" />
</geometry>
<origin xyz="-0.002 0.019 -0.0009" rpy="0 0 0" />
<material name="red" />
</collision>
<inertial>
<origin xyz="-0.007 0.00203 0.0003" rpy="0 0 0" />
<mass value="0.035143" />
<inertia ixx="0.000008214" ixy="0" ixz="0" iyx="0" iyy="0.000006164" iyz="0" izx="0" izy="0" izz="0.000003948" />
</inertial>
</link>
<joint name="joint_RF_2" type="revolute">
<parent link="Leg_Upper_RF" />
<child link="Leg_Mid_RF" />
<origin xyz="-0.013 0.021 -0.0093" rpy="0.785398163397 0 0" />
<axis xyz="1 0 0" />
<limit lower="-1.57" upper="1.57" effort="30" velocity="3.142" />
<dybamics damping="10000.0" friction="10000.0" />
</joint>
<link name="Leg_Mid_RF">
<visual>
<geometry>
<mesh filename="package://nx15/stl/Leg_Mid_RH.stl" />
</geometry>
<origin xyz="-0.035 0.007 -0.0557" rpy="0 0 0" />
<material name="green" />
</visual>
<collision>
<geometry>
<box size="0.032 0.014 0.0714" />
</geometry>
<origin xyz="-0.019 0 -0.020" rpy="0 0 0" />
<material name="green" />
</collision>
<inertial>
<origin xyz="-0.0166 0 -0.0229" rpy="0 0 0" />
<mass value="0.039138" />
<inertia ixx="0.000017266" ixy="0" ixz="0" iyx="0" iyy="0.000019967" iyz="0" izx="0" izy="0" izz="0.000003979" />
</inertial>
</link>
<joint name="joint_RF_3" type="revolute">
<parent link="Leg_Mid_RF" />
<child link="Leg_bottom_RF" />
<origin xyz="-0.022 0 -0.0707" rpy="-0.785398163397 0 0" />
<axis xyz="1 0 0" />
<limit lower="-1.57" upper="1.57" effort="30" velocity="3.142" />
<dybamics damping="10000.0" friction="10000.0" />
</joint>
<link name="Leg_bottom_RF">
<visual>
<geometry>
<mesh filename="package://nx15/stl/Leg_bottom_RH.stl" />
</geometry>
<origin xyz="-0.0045 0 0" rpy="0 0 0" />
<material name="yellow" />
</visual>
<collision>
<geometry>
<mesh filename="package://nx15/stl/Leg_bottom_RH_C.stl" />
</geometry>
<origin xyz="-0.0045 0 0" rpy="0 0 0" />
<material name="yellow" />
</collision>
<inertial>
<origin xyz="-0.0049 0.0072 -0.0156" rpy="0 0 0" />
<mass value="0.00816" />
<inertia ixx="0.000006633" ixy="-0.000000024" ixz="0.000000094" iyx="-0.000000024" iyy="0.000005740" iyz="0.000001963" izx="0.000000094" izy="0.000001963" izz="0.000000997" />
</inertial>
</link>
<joint name="joint_RF_4" type="revolute">
<parent link="Leg_bottom_RF" />
<child link="Rubber_RF" />
<origin xyz="-0.0023 -0.030 -0.063" rpy="0 0 0" />
<axis xyz="1 0 0" />
<limit lower="-1.57" upper="1.57" effort="30" velocity="3.142" />
<dybamics damping="10000.0" friction="10000.0" />
</joint>
<link name="Rubber_RF">
<visual>
<geometry>
<mesh filename="package://nx15/stl/Rubber.stl" />
</geometry>
<origin xyz="-0.0002 0 0" rpy="0 0 3.1415926535897931" />
<material name="white" />
</visual>
<collision>
<geometry>
<sphere radius="0.0075" />
</geometry>
<origin xyz="-0.0022 0 0" rpy="0 0 0" />
<material name="white" />
</collision>
<inertial>
<origin xyz="-0.0022 0 0" rpy="0 0 0" />
<mass value="0.0004" />
<inertia ixx="0.000000011" ixy="0" ixz="0" iyx="0" iyy="0.000000006" iyz="0" izx="0" izy="0" izz="0.000000006" />
</inertial>
</link>
<joint name="control_point_joint_RF" type="revolute">
<parent link="Rubber_RF" />
<child link="control_point_RF" />
<origin xyz="-0.0022 0 0" rpy="0 0 0" />
<axis xyz="0 1 0" />
<limit lower="-1.57" upper="1.57" effort="30" velocity="3.142" />
</joint>
<link name="control_point_RF">
<visual>
<geometry>
<sphere radius="0.004" />
</geometry>
<origin xyz="0 0 0" rpy="0 0 0" />
<material name="white" />
</visual>
<collision>
<geometry>
<sphere radius="0.004" />
</geometry>
<origin xyz="0 0 0" rpy="0 0 0" />
<material name="white" />
</collision>
<inertial>
<origin xyz="0 0 0" rpy="0 0 0" />
<mass value="0.0001" />
<inertia ixx="0.000000003" ixy="0" ixz="0" iyx="0" iyy="0.000000003" iyz="0" izx="0" izy="0" izz="0.000000003" />
</inertial>
</link>

<joint name="joint_RR_1" type="revolute">
<parent link="Body" />
<child link="Leg_Upper_RR" />
<origin xyz="-0.023 0.110 0" rpy="0 0 0" />
<axis xyz="0 1 0" />
<limit lower="-1.57" upper="1.57" effort="30" velocity="3.142" />
<dybamics damping="10000.0" friction="10000.0" />
</joint>
<link name="Leg_Upper_RR">
<visual>
<geometry>
<mesh filename="package://nx15/stl/Leg_Upper_RH.stl" />
</geometry>
<origin xyz="0.007 0.035 -0.022" rpy="0 0 0" />
<material name="red" />
</visual>
<collision>
<geometry>
<box size="0.018 0.032 0.0422" />
</geometry>
<origin xyz="-0.002 0.019 -0.0009" rpy="0 0 0" />
<material name="red" />
</collision>
<inertial>
<origin xyz="-0.007 0.00203 0.0003" rpy="0 0 0" />
<mass value="0.035143" />
<inertia ixx="0.000008214" ixy="0" ixz="0" iyx="0" iyy="0.000006164" iyz="0" izx="0" izy="0" izz="0.000003948" />
</inertial>
</link>
<joint name="joint_RR_2" type="revolute">
<parent link="Leg_Upper_RR" />
<child link="Leg_Mid_RR" />
<origin xyz="-0.013 0.021 -0.0093" rpy="0.785398163397 0 0" />
<axis xyz="1 0 0" />
<limit lower="-1.57" upper="1.57" effort="30" velocity="3.142" />
<dybamics damping="10000.0" friction="10000.0" />
</joint>
<link name="Leg_Mid_RR">
<visual>
<geometry>
<mesh filename="package://nx15/stl/Leg_Mid_RH.stl" />
</geometry>
<origin xyz="-0.035 0.007 -0.0557" rpy="0 0 0" />
<material name="green" />
</visual>
<collision>
<geometry>
<box size="0.032 0.014 0.0714" />
</geometry>
<origin xyz="-0.019 0 -0.020" rpy="0 0 0" />
<material name="green" />
</collision>
<inertial>
<origin xyz="-0.0166 0 -0.0229" rpy="0 0 0" />
<mass value="0.039138" />
<inertia ixx="0.000017266" ixy="0" ixz="0" iyx="0" iyy="0.000019967" iyz="0" izx="0" izy="0" izz="0.000003979" />
</inertial>
</link>
<joint name="joint_RR_3" type="revolute">
<parent link="Leg_Mid_RR" />
<child link="Leg_bottom_RR" />
<origin xyz="-0.022 0 -0.0707" rpy="-0.785398163397 0 0" />
<axis xyz="1 0 0" />
<limit lower="-1.57" upper="1.57" effort="30" velocity="3.142" />
<dybamics damping="10000.0" friction="10000.0" />
</joint>
<link name="Leg_bottom_RR">
<visual>
<geometry>
<mesh filename="package://nx15/stl/Leg_bottom_RH.stl" />
</geometry>
<origin xyz="-0.0045 0 0" rpy="0 0 0" />
<material name="yellow" />
</visual>
<collision>
<geometry>
<mesh filename="package://nx15/stl/Leg_bottom_RH_C.stl" />
</geometry>
<origin xyz="-0.0045 0 0" rpy="0 0 0" />
<material name="yellow" />
</collision>
<inertial>
<origin xyz="-0.0049 0.0072 -0.0156" rpy="0 0 0" />
<mass value="0.00816" />
<inertia ixx="0.000006633" ixy="-0.000000024" ixz="0.000000094" iyx="-0.000000024" iyy="0.000005740" iyz="0.000001963" izx="0.000000094" izy="0.000001963" izz="0.000000997" />
</inertial>
</link>
<joint name="joint_RR_4" type="revolute">
<parent link="Leg_bottom_RR" />
<child link="Rubber_RR" />
<origin xyz="-0.0023 -0.030 -0.063" rpy="0 0 0" />
<axis xyz="1 0 0" />
<limit lower="-1.57" upper="1.57" effort="30" velocity="3.142" />
<dybamics damping="10000.0" friction="10000.0" />
</joint>
<link name="Rubber_RR">
<visual>
<geometry>
<mesh filename="package://nx15/stl/Rubber.stl" />
</geometry>
<origin xyz="-0.0002 0 0" rpy="0 0 3.1415926535897931" />
<material name="white" />
</visual>
<collision>
<geometry>
<sphere radius="0.0075" />
</geometry>
<origin xyz="-0.0022 0 0" rpy="0 0 0" />
<material name="white" />
</collision>
<inertial>
<origin xyz="-0.0022 0 0" rpy="0 0 0" />
<mass value="0.0004" />
<inertia ixx="0.000000011" ixy="0" ixz="0" iyx="0" iyy="0.000000006" iyz="0" izx="0" izy="0" izz="0.000000006" />
</inertial>
</link>
<joint name="control_point_joint_RR" type="revolute">
<parent link="Rubber_RR" />
<child link="control_point_RR" />
<origin xyz="-0.0022 0 0" rpy="0 0 0" />
<axis xyz="0 1 0" />
<limit lower="-1.57" upper="1.57" effort="30" velocity="3.142" />
</joint>
<link name="control_point_RR">
<visual>
<geometry>
<sphere radius="0.004" />
</geometry>
<origin xyz="0 0 0" rpy="0 0 0" />
<material name="white" />
</visual>
<collision>
<geometry>
<sphere radius="0.004" />
</geometry>
<origin xyz="0 0 0" rpy="0 0 0" />
<material name="white" />
</collision>
<inertial>
<origin xyz="0 0 0" rpy="0 0 0" />
<mass value="0.0001" />
<inertia ixx="0.000000003" ixy="0" ixz="0" iyx="0" iyy="0.000000003" iyz="0" izx="0" izy="0" izz="0.000000003" />
</inertial>
</link>

<transmission name="trans_joint_LF_1">
<type>transmission_interface/SimpleTransmission</type>
<joint name="joint_LF_1">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
</joint>
<actuator name="joint_LF_1_motor">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="trans_joint_LF_2">
<type>transmission_interface/SimpleTransmission</type>
<joint name="joint_LF_2">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
</joint>
<actuator name="joint_LF_2_motor">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="trans_joint_LF_3">
<type>transmission_interface/SimpleTransmission</type>
<joint name="joint_LF_3">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
</joint>
<actuator name="joint_LF_3_motor">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="trans_joint_LF_4">
<type>transmission_interface/SimpleTransmission</type>
<joint name="joint_LF_4">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
</joint>
<actuator name="joint_LF_4_motor">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="trans_control_point_joint_LF">
<type>transmission_interface/SimpleTransmission</type>
<joint name="control_point_joint_LF">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
</joint>
<actuator name="control_point_joint_LF_motor">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="trans_joint_LR_1">
<type>transmission_interface/SimpleTransmission</type>
<joint name="joint_LR_1">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
</joint>
<actuator name="joint_LR_1_motor">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="trans_joint_LR_2">
<type>transmission_interface/SimpleTransmission</type>
<joint name="joint_LR_2">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
</joint>
<actuator name="joint_LR_2_motor">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="trans_joint_LR_3">
<type>transmission_interface/SimpleTransmission</type>
<joint name="joint_LR_3">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
</joint>
<actuator name="joint_LR_3_motor">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="trans_joint_LR_4">
<type>transmission_interface/SimpleTransmission</type>
<joint name="joint_LR_4">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
</joint>
<actuator name="joint_LR_4_motor">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="trans_control_point_joint_LR">
<type>transmission_interface/SimpleTransmission</type>
<joint name="control_point_joint_LR">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
</joint>
<actuator name="control_point_joint_LR_motor">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="trans_joint_RF_1">
<type>transmission_interface/SimpleTransmission</type>
<joint name="joint_RF_1">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
</joint>
<actuator name="joint_RF_1_motor">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="trans_joint_RF_2">
<type>transmission_interface/SimpleTransmission</type>
<joint name="joint_RF_2">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
</joint>
<actuator name="joint_RF_2_motor">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="trans_joint_RF_3">
<type>transmission_interface/SimpleTransmission</type>
<joint name="joint_RF_3">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
</joint>
<actuator name="joint_RF_3_motor">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="trans_joint_RF_4">
<type>transmission_interface/SimpleTransmission</type>
<joint name="joint_RF_4">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
</joint>
<actuator name="joint_RF_4_motor">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="trans_control_point_joint_RF">
<type>transmission_interface/SimpleTransmission</type>
<joint name="control_point_joint_RF">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
</joint>
<actuator name="control_point_joint_RF_motor">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="trans_joint_RR_1">
<type>transmission_interface/SimpleTransmission</type>
<joint name="joint_RR_1">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
</joint>
<actuator name="joint_RR_1_motor">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="trans_joint_RR_2">
<type>transmission_interface/SimpleTransmission</type>
<joint name="joint_RR_2">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
</joint>
<actuator name="joint_RR_2_motor">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="trans_joint_RR_3">
<type>transmission_interface/SimpleTransmission</type>
<joint name="joint_RR_3">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
</joint>
<actuator name="joint_RR_3_motor">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="trans_joint_RR_4">
<type>transmission_interface/SimpleTransmission</type>
<joint name="joint_RR_4">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
</joint>
<actuator name="joint_RR_4_motor">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<transmission name="trans_control_point_joint_RR">
<type>transmission_interface/SimpleTransmission</type>
<joint name="control_point_joint_RR">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
</joint>
<actuator name="control_point_joint_RR_motor">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<gazebo reference="Rubber_LF">
<kp>1000000.0</kp>
<kd>100.0</kd>
<mu1>1.5</mu1>
<mu2>1.5</mu2>
<fdir1>1 0 0</fdir1>
<maxVel>1.0</maxVel>
<minDepth>0.001</minDepth>
</gazebo>
<gazebo reference="Rubber_LR">
<kp>1000000.0</kp>
<kd>100.0</kd>
<mu1>1.5</mu1>
<mu2>1.5</mu2>
<fdir1>1 0 0</fdir1>
<maxVel>1.0</maxVel>
<minDepth>0.001</minDepth>
</gazebo>
<gazebo reference="Rubber_RF">
<kp>1000000.0</kp>
<kd>100.0</kd>
<mu1>1.5</mu1>
<mu2>1.5</mu2>
<fdir1>1 0 0</fdir1>
<maxVel>1.0</maxVel>
<minDepth>0.001</minDepth>
</gazebo>
<gazebo reference="Rubber_RR">
<kp>1000000.0</kp>
<kd>100.0</kd>
<mu1>1.5</mu1>
<mu2>1.5</mu2>
<fdir1>1 0 0</fdir1>
<maxVel>1.0</maxVel>
<minDepth>0.001</minDepth>
</gazebo>
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<robotNamespace>/</robotNamespace>
<robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
<legacyModeNS>true</legacyModeNS>
</plugin>
<plugin name="joint_state_publisher" filename="libgazebo_ros_joint_state_publisher.so">
<robotNamespace>/</robotNamespace>
<jointName>joint_LF_1, joint_LF_2, joint_LF_3, joint_LF_4, control_point_joint_LF, joint_LR_1, joint_LR_2, joint_LR_3, joint_LR_4, control_point_joint_LR, joint_RF_1, joint_RF_2, joint_RF_3, joint_RF_4, control_point_joint_RF, joint_RR_1, joint_RR_2, joint_RR_3, joint_RR_4, control_point_joint_RR</jointName>
</plugin>
</gazebo>
</robot>


nx15/controllers.yaml

leg_lf_controller:
type: position_controllers/JointTrajectoryController
publish_rate: 50
joints:
- joint_LF_1
- joint_LF_2
- joint_LF_3
- joint_LF_4
- control_point_joint_LF

leg_lr_controller:
type: position_controllers/JointTrajectoryController
publish_rate: 50
joints:
- joint_LR_1
- joint_LR_2
- joint_LR_3
- joint_LR_4
- control_point_joint_LR

leg_rf_controller:
type: position_controllers/JointTrajectoryController
publish_rate: 50
joints:
- joint_RF_1
- joint_RF_2
- joint_RF_3
- joint_RF_4
- control_point_joint_RF

leg_rr_controller:
type: position_controllers/JointTrajectoryController
publish_rate: 50
joints:
- joint_RR_1
- joint_RR_2
- joint_RR_3
- joint_RR_4
- control_point_joint_RR


nx15_moveit_config/config/controller.yaml

controller_manager_ns: /
controller_list:
- name: leg_lf_controller
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
joints:
- joint_LF_1
- joint_LF_2
- joint_LF_3
- joint_LF_4
- control_point_joint_LF

- name: leg_lr_controller
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
joints:
- joint_LR_1
- joint_LR_2
- joint_LR_3
- joint_LR_4
- control_point_joint_LR

- name: leg_rf_controller
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
joints:
- joint_RF_1
- joint_RF_2
- joint_RF_3
- joint_RF_4
- control_point_joint_RF

- name: leg_rr_controller
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
joints:
- joint_RR_1
- joint_RR_2
- joint_RR_3
- joint_RR_4
- control_point_joint_RR


nx15_moveit_config/launch/moveit_rivzlaunch

<launch>

<arg name="debug" default="false" />
<arg unless="$(arg debug)" name="launch_prefix" value="" />
<arg if="$(arg debug)" name="launch_prefix" value="gdb --ex run --args" />

<arg name="config" default="false" />
<arg unless="$(arg config)" name="command_args" value="" />
<arg if="$(arg config)" name="command_args" value="-d $(find nx15_moveit_config)/launch/moveit.rviz" />

<node name="$(anon rviz)" launch-prefix="$(arg launch_prefix)" pkg="rviz" type="rviz" respawn="false"
args="$(arg command_args)" output="screen">
<rosparam command="load" file="$(find nx15_moveit_config)/config/kinematics.yaml"/>
</node>

</launch>


nx15_moveit_config/launch/nx15_all.launch

<!-- Hand-made -->
<launch>
<include file="$(find nx15)/launch/nx15_simulation.launch"/>
<include file="$(find nx15_moveit_config)/launch/move_group.launch"/>
<include file="$(find nx15_moveit_config)/launch/moveit_rviz.launch">
<arg name="config" value="True"/>
</include>
<node pkg="rosserial_python" type="serial_node.py" name="serial_node">
<param name="port" value="tcp"/>
</node>
</launch>


nx15_moveit_config/launch/nx15_moveit_controller_manager.launch.xml

<launch>

<!-- loads moveit_controller_manager on the parameter server which is taken as argument
if no argument is passed, moveit_simple_controller_manager will be set -->
<arg name="moveit_controller_manager" default="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
<param name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/>

<param name="contoller_manager_name" value="/" />
<param name="use_controller_manager" value="true" />
<!-- loads ros_controllers to the param server -->
<rosparam file="$(find nx15_moveit_config)/config/controllers.yaml"/>
</launch>

スポンサードリンク

コメント

コメントの投稿

非公開コメント

プロフィール

RoboTakao

Author:RoboTakao
みなさんご訪問ありがとうございます。ロボット作りたいけどお小遣いそんなにないし、簡単でローコストでロボットを作るための私のプロジェクトを紹介します。

ウェブサイトもありますのでそちらもよろしくお願いします。
http://robotakao.jp/

スポンサーリンク