# Revision history [back]

### Problem with simple grasping simulation

Grasping is not working in this primitive case I've made with Gazebo 9 and ROS Kinetic. This question is a follow-up to a question I posted here earlier. The reason I've made a separate post is that I have some new data to include and the other question had already grown pretty long.

The object is a box of mass 1 and default friction and contact parameters. The gripper uses two prismatic joints to move two c-shaped links together. All these are shown in this image. Note the gripper-joint coordinate frames (the x- and z-direction vectors are visible).

When the gripper closes on the box and the robot then moves upward, the box does not get lifted, as shown in this animation (it's for a taller gripper than above but the behavior is identical, also the gripper in the animation used just one prismatic joint). Since the normal force between the gripper and box is 200 N (that's what I have the joint effort set at, as shown in the sdf below) then the lifting force should be at least 200 N, while 9.81 N should suffice to lift the box. Do you have an idea why the box doesn't go up?

The following plots were made via

rqt_plot ft_senr_topic_p/wrench/force/x, ft_sensor_topic_p/wrench/force/z, ft_sensor_topic_d/wrench/force/x, ft_sensor_topic_d/wrench/force/z


to graph the x and z components of the joint forces for the two prismatic gripper joints. In this one the joints are controlled with the EffortJointInterface. When the x components drop down, at about 28.6 seconds, that's when the gripper is higher than the box and it closes.

For this plot the gripper friction coefficient is changed to 100, which appears to have made no change.

The final plot is with mu=100 and using the PositionJointInterface. At about 24.4 seconds On this one, the x-components drop down. That's when the robot starts to move upward (prior to that it's stationary with the gripper closed around the box). The spikes after that, at 24.8, are when the gripper clears the box. It's odd to me that one joint has a positive z-component and the other has a negative z-component of force.

Here is the controller part from the gripper urdf xacro file.

<transmission name="gripper_joint_transmission_p">
<type>transmission_interface/SimpleTransmission</type>
<joint name="gripper_joint_p">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="gripper_joint_motor_p">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>

<transmission name="gripper_joint_transmission_d">
<type>transmission_interface/SimpleTransmission</type>
<joint name="gripper_joint_d">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="gripper_joint_motor_d">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>


The sdf it produces is pasted at the end, but first, here is the box sdf.

<?xml version="1.0" ?>
<sdf version="1.5">
<model name="table_block">
<static>false</static>

<inertial>
<mass>1</mass>
</inertial>

<collision name="collision">
<geometry>
<box>
<size>0.32 0.32 0.3</size>
</box>
</geometry>
<!--surface>
<friction>
<ode>
<mu>100.0</mu>
<mu2>100.0</mu2>
</ode>
</friction>
<contact>
<ode>
<kp>1e9</kp>
<kd>1e2</kd>
<max_vel>0</max_vel>
<min_depth>0.0</min_depth>
</ode>
</contact>
</surface-->
</collision>

<visual name="visual">
<geometry>
<box>
<size>0.32 0.32 0.3</size>
</box>
</geometry>
<material>
<ambient>1 0 0 1</ambient>
<diffuse>1 0 0 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>

<sensor name='my_contact' type='contact'>
<contact>
<collision>collision</collision>
</contact>
<update_rate> 5 </update_rate>
</sensor>
</model>
</sdf>


The surface tag is commented out because no changes there helped the simulation. Here is the gripper sdf (again, it came from gripper.urdf.xacro).

<sdf version='1.6'>
<model name='gripper'>
<pose frame=''>0 0 0 0 -0 0</pose>
<inertial>
<pose frame=''>0 0 0 0 -0 0</pose>
<mass>0.01</mass>
<inertia>
<ixx>0.0001</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.0001</iyy>
<iyz>0</iyz>
<izz>0.0001</izz>
</inertia>
</inertial>
<pose frame=''>0.14 0 0 0 -0 0</pose>
<inertial>
<pose frame=''>-0.0238 -0 0 0 -0 0</pose>
<mass>1.944</mass>
<inertia>
<ixx>0.0603698</ixx>
<ixy>3.30291e-19</ixy>
<ixz>0</ixz>
<iyy>0.00457571</iyy>
<iyz>0</iyz>
<izz>0.0617055</izz>
</inertia>
</inertial>
<collision name='gripper_wide_block_d_collision'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<box>
<size>0.01 0.468 0.1</size>
</box>
</geometry>
<surface>
<contact>
<ode>
<max_vel>0</max_vel>
<min_depth>0</min_depth>
</ode>
</contact>
<friction>
<ode>
<mu>100</mu>
<mu2>100</mu2>
</ode>
</friction>
</surface>
</collision>
<collision name='gripper_wide_block_d_fixed_joint_lump__collision_collision_1'>
<pose frame=''>-0.068 0.229 0 0 -0 0</pose>
<geometry>
<box>
<size>0.126 0.01 0.1</size>
</box>
</geometry>
</collision>
<collision name='gripper_wide_block_d_fixed_joint_lump__collision_collision_2'>
<pose frame=''>-0.068 -0.229 0 0 -0 0</pose>
<geometry>
<box>
<size>0.126 0.01 0.1</size>
</box>
</geometry>
</collision>
<visual name='gripper_wide_block_d_visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<box>
<size>0.01 0.468 0.1</size>
</box>
</geometry>
<material>
<script>
<uri>__default__</uri>
<name>__default__</name>
</script>
</material>
</visual>
<visual name='gripper_wide_block_d_fixed_joint_lump__gripper_side_block_d1_visual_1'>
<pose frame=''>-0.068 0.229 0 0 -0 0</pose>
<geometry>
<box>
<size>0.126 0.01 0.1</size>
</box>
</geometry>
</visual>
<visual name='gripper_wide_block_d_fixed_joint_lump__gripper_side_block_d2_visual_2'>
<pose frame=''>-0.068 -0.229 0 0 -0 0</pose>
<geometry>
<box>
<size>0.126 0.01 0.1</size>
</box>
</geometry>
</visual>
<gravity>1</gravity>
<velocity_decay/>
<joint name='gripper_joint_d' type='prismatic'>
<child>gripper_wide_block_d</child>
<parent>gripper_center</parent>
<axis>
<xyz>1 0 0</xyz>
<limit>
<lower>-100</lower>
<upper>100</upper>
<effort>200</effort>
<velocity>0.5</velocity>
</limit>
<dynamics>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
<use_parent_model_frame>1</use_parent_model_frame>
</axis>
<physics>
<provide_feedback>1</provide_feedback>
<ode>
<provide_feedback>1</provide_feedback>
<limit>
<cfm>0</cfm>
<erp>0.2</erp>
</limit>
</ode>
</physics>
</joint>
<pose frame=''>-0.14 0 0 0 -0 0</pose>
<inertial>
<pose frame=''>0.0238 -0 0 0 -0 0</pose>
<mass>1.944</mass>
<inertia>
<ixx>0.0603698</ixx>
<ixy>-3.30291e-19</ixy>
<ixz>0</ixz>
<iyy>0.00457571</iyy>
<iyz>0</iyz>
<izz>0.0617055</izz>
</inertia>
</inertial>
<collision name='gripper_wide_block_p_collision'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<box>
<size>0.01 0.468 0.1</size>
</box>
</geometry>
<surface>
<contact>
<ode>
<max_vel>0</max_vel>
<min_depth>0</min_depth>
</ode>
</contact>
<friction>
<ode>
<mu>100</mu>
<mu2>100</mu2>
</ode>
</friction>
</surface>
</collision>
<collision name='gripper_wide_block_p_fixed_joint_lump__collision_collision_1'>
<pose frame=''>0.068 0.229 0 0 -0 0</pose>
<geometry>
<box>
<size>0.126 0.01 0.1</size>
</box>
</geometry>
</collision>
<collision name='gripper_wide_block_p_fixed_joint_lump__collision_collision_2'>
<pose frame=''>0.068 -0.229 0 0 -0 0</pose>
<geometry>
<box>
<size>0.126 0.01 0.1</size>
</box>
</geometry>
</collision>
<visual name='gripper_wide_block_p_visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<box>
<size>0.01 0.468 0.1</size>
</box>
</geometry>
<material>
<script>
<uri>__default__</uri>
<name>__default__</name>
</script>
</material>
</visual>
<visual name='gripper_wide_block_p_fixed_joint_lump__gripper_side_block_p1_visual_1'>
<pose frame=''>0.068 0.229 0 0 -0 0</pose>
<geometry>
<box>
<size>0.126 0.01 0.1</size>
</box>
</geometry>
</visual>
<visual name='gripper_wide_block_p_fixed_joint_lump__gripper_side_block_p2_visual_2'>
<pose frame=''>0.068 -0.229 0 0 -0 0</pose>
<geometry>
<box>
<size>0.126 0.01 0.1</size>
</box>
</geometry>
</visual>
<gravity>1</gravity>
<velocity_decay/>
<joint name='gripper_joint_p' type='prismatic'>
<child>gripper_wide_block_p</child>
<parent>gripper_center</parent>
<axis>
<xyz>1 0 0</xyz>
<limit>
<lower>-100</lower>
<upper>100</upper>
<effort>200</effort>
<velocity>0.5</velocity>
</limit>
<dynamics>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
<use_parent_model_frame>1</use_parent_model_frame>
</axis>
<physics>
<provide_feedback>1</provide_feedback>
<ode>
<provide_feedback>1</provide_feedback>
<limit>
<cfm>0</cfm>
<erp>0.2</erp>
</limit>
</ode>
</physics>
</joint>
<plugin name='ft_sensor_p' filename='libgazebo_ros_ft_sensor.so'>
<updateRate>100.0</updateRate>
<topicName>ft_sensor_topic_p</topicName>
<jointName>gripper_joint_p</jointName>
</plugin>
<static>0</static>
<plugin name='ft_sensor_d' filename='libgazebo_ros_ft_sensor.so'>
<updateRate>100.0</updateRate>
<topicName>ft_sensor_topic_d</topicName>
<jointName>gripper_joint_d</jointName>
</plugin>
</model>
</sdf>


