Path Planning and Control

Sawyer Robot with Labeled Joints
Photo Credit: Rethink Robotics

Robot Hardware

Robot actuation was implemented on a Sawyer Black Robot, with 7 degrees of freedom and high movement and end effector precision. A custom dart launcher was attached to the end effector, and the head camera was utilized for vision.

Robot Control

Robot control was implemented with the MoveIt path planner and inverse kinematics. This controller library utilizes the Sawyer robots URDF geometry file to plan the movement by computing the different joint angles with inverse kinematics from the desired end effector pose. The planned path trajectory with setpoints is then executed with a PID controller by sending the joint angles to the robot. In order to mitigate the issue of path planner returning suboptimal paths that either risked wall collisions or had too long of a path, we decided to increase the number of path planner iterations and the joint velocities of the arm which mitigated most of the path planning issues present. Trajectories can also be checked in the RViz path planner visualizer.

RViz Path Planner Visualizer for Sawyer RobotPhoto Credit: Rethink Robotics Forum

PID Control Equation

PID Controller

PID controller applies an accurate and responsive correction to a control function by employing feedback. It accounts for the error between the reference and output values in various ways (proportional, integral, and derivative) so that the output of the system can track the reference reliably. The error term is e＝| ref - 𝑦 |.

In the equation on the left-hand side, 𝐾_p, 𝐾_i, and 𝐾_d are the non-negative coefficients for the proportional, integral, and derivative terms respectively (and thus P, I, D).

P: the proportional term controls how quickly (or slowly) you want the controller to act on the error term. It essentially pulls the output state towards the desired state.
I: the integral term governs how much accumulative error should exist in the controller. It is mainly used to account for constant disturbance.
D: the derivative term is used to stabilize the controller. In essence, it acts as a damper between the output state and the desired state.

Path Planning Procedure

Start the intera_interface joint_trajectory_action_server and the Sawyer MoveIt launch file to start the RViz visualizer.
Start PP_move and wait to receive target pose message from the target acquisition node.
Initialize PID controller and path planner with MoveIt Commander and add obstacles to limit range of motion.
Plan path to desired pose from received message using path planner, finding shortest path over 5000 iterations.
Execute planned path with PID controller, then actuate robot gripper to close to trigger the dart launcher.

Page updated

Report abuse