Object not gripping during simulation

[tumoney84]

Hi, I found this issue while running the simulation. The gripper closes fine and opens but now the problem is that after approaching the object, the arm instead of picking the object up it seems to colide with the object and make it fly away(it feels like the object is very light and at the slightest contact with the arm it moves, falls or flies away). This is the result from when I executed approach and retreat:

Cpp code for approach and retreat:

#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>

#include <moveit_msgs/msg/display_robot_state.hpp>
#include <moveit_msgs/msg/display_trajectory.hpp>

static const rclcpp::Logger LOGGER = rclcpp::get_logger(“move_group_demo”);

int main(int argc, char **argv) {
rclcpp::init(argc, argv);
rclcpp::NodeOptions node_options;
node_options.automatically_declare_parameters_from_overrides(true);
auto move_group_node =
rclcpp::Node::make_shared(“move_group_interface_tutorial”, node_options);

rclcpp::executors::SingleThreadedExecutor executor;
executor.add_node(move_group_node);
std::thread(&executor { executor.spin(); }).detach();

static const std::string PLANNING_GROUP_ARM = “ur_manipulator”;
static const std::string PLANNING_GROUP_GRIPPER = “gripper”;

moveit::planning_interface::MoveGroupInterface move_group_arm(
move_group_node, PLANNING_GROUP_ARM);
moveit::planning_interface::MoveGroupInterface move_group_gripper(
move_group_node, PLANNING_GROUP_GRIPPER);

const moveit::core::JointModelGroup *joint_model_group_arm =
move_group_arm.getCurrentState()->getJointModelGroup(PLANNING_GROUP_ARM);
const moveit::core::JointModelGroup *joint_model_group_gripper =
move_group_gripper.getCurrentState()->getJointModelGroup(
PLANNING_GROUP_GRIPPER);

// Get Current State
moveit::core::RobotStatePtr current_state_arm =
move_group_arm.getCurrentState(10);
moveit::core::RobotStatePtr current_state_gripper =
move_group_gripper.getCurrentState(10);

std::vector joint_group_positions_arm;
std::vector joint_group_positions_gripper;
current_state_arm->copyJointGroupPositions(joint_model_group_arm,
joint_group_positions_arm);
current_state_gripper->copyJointGroupPositions(joint_model_group_gripper,
joint_group_positions_gripper);

// Go Home
move_group_arm.setStartStateToCurrentState();
move_group_gripper.setStartStateToCurrentState();
RCLCPP_INFO(LOGGER, “Going Home”);

// joint_group_positions_arm[0] = 0.00; // Shoulder Pan
joint_group_positions_arm[1] = -2.50; // Shoulder Lift
joint_group_positions_arm[2] = 1.50; // Elbow
joint_group_positions_arm[3] = -1.50; // Wrist 1
joint_group_positions_arm[4] = -1.55; // Wrist 2
// joint_group_positions_arm[5] = 0.00; // Wrist 3

move_group_arm.setJointValueTarget(joint_group_positions_arm);

moveit::planning_interface::MoveGroupInterface::Plan my_plan_arm;
bool success_arm = (move_group_arm.plan(my_plan_arm) ==
moveit::core::MoveItErrorCode::SUCCESS);

move_group_arm.execute(my_plan_arm);

// Pregrasp

RCLCPP_INFO(LOGGER, “Pregrasp Position”);
current_state_arm = move_group_arm.getCurrentState(10);
current_state_gripper = move_group_gripper.getCurrentState(10);
current_state_arm->copyJointGroupPositions(joint_model_group_arm,
joint_group_positions_arm);
current_state_gripper->copyJointGroupPositions(joint_model_group_gripper,
joint_group_positions_gripper);

geometry_msgs::msg::Pose target_pose1;
target_pose1.orientation.x = -1.0;
target_pose1.orientation.y = 0.00;
target_pose1.orientation.z = 0.00;
target_pose1.orientation.w = 0.00;
target_pose1.position.x = 0.343;
target_pose1.position.y = 0.132;
target_pose1.position.z = 0.264;
move_group_arm.setPoseTarget(target_pose1);

success_arm = (move_group_arm.plan(my_plan_arm) ==
moveit::core::MoveItErrorCode::SUCCESS);

// Execute
move_group_arm.execute(my_plan_arm);

// Open Gripper

RCLCPP_INFO(LOGGER, “Open Gripper!”);

move_group_gripper.setNamedTarget(“gripper_open”);

moveit::planning_interface::MoveGroupInterface::Plan my_plan_gripper;
bool success_gripper = (move_group_gripper.plan(my_plan_gripper) ==
moveit::core::MoveItErrorCode::SUCCESS);

// Execute
move_group_gripper.execute(my_plan_gripper);

// Approach

current_state_arm = move_group_arm.getCurrentState(10);
current_state_gripper = move_group_gripper.getCurrentState(10);
current_state_arm->copyJointGroupPositions(joint_model_group_arm,
joint_group_positions_arm);
current_state_gripper->copyJointGroupPositions(joint_model_group_gripper,
joint_group_positions_gripper);
RCLCPP_INFO(LOGGER, “Approach to object”);

float delta = 0.04;
target_pose1.orientation.x = -1.0;
target_pose1.orientation.y = 0.00;
target_pose1.orientation.z = 0.00;
target_pose1.orientation.w = 0.00;
target_pose1.position.x = 0.343;
target_pose1.position.y = 0.132;
target_pose1.position.z = target_pose1.position.z - delta;
move_group_arm.setPoseTarget(target_pose1);

success_arm = (move_group_arm.plan(my_plan_arm) ==
moveit::core::MoveItErrorCode::SUCCESS);

// Execute
move_group_arm.execute(my_plan_arm);

// Close Gripper

RCLCPP_INFO(LOGGER, “Close Gripper!”);

move_group_gripper.setNamedTarget(“gripper_close”);

success_gripper = (move_group_gripper.plan(my_plan_gripper) ==
moveit::core::MoveItErrorCode::SUCCESS);

// Execute
move_group_gripper.execute(my_plan_gripper);

// Retreat

current_state_arm = move_group_arm.getCurrentState(10);
current_state_gripper = move_group_gripper.getCurrentState(10);
current_state_arm->copyJointGroupPositions(joint_model_group_arm,
joint_group_positions_arm);
current_state_gripper->copyJointGroupPositions(joint_model_group_gripper,
joint_group_positions_gripper);

target_pose1.orientation.x = -1.0;
target_pose1.orientation.y = 0.00;
target_pose1.orientation.z = 0.00;
target_pose1.orientation.w = 0.00;
target_pose1.position.x = 0.343;
target_pose1.position.y = 0.132;
target_pose1.position.z = target_pose1.position.z + delta;
move_group_arm.setPoseTarget(target_pose1);

success_arm = (move_group_arm.plan(my_plan_arm) ==
moveit::core::MoveItErrorCode::SUCCESS);

// Execute
move_group_arm.execute(my_plan_arm);

// Open Gripper

RCLCPP_INFO(LOGGER, “Open Gripper!”);

move_group_gripper.setNamedTarget(“gripper_open”);

success_gripper = (move_group_gripper.plan(my_plan_gripper) ==
moveit::core::MoveItErrorCode::SUCCESS);

// Execute
move_group_gripper.execute(my_plan_gripper);

rclcpp::shutdown();
return 0;
}

