search

doosan-robotics / doosan-robot2

ROS 2 for Doosan Robot
BSD 3-Clause "New" or "Revised" License
80 stars 56 forks source link

두산로보틱스 git을 클론하여 mobile base를 추가 후 setup_assistant를 사용하여 motion플래닝 plan&execute 실패에 관하여 #118

Open MinSeokCSE opened 1 day ago

MinSeokCSE commented 1 day ago

스크린캐스트 11-24-2024 11:25:17 PM.webm

모바일 베이스는 팀원이 ros1에서 사용하던 모델을 불러왔습니다. 로봇의 모델은 h2017모델입니다.

추측 에러 [rviz2-4] [ERROR] [1732458272.094563219] [move_group_interface]: MoveGroupInterface::execute() failed or timeout reached 에러 사항 입니다. plan과 execute를 진행할 경우 무한정 플랜이 걸리는 현상입니다. 그리고 execute는 실패합니다. 현재 doosan robot2에 클론하여 mobile 베이스를 추가하였습니다. 


  <?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xacro from h2017.urdf.xacro               | -->
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->
<robot name="h2017">
  <material name="black">
    <color rgba="0.0 0.0 0.0 1.0"/>
  </material>
  <material name="white">
    <color rgba="1.0 1.0 1.0 1.0"/>
  </material>
  <material name="orange">
    <color rgba="1.0 0.4 0.0 1.0"/>
  </material>
  <material name="dr_gray">
    <color rgba="0.490 0.490 0.486 1.0"/>
  </material>
  <material name="dr_white">
    <color rgba="0.941 0.941 0.941 1.0"/>
  </material>
  <link name="base_link">
    <inertial>
      <mass value="8.537"/>
      <origin xyz="1.8e-05   -0.002377    0.095608"/>
      <inertia ixx="0.079499" ixy="2.9382e-05" ixz="1.7529e-05" iyy="0.076993" iyz="-0.0009938" izz="0.067833"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="file:////home/minseok/ws_moveit2/install/dsr_description2/share/dsr_description2/meshes/h2017_white/H2017_0_0.dae" scale="0.001 0.001 0.001"/>
      </geometry>
    </visual>
  </link>
  <!-- joint 1 -->
  <joint name="joint_1" type="revolute">
    <parent link="base_link"/>
    <child link="link_1"/>
    <origin rpy="0 0 0" xyz="0 0 0.3443"/>
    <axis xyz="0 0 1"/>
    <limit effort="372" lower="-6.2832" upper="6.2832" velocity="1.7453"/>
    <dynamics friction="0"/>
  </joint>
  <link name="link_1">
    <inertial>
      <mass value="20.253"/>
      <origin xyz="-5e-05     0.11854   -0.059979"/>
      <inertia ixx="0.5323" ixy="-6.2817e-05" ixz="-7.5664e-06" iyy="0.2807" iyz="-0.14899" izz="0.34857"/>
    </inertial>
    <collision>
      <geometry>
        <cylinder length="0.2" radius="0.1"/>
      </geometry>
    </collision>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="file:////home/minseok/ws_moveit2/install/dsr_description2/share/dsr_description2/meshes/h2017_white/H2017_1_0.dae" scale="0.001 0.001 0.001"/>
      </geometry>
    </visual>
  </link>
  <!-- joint2 -->
  <joint name="joint_2" type="revolute">
    <parent link="link_1"/>
    <child link="link_2"/>
    <origin rpy="0 -1.571 -1.571" xyz="0 0.0099 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="372" lower="-2.1817" upper="2.1817" velocity="1.3963"/>
    <dynamics friction="0"/>
  </joint>
  <link name="link_2">
    <inertial>
      <mass value="28.536"/>
      <origin xyz="0.38055    -1.7e-05     0.22527"/>
      <inertia ixx="0.15388" ixy="-0.00022213" ixz="0.11801" iyy="3.0424" iyz="-5.3325e-05" izz="3.0015"/>
    </inertial>
    <collision>
      <origin rpy="0 -1.571 0" xyz="0.3 0 0.1525"/>
      <geometry>
        <cylinder length="0.75" radius="0.08"/>
      </geometry>
    </collision>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="file:////home/minseok/ws_moveit2/install/dsr_description2/share/dsr_description2/meshes/h2017_white/H2017_2_0.dae" scale="0.001 0.001 0.001"/>
      </geometry>
    </visual>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="file:////home/minseok/ws_moveit2/install/dsr_description2/share/dsr_description2/meshes/h2017_white/H2017_2_1.dae" scale="0.001 0.001 0.001"/>
      </geometry>
    </visual>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="file:////home/minseok/ws_moveit2/install/dsr_description2/share/dsr_description2/meshes/h2017_white/H2017_2_2.dae" scale="0.001 0.001 0.001"/>
      </geometry>
    </visual>
  </link>
  <!-- joint3 -->
  <joint name="joint_3" type="revolute">
    <parent link="link_2"/>
    <child link="link_3"/>
    <origin rpy="0 0 1.571" xyz="0.845 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="372" lower="-2.7925" upper="2.7925" velocity="1.7453"/>
    <dynamics friction="0"/>
  </joint>
  <link name="link_3">
    <inertial>
      <mass value="6.421"/>
      <origin xyz="-5.9e-05   -0.008325    0.049527"/>
      <inertia ixx="0.04463" ixy="3.0207e-05" ixz="-1.0987e-05" iyy="0.041031" iyz="-0.0029929" izz="0.028983"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="file:////home/minseok/ws_moveit2/install/dsr_description2/share/dsr_description2/meshes/h2017_white/H2017_3_0.dae" scale="0.001 0.001 0.001"/>
      </geometry>
    </visual>
  </link>
  <!-- joint4 -->
  <joint name="joint_4" type="revolute">
    <parent link="link_3"/>
    <child link="link_4"/>
    <origin rpy="1.571 0 0" xyz="0 -0.734 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="163" lower="-6.2832" upper="6.2832" velocity="3.1416"/>
    <dynamics friction="0"/>
  </joint>
  <link name="link_4">
    <inertial>
      <mass value="7.59"/>
      <origin xyz="7.7e-05     0.12896    -0.22056"/>
      <inertia ixx="0.60329" ixy="-6.8213e-05" ixz="-0.00013439" iyy="0.54966" iyz="-0.159" izz="0.078126"/>
    </inertial>
    <collision>
      <origin rpy="-0.55 0 0" xyz="0 0.07 -0.3"/>
      <geometry>
        <cylinder length="0.4" radius="0.07"/>
      </geometry>
    </collision>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="file:////home/minseok/ws_moveit2/install/dsr_description2/share/dsr_description2/meshes/h2017_white/H2017_4_0.dae" scale="0.001 0.001 0.001"/>
      </geometry>
    </visual>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="file:////home/minseok/ws_moveit2/install/dsr_description2/share/dsr_description2/meshes/h2017_white/H2017_4_1.dae" scale="0.001 0.001 0.001"/>
      </geometry>
    </visual>
  </link>
  <!-- joint5 -->
  <joint name="joint_5" type="revolute">
    <parent link="link_4"/>
    <child link="link_5"/>
    <origin rpy="-1.571 0 0" xyz="0 0 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="96" lower="-6.2832" upper="6.2832" velocity="3.1416"/>
    <dynamics friction="0"/>
  </joint>
  <link name="link_5">
    <inertial>
      <mass value="4.099"/>
      <origin xyz="-6.1e-05   -0.000589    0.048812"/>
      <inertia ixx="0.021303" ixy="2.545e-05" ixz="-4.3439e-06" iyy="0.023894" iyz="6.0426e-05" izz="0.014587"/>
    </inertial>
    <collision>
      <origin rpy="1.571 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder length="0.15" radius="0.08"/>
      </geometry>
    </collision>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="file:////home/minseok/ws_moveit2/install/dsr_description2/share/dsr_description2/meshes/h2017_white/H2017_5_0.dae" scale="0.001 0.001 0.001"/>
      </geometry>
    </visual>
  </link>
  <!-- joint6 -->
  <joint name="joint_6" type="revolute">
    <parent link="link_5"/>
    <child link="link_6"/>
    <origin rpy="1.571 0 0" xyz="0 -0.121 0"/>
    <axis xyz="0 0 1"/>
    <limit effort="50" lower="-6.2832" upper="6.2832" velocity="3.927"/>
    <dynamics friction="0"/>
  </joint>
  <link name="link_6">
    <inertial>
      <mass value="0.897"/>
      <origin xyz="-3.3e-05     3.6e-05   -0.049443"/>
      <inertia ixx="0.0086582" ixy="7.8198e-06" ixz="2.2495e-06" iyy="0.0068184" iyz="5.6115e-06" izz="0.0080729"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="file:////home/minseok/ws_moveit2/install/dsr_description2/share/dsr_description2/meshes/h2017_white/H2017_6_0.dae" scale="0.001 0.001 0.001"/>
      </geometry>
    </visual>
  </link>
  <!-- Transmission 1 -->
  <transmission name="tran1">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="joint1">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor1">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <!-- Transmission 2 -->
  <transmission name="tran2">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="joint2">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor2">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <!-- Transmission 3 -->
  <transmission name="tran3">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="joint3">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor3">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <!-- Transmission 4 -->
  <transmission name="tran4">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="joint4">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor4">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <!-- Transmission 5 -->
  <transmission name="tran5">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="joint5">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor5">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <!-- Transmission 6 -->
  <transmission name="tran6">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="joint6">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
    </joint>
    <actuator name="motor6">
      <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>
  <!-- ros_control plugin -->
  <gazebo>
    <plugin filename="ign_ros2_control-system" name="ign_ros2_control::IgnitionROS2ControlPlugin">
      <ros>
        <namespace></namespace>
      </ros>
      <parameters>/home/minseok/ws_moveit2/install/dsr_controller2/share/dsr_controller2/config/dsr_controller2_gz.yaml</parameters>
      <!-- <controller_manager_name>asd</controller_manager_name> -->
    </plugin>
  </gazebo>
  <!-- Base -->
  <gazebo reference="base_0">
    <kp>100000.0</kp>
    <kd>100.0</kd>
    <mu1>30.0</mu1>
    <mu2>30.0</mu2>
    <maxVel>1.0</maxVel>
    <minDepth>0.001</minDepth>
  </gazebo>
  <!-- Link1 -->
  <gazebo reference="link1">
    <kp>100000.0</kp>
    <kd>100.0</kd>
    <mu1>30.0</mu1>
    <mu2>30.0</mu2>
    <maxVel>1.0</maxVel>
    <minDepth>0.001</minDepth>
  </gazebo>
  <!-- Link2 -->
  <gazebo reference="link2">
    <kp>100000.0</kp>
    <kd>100.0</kd>
    <mu1>30.0</mu1>
    <mu2>30.0</mu2>
    <maxVel>1.0</maxVel>
    <minDepth>0.001</minDepth>
  </gazebo>
  <!-- Link3 -->
  <gazebo reference="link3">
    <kp>100000.0</kp>
    <kd>100.0</kd>
    <mu1>30.0</mu1>
    <mu2>30.0</mu2>
    <maxVel>1.0</maxVel>
    <minDepth>0.001</minDepth>
  </gazebo>
  <!-- Link4 -->
  <gazebo reference="link4">
    <kp>100000.0</kp>
    <kd>100.0</kd>
    <mu1>30.0</mu1>
    <mu2>30.0</mu2>
    <maxVel>1.0</maxVel>
    <minDepth>0.001</minDepth>
  </gazebo>
  <!-- Link5 -->
  <gazebo reference="link5">
    <kp>100000.0</kp>
    <kd>100.0</kd>
    <mu1>30.0</mu1>
    <mu2>30.0</mu2>
    <maxVel>1.0</maxVel>
    <minDepth>0.001</minDepth>
  </gazebo>
  <!-- Link6 -->
  <gazebo reference="link6">
    <kp>100000.0</kp>
    <kd>100.0</kd>
    <mu1>30.0</mu1>
    <mu2>30.0</mu2>
    <maxVel>1.0</maxVel>
    <minDepth>0.001</minDepth>
  </gazebo>
  <!-- World Link -->
  <link name="world"/>
  <!-- Mobile Base -->
  <link name="mobile_base">
    <inertial>
      <mass value="20"/>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <inertia ixx="0.079499" ixy="2.9382e-05" ixz="1.7529e-05" iyy="0.076993" iyz="-0.0009938" izz="0.067833"/>
    </inertial>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://dsr_description2/meshes/h2017_collision/mobile_base.dae" scale="0.001 0.001 0.001"/>
      </geometry>
    </collision>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://dsr_description2/meshes/h2017_visual/mobile_base.dae" scale="0.001 0.001 0.001"/>
      </geometry>
    </visual>
  </link>
  <!-- Mobile Base Joint -->
  <joint name="world_mobile_base" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <parent link="world"/>
    <child link="mobile_base"/>
  </joint>
  <!-- Mobile Base to Base Link Joint -->
  <joint name="mobile_base_base_link" type="fixed">
    <origin rpy="0 0 0" xyz="0 0 0.8"/>
    <parent link="mobile_base"/>
    <child link="base_link"/>
  </joint>
  <ros2_control name="h2017" type="system">
    <hardware>
      <plugin>dsr_hardware2/DRHWInterface</plugin>
      <param name="name">h2017</param>
      <param name="rate">100</param>
      <param name="standby">ture</param>
      <param name="command">none</param>
      <param name="host">0.0.0.0</param>
      <param name="port">12345</param>
      <param name="mode">real</param>
      <param name="model">h2017</param>
      <param name="gripper">none</param>
      <param name="mobile">none</param>
    </hardware>
    <joint name="joint_1">
      <command_interface name="position">
        <param name="min">{-2*pi}</param>
        <param name="max">{2*pi}</param>
      </command_interface>
      <command_interface name="velocity">
        <param name="min">-3.15</param>
        <param name="max">3.15</param>
      </command_interface>
      <state_interface name="position">
        <param name="initial_value">0.0</param>
      </state_interface>
      <state_interface name="velocity"/>
    </joint>
    <joint name="joint_2">
      <command_interface name="position">
        <param name="min">{-2*pi}</param>
        <param name="max">{2*pi}</param>
      </command_interface>
      <command_interface name="velocity">
        <param name="min">-3.15</param>
        <param name="max">3.15</param>
      </command_interface>
      <state_interface name="position">
        <param name="initial_value">0.0</param>
      </state_interface>
      <state_interface name="velocity"/>
    </joint>
    <joint name="joint_3">
      <command_interface name="position">
        <param name="min">{-pi}</param>
        <param name="max">{pi}</param>
      </command_interface>
      <command_interface name="velocity">
        <param name="min">-3.15</param>
        <param name="max">3.15</param>
      </command_interface>
      <state_interface name="position">
        <param name="initial_value">0.0</param>
      </state_interface>
      <state_interface name="velocity"/>
    </joint>
    <joint name="joint_4">
      <command_interface name="position">
        <param name="min">{-2*pi}</param>
        <param name="max">{2*pi}</param>
      </command_interface>
      <command_interface name="velocity">
        <param name="min">-3.2</param>
        <param name="max">3.2</param>
      </command_interface>
      <state_interface name="position">
        <param name="initial_value">0.0</param>
      </state_interface>
      <state_interface name="velocity"/>
    </joint>
    <joint name="joint_5">
      <command_interface name="position">
        <param name="min">{-2*pi}</param>
        <param name="max">{2*pi}</param>
      </command_interface>
      <command_interface name="velocity">
        <param name="min">-3.2</param>
        <param name="max">3.2</param>
      </command_interface>
      <state_interface name="position">
        <param name="initial_value">0.0</param>
      </state_interface>
      <state_interface name="velocity"/>
    </joint>
    <joint name="joint_6">
      <command_interface name="position">
        <param name="min">{-2*pi}</param>
        <param name="max">{2*pi}</param>
      </command_interface>
      <command_interface name="velocity">
        <param name="min">-3.2</param>
        <param name="max">3.2</param>
      </command_interface>
      <state_interface name="position">
        <param name="initial_value">0.0</param>
      </state_interface>
      <state_interface name="velocity"/>
    </joint>
  </ros2_control>
</robot>```

현재 urdf 코드이며 어디가 잘 못되고 모션플래닝이 안되는지 모르겠습니다. 직접 setup_assitant를 사용하여 config파일을 생성하여 demo.py를 실행했을 때의 위 영상의 결과입니다. 몇 주째 시도 중인데 안되는 이유를 모르겠습니