medium Preview An early lesson: it works, but its text, code or grading may still change. manipulationgraspingpose estimationpca

Oriented Grasping

Estimate a can's orientation from a point cloud and turn it into a top-down grasp for a UR5e.

Run this lesson in Webots Read the theory

Free. Runs in your browser; you'll be asked to sign in first.

The Oriented Grasping simulation in Webots

CSCI 4302 Lab 3 - Oriented Grasping

Overview

A UR5e arm with a Robotiq three-finger gripper stands on a table next to a conveyor. A soda can lies on its side in front of it, and every run turns the can to a different, random direction. The camera on the wrist recognizes the can and reports its pose. Your job is to turn that pose into a grasp: a gripper pose whose fingers close across the can, that the arm can reach, and that lifts the can off the table.

The lesson covers rigid transforms and frames, reading an object's axis from its rotation matrix, building a tool frame from task constraints, and choosing among grasp candidates with inverse kinematics. Lab 1's motion primitives (MoveJ, MoveL, inverse kinematics, the keep-out plane) and the camera's recognition are provided.

Theoretical Background

Poses as Homogeneous Transforms

The pose of a rigid body (a can, the tool flange, the camera) in a frame is a \(4 \times 4\) homogeneous transform:

\[ T = \begin{bmatrix} R & p \\ 0 & 1 \end{bmatrix} ,\qquad R = \begin{bmatrix} \hat{x} & \hat{y} & \hat{z} \end{bmatrix} \]

where \(p\) is the position of the body's origin and the columns \(\hat{x}, \hat{y}, \hat{z}\) of the rotation matrix \(R\) are the body's own axes, expressed in the frame. Transforms chain by multiplication: if \(T_{AB}\) is the pose of frame \(B\) in frame \(A\), then \(T_{AC} = T_{AB}\, T_{BC}\).

From the Camera to the World