Thanks for your time!

### Problem with simple grasping simulation

Grasping is not working in this primitive case I've made with Gazebo 9 and ROS Kinetic. This question is a follow-up to a question I posted here earlier. The reason I've made a separate post is that I have some new data to include and the other question had already grown pretty long.

The object is a box of mass 1 and default friction and contact parameters. The gripper uses two prismatic joints to move two c-shaped links together. All these are shown in this image. Note the gripper-joint coordinate frames (the x- and z-direction vectors are visible).

When the gripper closes on the box and the robot then moves upward, the box does not get lifted, as shown in this animation (it's for a taller gripper than above but the behavior is identical, also the gripper in the animation used just one prismatic joint). Since the normal force between the gripper and box is 200 N (that's what I have the joint effort set at, as shown in the sdf below) then the lifting force should be at least 200 N, while 9.81 N should suffice to lift the box. Do you have an idea why the box doesn't go up?

The following plots were made via

rqt_plot ft_senr_topic_p/wrench/force/x, ft_sensor_topic_p/wrench/force/z, ft_sensor_topic_d/wrench/force/x, ft_sensor_topic_d/wrench/force/z


to graph the x and z components of the joint forces for the two prismatic gripper joints. In this one the joints are controlled with the EffortJointInterface. When the x components drop down, at about 28.6 seconds, that's when the gripper is higher than the box and it closes.

For this plot the gripper friction coefficient is changed to 100, which appears to have made no change.

The final plot is with mu=100 and using the PositionJointInterface. At about 24.4 seconds On this one, seconds, the x-components drop down. That's when the robot starts to move upward (prior to that it's stationary with the gripper closed around the box). The spikes after that, at 24.8, are when the gripper clears the box. It's odd to me that one joint has a positive z-component and the other has a negative z-component of force.force since one would think both joints would have z-components of force in the same direction.

Here is the controller part from the gripper urdf xacro file.

<transmission name="gripper_joint_transmission_p">
<type>transmission_interface/SimpleTransmission</type>
<joint name="gripper_joint_p">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="gripper_joint_motor_p">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>

<transmission name="gripper_joint_transmission_d">
<type>transmission_interface/SimpleTransmission</type>
<joint name="gripper_joint_d">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="gripper_joint_motor_d">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>


The sdf it produces is pasted at the end, but first, here is the box sdf.

<?xml version="1.0" ?>
<sdf version="1.5">
<model name="table_block">
<static>false</static>

<inertial>
<mass>1</mass>
</inertial>

<collision name="collision">
<geometry>
<box>
<size>0.32 0.32 0.3</size>
</box>
</geometry>
<!--surface>
<friction>
<ode>
<mu>100.0</mu>
<mu2>100.0</mu2>
</ode>
</friction>
<contact>
<ode>
<kp>1e9</kp>
<kd>1e2</kd>
<max_vel>0</max_vel>
<min_depth>0.0</min_depth>
</ode>
</contact>
</surface-->
</collision>

<visual name="visual">
<geometry>
<box>
<size>0.32 0.32 0.3</size>
</box>
</geometry>
<material>
<ambient>1 0 0 1</ambient>
<diffuse>1 0 0 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>

<sensor name='my_contact' type='contact'>
<contact>
<collision>collision</collision>
</contact>
<update_rate> 5 </update_rate>
</sensor>
</model>
</sdf>


The surface tag is commented out because no changes there helped the simulation. Here is the gripper sdf (again, it came from gripper.urdf.xacro).

