Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Vibration in joints and poor set point control #2

Open
thejose5 opened this issue Jun 19, 2020 · 0 comments
Open

Vibration in joints and poor set point control #2

thejose5 opened this issue Jun 19, 2020 · 0 comments

Comments

@thejose5
Copy link

I am using the xacro file given below to spawn a kuka robot in gazebo. Whenever I spawn the robot with this xacro file, I see small vibrations in some of the joints. However, if I comment out the < gazebo > tag that defines the control plugin (line 6 to 11), this vibration disappears.

It seems that the problem is with joints 4, 5 and 6 (joint controllers 5, 6 and 7). If I remove these joints from the xacro the vibration disappears. Moreover, for joints 4 and 6, the controller is not able to move it to the set point correctly. No matter what set point is given, both joints stabilize at some other random point.

I was not able to add the videos to this question. So I have uploaded them to a google drive. Here's the link:
https://drive.google.com/drive/folders/1Rtpe_22l3jOnsukvlULb_VTK5IvRVLMJ?usp=sharing

I have also pasted my launch files below the xacro file.

The xacro file is as follows:

<?xml version="1.0"?>

 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="kuka_lwr">

 <xacro:property name="M_PI" value="3.1415926535897931" />
 <xacro:property name="name" value="kuka" />
 <xacro:property name="inertia_scaling" value="1" />
  <gazebo>
      <plugin filename="libgazebo_ros_control.so" name="gazebo_ros_control">
        <robotNamespace>/kuka</robotNamespace>
        <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
      </plugin>
  </gazebo>

  <xacro:macro name="cuboid_inertia_def" params="width height length mass">
    <inertia ixx="${mass * (height * height + length * length) / 12 * inertia_scaling}"
             iyy="${mass * (width * width + height * height) / 12 * inertia_scaling}"
             izz="${mass * (width * width + length * length) / 12 * inertia_scaling}"
             ixy="0" iyz="0" ixz="0"/>
  </xacro:macro>

  <!-- length is along the y-axis! -->
  <xacro:macro name="cylinder_inertia_def" params="radius length mass">
    <inertia ixx="${mass * (3 * radius * radius + length * length) / 12 * inertia_scaling}"
             iyy="${mass * radius* radius / 2 * inertia_scaling}"
             izz="${mass * (3 * radius * radius + length * length) / 12 * inertia_scaling}"
             ixy="0" iyz="0" ixz="0"/>
  </xacro:macro>

  <xacro:property name="arm_elem_link_mass" value="2.0"/>
  <xacro:property name="arm_elem_ball_link_mass" value="2.0"/>
  <xacro:property name="arm_elem_end_link_mass" value="2.0"/>
  <xacro:property name="safety_controller_k_pos" value="100" />
  <xacro:property name="safety_controller_k_vel" value="2" />
  <xacro:property name="joint_damping" value="1" />

  <xacro:property name="arm_velocity_scale_factor" value="1"/>
  <xacro:property name="right" value="0" />
  <!-- right is either 1 (for right arm) or -1 (for left arm) -->
    <link name="world"/>
    <joint name="base_joint" type="fixed">
      <origin xyz="0 0 0.055" rpy="0 0 0"/>
      <axis xyz="0 0 1"/>
      <child link="calib_${name}_arm_base_link"/>
      <parent link="world"/>
    </joint>
<link name="calib_${name}_arm_base_link">
  <inertial>
    <mass value="2"/> <!-- static base, disable dynamics for this link -->
    <origin xyz="0 0 0.055" rpy="0 0 0"/>
    <xacro:cylinder_inertia_def radius="0.06" length="0.11"
                          mass="2"/>
  </inertial>
  <visual>
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <geometry>
      <mesh filename="package://lmtlwr/model/meshes_arm/arm_base.stl"/>
    </geometry>
    <material name="Orange"/>
  </visual>
  <collision>
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <geometry>
      <mesh filename="package://lmtlwr/model/meshes_arm/convex/arm_base_convex.stl"/>
    </geometry>
  </collision>
</link>