This is the result of when I executed pick and place:

Cpp code for pick and place:

#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>

#include <moveit_msgs/msg/display_robot_state.hpp>
#include <moveit_msgs/msg/display_trajectory.hpp>

static const rclcpp::Logger LOGGER = rclcpp::get_logger(“move_group_demo”);

int main(int argc, char **argv) {
rclcpp::init(argc, argv);
rclcpp::NodeOptions node_options;
node_options.automatically_declare_parameters_from_overrides(true);
auto move_group_node =
rclcpp::Node::make_shared(“move_group_interface_tutorial”, node_options);

rclcpp::executors::SingleThreadedExecutor executor;
executor.add_node(move_group_node);
std::thread(&executor { executor.spin(); }).detach();

static const std::string PLANNING_GROUP_ARM = “ur_manipulator”;
static const std::string PLANNING_GROUP_GRIPPER = “gripper”;

moveit::planning_interface::MoveGroupInterface move_group_arm(
move_group_node, PLANNING_GROUP_ARM);
moveit::planning_interface::MoveGroupInterface move_group_gripper(
move_group_node, PLANNING_GROUP_GRIPPER);

const moveit::core::JointModelGroup *joint_model_group_arm =
move_group_arm.getCurrentState()->getJointModelGroup(PLANNING_GROUP_ARM);
const moveit::core::JointModelGroup *joint_model_group_gripper =
move_group_gripper.getCurrentState()->getJointModelGroup(
PLANNING_GROUP_GRIPPER);

// Get Current State
moveit::core::RobotStatePtr current_state_arm =
move_group_arm.getCurrentState(10);
moveit::core::RobotStatePtr current_state_gripper =
move_group_gripper.getCurrentState(10);

std::vector joint_group_positions_arm;
std::vector joint_group_positions_gripper;
current_state_arm->copyJointGroupPositions(joint_model_group_arm,
joint_group_positions_arm);
current_state_gripper->copyJointGroupPositions(joint_model_group_gripper,
joint_group_positions_gripper);

move_group_arm.setStartStateToCurrentState();
move_group_gripper.setStartStateToCurrentState();

// Go Home
RCLCPP_INFO(LOGGER, “Going Home”);

// joint_group_positions_arm[0] = 0.00; // Shoulder Pan
joint_group_positions_arm[1] = -2.50; // Shoulder Lift
joint_group_positions_arm[2] = 1.50; // Elbow
joint_group_positions_arm[3] = -1.50; // Wrist 1
joint_group_positions_arm[4] = -1.55; // Wrist 2
// joint_group_positions_arm[5] = 0.00; // Wrist 3

move_group_arm.setJointValueTarget(joint_group_positions_arm);

moveit::planning_interface::MoveGroupInterface::Plan my_plan_arm;
bool success_arm = (move_group_arm.plan(my_plan_arm) ==
moveit::core::MoveItErrorCode::SUCCESS);

move_group_arm.execute(my_plan_arm);

// Pregrasp
RCLCPP_INFO(LOGGER, “Pregrasp Position”);

geometry_msgs::msg::Pose target_pose1;
target_pose1.orientation.x = -1.0;
target_pose1.orientation.y = 0.00;
target_pose1.orientation.z = 0.00;
target_pose1.orientation.w = 0.00;
target_pose1.position.x = 0.343;
target_pose1.position.y = 0.132;
target_pose1.position.z = 0.264;
move_group_arm.setPoseTarget(target_pose1);

success_arm = (move_group_arm.plan(my_plan_arm) ==
moveit::core::MoveItErrorCode::SUCCESS);

move_group_arm.execute(my_plan_arm);

// Open Gripper

RCLCPP_INFO(LOGGER, “Open Gripper!”);

move_group_gripper.setNamedTarget(“gripper_open”);

moveit::planning_interface::MoveGroupInterface::Plan my_plan_gripper;
bool success_gripper = (move_group_gripper.plan(my_plan_gripper) ==
moveit::core::MoveItErrorCode::SUCCESS);

move_group_gripper.execute(my_plan_gripper);

// Approach
RCLCPP_INFO(LOGGER, “Approach to object!”);

std::vector<geometry_msgs::msg::Pose> approach_waypoints;
target_pose1.position.z -= 0.03;
approach_waypoints.push_back(target_pose1);

target_pose1.position.z -= 0.03;
approach_waypoints.push_back(target_pose1);

moveit_msgs::msg::RobotTrajectory trajectory_approach;
const double jump_threshold = 0.0;
const double eef_step = 0.01;

double fraction = move_group_arm.computeCartesianPath(
approach_waypoints, eef_step, jump_threshold, trajectory_approach);

move_group_arm.execute(trajectory_approach);

// Close Gripper

RCLCPP_INFO(LOGGER, “Close Gripper!”);

move_group_gripper.setNamedTarget(“gripper_close”);

success_gripper = (move_group_gripper.plan(my_plan_gripper) ==
moveit::core::MoveItErrorCode::SUCCESS);

move_group_gripper.execute(my_plan_gripper);

// Retreat

RCLCPP_INFO(LOGGER, “Retreat from object!”);

std::vector<geometry_msgs::msg::Pose> retreat_waypoints;
target_pose1.position.z += 0.03;
retreat_waypoints.push_back(target_pose1);

target_pose1.position.z += 0.03;
retreat_waypoints.push_back(target_pose1);

moveit_msgs::msg::RobotTrajectory trajectory_retreat;

fraction = move_group_arm.computeCartesianPath(
retreat_waypoints, eef_step, jump_threshold, trajectory_retreat);

move_group_arm.execute(trajectory_retreat);

// Place

RCLCPP_INFO(LOGGER, “Rotating Arm”);

current_state_arm = move_group_arm.getCurrentState(10);
current_state_arm->copyJointGroupPositions(joint_model_group_arm,
joint_group_positions_arm);

joint_group_positions_arm[0] = 1.57; // Shoulder Pan

move_group_arm.setJointValueTarget(joint_group_positions_arm);

success_arm = (move_group_arm.plan(my_plan_arm) ==
moveit::core::MoveItErrorCode::SUCCESS);

move_group_arm.execute(my_plan_arm);

// Open Gripper

RCLCPP_INFO(LOGGER, “Release Object!”);

move_group_gripper.setNamedTarget(“gripper_open”);

success_gripper = (move_group_gripper.plan(my_plan_gripper) ==
moveit::core::MoveItErrorCode::SUCCESS);

move_group_gripper.execute(my_plan_gripper);

rclcpp::shutdown();
return 0;
}