<sdf version='1.6'>
<model name='gripper'>
<pose frame=''>0 0 0 0 -0 0</pose>
<inertial>
<pose frame=''>0 0 0 0 -0 0</pose>
<mass>0.01</mass>
<inertia>
<ixx>0.0001</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.0001</iyy>
<iyz>0</iyz>
<izz>0.0001</izz>
</inertia>
</inertial>
<pose frame=''>0.14 0 0 0 -0 0</pose>
<inertial>
<pose frame=''>-0.0238 -0 0 0 -0 0</pose>
<mass>1.944</mass>
<inertia>
<ixx>0.0603698</ixx>
<ixy>3.30291e-19</ixy>
<ixz>0</ixz>
<iyy>0.00457571</iyy>
<iyz>0</iyz>
<izz>0.0617055</izz>
</inertia>
</inertial>
<collision name='gripper_wide_block_d_collision'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<box>
<size>0.01 0.468 0.1</size>
</box>
</geometry>
<surface>
<contact>
<ode>
<max_vel>0</max_vel>
<min_depth>0</min_depth>
</ode>
</contact>
<friction>
<ode>
<mu>100</mu>
<mu2>100</mu2>
</ode>
</friction>
</surface>
</collision>
<collision name='gripper_wide_block_d_fixed_joint_lump__collision_collision_1'>
<pose frame=''>-0.068 0.229 0 0 -0 0</pose>
<geometry>
<box>
<size>0.126 0.01 0.1</size>
</box>
</geometry>
</collision>
<collision name='gripper_wide_block_d_fixed_joint_lump__collision_collision_2'>
<pose frame=''>-0.068 -0.229 0 0 -0 0</pose>
<geometry>
<box>
<size>0.126 0.01 0.1</size>
</box>
</geometry>
</collision>
<visual name='gripper_wide_block_d_visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<box>
<size>0.01 0.468 0.1</size>
</box>
</geometry>
<material>
<script>
<uri>__default__</uri>
<name>__default__</name>
</script>
</material>
</visual>
<visual name='gripper_wide_block_d_fixed_joint_lump__gripper_side_block_d1_visual_1'>
<pose frame=''>-0.068 0.229 0 0 -0 0</pose>
<geometry>
<box>
<size>0.126 0.01 0.1</size>
</box>
</geometry>
</visual>
<visual name='gripper_wide_block_d_fixed_joint_lump__gripper_side_block_d2_visual_2'>
<pose frame=''>-0.068 -0.229 0 0 -0 0</pose>
<geometry>
<box>
<size>0.126 0.01 0.1</size>
</box>
</geometry>
</visual>
<gravity>1</gravity>
<velocity_decay/>
<joint name='gripper_joint_d' type='prismatic'>
<child>gripper_wide_block_d</child>
<parent>gripper_center</parent>
<axis>
<xyz>1 0 0</xyz>
<limit>
<lower>-100</lower>
<upper>100</upper>
<effort>200</effort>
<velocity>0.5</velocity>
</limit>
<dynamics>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
<use_parent_model_frame>1</use_parent_model_frame>
</axis>
<physics>
<provide_feedback>1</provide_feedback>
<ode>
<provide_feedback>1</provide_feedback>
<limit>
<cfm>0</cfm>
<erp>0.2</erp>
</limit>
</ode>
</physics>
</joint>
<pose frame=''>-0.14 0 0 0 -0 0</pose>
<inertial>
<pose frame=''>0.0238 -0 0 0 -0 0</pose>
<mass>1.944</mass>
<inertia>
<ixx>0.0603698</ixx>
<ixy>-3.30291e-19</ixy>
<ixz>0</ixz>
<iyy>0.00457571</iyy>
<iyz>0</iyz>
<izz>0.0617055</izz>
</inertia>
</inertial>
<collision name='gripper_wide_block_p_collision'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<box>
<size>0.01 0.468 0.1</size>
</box>
</geometry>
<surface>
<contact>
<ode>
<max_vel>0</max_vel>
<min_depth>0</min_depth>
</ode>
</contact>
<friction>
<ode>
<mu>100</mu>
<mu2>100</mu2>
</ode>
</friction>
</surface>
</collision>
<collision name='gripper_wide_block_p_fixed_joint_lump__collision_collision_1'>
<pose frame=''>0.068 0.229 0 0 -0 0</pose>
<geometry>
<box>
<size>0.126 0.01 0.1</size>
</box>
</geometry>
</collision>
<collision name='gripper_wide_block_p_fixed_joint_lump__collision_collision_2'>
<pose frame=''>0.068 -0.229 0 0 -0 0</pose>
<geometry>
<box>
<size>0.126 0.01 0.1</size>
</box>
</geometry>
</collision>
<visual name='gripper_wide_block_p_visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<box>
<size>0.01 0.468 0.1</size>
</box>
</geometry>
<material>
<script>
<uri>__default__</uri>
<name>__default__</name>
</script>
</material>
</visual>
<visual name='gripper_wide_block_p_fixed_joint_lump__gripper_side_block_p1_visual_1'>
<pose frame=''>0.068 0.229 0 0 -0 0</pose>
<geometry>
<box>
<size>0.126 0.01 0.1</size>
</box>
</geometry>
</visual>
<visual name='gripper_wide_block_p_fixed_joint_lump__gripper_side_block_p2_visual_2'>
<pose frame=''>0.068 -0.229 0 0 -0 0</pose>
<geometry>
<box>
<size>0.126 0.01 0.1</size>
</box>
</geometry>
</visual>
<gravity>1</gravity>
<velocity_decay/>
<joint name='gripper_joint_p' type='prismatic'>
<child>gripper_wide_block_p</child>
<parent>gripper_center</parent>
<axis>
<xyz>1 0 0</xyz>
<limit>
<lower>-100</lower>
<upper>100</upper>
<effort>200</effort>
<velocity>0.5</velocity>
</limit>
<dynamics>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
<use_parent_model_frame>1</use_parent_model_frame>
</axis>
<physics>
<provide_feedback>1</provide_feedback>
<ode>
<provide_feedback>1</provide_feedback>
<limit>
<cfm>0</cfm>
<erp>0.2</erp>
</limit>
</ode>
</physics>
</joint>
<plugin name='ft_sensor_p' filename='libgazebo_ros_ft_sensor.so'>
<updateRate>100.0</updateRate>
<topicName>ft_sensor_topic_p</topicName>
<jointName>gripper_joint_p</jointName>
</plugin>
<static>0</static>
<plugin name='ft_sensor_d' filename='libgazebo_ros_ft_sensor.so'>
<updateRate>100.0</updateRate>
<topicName>ft_sensor_topic_d</topicName>
<jointName>gripper_joint_d</jointName>
</plugin>
</model>
</sdf>


Thanks for your time!

### Problem with simple grasping simulation

Grasping is not working in this primitive case I've made with Gazebo 9 and ROS Kinetic. This question is a follow-up to a question I posted here earlier. The reason I've made a separate post is that I have some new data to include and the other question had already grown pretty long.

made. The object is a box of mass 1 and default friction and contact parameters. The gripper uses two prismatic joints to move two c-shaped links together. All these are shown in this image. Note the gripper-joint coordinate frames (the x- and z-direction vectors are visible).

When the gripper closes on the box and the robot then moves upward, the box does not get lifted, as shown in this animation (it's for a taller gripper than above but the behavior is identical, also the gripper in the animation used just one prismatic joint). Since the normal force between the gripper and box is 200 N (that's what I have the joint effort set at, as shown in the sdf below) then the lifting force should be at least 200 N, while 9.81 N should suffice to lift the box. Do you have an idea why the box doesn't go up?

The following plots were made via

rqt_plot ft_senr_topic_p/wrench/force/x, ft_sensor_topic_p/wrench/force/z, ft_sensor_topic_d/wrench/force/x, ft_sensor_topic_d/wrench/force/z


to graph the x and z components of the joint forces for the two prismatic gripper joints. In this one the joints are controlled with the EffortJointInterface. EffortJointInterface. When the x components drop down, at about 28.6 seconds, that's when the gripper is higher than the box and it closes.

For this plot the gripper friction coefficient is changed to 100, which appears to have made no change.

The final plot is with mu=100 and using the PositionJointInterface. PositionJointInterface. At about 24.4 seconds, seconds On this one, the x-components drop down. That's when the robot starts to move upward (prior to that it's stationary with the gripper closed around the box). The spikes after that, at 24.8, are when the gripper clears the box. It's odd to me that one joint has a positive z-component and the other has a negative z-component of force since one force.

Update

In an attempt to circumvent this problem I made an object that's wider in the middle and gripper blocks to match it. In theory, if the gripper stayed at its closed position then friction would think not be required for lifting the object. I increased the effort limit on the joints from 200 to 500.

With 'EffortJointInterface' the same problem occurs as with the box, and with the PositionJointInterface the grasp is unstable and the object goes flying away.

To battle the instability I changed max_vel and min_depth on both joints would have z-components of force in the same direction.

the gripper and object to 0 and 0.001, respectively. Alas, the simulation remained unstable with PositionJointInterface even with these changes.

Here is the controller part from the gripper urdf xacro file.

an animation and force plot when using <transmission name="gripper_joint_transmission_p">
<type>transmission_interface/SimpleTransmission</type>
<joint name="gripper_joint_p">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="gripper_joint_motor_p">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>

<transmission name="gripper_joint_transmission_d">
<type>transmission_interface/SimpleTransmission</type>
<joint name="gripper_joint_d">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="gripper_joint_motor_d">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
EffortJointInterface

The sdf it produces is pasted at the end, but first, here is the box sdf.

<?xml version="1.0" ?>
<sdf version="1.5">
<model name="table_block">
<static>false</static>

<inertial>
<mass>1</mass>
</inertial>

<collision name="collision">
<geometry>
<box>
<size>0.32 0.32 0.3</size>
</box>
</geometry>
<!--surface>
<friction>
<ode>
<mu>100.0</mu>
<mu2>100.0</mu2>
</ode>
</friction>
<contact>
<ode>
<kp>1e9</kp>
<kd>1e2</kd>
<max_vel>0</max_vel>
<min_depth>0.0</min_depth>
</ode>
</contact>
</surface-->
</collision>

<visual name="visual">
<geometry>
<box>
<size>0.32 0.32 0.3</size>
</box>
</geometry>
<material>
<ambient>1 0 0 1</ambient>
<diffuse>1 0 0 1</diffuse>
<specular>0.1 0.1 0.1 1</specular>
<emissive>0 0 0 0</emissive>
</material>
</visual>

<sensor name='my_contact' type='contact'>
<contact>
<collision>collision</collision>
</contact>
<update_rate> 5 </update_rate>
</sensor>
</model>
</sdf>


The surface tag is commented out because no changes there helped the simulation. .

Here is the gripper an animation and force plot when using PositionJointInterface.

Here is the sdf (again, it came from gripper.urdf.xacro).of the object.

