Uncontrallable urdf of 2 link arm in Gazebo
Hello, I'd like to make a 2 link arm model by using urdf to simulate it in Gazebo. I made urdf of 2 link arm and launch file that execute simulation in Gazebo. But my model is uncontrollable like the attached pictures. Please let me know where I should improve the urdf model.
My ideal state of 2 link arm is the following. When I change the ensor of inertia as the following , it is better than the above model.
Before
<inertia
ixx="${mass / 12.0 * (width*width + width*width)}" ixy="0.0" ixz="0.0"
iyy="${mass / 12.0 * (height1*height1 + width*width)}" iyz="0.0"
izz="${mass / 12.0 * (height1*height1 + width*width)}"/>
</inertial>
After
<inertia
ixx="${mass / 12.0 * (width*width + height1*height1)}" ixy="0.0" ixz="0.0"
iyy="${mass / 12.0 * (height1*height1 + width*width)}" iyz="0.0"
izz="${mass / 12.0 * (width*width + width*width)}"/>
</inertial>
But it is still uncontrollable. I also think the former ensor of inertia is proper.
I show the tree structure of my package.
├── CMakeLists.txt
├── config
│ └── controller.yaml
├── include
├── launch
│ ├── control.launch
│ ├── display.launch
│ └── gazebo.launch
├── materials.xacro
├── one_link_arm2.gazebo
├── one_link_arm2.urdf
├── package.xml
└── src
one_link_arm2.urdf is the following
<?xml version="1.0"?>
<!-- Revolute-Revolute Manipulator -->
<robot name="one_link_arm2" xmlns:xacro="http://www.ros.org/wiki/xacro">
<!-- Constants for robot dimensions -->
<xacro:property name="PI" value="3.1415926535897931"/>
<xacro:property name="mass" value="1" /> <!-- arbitrary value for mass -->
<xacro:property name="width" value="0.1" /> <!-- Square dimensions (widthxwidth) of beams -->
<xacro:property name="height1" value="2" /> <!-- Link 1 -->
<xacro:property name="height2" value="1" /> <!-- Link 2 -->
<xacro:property name="axel_offset" value="0.05" /> <!-- Space btw top of beam and the each joint -->
<!-- Import all Gazebo-customization elements, including Gazebo colors -->
<xacro:include filename="$(find one_link_arm2)/one_link_arm2.gazebo" />
<!-- Import Rviz colors -->
<xacro:include filename="$(find one_link_arm2)/materials.xacro" />
<!-- Used for fixing robot to Gazebo 'base_link' -->
<link name="world"/>
<joint name="fixed" type="fixed">
<parent link="world"/>
<child link="link1"/>
</joint>
<!-- Base Link -->
<link name="link1">
<collision>
<origin xyz="${height1/2} 0 0" rpy="0 0 0"/>
<geometry>
<box size="${height1} ${width} ${width}"/>
</geometry>
</collision>
<visual>
<origin xyz="${height1/2} 0 0" rpy="0 0 0"/>
<geometry>
<box size="${height1} ${width} ${width}"/>
</geometry>
<material name="orange"/>
</visual>
<inertial>
<origin xyz="${height1/2} 0 0" rpy="0 0 0"/>
<mass value="${2*mass}"/>
<inertia
ixx="${mass / 12.0 * (width*width + height1*height1)}" ixy="0.0" ixz="0.0"
iyy="${mass / 12.0 * (height1*height1 + width*width)}" iyz="0.0"
izz="${mass / 12.0 * (width*width + width*width)}"/>
</inertial>
</link>
<joint name="joint1" type="continuous">
<parent link="link1"/>
<child link="link2"/>
<origin xyz="${height1 - axel_offset} 0 ${width}" rpy="0 0 0"/>
<axis xyz="0 0 1"/>
<dynamics damping="0.7"/>
</joint>
<!-- Middle Link -->
<link name="link2">
<collision>
<origin xyz="${height2/2 - axel_offset} 0 0" rpy="0 0 0"/>
<geometry>
<box size="${height2} ${width} ${width}"/>
</geometry>
</collision>
<visual>
<origin xyz="${height2/2 - axel_offset} 0 0" rpy="0 0 0"/>
<geometry>
<box size="${height2 ...