Is there any explanation and or fix of why is this occuring?
If you would like to have a look at the animaton please let me know if there is a way to upload a small clip of the arm executing the trajectory.
Thanks again for your help.

[skynguyen1402]

Hi @tumoney84
When I implemented this task, I encountered the same problem you’re facing. Successful object picking depends on several factors. For instance, it’s crucial to determine how far the gripper should descend to ensure adequate contact for holding the object. In this case, the issue seems to be that the gripper descends and closes too quickly, leading to collisions. My solution involved adjusting the gripper’s velocity and acceleration to slow down its closing action and introducing some delay before execution.

You can adapt or modify the pick_place.cpp below to resolve your issue:

#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>

#include <moveit_msgs/msg/display_robot_state.hpp>
#include <moveit_msgs/msg/display_trajectory.hpp>

static const rclcpp::Logger LOGGER = rclcpp::get_logger("move_group_demo");

int main(int argc, char **argv) {
  rclcpp::init(argc, argv);
  rclcpp::NodeOptions node_options;
  node_options.automatically_declare_parameters_from_overrides(true);
  auto move_group_node =
      rclcpp::Node::make_shared("move_group_interface_tutorial", node_options);

  rclcpp::executors::SingleThreadedExecutor executor;
  executor.add_node(move_group_node);
  std::thread([&executor]() { executor.spin(); }).detach();

  static const std::string PLANNING_GROUP_ARM = "ur_manipulator";
  static const std::string PLANNING_GROUP_GRIPPER = "gripper";

  moveit::planning_interface::MoveGroupInterface move_group_arm(
      move_group_node, PLANNING_GROUP_ARM);
  moveit::planning_interface::MoveGroupInterface move_group_gripper(
      move_group_node, PLANNING_GROUP_GRIPPER);

  const moveit::core::JointModelGroup *joint_model_group_arm =
      move_group_arm.getCurrentState()->getJointModelGroup(PLANNING_GROUP_ARM);
  const moveit::core::JointModelGroup *joint_model_group_gripper =
      move_group_gripper.getCurrentState()->getJointModelGroup(
          PLANNING_GROUP_GRIPPER);

  // Get Current State
  moveit::core::RobotStatePtr current_state_arm =
      move_group_arm.getCurrentState(10);
  moveit::core::RobotStatePtr current_state_gripper =
      move_group_gripper.getCurrentState(10);

  std::vector<double> joint_group_positions_arm;
  std::vector<double> joint_group_positions_gripper;
  current_state_arm->copyJointGroupPositions(joint_model_group_arm,
                                             joint_group_positions_arm);
  current_state_gripper->copyJointGroupPositions(joint_model_group_gripper,
                                                 joint_group_positions_gripper);

  move_group_arm.setStartStateToCurrentState();
  move_group_gripper.setStartStateToCurrentState();

  // Go Home
  RCLCPP_INFO(LOGGER, "Going Home");

  // joint_group_positions_arm[0] = 0.00;  // Shoulder Pan
  joint_group_positions_arm[1] = -2.50; // Shoulder Lift
  joint_group_positions_arm[2] = 1.50;  // Elbow
  joint_group_positions_arm[3] = -1.50; // Wrist 1
  joint_group_positions_arm[4] = -1.55; // Wrist 2
  // joint_group_positions_arm[5] = 0.00;  // Wrist 3

  move_group_arm.setJointValueTarget(joint_group_positions_arm);

  moveit::planning_interface::MoveGroupInterface::Plan my_plan_arm;
  bool success_arm = (move_group_arm.plan(my_plan_arm) ==
                      moveit::core::MoveItErrorCode::SUCCESS);

  move_group_arm.execute(my_plan_arm);

  // Pregrasp
  RCLCPP_INFO(LOGGER, "Pregrasp Position");

  geometry_msgs::msg::Pose target_pose1;
  target_pose1.orientation.x = -1.0;
  target_pose1.orientation.y = 0.00;
  target_pose1.orientation.z = 0.00;
  target_pose1.orientation.w = 0.00;
  target_pose1.position.x = 0.343;
  target_pose1.position.y = 0.132;
  target_pose1.position.z = 0.264;
  move_group_arm.setPoseTarget(target_pose1);

  success_arm = (move_group_arm.plan(my_plan_arm) ==
                 moveit::core::MoveItErrorCode::SUCCESS);

  move_group_arm.execute(my_plan_arm);

  // Open Gripper

  RCLCPP_INFO(LOGGER, "Open Gripper!");

  move_group_gripper.setNamedTarget("gripper_open");

  moveit::planning_interface::MoveGroupInterface::Plan my_plan_gripper;
  bool success_gripper = (move_group_gripper.plan(my_plan_gripper) ==
                          moveit::core::MoveItErrorCode::SUCCESS);

  move_group_gripper.execute(my_plan_gripper);

  // Approach
  RCLCPP_INFO(LOGGER, "Approach to object!");

  std::vector<geometry_msgs::msg::Pose> approach_waypoints;
  target_pose1.position.z -= 0.03;
  approach_waypoints.push_back(target_pose1);

  target_pose1.position.z -= 0.03;
  approach_waypoints.push_back(target_pose1);

  moveit_msgs::msg::RobotTrajectory trajectory_approach;
  const double jump_threshold = 0.0;
  const double eef_step = 0.01;

  double fraction = move_group_arm.computeCartesianPath(
      approach_waypoints, eef_step, jump_threshold, trajectory_approach);

  move_group_arm.execute(trajectory_approach);
   std::this_thread::sleep_for(std::chrono::milliseconds(3000));
//   Close Gripper
  move_group_gripper.setMaxVelocityScalingFactor(0.001); // For the gripper
  move_group_gripper.setMaxAccelerationScalingFactor(0.001); // For the gripper
  RCLCPP_INFO(LOGGER, "Close Gripper!!");

  move_group_gripper.setNamedTarget("gripper_close");

  success_gripper = (move_group_gripper.plan(my_plan_gripper) ==
                     moveit::core::MoveItErrorCode::SUCCESS);

  move_group_gripper.execute(my_plan_gripper);

  // Sleep for some seconds
  std::this_thread::sleep_for(std::chrono::milliseconds(1000));
  // Retreat

  RCLCPP_INFO(LOGGER, "Retreat from object!");

  std::vector<geometry_msgs::msg::Pose> retreat_waypoints;
  target_pose1.position.z += 0.03;
  retreat_waypoints.push_back(target_pose1);

  target_pose1.position.z += 0.03;
  retreat_waypoints.push_back(target_pose1);

  moveit_msgs::msg::RobotTrajectory trajectory_retreat;

  fraction = move_group_arm.computeCartesianPath(
      retreat_waypoints, eef_step, jump_threshold, trajectory_retreat);

  move_group_arm.execute(trajectory_retreat);

  // Place

  RCLCPP_INFO(LOGGER, "Rotating Arm");

  current_state_arm = move_group_arm.getCurrentState(10);
  current_state_arm->copyJointGroupPositions(joint_model_group_arm,
                                             joint_group_positions_arm);

  joint_group_positions_arm[0] = 1.57; // Shoulder Pan

  move_group_arm.setJointValueTarget(joint_group_positions_arm);

  success_arm = (move_group_arm.plan(my_plan_arm) ==
                 moveit::core::MoveItErrorCode::SUCCESS);

  move_group_arm.execute(my_plan_arm);

  // Open Gripper

  RCLCPP_INFO(LOGGER, "Release Object!");

  move_group_gripper.setNamedTarget("gripper_open");

  success_gripper = (move_group_gripper.plan(my_plan_gripper) ==
                     moveit::core::MoveItErrorCode::SUCCESS);

  move_group_gripper.execute(my_plan_gripper);

  rclcpp::shutdown();
  return 0;
}

Best regard,
Eric

[tumoney84]

I understand. Thanks for your help!

[tumoney84]

Managed to make it work by adjusting the velocity and acceleration values, thank you for your feedback and explanation.