<sdf version='1.6'>
<model name='gripper'>
name='wideCone'>
name='wideCone'>
<pose frame=''>0 0 0 0 -0 0</pose>
<inertial>
<pose frame=''>0 frame=''>-0 0 0 0.145 0 -0 0</pose>
<mass>0.01</mass>
<mass>0.1</mass>
<inertia>
<ixx>0.0001</ixx>
<ixx>0</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.0001</iyy>
<iyy>0</iyy>
<iyz>0</iyz>
<izz>0.0001</izz>
<izz>0</izz>
</inertia>
<!--mass>9</mass>
<inertia>
<ixx>0.443658</ixx>
<ixy>-4.45426e-18</ixy>
<ixz>-4.19225e-18</ixz>
<iyy>0.443658</iyy>
<iyz>2.2175e-17</iyz>
<izz>0.52339</izz>
</inertia-->
</inertial>
<pose frame=''>0.14 0 0 0 -0 0</pose>
<inertial>
<pose frame=''>-0.0238 -0 0 0 -0 0</pose>
<mass>1.944</mass>
<inertia>
<ixx>0.0603698</ixx>
<ixy>3.30291e-19</ixy>
<ixz>0</ixz>
<iyy>0.00457571</iyy>
<iyz>0</iyz>
<izz>0.0617055</izz>
</inertia>
</inertial>
<collision name='gripper_wide_block_d_collision'>
name='wideCone_collision'>
<pose frame=''>0 0 0 0 1.5708 -0 0</pose>
<geometry>
<box>
<size>0.01 0.468 0.1</size>
</box>
<mesh>
<scale>1 1 1</scale>
<uri>model://wideCone/meshes/wideCone.STL</uri>
</mesh>
</geometry>
<surface>
<!--friction>
<ode>
<mu>100.0</mu>
<mu2>100.0</mu2>
</ode>
</friction-->
<contact>
<ode>
<!--kp>1e9</kp>
<kd>1e2</kd-->
<max_vel>0</max_vel>
<min_depth>0</min_depth>
<min_depth>0.001</min_depth>
</ode>
</contact>
<friction>
<ode>
<mu>100</mu>
<mu2>100</mu2>
</ode>
</friction>
</surface>
</collision>
<collision name='gripper_wide_block_d_fixed_joint_lump__collision_collision_1'>
<pose frame=''>-0.068 0.229 0 0 -0 0</pose>
<geometry>
<box>
<size>0.126 0.01 0.1</size>
</box>
</geometry>
</collision>
<collision name='gripper_wide_block_d_fixed_joint_lump__collision_collision_2'>
<pose frame=''>-0.068 -0.229 0 0 -0 0</pose>
<geometry>
<box>
<size>0.126 0.01 0.1</size>
</box>
</geometry>
</collision>
<visual name='gripper_wide_block_d_visual'>
name='wideCone_visual'>
<pose frame=''>0 0 0 0 1.5708 -0 0</pose>
<geometry>
<box>
<size>0.01 0.468 0.1</size>
</box>
<mesh>
<scale>1 1 1</scale>
<uri>model://wideCone/meshes/wideCone.STL</uri>
</mesh>
</geometry>
<material>
<script>
<uri>__default__</uri>
<name>__default__</name>
</script>
</material>
</visual>
<visual name='gripper_wide_block_d_fixed_joint_lump__gripper_side_block_d1_visual_1'>
<pose frame=''>-0.068 0.229 0 0 -0 0</pose>
<geometry>
<box>
<size>0.126 0.01 0.1</size>
</box>
</geometry>
</visual>
<visual name='gripper_wide_block_d_fixed_joint_lump__gripper_side_block_d2_visual_2'>
<pose frame=''>-0.068 -0.229 0 0 -0 0</pose>
<geometry>
<box>
<size>0.126 0.01 0.1</size>
</box>
</geometry>
</visual>
<gravity>1</gravity>
<velocity_decay/>
<joint name='gripper_joint_d' type='prismatic'>
<child>gripper_wide_block_d</child>
<parent>gripper_center</parent>
<axis>
<xyz>1 0 0</xyz>
<limit>
<lower>-100</lower>
<upper>100</upper>
<effort>200</effort>
<velocity>0.5</velocity>
</limit>
<dynamics>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
<use_parent_model_frame>1</use_parent_model_frame>
</axis>
<physics>
<provide_feedback>1</provide_feedback>
<ode>
<provide_feedback>1</provide_feedback>
<limit>
<cfm>0</cfm>
<erp>0.2</erp>
</limit>
</ode>
</physics>
</joint>
<pose frame=''>-0.14 0 0 0 -0 0</pose>
<inertial>
<pose frame=''>0.0238 -0 0 0 -0 0</pose>
<mass>1.944</mass>
<inertia>
<ixx>0.0603698</ixx>
<ixy>-3.30291e-19</ixy>
<ixz>0</ixz>
<iyy>0.00457571</iyy>
<iyz>0</iyz>
<izz>0.0617055</izz>
</inertia>
</inertial>
<collision name='gripper_wide_block_p_collision'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<box>
<size>0.01 0.468 0.1</size>
</box>
</geometry>
<surface>
<contact>
<ode>
<max_vel>0</max_vel>
<min_depth>0</min_depth>
</ode>
</contact>
<friction>
<ode>
<mu>100</mu>
<mu2>100</mu2>
</ode>
</friction>
</surface>
</collision>
<collision name='gripper_wide_block_p_fixed_joint_lump__collision_collision_1'>
<pose frame=''>0.068 0.229 0 0 -0 0</pose>
<geometry>
<box>
<size>0.126 0.01 0.1</size>
</box>
</geometry>
</collision>
<collision name='gripper_wide_block_p_fixed_joint_lump__collision_collision_2'>
<pose frame=''>0.068 -0.229 0 0 -0 0</pose>
<geometry>
<box>
<size>0.126 0.01 0.1</size>
</box>
</geometry>
</collision>
<visual name='gripper_wide_block_p_visual'>
<pose frame=''>0 0 0 0 -0 0</pose>
<geometry>
<box>
<size>0.01 0.468 0.1</size>
</box>
</geometry>
<material>
<script>
<uri>__default__</uri>
<name>__default__</name>
</script>
</material>
</visual>
<visual name='gripper_wide_block_p_fixed_joint_lump__gripper_side_block_p1_visual_1'>
<pose frame=''>0.068 0.229 0 0 -0 0</pose>
<geometry>
<box>
<size>0.126 0.01 0.1</size>
</box>
</geometry>
</visual>
<visual name='gripper_wide_block_p_fixed_joint_lump__gripper_side_block_p2_visual_2'>
<pose frame=''>0.068 -0.229 0 0 -0 0</pose>
<geometry>
<box>
<size>0.126 0.01 0.1</size>
</box>
</geometry>
</visual>
<gravity>1</gravity>
<velocity_decay/>
<joint name='gripper_joint_p' type='prismatic'>
<child>gripper_wide_block_p</child>
<parent>gripper_center</parent>
<axis>
<xyz>1 0 0</xyz>
<limit>
<lower>-100</lower>
<upper>100</upper>
<effort>200</effort>
<velocity>0.5</velocity>
</limit>
<dynamics>
<spring_reference>0</spring_reference>
<spring_stiffness>0</spring_stiffness>
</dynamics>
<use_parent_model_frame>1</use_parent_model_frame>
</axis>
<physics>
<provide_feedback>1</provide_feedback>
<ode>
<provide_feedback>1</provide_feedback>
<limit>
<cfm>0</cfm>
<erp>0.2</erp>
</limit>
</ode>
</physics>
</joint>
<plugin name='ft_sensor_p' filename='libgazebo_ros_ft_sensor.so'>
<updateRate>100.0</updateRate>
<topicName>ft_sensor_topic_p</topicName>
<jointName>gripper_joint_p</jointName>
</plugin>
<static>0</static>
<plugin name='ft_sensor_d' filename='libgazebo_ros_ft_sensor.so'>
<updateRate>100.0</updateRate>
<topicName>ft_sensor_topic_d</topicName>
<jointName>gripper_joint_d</jointName>
</plugin>
</model>
</sdf>


Here is the gripper urdf xacro.

