Main Content

Pick and Place Robot Using Forward and Inverse Kinematics

Open Model

This example models a delta robot performing a pick and place task. The robot picks up a part using a vacuum gripper, moves the part to each of the four markers on the table, drops the part at the first marker, and then returns to the home position. This example demonstrates how to:

Model

Delta Robot Subsystem

The Delta Robot subsystem models a 3-DOF delta robot. The motion of the end effector is purely translational due to the kinematic structure of the robot. The robot's actuators correspond to the three torque-actuated revolute joints mounted to the upper base plate. To mimic encoder data, the subsystem outputs the positions (angles) of the actuators. A camera frame is mounted underneath the base plate and looks down towards the end effector. The geometry associated with the end effector is exported via a Simscape Bus to facilitate contact modeling. See the block mask for more information.

Planning and Control Subsystem: Forward and Inverse Kinematics

Because trajectory planning for the end effector is done with respect to the xyz coordinates of the robot's camera frame, a forward kinematics map is needed to transform the positions and velocities of the actuators to the position and velocity of the end effector. Similarly, an inverse kinematics map is needed to transform the desired position of the end effector computed by the planner to the corresponding positions of the three actuators. These forward and inverse kinematics computations are done using KinematicsSolver objects. The objects are defined as persistent variables in the functions PickAndPlaceRobotFk and PickAndPlaceRobotIk. These functions are called by the MATLAB function blocks Planning and Control/Forward Kinematics and Planning and Control/Inverse Kinematics highlighted below. To speed up computation and help ensure the KinematicsSolver object for the inverse kinematics problem finds the desired solutions, the previous solution is used as the initial guess for the current problem. Whenever the parameters of the Delta Robot subsystem change, the PickAndPlaceRobotFk and PickAndPlaceRobotIk functions are cleared from memory so that the KinematicsSolver objects are regenerated at the beginning of the next simulation. This ensures that the KinematicsSolver objects and the model stay in sync.

Planning and Control Subsystem: Path Planner

Planning occurs in the MATLAB Function block Planning and Control/Path Planner highlighted below. The planner transitions the robot between three different modes:

  • go to location directly above part

  • grasp part and move to goal location

  • go home

Whenever a mode begins, a trajectory is computed that takes the end effector from its current position to the mode's goal position in a fixed amount of time. The trajectory is generated in two stages: first, a third-order polynomial is computed corresponding to the path of the end effector in xyz camera coordinates from its current position to the goal position; second, a fifth-order polynomial is computed which is used to scale the time along the path such that the initial and final velocities and accelerations are all zero. A mode transition occurs when the position and velocity of the end effector are sufficiently close to the goal values. Given the current time, the planner returns the desired position and velocity of the end effector along the trajectory as well as the desired state of the vacuum.

Planning and Control Subsystem: Controller

The Planning and Control/Controller subsystem highlighted below contains a discrete time PID controller that drives the actual positions of the actuators to their desired values.

Vacuum Subsystem

To grasp the part, a simple vacuum is modeled between the part and the robot's end effector. Whenever the planner commands suction, the vacuum applies a constant force between the center of mass of the part and the tip of the end effector.

Contact Subsystems

Spatial Contact Force blocks inside the End Effector-Part Contact Forces and Part-Table Contact Forces subsystems are used to model contact. To speed up the simulation, three contact points equally spaced around the tip of the end effector are used as proxies for full cylindrical geometry when it is in contact with the part. Similarly, three equally spaced contact points around the bottom edge of the part are used as contact proxies for when it makes contact with the table. The vacuum force keeps the part in contact with the end effector, and friction prevents it from slipping during transport.

Image Processor

The Image Processor subsystem uses Transform Sensor blocks to simulate the processing of camera data to track the locations of the part and the markers on the table.

See Also

|

Related Topics