The camera sits on the gripper. Webots' recognition reports the can's pose in the camera frame, \(T_{\text{cam,can}}\). Forward kinematics gives the flange pose \(T_{\text{w,flange}}(q)\) from the joint angles \(q\), and the camera is mounted at a fixed offset \(T_{\text{flange,cam}}\) (9 cm along the tool's approach axis). So the can's pose in the world is

\[T_{\text{w,can}} = T_{\text{w,flange}}(q)\; T_{\text{flange,cam}}\; T_{\text{cam,can}}\]

This is what perception.py computes. The world frame is the Webots world: the base pose in motion.py is the UR5e node's pose, so the numbers you print are the ones the scene tree shows.

The Can's Axis

A can is a cylinder, symmetric about its axis. In this model the axis is the can's local \(z\) axis, the third column of its rotation matrix: \(a = R_{\text{can}}\,[0, 0, 1]^T = (a_x, a_y, a_z)\). The rest of the can's rotation (how it is rolled about its axis) does not matter for grasping.

For a top-down grasp only the direction of the axis on the table matters. Projecting \(a\) onto the table plane keeps \((a_x, a_y)\), and its direction is the axis yaw

\[\psi = \operatorname{atan2}(a_y, a_x)\]

Two cases need care:

  • An upright can has \(a \approx (0, 0, \pm 1)\), so \((a_x, a_y) \approx 0\) and \(\psi\) is undefined. Every yaw works for a round object seen from above; the code returns \(\psi = 0\) when \(\sqrt{a_x^2 + a_y^2}\) is below a small threshold.
  • The axis has no sign. \(a\) and \(-a\) describe the same can, so \(\psi\) and \(\psi + \pi\) are the same grasp problem. That is one reason the planner always looks at two candidates, half a turn apart.

A Top-Down Grasp Frame

The tool frame of the UR5e flange (the frame of forward_kinematics()) has its \(y\) axis along the approach direction, out of the flange. The Robotiq 3F gripper closes along the tool's \(x\) axis: the middle finger sits on the \(+x\) side, the two paired fingers on the \(-x\) side, spread along the tool's \(z\) axis.

A vertical grasp points the tool's \(y\) axis straight down. With the closing direction along the world's \(x\) axis it is

\[ R_{\downarrow} = \begin{bmatrix} \hat{x}_t & \hat{y}_t & \hat{z}_t \end{bmatrix} = \begin{bmatrix} 1 & 0 & 0 \\ 0 & 0 & 1 \\ 0 & -1 & 0 \end{bmatrix} \qquad (\hat{x}_t = \hat{x}_w,\;\; \hat{y}_t = -\hat{z}_w,\;\; \hat{z}_t = \hat{y}_w) \]

To grasp a lying can, the fingers must close across it, perpendicular to its axis: the paired fingers then lie along the can and the middle finger presses from the other side. The closing direction must point at \(\psi + \pi/2\). Turning \(R_{\downarrow}\) about the world's vertical axis by the gripper yaw \(\theta\) keeps the tool vertical and turns the closing direction:

\[R_{\text{tool}} = R_z(\theta)\, R_{\downarrow},\qquad \theta = \psi + \frac{\pi}{2} + k\pi,\quad k \in \{0, 1\}\]
\[ R_z(\theta) = \begin{bmatrix} \cos\theta & -\sin\theta & 0 \\ \sin\theta & \cos\theta & 0 \\ 0 & 0 & 1 \end{bmatrix} \]

\(R_z\) multiplies from the left because it turns about an axis of the world frame. Multiplying from the right would turn the tool about its own \(z\) axis, which is horizontal, and tilt the gripper. Copying the can's rotation matrix as the tool's rotation would be wrong for a similar reason: the object's pose is an input to the grasp, not the grasp itself.

The angle between the closing direction and the can's axis measures the orientation of a grasp:

\[\gamma = \arccos\left|\hat{x}_t \cdot a\right|,\qquad \gamma = 90^\circ \text{ is ideal}\]

Pre-Grasp, Grasp and Lift Poses

With the can's centre \(c\) and the rotation \(R_{\text{tool}}\), three poses share one rotation, so the wrist does not turn while the fingers are near the can:

\[ p_{\text{grasp}} = c + h\,\hat{z}_w,\qquad p_{\text{pre}} = p_{\text{grasp}} + d\,\hat{z}_w,\qquad p_{\text{lift}} = p_{\text{grasp}} + l\,\hat{z}_w \]

with \(h\) = FLANGE_ABOVE_CAN_CENTRE (0.18 m), \(d\) = PREGRASP_CLEARANCE (0.08 m) and \(l\) = LIFT_HEIGHT (0.15 m). The grasp height follows from the gripper: the open fingertips reach about 0.17 m below the flange, so at \(h = 0.18\) m they close around the lower half of the lying can (radius 3.2 cm) without touching the table. The fingers' proximal joints close to 70 % of their travel; closing them fully would sweep the straight fingers up past the can, which then slips out when the arm lifts. The arm moves to the pre-grasp pose in joint space (MoveJointTargets), descends in a straight line (MoveL), closes the gripper and lifts in a straight line.

Reachability and Choosing a Candidate

A grasp pose is only useful if the arm can reach it. Inverse kinematics (Lab 1: damped least squares on the Jacobian) either returns joint angles or fails, because the pose is out of reach or a joint centre would cross the keep-out plane 2 cm above the table. A candidate counts only if all three poses are solved, each seeded with the previous solution, so the descent and the lift stay on the same arm configuration.

Among the reachable candidates the planner keeps the one with the least joint motion:

\[C = \left\| \Delta(q_{\text{pre}}, q_{\text{now}}) \right\| + \left\| \Delta(q_{\text{grasp}}, q_{\text{pre}}) \right\|\]

where \(\Delta\) wraps every joint difference to \((-\pi, \pi]\), so a joint turning from \(+3.1\) to \(-3.1\) rad counts as the small move it is. The two candidates differ mostly in the last wrist joint; the cheaper one turns it less.

flowchart LR
    A[Recognition:<br/>can pose in the camera] --> B[Can pose in the world]
    B --> C[Axis yaw psi]
    C --> D[Two tool rotations<br/>k = 0, 1]
    D --> E[Pre-grasp, grasp,<br/>lift poses]
    E --> F{IK for all three?}
    F -- no --> G[Drop the candidate]
    F -- yes --> H[Joint motion cost]
    H --> I[Cheapest reachable grasp]
    I --> J[Approach, descend,<br/>close, lift]

Task Description

Objective

Implement the four functions marked IMPLEMENT HERE in controllers/lab3_oriented_grasping/grasp_planning.py:

  1. can_axis_yaw(can_pose): the yaw \(\psi\) of the can's axis on the table, \(0\) for an upright can.
  2. grasp_rotation(axis_yaw, yaw_offset): the top-down tool rotation \(R_z(\psi + \pi/2 + \text{offset})\,R_{\downarrow}\).
  3. grasp_poses(can_pose, rotation): the pre-grasp, grasp and lift poses above the can's centre.
  4. joint_motion_cost(current_joints, pregrasp_joints, grasp_joints): the joint motion \(C\) of a candidate.

plan_oriented_grasp(), in the same file, ties them together with inverse kinematics; lab3_oriented_grasping.py runs perception, calls the planner and executes the plan.

System Configuration

  • Robot: Universal Robots UR5e, its base on the table at \((-1.53, -1.04, 0.80)\) m, turned by \(-90^\circ\) about \(z\).
  • Gripper: Robotiq 3F (finger_1_joint_1, finger_2_joint_1, finger_middle_joint_1), closed to 70 % of their travel for this grasp.
  • Sensors: six joint position sensors; a 320 x 240 camera with recognition and segmentation on the wrist; a range finder (0.11-0.8 m, not needed in this lab).
  • Environment: one can (DEF can0, 0.35 kg, 12.2 cm long, 6.35 cm in diameter) lying on the table at \((-1.46, -0.57)\) m. The supervisor turns it to a random axis yaw in \([-90^\circ, 90^\circ)\) at the start of each run.
  • Files: grasp_planning.py (your work), lab3_oriented_grasping.py (the main flow), perception.py (recognition to world poses), motion.py (Lab 1's kinematics and motions), robot.py (the devices), grasp_plot.py (the Visualization window: the can from above and where each candidate's fingers would land).

Performance Metrics

The supervisor (controllers/lab3_oriented_grasping_benchmark) grades from the simulated world and writes metrics.json:

Metric Meaning
success the can was lifted at least 0.10 m and held for 1 s, by fingers that closed across it (\(\gamma \ge 70^\circ\))
ended_reason can_lifted, can_pushed_away, can_dropped, can_fell_off_table, robot_stopped or time_limit
time_s simulated time when the run ended (s)
can_axis_yaw_deg the can's random axis yaw in this run (deg)
closing_angle_deg \(\gamma\), the angle between the gripper's closing direction and the can's axis at the grasp (deg)
grasp_aligned \(\gamma \ge 70^\circ\)
can_lift_m, max_can_lift_m the can's final and highest rise above its rest height (m)
score 100 on success; 60 when the can was lifted without an oriented grasp; otherwise up to 50 for a partial lift

The run ends as soon as the result is decided, at the latest after 30 s of simulated time.

Suggested Algorithm

  1. Sense: at the home pose, read the recognized can and build its world pose (provided).
  2. Take the third column of the can's rotation matrix and compute \(\psi\) with atan2.
  3. For each yaw offset in CANDIDATE_YAW_OFFSETS (\(0\) and \(\pi\)):
  4. build \(R_{\text{tool}} = R_z(\psi + \pi/2 + \text{offset})\,R_{\downarrow}\);
  5. build the pre-grasp, grasp and lift poses;
  6. solve inverse kinematics for all three, and drop the candidate if one fails;
  7. compute its joint motion cost.
  8. Plan: keep the reachable candidate with the lowest cost.
  9. Act: open the gripper, move to the pre-grasp joints, descend in a straight line, close, lift.

Validation Checklist

  • The console's Can axis yaw matches the supervisor's axis yaw (or differs from it by \(180^\circ\)).
  • In the Visualization window the fingers of both candidates sit on either side of the can, the line between them perpendicular to the can's axis.
  • At least one candidate is reachable, and the chosen one turns the wrist the shorter way.
  • The gripper comes straight down and does not touch the can before it closes.
  • The can rises with the gripper and stays in it; closing_angle_deg is close to 90.
  • Run it several times: every random orientation must work.

References

Extension Ideas

  • Stand the can upright (its axis along the world \(z\)): which gripper yaw works now, and would a side grasp be better?
  • Add candidates with a tilted approach, and a cost that weighs joint motion against how far the tool leans.
  • Put several cans on the table and choose the one the arm can pick with the least motion.