<link name="${name}_arm_1_link">
  <inertial>
    <mass value="${arm_elem_link_mass}"/>
    <origin rpy="0 0 0" xyz="0 0.04 0.130"/>
    <xacro:cuboid_inertia_def length="0.12" width="0.06" height="0.260"
                        mass="${arm_elem_link_mass}"/>
  </inertial>
  <visual>
    <origin xyz="0 0 0" rpy="0 0 ${M_PI}"/>
    <geometry>
      <mesh filename="package://lmtlwr/model/meshes_arm/arm_segment_a.stl"/>
    </geometry>
    <material name="Orange"/>
  </visual>
  <collision>
    <origin xyz="0 0 0" rpy="0 0 ${M_PI}"/>
    <geometry>
      <mesh filename="package://lmtlwr/model/meshes_arm/convex/arm_segment_a_convex.stl"/>
    </geometry>
  </collision>
</link>

<joint name="${name}_arm_0_joint" type="revolute">
  <origin xyz="0 0 0.11" rpy="0 0 0"/>
  <!-- <origin xyz="0 0.04 0.13" rpy="0 0 0"/> -->
  <axis xyz="0 0 1"/>
  <limit lower="${-170 * M_PI / 180}" upper="${170 * M_PI / 180}"
         effort="204" velocity="${arm_velocity_scale_factor * 110 * M_PI / 180}" />
  <dynamics damping="${joint_damping}"/>
  <parent link="calib_${name}_arm_base_link"/>
  <child link="${name}_arm_1_link"/>
</joint>

<transmission name="${name}_arm_0_trans">
  <type>transmission_interface/SimpleTransmission</type>
  <actuator name="${name}_arm_0_motor">
    <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    <mechanicalReduction>1.0</mechanicalReduction>
  </actuator>
  <joint name="${name}_arm_0_joint">
    <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
  </joint>

</transmission>

<link name="${name}_arm_2_link">
  <inertial>
    <mass value="${arm_elem_link_mass}"/>
    <origin rpy="0 0 0" xyz="0 -0.04 ${0.130 - 0.06}"/>
    <xacro:cuboid_inertia_def length="0.12" width="0.06" height="0.260"
                        mass="${arm_elem_link_mass}"/>
  </inertial>

  <visual>
    <origin xyz="0 0 0.2" rpy="${M_PI} 0 ${M_PI}"/>
    <geometry>
      <mesh filename="package://lmtlwr/model/meshes_arm/arm_segment_b.stl"/>
    </geometry>
    <material name="Orange"/>
  </visual>

  <collision>
    <origin xyz="0 0 0.2" rpy="${M_PI} 0 ${M_PI}"/>
    <geometry>
      <mesh filename="package://lmtlwr/model/meshes_arm/convex/arm_segment_b_convex.stl"/>
    </geometry>
  </collision>
</link>

<joint name="${name}_arm_1_joint" type="revolute">
  <origin xyz="0 0 0.20" rpy="0 0 0"/>
  <origin rpy="0 0 0" xyz="0 -0.04 ${0.130 - 0.06}"/>
  <axis xyz="0 -1 0"/>
  <limit lower="${-90 * M_PI / 180}" upper="${90 * M_PI / 180}"
         effort="306" velocity="${arm_velocity_scale_factor * 0 * M_PI / 180}" />
  <dynamics damping="${joint_damping}"/>
  <parent link="${name}_arm_1_link"/>
  <child link="${name}_arm_2_link"/>
</joint>

<transmission name="${name}_arm_1_trans">
  <type>transmission_interface/SimpleTransmission</type>
  <actuator name="${name}_arm_1_motor">
    <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    <mechanicalReduction>1.0</mechanicalReduction>
  </actuator>
  <joint name="${name}_arm_1_joint">
    <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
  </joint>
</transmission>

<link name="${name}_arm_3_link">
  <inertial>
    <mass value="${arm_elem_link_mass}"/>
    <origin rpy="0 0 0" xyz="0 -0.04 0.130"/>
    <xacro:cuboid_inertia_def length="0.12" width="0.06" height="0.260"
                        mass="${arm_elem_link_mass}"/>
  </inertial>

  <visual>
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <geometry>
      <mesh filename="package://lmtlwr/model/meshes_arm/arm_segment_a.stl"/>
    </geometry>
    <material name="Orange"/>
  </visual>

  <collision>
    <origin xyz="0 0 0" rpy="0 0 0"/>
    <geometry>
      <mesh filename="package://lmtlwr/model/meshes_arm/convex/arm_segment_a_convex.stl"/>
    </geometry>
  </collision>
