Pick & Place Robot Arm

December 2024

Abstract

This project involved designing and implementing advanced planning and control algorithms for the Franka Emika Panda 7-DOF robotic arm to perform efficient pick-and-place tasks. The system focused on stacking both static and dynamic blocks to form stable towers of maximum height. Leveraging ROS for robot control and Gazebo for simulation [1], the project demonstrated precise performance in both controlled and dynamic scenarios, showcasing the potential of robotic systems in industrial applications.

Introduction

The project aimed to develop a robust robotic system capable of performing efficient and accurate pick-and-place tasks using the Franka Emika Panda robotic arm. The advanced 7-DOF robot has high flexibility and precision, making it ideal for handling both static and dynamic environments. The system's applications extend to industrial automation, logistics, and manufacturing, where high precision and adaptability are critical.

Methodology

The workflow included:
  • Static Grasping: Stacking four static blocks within ~1:30 minutes.
  • Dynamic Grasping: Detecting, grasping, and stacking up to two moving blocks in another ~1:30 minutes.

Static Grasping

Detection: A camera was used to detect static blocks in the workspace. Block poses were represented as transformation matrices relative to the camera frame and transformed to the robot's base frame using the equation:

Tbaseblock=Tcamerabase⋅TblockcameraT_{base}^{block} = T_{camera}^{base} \cdot T_{block}^{camera}

Grasping: The robot used forward kinematics (FK) to calculate the end effector's current pose and inverse kinematics (IK) to compute the joint angles required for grasping. To ensure alignment, Rodrigues' formula [2] was used to adjust the gripper's orientation:

R=cos⁡(θ)I+sin⁡(θ)[axis]x+(1−cos⁡(θ))axis⋅axisTR = \cos(\theta)I + \sin(\theta)[axis]_x + (1 - \cos(\theta))axis \cdot axis^T

This rotation matrix adjusts the grippers' orientation to align its z-axis with the downward direction. The adjusted rotation matrix is then applied to TbaseapproachT_{base}^{approach}(at the position 10cm above the block), ensuring the gripper's z-axis always points downward before the robot moves to the block’s approach position.

Placement: Blocks were stacked at predefined positions with heights incremented by 5 cm per block, starting at z = 0.22m.

Dynamic Grasping

Pre-Grasping: The end effector was positioned at a predefined location, anticipating the arrival of the next block.

Block Alignment: The robot calculated the block's circular trajectory and its intersection with the gripper's motion line. The intersection point was transformed to the end-effector frame to compute the alignment offset.

Grasp Execution: The grasp was triggered when the block crossed a threshold plane defined by two points on the gripper tips. Validation was performed using the cross-product of vectors to ensure proper alignment:

IF (cross product of vectors AB and AC) < 0:
    Block is to the right
ELSE IF (cross product of vectors AB and AC) > 0:
    Block is to the left

Grasp Success Validation: Grasp success was determined by measuring the gripper's finger positions. If left finger position + right finger position > 0.3, the block is grasped successfully.

IK Solver Enhancements

The inverse kinematics (IK) solver was a critical component of the project. Several modifications were made to enhance its reliability, precision, and overall performance:

  • Decaying Learning Rates: Reduced oscillations and overshooting by halving the learning rate after 2500, 5000, and 7500 iterations.
  • Higher Precision: Reduced the minimum step size by a factor of 10 (1 × 10−6) to improve convergence for high-precision tasks.
  • Joint-Centering Rate: Increased to 15 × 10−1 to ensure smoother trajectories and better convergence near joint limits.

Results

Static Grasping: Successfully stacked static blocks with 100% accuracy in simulation. Real-world challenges included detection noise and platform height mismatches.

Dynamic Grasping: Achieved a 90% success rate in block alignment and 80% in grasping during simulation.

Sim-to-Real Gap: Highlighted discrepancies in predefined positions between simulation and the real-world, addressed by proposed real-time depth camera integration. Due to a big difference between simulation and real environment, the robot was not able to grasp many dynamic blocks.

The robustness of the system is demonstrated in the video at the start of the page. Below are some additional photos.

Dynamic grasping
Dynamic stacking
Figure 1: The robot arm picking and stacking the dynamic blocks after successfully stacked all static blocks

Key Challenges and Possible Solutions

  • Insufficient Alignment Time: Introduce ignore-distance parameters to skip unreachable blocks.
  • Incorrect Block Selection: Add tighter alignment thresholds to limit target range.
  • Noise in Detection: Iterative camera scans closer to the platform for better accuracy.
  • IK Solver Reliability: Improve solver stability with decaying learning rates and finer step sizes.

Conclusion

  • Flexible algorithms are essential to address real-world discrepancies.
  • Retry mechanisms significantly enhance robustness in dynamic tasks.
  • Comprehensive testing under time constraints is critical for success.

References

[1] franka_ros — Franka Control Interface (FCI) documentation, Franka Robotics GmbH
[2] CS184: Computing Rotations in 3D, UC Berkeley @EECS

Appendices

The project's Franka Emika Panda 7-DOF robot arm and detailed setup instructions are provided as part of the MEAM5200: Introduction to Robotics project at the University of Pennsylvania.

Supplementary materials, such as the detailed report, extended algorithms, and source code, are available upon request.