<?xml version="1.0"?>
<robot name="gripper"
xmlns:xacro="http://ros.org/wiki/xacro">

<!-- Switch between EffortJointInterface and PositionJointInterface, still requires change in yaml files -->
<xacro:property name="using_position_not_effort" value="true" />

<!-- Constants for gripper joint controllers -->
<xacro:property name="effort_limit" value="500" />
<xacro:property name="gripper_friction" value="100" />
<xacro:property name="gripper_min_depth" value="0.001" />

<!-- Phantom link for center of gripper -->
<inertial>
<mass value="0.01"/>
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
</inertial>

<!-- One side of gripper -->
<visual>
<origin xyz="0 0.177 0" rpy="${pi / 2} 0${5 * pi / 8}" />
<geometry>
<mesh filename="package://mobile_manipulator/models/gripper_fake/meshes/gripperClaw.STL"/>
</geometry>
</visual>
<collision>
<origin xyz="0 0.177 0" rpy="${pi / 2} 0${5 * pi / 8}" />
<geometry>
<mesh filename="package://mobile_manipulator/models/gripper_fake/meshes/gripperClaw.STL"/>
</geometry>
</collision>
<inertial>
<mass value="1" />
<inertia ixx="0.01" ixy="0.005" ixz="0.0003" iyy="0.02" iyz="0.00004" izz="0.02" />
</inertial>

<!-- Second side of gripper -->
<visual>
<origin xyz="0 -0.177 0" rpy="${pi / 2} 0${-3 * pi / 8}" />
<geometry>
<mesh filename="package://mobile_manipulator/models/gripper_fake/meshes/gripperClaw.STL"/>
</geometry>
</visual>
<collision>
<origin xyz="0 -0.177 0" rpy="${pi / 2} 0${-3 * pi / 8}" />
<geometry>
<mesh filename="package://mobile_manipulator/models/gripper_fake/meshes/gripperClaw.STL"/>
</geometry>
</collision>
<inertial>
<mass value="1" />
<inertia ixx="0.01" ixy="0.005" ixz="0.0003" iyy="0.02" iyz="0.00004" izz="0.02" />
</inertial>

<!-- Prismatic joint -->
<joint name="gripper_joint_p" type="prismatic">
<origin xyz="0 0 0" rpy="0 0 0" />
<limit effort="${effort_limit}" lower="-100" upper="100" velocity="0.5"/> </joint> <joint name="gripper_joint_d" type="prismatic"> <origin xyz="0 0 0" rpy="0 0 0" /> <parent link="gripper_center" /> <child link="side_d" /> <limit effort="${effort_limit}" lower="-100" upper="100" velocity="0.5"/>
</joint>

<!-- Transmission for ROS control -->
<xacro:if value="${using_position_not_effort == 'true'}"> <transmission name="gripper_joint_transmission_p"> <type>transmission_interface/SimpleTransmission</type> <joint name="gripper_joint_p"> <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface> </joint> <actuator name="gripper_joint_motor_p"> <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface> <mechanicalReduction>1</mechanicalReduction> </actuator> </transmission> <transmission name="gripper_joint_transmission_d"> <type>transmission_interface/SimpleTransmission</type> <joint name="gripper_joint_d"> <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface> </joint> <actuator name="gripper_joint_motor_d"> <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface> <mechanicalReduction>1</mechanicalReduction> </actuator> </transmission> </xacro:if> <xacro:if value="${using_position_not_effort == 'false'}">
<transmission name="gripper_joint_transmission_p">
<type>transmission_interface/SimpleTransmission</type>
<joint name="gripper_joint_p">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="gripper_joint_motor_p">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>

<transmission name="gripper_joint_transmission_d">
<type>transmission_interface/SimpleTransmission</type>
<joint name="gripper_joint_d">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="gripper_joint_motor_d">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
</xacro:if>

<!-- Enable joint feedback -->
<gazebo reference="gripper_joint_p">
<provideFeedback>true</provideFeedback>
</gazebo>

<gazebo reference="gripper_joint_d">
<provideFeedback>true</provideFeedback>
</gazebo>

<!-- ft_sensor plugin -->
<gazebo>
<plugin name="ft_sensor_p" filename="libgazebo_ros_ft_sensor.so">
<updateRate>100.0</updateRate>
<topicName>ft_sensor_topic_p</topicName>
<jointName>gripper_joint_p</jointName>
</plugin>
</gazebo>

<gazebo>
<plugin name="ft_sensor_d" filename="libgazebo_ros_ft_sensor.so">
<updateRate>100.0</updateRate>
<topicName>ft_sensor_topic_d</topicName>
<jointName>gripper_joint_d</jointName>
</plugin>
</gazebo>

<!-- High friction on gripper -->
<gazebo reference="gripper_wide_block_p">
<mu1>${gripper_friction}</mu1> <mu2>${gripper_friction}</mu2>
<minDepth>${gripper_min_depth}</minDepth> <maxVel>0</maxVel> </gazebo> <gazebo reference="gripper_wide_block_d"> <mu1>${gripper_friction}</mu1>
<mu2>${gripper_friction}</mu2> <minDepth>${gripper_min_depth}</minDepth>
<maxVel>0</maxVel>
</gazebo>

</robot>


Thanks for your time!

### Problem with simple grasping simulation

Edit Do you think this strange behavior could be from using Gazeb 9 with the latest version of gazebo_ros_pkgs, which was done as a fix for an earlier problem?

Grasping is not working in this primitive case I've made. The object is a box of mass 1 and default friction and contact parameters. The gripper uses two prismatic joints to move two c-shaped links together. All these are shown in this image. Note the gripper-joint coordinate frames (the x- and z-direction vectors are visible).

When the gripper closes on the box and the robot then moves upward, the box does not get lifted, as shown in this animation (it's for a taller gripper than above but the behavior is identical, also the gripper in the animation used just one prismatic joint). Since the normal force between the gripper and box is 200 N (that's what I have the joint effort set at, as shown in the sdf below) then the lifting force should be at least 200 N, while 9.81 N should suffice to lift the box. Do you have an idea why the box doesn't go up?

The following plots were made via

rqt_plot ft_senr_topic_p/wrench/force/x, ft_sensor_topic_p/wrench/force/z, ft_sensor_topic_d/wrench/force/x, ft_sensor_topic_d/wrench/force/z


to graph the x and z components of the joint forces for the two prismatic gripper joints. In this one the joints are controlled with the EffortJointInterface. When the x components drop down, at about 28.6 seconds, that's when the gripper is higher than the box and it closes.

For this plot the gripper friction coefficient is changed to 100, which appears to have made no change.

The final plot is with mu=100 and using the PositionJointInterface. At about 24.4 seconds On this one, the x-components drop down. That's when the robot starts to move upward (prior to that it's stationary with the gripper closed around the box). The spikes after that, at 24.8, are when the gripper clears the box. It's odd to me that one joint has a positive z-component and the other has a negative z-component of force.

Update

In an attempt to circumvent this problem I made an object that's wider in the middle and gripper blocks to match it. In theory, if the gripper stayed at its closed position then friction would not be required for lifting the object. I increased the effort limit on the joints from 200 to 500.

With 'EffortJointInterface' the same problem occurs as with the box, and with the PositionJointInterface the grasp is unstable and the object goes flying away.

To battle the instability I changed max_vel and min_depth on both the gripper and object to 0 and 0.001, respectively. Alas, the simulation remained unstable with PositionJointInterface even with these changes.

Here is an animation and force plot when using EffortJointInterface.

Here is an animation and force plot when using PositionJointInterface.

Here is the sdf of the object.