</link>

<joint name="${name}_arm_2_joint" type="revolute">
  <origin xyz="0 0 0.20" rpy="0 0 0"/>
  <origin rpy="0 0 0" xyz="0 -0.04 0.130"/>
  <axis xyz="0 0 1"/>
  <limit lower="${-170 * M_PI / 180}" upper="${170 * M_PI / 180}"
         effort="204" velocity="${arm_velocity_scale_factor * 130 * M_PI / 180}" />
  <dynamics damping="${joint_damping}"/>
  <parent link="${name}_arm_2_link"/>
  <child link="${name}_arm_3_link"/>
</joint>

<transmission name="${name}_arm_2_trans">
  <type>transmission_interface/SimpleTransmission</type>
  <actuator name="${name}_arm_2_motor">
    <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    <mechanicalReduction>1.0</mechanicalReduction>
  </actuator>
  <joint name="${name}_arm_2_joint">
    <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
  </joint>
</transmission>

<link name="${name}_arm_4_link">
  <inertial>
    <mass value="${arm_elem_link_mass}"/>
    <origin rpy="0 0 0" xyz="0 0.04 ${0.130 - 0.06}"/>
    <xacro:cuboid_inertia_def length="0.12" width="0.06" height="0.2600"
                        mass="${arm_elem_link_mass}"/>
  </inertial>

  <visual>
    <origin xyz="0 0 0.2" rpy="0 ${M_PI} ${M_PI}"/>
    <geometry>
      <mesh filename="package://lmtlwr/model/meshes_arm/arm_segment_b.stl"/>
    </geometry>
    <material name="Orange"/>
  </visual>

  <collision>
    <origin xyz="0 0 0.2" rpy="0 ${M_PI} ${M_PI}"/>
    <geometry>
      <mesh filename="package://lmtlwr/model/meshes_arm/convex/arm_segment_b_convex.stl"/>
    </geometry>
  </collision>
</link>


<joint name="${name}_arm_3_joint" type="revolute">
  <origin xyz="0 0 0.20" rpy="0 0 0"/>
  <origin rpy="0 0 0" xyz="0 0.04 ${0.130 - 0.06}"/>
  <axis xyz="0 1 0"/>
  <limit lower="${-120 * M_PI / 180}" upper="${120 * M_PI / 180}"
         effort="306" velocity="${arm_velocity_scale_factor * 130 * M_PI / 180}" />
  <dynamics damping="${joint_damping}"/>
  <parent link="${name}_arm_3_link"/>
  <child link="${name}_arm_4_link"/>
</joint>

<transmission name="${name}_arm_3_trans">
  <type>transmission_interface/SimpleTransmission</type>
  <actuator name="${name}_arm_3_motor">
    <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    <mechanicalReduction>1.0</mechanicalReduction>
  </actuator>
  <joint name="${name}_arm_3_joint">
    <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
  </joint>
</transmission>

<link name="${name}_arm_5_link">
  <inertial>
    <mass value="${arm_elem_link_mass}"/>
    <origin rpy="0 0 0" xyz="0 0.02 0.124"/>
    <xacro:cuboid_inertia_def length="0.12" width="0.06" height="0.248"
                        mass="${arm_elem_link_mass}"/>
  </inertial>

  <visual>
    <origin xyz="0 0 0" rpy="0 0 ${M_PI}"/>
    <geometry name="${name}_arm_5_geom">
      <mesh filename="package://lmtlwr/model/meshes_arm/arm_segment_last.stl"/>
    </geometry>
    <material name="Orange"/>
  </visual>

  <collision>
    <origin xyz="0 0 0" rpy="0 0 ${M_PI}"/>
    <geometry>
      <mesh filename="package://lmtlwr/model/meshes_arm/convex/arm_segment_last_convex.stl"/>
    </geometry>
  </collision>
