Cannot control gripper to grasp object in gazebo

edit

Hi everybody,I had this problem when doing gazebo simulation:when I use an gripper to grasp a small box in gazebo,the gripper is not able to move the box,and has a deformation(shown as follows)

I wonder if the problem is about the configuration of PID and inertial parameters

I control the gripper using an action client to send a FollowJointTrajectoryGoal,the code is as follows:

  # close griper
    gripper_goal = FollowJointTrajectoryGoal()
    gripper_goal.trajectory = gripper_traj_close
    gripper_goal.goal_time_tolerance = rospy.Duration(0.0)
    gripper_client.send_goal(gripper_goal)
    gripper_client.wait_for_result(rospy.Duration(5.0))

the fripper controller file is as follows:

gripper_controller:
type: position_controllers/JointTrajectoryController
joints:
    - finger_joint
gains:
  finger_joint: {p: 50,  d: 1, i: 0.01, i_clamp: 1}

constraints:
  goal_time: 0.5                   # Override default

state_publish_rate:  25            # Override default
action_monitor_rate: 30            # Override default
stop_trajectory_duration: 0        # Override default

the xacro file of the gripper is as follows:

<?xml version="1.0"?>

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

<xacro:include filename="$(find iiwa_description)/urdf/robotiq_arg2f_transmission.xacro"/>

<xacro:macro name="outer_knuckle" params="prefix fingerprefix stroke">

<link name="${prefix}${fingerprefix}_outer_knuckle">

  <inertial>

    <origin xyz="-0.000200000000003065 0.0199435877845359 0.0292245259211331" rpy="0 0 0" />

    <mass value="0.00853198276973456" />

    <inertia ixx="2.89328108496468E-06" ixy="-1.57935047237397E-19" ixz="-1.93980378593255E-19" iyy="1.86719750325683E-06" iyz="-1.21858577871576E-06" izz="1.21905238907251E-06" />

  </inertial>

  <visual>

    <origin xyz="0 0 0" rpy="0 0 0" />

    <geometry>

      <mesh filename="package://iiwa_description/meshes/gripper/visual/robotiq_arg2f_85_outer_knuckle.dae" scale="0.001 0.001 0.001"/>

    </geometry>

    <material name="">

      <color rgba="0.792156862745098 0.819607843137255 0.933333333333333 1" />

    </material>

  </visual>

  <collision>

    <origin xyz="0 0 0" rpy="0 0 0" />

    <geometry>

      <mesh filename="package://iiwa_description/meshes/gripper/collision/robotiq_arg2f_85_outer_knuckle.dae" scale="0.001 0.001 0.001"/>

    </geometry>

  </collision>

</link>

</xacro:macro>

<xacro:macro name="outer_finger" params="prefix fingerprefix stroke">

<link name="${prefix}${fingerprefix}_outer_finger">

  <inertial>

    <origin xyz="0.00030115855001899 0.0373907951953854 -0.0208027427000385" rpy="0 0 0" />

    <mass value="0.022614240507152" />

    <inertia ixx="1.52518312458174E-05" ixy="9.76583423954399E-10" ixz="-5.43838577022588E-10" iyy="6.17694243867776E-06" iyz="6.78636130740228E-06" izz="1.16494917907219E-05" />

  </inertial>

  <visual>

    <origin xyz="0 0 0" rpy="0 0 0" />

    <geometry>

      <mesh filename="package://iiwa_description/meshes/gripper/visual/robotiq_arg2f_85_outer_finger.dae" scale="0.001 0.001 0.001"/>

    </geometry>

    <material name="">

      <color rgba="0.1 0.1 0.1 1" />

    </material>

  </visual>

  <collision>

    <origin xyz="0 0 0" rpy="0 0 0" />

    <geometry>

      <mesh filename="package://iiwa_description/meshes/gripper/collision/robotiq_arg2f_85_outer_finger.dae" scale="0.001 0.001 0.001"/>

    </geometry>

  </collision>

</link>

</xacro:macro>

<xacro:macro name="inner_knuckle" params="prefix fingerprefix stroke">

<link name="${prefix}${fingerprefix}_inner_knuckle">

  <inertial>

    <origin xyz="0.000123011831763771 0.0507850843201817 0.00103968640075166" rpy="0 0 0" />

   <mass value="0.0271177346495152" />

    <inertia ixx="2.61910379223783E-05" ixy="-2.43616858946494E-07" ixz="-6.37789906117123E-09" iyy="2.8270243746167E-06" iyz="-5.37200748039765E-07" izz="2.83695868220296E-05" />

  </inertial>

  <visual>

    <origin xyz="0 0 0" rpy="0 0 0" />

    <geometry>

      <mesh filename="package://iiwa_description/meshes/gripper/visual/robotiq_arg2f_85_inner_knuckle.dae" scale="0.001 0.001 0.001"/>

    </geometry>

    <material name="">

      <color rgba="0.1 0.1 0.1 1" />

    </material>

  </visual>

  <collision ...