<sdf version='1.6'>
<model name='wideCone'>
<pose frame=''>0 0 0 0 -0 0</pose>
<inertial>
<pose frame=''>-0 0 0.145 0 -0 0</pose>
<mass>0.1</mass>
<inertia>
<ixx>0</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0</iyy>
<iyz>0</iyz>
<izz>0</izz>
</inertia>
<!--mass>9</mass>
<inertia>
<ixx>0.443658</ixx>
<ixy>-4.45426e-18</ixy>
<ixz>-4.19225e-18</ixz>
<iyy>0.443658</iyy>
<iyz>2.2175e-17</iyz>
<izz>0.52339</izz>
</inertia-->
</inertial>
<collision name='wideCone_collision'>
<pose frame=''>0 0 0 1.5708 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>model://wideCone/meshes/wideCone.STL</uri>
</mesh>
</geometry>
<surface>
<!--friction>
<ode>
<mu>100.0</mu>
<mu2>100.0</mu2>
</ode>
</friction-->
<contact>
<ode>
<!--kp>1e9</kp>
<kd>1e2</kd-->
<max_vel>0</max_vel>
<min_depth>0.001</min_depth>
</ode>
</contact>
</surface>
</collision>
<visual name='wideCone_visual'>
<pose frame=''>0 0 0 1.5708 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>model://wideCone/meshes/wideCone.STL</uri>
</mesh>
</geometry>
</visual>
</model>
</sdf>


Here is the gripper urdf xacro.

<?xml version="1.0"?>
<robot name="gripper"
xmlns:xacro="http://ros.org/wiki/xacro">

<!-- Switch between EffortJointInterface and PositionJointInterface, still requires change in yaml files -->
<xacro:property name="using_position_not_effort" value="true" />

<!-- Constants for gripper joint controllers -->
<xacro:property name="effort_limit" value="500" />
<xacro:property name="gripper_friction" value="100" />
<xacro:property name="gripper_min_depth" value="0.001" />

<!-- Phantom link for center of gripper -->
<inertial>
<mass value="0.01"/>
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
</inertial>

<!-- One side of gripper -->
<visual>
<origin xyz="0 0.177 0" rpy="${pi / 2} 0${5 * pi / 8}" />
<geometry>
<mesh filename="package://mobile_manipulator/models/gripper_fake/meshes/gripperClaw.STL"/>
</geometry>
</visual>
<collision>
<origin xyz="0 0.177 0" rpy="${pi / 2} 0${5 * pi / 8}" />
<geometry>
<mesh filename="package://mobile_manipulator/models/gripper_fake/meshes/gripperClaw.STL"/>
</geometry>
</collision>
<inertial>
<mass value="1" />
<inertia ixx="0.01" ixy="0.005" ixz="0.0003" iyy="0.02" iyz="0.00004" izz="0.02" />
</inertial>

<!-- Second side of gripper -->
<visual>
<origin xyz="0 -0.177 0" rpy="${pi / 2} 0${-3 * pi / 8}" />
<geometry>
<mesh filename="package://mobile_manipulator/models/gripper_fake/meshes/gripperClaw.STL"/>
</geometry>
</visual>
<collision>
<origin xyz="0 -0.177 0" rpy="${pi / 2} 0${-3 * pi / 8}" />
<geometry>
<mesh filename="package://mobile_manipulator/models/gripper_fake/meshes/gripperClaw.STL"/>
</geometry>
</collision>
<inertial>
<mass value="1" />
<inertia ixx="0.01" ixy="0.005" ixz="0.0003" iyy="0.02" iyz="0.00004" izz="0.02" />
</inertial>

<!-- Prismatic joint -->
<joint name="gripper_joint_p" type="prismatic">
<origin xyz="0 0 0" rpy="0 0 0" />
<limit effort="${effort_limit}" lower="-100" upper="100" velocity="0.5"/> </joint> <joint name="gripper_joint_d" type="prismatic"> <origin xyz="0 0 0" rpy="0 0 0" /> <parent link="gripper_center" /> <child link="side_d" /> <limit effort="${effort_limit}" lower="-100" upper="100" velocity="0.5"/>
</joint>

<!-- Transmission for ROS control -->
<xacro:if value="${using_position_not_effort == 'true'}"> <transmission name="gripper_joint_transmission_p"> <type>transmission_interface/SimpleTransmission</type> <joint name="gripper_joint_p"> <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface> </joint> <actuator name="gripper_joint_motor_p"> <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface> <mechanicalReduction>1</mechanicalReduction> </actuator> </transmission> <transmission name="gripper_joint_transmission_d"> <type>transmission_interface/SimpleTransmission</type> <joint name="gripper_joint_d"> <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface> </joint> <actuator name="gripper_joint_motor_d"> <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface> <mechanicalReduction>1</mechanicalReduction> </actuator> </transmission> </xacro:if> <xacro:if value="${using_position_not_effort == 'false'}">
<transmission name="gripper_joint_transmission_p">
<type>transmission_interface/SimpleTransmission</type>
<joint name="gripper_joint_p">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="gripper_joint_motor_p">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>

<transmission name="gripper_joint_transmission_d">
<type>transmission_interface/SimpleTransmission</type>
<joint name="gripper_joint_d">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="gripper_joint_motor_d">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
</xacro:if>

<!-- Enable joint feedback -->
<gazebo reference="gripper_joint_p">
<provideFeedback>true</provideFeedback>
</gazebo>

<gazebo reference="gripper_joint_d">
<provideFeedback>true</provideFeedback>
</gazebo>

<!-- ft_sensor plugin -->
<gazebo>
<plugin name="ft_sensor_p" filename="libgazebo_ros_ft_sensor.so">
<updateRate>100.0</updateRate>
<topicName>ft_sensor_topic_p</topicName>
<jointName>gripper_joint_p</jointName>
</plugin>
</gazebo>

<gazebo>
<plugin name="ft_sensor_d" filename="libgazebo_ros_ft_sensor.so">
<updateRate>100.0</updateRate>
<topicName>ft_sensor_topic_d</topicName>
<jointName>gripper_joint_d</jointName>
</plugin>
</gazebo>

<!-- High friction on gripper -->
<gazebo reference="gripper_wide_block_p">
<mu1>${gripper_friction}</mu1> <mu2>${gripper_friction}</mu2>
<minDepth>${gripper_min_depth}</minDepth> <maxVel>0</maxVel> </gazebo> <gazebo reference="gripper_wide_block_d"> <mu1>${gripper_friction}</mu1>
<mu2>${gripper_friction}</mu2> <minDepth>${gripper_min_depth}</minDepth>
<maxVel>0</maxVel>
</gazebo>

</robot>


Thanks for your time!

### Problem with simple grasping simulation

Edit Do you think this strange behavior could be from using Gazeb 9 with the latest version of gazebo_ros_pkgs, which was done as a fix for an earlier problem?

Grasping is not working in this primitive case I've made. The object is a box of mass 1 and default friction and contact parameters. The gripper uses two prismatic joints to move two c-shaped links together. All these are shown in this image. Note the gripper-joint coordinate frames (the x- and z-direction vectors are visible).

When the gripper closes on the box and the robot then moves upward, the box does not get lifted, as shown in this animation (it's for a taller gripper than above but the behavior is identical, also the gripper in the animation used just one prismatic joint). Since the normal force between the gripper and box is 200 N (that's what I have the joint effort set at, as shown in the sdf below) then the lifting force should be at least 200 N, while 9.81 N should suffice to lift the box. Do you have an idea why the box doesn't go up?

The following plots were made via

rqt_plot ft_senr_topic_p/wrench/force/x, ft_sensor_topic_p/wrench/force/z, ft_sensor_topic_d/wrench/force/x, ft_sensor_topic_d/wrench/force/z


to graph the x and z components of the joint forces for the two prismatic gripper joints. In this one the joints are controlled with the EffortJointInterface. When the x components drop down, at about 28.6 seconds, that's when the gripper is higher than the box and it closes.

For this plot the gripper friction coefficient is changed to 100, which appears to have made no change.

The final plot is with mu=100 and using the PositionJointInterface. At about 24.4 seconds On this one, the x-components drop down. That's when the robot starts to move upward (prior to that it's stationary with the gripper closed around the box). The spikes after that, at 24.8, are when the gripper clears the box. It's odd to me that one joint has a positive z-component and the other has a negative z-component of force.

Update

In an attempt to circumvent this problem I made an object that's wider in the middle and gripper blocks to match it. In theory, if the gripper stayed at its closed position then friction would not be required for lifting the object. I increased the effort limit on the joints from 200 to 500.

With 'EffortJointInterface' the same problem occurs as with the box, and with the PositionJointInterface the grasp is unstable and the object goes flying away.