</link>

<joint name="${name}_arm_4_joint" type="revolute">
  <origin xyz="0 0 0.20" rpy="0 0 0"/>
  <origin rpy="0 0 0" xyz="0 0.02 0.124"/>
  <axis xyz="0 0 1"/>
  <limit lower="${-170 * M_PI / 180}" upper="${170 * M_PI / 180}"
         effort="204" velocity="${arm_velocity_scale_factor * 130 * M_PI / 180}" />
  <dynamics damping="${joint_damping}"/>
  <parent link="${name}_arm_4_link"/>
  <child link="${name}_arm_5_link"/>
</joint>

<transmission name="${name}_arm_4_trans">
  <type>transmission_interface/SimpleTransmission</type>
  <actuator name="${name}_arm_4_motor">
    <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    <mechanicalReduction>1.0</mechanicalReduction>
  </actuator>
  <joint name="${name}_arm_4_joint">
    <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
  </joint>
</transmission>

<link name="${name}_arm_6_link">
  <inertial>
    <mass value="${arm_elem_ball_link_mass}"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <xacro:cuboid_inertia_def length="0.125" width="0.125" height="0.125"
                        mass="${arm_elem_ball_link_mass}"/>
  </inertial>

  <visual>
    <origin xyz="0 0 0" rpy="0 0 ${M_PI}"/>
    <geometry>
      <mesh filename="package://lmtlwr/model/meshes_arm/arm_wrist.stl"/>
    </geometry>
    <material name="Grey"/>
  </visual>

  <collision>
    <origin xyz="0 0 0" rpy="0 0 ${M_PI}"/>
    <geometry>
      <mesh filename="package://lmtlwr/model/meshes_arm/convex/arm_wrist_convex.stl"/>
    </geometry>
  </collision>
</link>

<joint name="${name}_arm_5_joint" type="revolute">
  <origin xyz="0 0 0.19" rpy="0 0 0"/>
  <origin rpy="0 0 0" xyz="0 0 0"/>
  <axis xyz="0 -1 0"/>
  <limit lower="${-120 * M_PI / 180}" upper="${120 * M_PI / 180}"
         effort="306" velocity="${arm_velocity_scale_factor * 180 * M_PI / 180}" />
  <dynamics damping="${joint_damping}"/>
  <parent link="${name}_arm_5_link"/>
  <child link="${name}_arm_6_link"/>
</joint>

<transmission name="${name}_arm_5_trans">
  <type>transmission_interface/SimpleTransmission</type>
  <actuator name="${name}_arm_5_motor">
    <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    <mechanicalReduction>1.0</mechanicalReduction>
  </actuator>
  <joint name="${name}_arm_5_joint">
    <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
  </joint>
</transmission>

<link name="${name}_arm_7_link">
  <inertial>
    <mass value="${arm_elem_end_link_mass}"/>
    <origin xyz="0 0 0"/>
    <xacro:cuboid_inertia_def length="0.05" width="0.05" height="0.05"
                        mass="${arm_elem_end_link_mass}"/>
  </inertial>
  <visual>
    <origin xyz="0 0 0" rpy="0 0 ${right * -1/4 * M_PI  +  M_PI}"/>
    <geometry>
      <mesh filename="package://lmtlwr/model/meshes_arm/arm_flanche.stl"/>
    </geometry>
    <material name="Grey"/>
  </visual>
  <collision>
    <origin xyz="0 0 0" rpy="0 0 ${right * -1/4 * M_PI  +  M_PI}"/>
    <geometry>
      <mesh filename="package://lmtlwr/model/meshes_arm/convex/arm_flanche_convex.stl"/>
    </geometry>
  </collision>
</link>

<joint name="${name}_arm_6_joint" type="revolute">
  <origin xyz="0 0 0.078" rpy="0 0 0"/>
  <origin xyz="0 0 0"/>
  <axis xyz="0 0 1"/>
  <limit lower="${-170 * M_PI / 180}" upper="${170 * M_PI / 180}"
         effort="204" velocity="${arm_velocity_scale_factor * 180 * M_PI / 180}" />
  <dynamics damping="${joint_damping}"/>
  <parent link="${name}_arm_6_link"/>
  <child link="${name}_arm_7_link"/>