To battle the instability I changed max_vel and min_depth on both the gripper and object to 0 and 0.001, respectively. Alas, the simulation remained unstable with PositionJointInterface even with these changes.

Here is an animation and force plot when using EffortJointInterface.

Here is an animation and force plot when using PositionJointInterface.

Here is the sdf of the object.

<sdf version='1.6'>
<model name='wideCone'>
<pose frame=''>0 0 0 0 -0 0</pose>
<inertial>
<pose frame=''>-0 0 0.145 0 -0 0</pose>
<mass>0.1</mass>
<inertia>
<ixx>0</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0</iyy>
<iyz>0</iyz>
<izz>0</izz>
</inertia>
<!--mass>9</mass>
<inertia>
<ixx>0.443658</ixx>
<ixy>-4.45426e-18</ixy>
<ixz>-4.19225e-18</ixz>
<iyy>0.443658</iyy>
<iyz>2.2175e-17</iyz>
<izz>0.52339</izz>
</inertia-->
</inertial>
<collision name='wideCone_collision'>
<pose frame=''>0 0 0 1.5708 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>model://wideCone/meshes/wideCone.STL</uri>
</mesh>
</geometry>
<surface>
<!--friction>
<ode>
<mu>100.0</mu>
<mu2>100.0</mu2>
</ode>
</friction-->
<contact>
<ode>
<!--kp>1e9</kp>
<kd>1e2</kd-->
<max_vel>0</max_vel>
<min_depth>0.001</min_depth>
</ode>
</contact>
</surface>
</collision>
<visual name='wideCone_visual'>
<pose frame=''>0 0 0 1.5708 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>model://wideCone/meshes/wideCone.STL</uri>
</mesh>
</geometry>
</visual>
</model>
</sdf>


Here is the gripper urdf xacro.

<?xml version="1.0"?>
<robot name="gripper"
xmlns:xacro="http://ros.org/wiki/xacro">

<!-- Switch between EffortJointInterface and PositionJointInterface, still requires change in yaml files -->
<xacro:property name="using_position_not_effort" value="true" />

<!-- Constants for gripper joint controllers -->
<xacro:property name="effort_limit" value="500" />
<xacro:property name="gripper_friction" value="100" />
<xacro:property name="gripper_min_depth" value="0.001" />

<!-- Phantom link for center of gripper -->
<inertial>
<mass value="0.01"/>
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
</inertial>

<!-- One side of gripper -->
<visual>
<origin xyz="0 0.177 0" rpy="${pi / 2} 0${5 * pi / 8}" />
<geometry>
<mesh filename="package://mobile_manipulator/models/gripper_fake/meshes/gripperClaw.STL"/>
</geometry>
</visual>
<collision>
<origin xyz="0 0.177 0" rpy="${pi / 2} 0${5 * pi / 8}" />
<geometry>
<mesh filename="package://mobile_manipulator/models/gripper_fake/meshes/gripperClaw.STL"/>
</geometry>
</collision>
<inertial>
<mass value="1" />
<inertia ixx="0.01" ixy="0.005" ixz="0.0003" iyy="0.02" iyz="0.00004" izz="0.02" />
</inertial>

<!-- Second side of gripper -->
<visual>
<origin xyz="0 -0.177 0" rpy="${pi / 2} 0${-3 * pi / 8}" />
<geometry>
<mesh filename="package://mobile_manipulator/models/gripper_fake/meshes/gripperClaw.STL"/>
</geometry>
</visual>
<collision>
<origin xyz="0 -0.177 0" rpy="${pi / 2} 0${-3 * pi / 8}" />
<geometry>
<mesh filename="package://mobile_manipulator/models/gripper_fake/meshes/gripperClaw.STL"/>
</geometry>
</collision>
<inertial>
<mass value="1" />
<inertia ixx="0.01" ixy="0.005" ixz="0.0003" iyy="0.02" iyz="0.00004" izz="0.02" />
</inertial>

<!-- Prismatic joint -->
<joint name="gripper_joint_p" type="prismatic">
<origin xyz="0 0 0" rpy="0 0 0" />
<limit effort="${effort_limit}" lower="-100" upper="100" velocity="0.5"/> </joint> <joint name="gripper_joint_d" type="prismatic"> <origin xyz="0 0 0" rpy="0 0 0" /> <parent link="gripper_center" /> <child link="side_d" /> <limit effort="${effort_limit}" lower="-100" upper="100" velocity="0.5"/>
</joint>

<!-- Transmission for ROS control -->
<xacro:if value="${using_position_not_effort == 'true'}"> <transmission name="gripper_joint_transmission_p"> <type>transmission_interface/SimpleTransmission</type> <joint name="gripper_joint_p"> <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface> </joint> <actuator name="gripper_joint_motor_p"> <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface> <mechanicalReduction>1</mechanicalReduction> </actuator> </transmission> <transmission name="gripper_joint_transmission_d"> <type>transmission_interface/SimpleTransmission</type> <joint name="gripper_joint_d"> <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface> </joint> <actuator name="gripper_joint_motor_d"> <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface> <mechanicalReduction>1</mechanicalReduction> </actuator> </transmission> </xacro:if> <xacro:if value="${using_position_not_effort == 'false'}">
<transmission name="gripper_joint_transmission_p">
<type>transmission_interface/SimpleTransmission</type>
<joint name="gripper_joint_p">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="gripper_joint_motor_p">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>

<transmission name="gripper_joint_transmission_d">
<type>transmission_interface/SimpleTransmission</type>
<joint name="gripper_joint_d">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="gripper_joint_motor_d">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
</xacro:if>

<!-- Enable joint feedback -->
<gazebo reference="gripper_joint_p">
<provideFeedback>true</provideFeedback>
</gazebo>

<gazebo reference="gripper_joint_d">
<provideFeedback>true</provideFeedback>
</gazebo>

<!-- ft_sensor plugin -->
<gazebo>
<plugin name="ft_sensor_p" filename="libgazebo_ros_ft_sensor.so">
<updateRate>100.0</updateRate>
<topicName>ft_sensor_topic_p</topicName>
<jointName>gripper_joint_p</jointName>
</plugin>
</gazebo>

<gazebo>
<plugin name="ft_sensor_d" filename="libgazebo_ros_ft_sensor.so">
<updateRate>100.0</updateRate>
<topicName>ft_sensor_topic_d</topicName>
<jointName>gripper_joint_d</jointName>
</plugin>
</gazebo>

<!-- High friction on gripper -->
<gazebo reference="gripper_wide_block_p">
<mu1>${gripper_friction}</mu1> <mu2>${gripper_friction}</mu2>
<minDepth>${gripper_min_depth}</minDepth> <maxVel>0</maxVel> </gazebo> <gazebo reference="gripper_wide_block_d"> <mu1>${gripper_friction}</mu1>
<mu2>${gripper_friction}</mu2> <minDepth>${gripper_min_depth}</minDepth>
<maxVel>0</maxVel>
</gazebo>

</robot>


Thanks for your time!

Edit Do you think this strange behavior could be from using Gazeb 9 with the latest version of gazebo_ros_pkgs, which was done as a fix for an earlier problem?

### Problem with simple grasping simulation

Grasping is not working in this primitive case I've made. The object is a box of mass 1 and default friction and contact parameters. The gripper uses two prismatic joints to move two c-shaped links together. All these are shown in this image. Note the gripper-joint coordinate frames (the x- and z-direction vectors are visible).

When the gripper closes on the box and the robot then moves upward, the box does not get lifted, as shown in this animation (it's for a taller gripper than above but the behavior is identical, also the gripper in the animation used just one prismatic joint). Since the normal force between the gripper and box is 200 N (that's what I have the joint effort set at, as shown in the sdf below) then the lifting force should be at least 200 N, while 9.81 N should suffice to lift the box. Do you have an idea why the box doesn't go up?

The following plots were made via

rqt_plot ft_senr_topic_p/wrench/force/x, ft_sensor_topic_p/wrench/force/z, ft_sensor_topic_d/wrench/force/x, ft_sensor_topic_d/wrench/force/z


to graph the x and z components of the joint forces for the two prismatic gripper joints. In this one the joints are controlled with the EffortJointInterface. When the x components drop down, at about 28.6 seconds, that's when the gripper is higher than the box and it closes.