</joint>

<transmission name="${name}_arm_6_trans">
  <type>transmission_interface/SimpleTransmission</type>
  <actuator name="${name}_arm_6_motor">
    <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
    <mechanicalReduction>1.0</mechanicalReduction>
  </actuator>
  <joint name="${name}_arm_6_joint">
    <hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
  </joint>
</transmission>

</robot>

The main launch file is as follows:

<?xml version="1.0"?>
<launch>
  <!-- Launch Gazebo World -->
  <include file="$(find gazebo_ros)/launch/empty_world.launch">
    <arg name="use_sim_time" value="true"/>
    <arg name="debug" value="false"/>
    <arg name="gui" value="true" />
    <arg name="paused" value="true"/>
  </include>

  <!-- Launch robot -->
  <!-- <param name="robot_description" command="cat '$(find lmtlwr)/model/sample.urdf.xacro'" /> -->
  <arg name="model" default="$(find multimodal_perception)/model/kuka_lwr_arm.urdf.xacro"/>
  <param name="robot_description" command="$(find xacro)/xacro --inorder $(arg model)" />

  <node name="LWR" pkg="gazebo_ros" type="spawn_model" output="screen"
        args="-urdf -param robot_description -model LWR" />
  <include file="$(find multimodal_perception)/control/kuka_control.launch"></include>

</launch>

My controller launch file is as follows:

<launch>
  <!-- Load joint controller configurations from YAML file to parameter server -->
  <rosparam file="$(find multimodal_perception)/control/kuka_control.yaml" command="load"/>

  <!-- load the controllers -->
  <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
    output="screen" ns="/kuka" args="joint1_position_controller joint2_position_controller joint3_position_controller joint4_position_controller joint5_position_controller joint6_position_controller joint7_position_controller joint_state_controller"/>

  <!-- convert joint states to TF transforms for rviz, etc -->
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
    respawn="false" output="screen">
    <remap from="/joint_states" to="/kuka/joint_states" />
  </node>

</launch>

My controller config (yaml) file is as follows:

kuka:
  joint_state_controller:
    type: joint_state_controller/JointStateController
    publish_rate: 50

  joint1_position_controller:
    type: effort_controllers/JointPositionController
    joint: kuka_arm_0_joint
    # pid: {p: 0, i: 0, d: 0}
    pid: {p: 200.0, i: 0.01, d: 5.0, i_clamp: 10}
  joint2_position_controller:
    type: effort_controllers/JointPositionController
    joint: kuka_arm_1_joint
    # pid: {p: 0, i: 0, d: 0}
    pid: {p: 200.0, i: 0.01, d: 5.0, i_clamp: 10}
  joint3_position_controller:
    type: effort_controllers/JointPositionController
    joint: kuka_arm_2_joint
    # pid: {p: 0, i: 0, d: 0}
    pid: {p: 200.0, i: 0.01, d: 5.0, i_clamp: 10}
  joint4_position_controller:
    type: effort_controllers/JointPositionController
    joint: kuka_arm_3_joint
    # pid: {p: 0, i: 0, d: 0}
    pid: {p: 200.0, i: 0.01, d: 10.0, i_clamp: 10}
  joint5_position_controller:
    type: effort_controllers/JointPositionController
    joint: kuka_arm_4_joint
    # pid: {p: 0, i: 0, d: 0}
    pid: {p: 200.0, i: 1, d: 5.0, i_clamp: 10}
  joint6_position_controller:
    type: effort_controllers/JointPositionController
    joint: kuka_arm_5_joint
    # pid: {p: 0, i: 0, d: 0}
    pid: {p: 100.0, i: 0.01, d: 10.0, i_clamp: 10}
  joint7_position_controller:
    type: effort_controllers/JointPositionController
    joint: kuka_arm_6_joint
    # pid: {p: 0, i: 0, d: 0}
    pid: {p: 100.0, i: 1, d: 10.0, i_clamp: 10}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Assignees
No one assigned
Labels
None yet
Projects
None yet
Milestone
No milestone
Development

No branches or pull requests
1 participant
@thejose5