For this plot the gripper friction coefficient is changed to 100, which appears to have made no change.

The final plot is with mu=100 and using the PositionJointInterface. At about 24.4 seconds On this one, the x-components drop down. That's when the robot starts to move upward (prior to that it's stationary with the gripper closed around the box). The spikes after that, at 24.8, are when the gripper clears the box. It's odd to me that one joint has a positive z-component and the other has a negative z-component of force.

Update

In an attempt to circumvent this problem I made an object that's wider in the middle and gripper blocks to match it. In theory, if the gripper stayed at its closed position then friction would not be required for lifting the object. I increased the effort limit on the joints from 200 to 500.

With 'EffortJointInterface' the same problem occurs as with the box, and with the PositionJointInterface the grasp is unstable and the object goes flying away.

To battle the instability I changed max_vel and min_depth on both the gripper and object to 0 and 0.001, respectively. Alas, the simulation remained unstable with PositionJointInterface even with these changes.

Here is an animation and force plot when using EffortJointInterface.

Here is an animation and force plot when using PositionJointInterface.

Here is the sdf of the object.

<sdf version='1.6'>
<model name='wideCone'>
<pose frame=''>0 0 0 0 -0 0</pose>
<inertial>
<pose frame=''>-0 0 0.145 0 -0 0</pose>
<mass>0.1</mass>
<inertia>
<ixx>0</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0</iyy>
<iyz>0</iyz>
<izz>0</izz>
</inertia>
<!--mass>9</mass>
<inertia>
<ixx>0.443658</ixx>
<ixy>-4.45426e-18</ixy>
<ixz>-4.19225e-18</ixz>
<iyy>0.443658</iyy>
<iyz>2.2175e-17</iyz>
<izz>0.52339</izz>
</inertia-->
</inertial>
<collision name='wideCone_collision'>
<pose frame=''>0 0 0 1.5708 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>model://wideCone/meshes/wideCone.STL</uri>
</mesh>
</geometry>
<surface>
<!--friction>
<ode>
<mu>100.0</mu>
<mu2>100.0</mu2>
</ode>
</friction-->
<contact>
<ode>
<!--kp>1e9</kp>
<kd>1e2</kd-->
<max_vel>0</max_vel>
<min_depth>0.001</min_depth>
</ode>
</contact>
</surface>
</collision>
<visual name='wideCone_visual'>
<pose frame=''>0 0 0 1.5708 -0 0</pose>
<geometry>
<mesh>
<scale>1 1 1</scale>
<uri>model://wideCone/meshes/wideCone.STL</uri>
</mesh>
</geometry>
</visual>
</model>
</sdf>


Here is the gripper urdf xacro.

<?xml version="1.0"?>
<robot name="gripper"
xmlns:xacro="http://ros.org/wiki/xacro">

<!-- Switch between EffortJointInterface and PositionJointInterface, still requires change in yaml files -->
<xacro:property name="using_position_not_effort" value="true" />

<!-- Constants for gripper joint controllers -->
<xacro:property name="effort_limit" value="500" />
<xacro:property name="gripper_friction" value="100" />
<xacro:property name="gripper_min_depth" value="0.001" />

<!-- Phantom link for center of gripper -->
<inertial>
<mass value="0.01"/>
<inertia ixx="0.0001" ixy="0.0" ixz="0.0" iyy="0.0001" iyz="0.0" izz="0.0001"/>
</inertial>

<!-- One side of gripper -->
<visual>
<origin xyz="0 0.177 0" rpy="${pi / 2} 0${5 * pi / 8}" />
<geometry>
<mesh filename="package://mobile_manipulator/models/gripper_fake/meshes/gripperClaw.STL"/>
</geometry>
</visual>
<collision>
<origin xyz="0 0.177 0" rpy="${pi / 2} 0${5 * pi / 8}" />
<geometry>
<mesh filename="package://mobile_manipulator/models/gripper_fake/meshes/gripperClaw.STL"/>
</geometry>
</collision>
<inertial>
<mass value="1" />
<inertia ixx="0.01" ixy="0.005" ixz="0.0003" iyy="0.02" iyz="0.00004" izz="0.02" />
</inertial>

<!-- Second side of gripper -->
<visual>
<origin xyz="0 -0.177 0" rpy="${pi / 2} 0${-3 * pi / 8}" />
<geometry>
<mesh filename="package://mobile_manipulator/models/gripper_fake/meshes/gripperClaw.STL"/>
</geometry>
</visual>
<collision>
<origin xyz="0 -0.177 0" rpy="${pi / 2} 0${-3 * pi / 8}" />
<geometry>
<mesh filename="package://mobile_manipulator/models/gripper_fake/meshes/gripperClaw.STL"/>
</geometry>
</collision>
<inertial>
<mass value="1" />
<inertia ixx="0.01" ixy="0.005" ixz="0.0003" iyy="0.02" iyz="0.00004" izz="0.02" />
</inertial>

<!-- Prismatic joint -->
<joint name="gripper_joint_p" type="prismatic">
<origin xyz="0 0 0" rpy="0 0 0" />
<limit effort="${effort_limit}" lower="-100" upper="100" velocity="0.5"/> </joint> <joint name="gripper_joint_d" type="prismatic"> <origin xyz="0 0 0" rpy="0 0 0" /> <parent link="gripper_center" /> <child link="side_d" /> <limit effort="${effort_limit}" lower="-100" upper="100" velocity="0.5"/>
</joint>

<!-- Transmission for ROS control -->
<xacro:if value="${using_position_not_effort == 'true'}"> <transmission name="gripper_joint_transmission_p"> <type>transmission_interface/SimpleTransmission</type> <joint name="gripper_joint_p"> <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface> </joint> <actuator name="gripper_joint_motor_p"> <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface> <mechanicalReduction>1</mechanicalReduction> </actuator> </transmission> <transmission name="gripper_joint_transmission_d"> <type>transmission_interface/SimpleTransmission</type> <joint name="gripper_joint_d"> <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface> </joint> <actuator name="gripper_joint_motor_d"> <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface> <mechanicalReduction>1</mechanicalReduction> </actuator> </transmission> </xacro:if> <xacro:if value="${using_position_not_effort == 'false'}">
<transmission name="gripper_joint_transmission_p">
<type>transmission_interface/SimpleTransmission</type>
<joint name="gripper_joint_p">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="gripper_joint_motor_p">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>

<transmission name="gripper_joint_transmission_d">
<type>transmission_interface/SimpleTransmission</type>
<joint name="gripper_joint_d">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
</joint>
<actuator name="gripper_joint_motor_d">
<hardwareInterface>hardware_interface/EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
</xacro:if>

<!-- Enable joint feedback -->
<gazebo reference="gripper_joint_p">
<provideFeedback>true</provideFeedback>
</gazebo>

<gazebo reference="gripper_joint_d">
<provideFeedback>true</provideFeedback>
</gazebo>

<!-- ft_sensor plugin -->
<gazebo>
<plugin name="ft_sensor_p" filename="libgazebo_ros_ft_sensor.so">
<updateRate>100.0</updateRate>
<topicName>ft_sensor_topic_p</topicName>
<jointName>gripper_joint_p</jointName>
</plugin>
</gazebo>

<gazebo>
<plugin name="ft_sensor_d" filename="libgazebo_ros_ft_sensor.so">
<updateRate>100.0</updateRate>
<topicName>ft_sensor_topic_d</topicName>
<jointName>gripper_joint_d</jointName>
</plugin>
</gazebo>

<!-- High friction on gripper -->
<gazebo reference="gripper_wide_block_p">
<mu1>${gripper_friction}</mu1> <mu2>${gripper_friction}</mu2>
<minDepth>${gripper_min_depth}</minDepth> <maxVel>0</maxVel> </gazebo> <gazebo reference="gripper_wide_block_d"> <mu1>${gripper_friction}</mu1>
<mu2>${gripper_friction}</mu2> <minDepth>${gripper_min_depth}</minDepth>
<maxVel>0</maxVel>
</gazebo>

</robot>


Thanks for your time!

Edit Do you think this strange behavior could be from using Gazeb 9 with the latest version of gazebo_ros_pkgs, which was done as a fix for an earlier problem?