Robotic Arm Manipulation to Perform Rock Skipping in Simulation

The first step to our project was setting up our simulation. We decided to use PyDrake [4] as the foundation for our project since we have been using it all year in class. Within PyDrake, we utilized the manipulation station which consists of a Kuka IIWA LWR for our robotic arm and a simple two-finger Schunk WSG 50 for our gripper. We figured the IIWA arm and Schunk gripper provided enough flexibility for our rock skipping task. In addition to choosing the manipulation station, we created model instances using primitive geometries in SDFormat for the water, table, and rock objects. These primitive geometries are parsed by drake to create the model in simulation. The entire setup is displayed below in Figure 1. It is important to note that we assume an abundant number of camera perspectives for this project and known location of objects.

We had to make design choices when creating the models for the water, table, and rock. Both the water and table were relatively easy choices since any reasonable choice would have no significant impact on the rock skipping task. We simply placed them in locations advantageous for our manipulation task. The water is a long rectangular model in the positive x direction with a height at 0 m so its ground level. The table is a box shape with its most important parameter being its height. On the other hand, the design of the rock has a significant impact on the performance of our system. This conclusion was reached in both of the previous works [3, 5] as both works found that a flat rock geometry resulted in more skips. This will be explained more in the context of the skipping dynamics in the next section. As a result, we choose to make our rock resemble a flat hockey puck shape. This shape matches the flat rock geometry requirement and also provides a large top and bottom surface area for the gripper. The most important parameters and its values for the system’s models are shown in Table I.

Lastly, our simulation setup includes a force system controller to apply the spatial forces from the water onto the rock. Collision geometry of the water is ignored and all impact forces are computed according to a later described model. The force system takes in the calculated water force and converts it to an ExternallyAppliedSpatialForce type in PyDrake. This type enables us to indicate what body in the simulation the force is applied to. In order to utilize this output from the force system, we had to expose the internal spatial force port within the station. This is accomplished during the creation of the station by exporting the internal spatial force port as an input port.

Rock skipping occurs when an upward force on the surface of water counteracts the force of gravity. We define this force as $F_{lift}$. If executed properly, this creates an upward acceleration to bounce the rock off of the water and back into the air. The free-body diagram of a flat rock colliding with the water displaying $F_{lift}$ is shown in Figure 2.

Upward force $F_{lift}$ can be modeled as a drag force. Previous papers [5] have employed this technique and used Equation 1.

The drag force acts in the upward normal direction $\hat{n}$. It is directly proportional to a coefficient of drag $C_{D}$. For our use case, we will take this coefficient to be equal to 0.5. The force is also proportional to the density of the liquid it is skipping over $\rho_{water}$. This makes sense because if the liquid is more dense, the rock is more likely to bounce off of it.

The force is proportional to the wetted area of the rock $S_{wet}$. As you increase the contact area between the rock and water, you get a higher upward force. This is intuitive. It also implies that a very flat rock geometry will give us more skips because it can achieve a larger wetted area for a given mass. Parameters of $F_{lift}$ are displayed in Figure 3.

The angle $\alpha$ is defined as the angle between the surface of the rock and the surface of the water. This measures the flatness of the rock on impact. It can be calculated according to Equation 2.

The angle $\beta$ is defined as the angle between the rock’s planar velocity $\bar{V}$ and the surface of the water. This measures the flatness of the rock’s velocity on impact. It can be calculated according to Equation 3.

The upward skipping force is proportional to the vector norm of the rock’s planar velocity $\bar{V}$. This tells us that the faster you throw a rock, the more likely it is to skip. However, in accordance with angle $\beta$, this velocity should be as flat as possible with minimum element in the $\hat{z}$ direction.

We can take advantage of these force model parameters in planning to create an advantageous system for rock skipping.

Outside of the lift force $F_{lift}$, we also supply a damping force on impact to account for frictional losses. This force is defined in Equation 4 and is intended to slow the rock down as it skips. This should increase the accuracy of our simulation. Damping coefficient $D$ is chosen based on prior experimental studies[1].

With $F_{damping}$ defined, we simply add this to $F_{lift}$ to get the total force $F_{rock}$ that should be supplied to the rock on surface impact.

The task of rock skipping can be broken down into 4 main stages: bringing the rock to an ideal pickup position, picking it up, loading up the rock before throwing, and actually throwing the rock. Since our robotic arm is required to go through these different stages, we implemented a state machine to handle the planning of the arm. By utilizing a state machine, we can easily switch the way we command and control the arm. These different stages and the underlying control methods used to command the arm are shown in Figure 4. It’s important to note that we have a terminal state where the arm is no longer commanded by anything. We enter this state while we are simulating the rock skipping after it has been thrown.

Since the rock starts at the center of the table, the first state is designed to get the rock to a better position to pick it up. It is important to restate that we assume perfect perception with an abundant number of camera perspectives and a known rock location for this project. We manipulate the rock by dragging it to the edge of the table as seen in Figure 5. In order to drag the rock, we used simple kinematic planning and differential inverse kinematics as the underlying control method for the arm. Specifically, we created a sequence of desired end-effector poses and specified the difference of time $\Delta t$ between each pose. Differential inverse kinematics is then used to determine the necessary joint commands to achieve those poses. We drag the rock to the edge of the table in order to get a strong antipodal grasp with the gripper grasping the two large flat surfaces of the rock (top and bottom).

After dragging the rock, we enter the second state which is picking up the rock. The antipodal grasp mentioned before is shown in Figure 6. Similarly to the dragging state, we continue to use simple kinematic planning and differential inverse kinematics during this state to command the arm. The third state is loading up the rock to throw. In order to gain the most speed during the throw, we position the arm the farthest away from the water we can. The motion planning and control method is the same as the two other states.

The final state that requires control of the arm is the throwing state. The exact optimization is mentioned explicitly below in the Throwing Optimization section. Ultimately, we utilize Kinematic Trajectory Optimization as the underlying control method within this state. This control method is required to properly consider velocity constraints that could not be accomplished with a simple inverse kinematics approach.

Once the rock has been gripped, we must throw it at a proper speed to cause skipping. During pick-up, we grab the rock in such a way to take advantage of the rock’s cylinder geometry. By clamping down on the rock’s flat sides, we have an antipodal grasp that can effectively hold the rock with high normal force throughout the duration of the throw until release.

The objective of rock skipping is to throw the rock at a desired velocity from a desired release point. Originally, we attempted to throw the rock using a simple differential inverse kinematics approach. This is the same approach we used to plan the pickup of the rock. We commanded a set of desired end-effector poses wit varying $\Delta t$ between them that speed up to a desired velocity until the rock’s release. However, this approach did not work. Differential inverse kinematics does not fully consider the set of desired poses, it only maps a path between sequential desired poses. Therefore, this approach has a difficulty achieving the high velocities required to skip a rock. It may successfully follow the first few desired poses, but eventually it’s joints may reach an unadvantageous position that restricts the solver from planning paths to future poses. This is compounded by the fact that later poses are required to reach a higher speeds across the throw. After this approach failed, we had to find a more comprehensive approach that planned across the whole path and had a better intuition of desired velocity.

Instead, we used kinematic trajectory optimization to find the optimal path for throwing at a desired velocity from a desired release point. This approach considers all constraints when finding the optimal, comprehensive throwing trajectory. From a high-level view, we specify an initial load-up position $p_{0}$, a release position $p_{r}$, and an end position of the end-effector to follow through to after release $p_{f}$. These positions follow an approximately radial path about the origin of the robot frame. The trajectory path and constraint poses can be seen in Figure 7.

The trajectory optimization formulation is outlined in Equation 6. We impose loose position constraints of the end-effector around the three specified poses with error bound $\epsilon_{p}$. These create a box around each that the gripper must pass through. We impose more strict constraints around the $\hat{z}$ coordinate of the release position. This is because the release height has a great effect on the resulting trajectory of the rock. We do not care as much about the rock’s release point along the $\hat{x}$ and $\hat{y}$ axes.

Similarly, we impose an orientation constraint with very small angular bound $\epsilon_{\theta}$ at the release point to make sure that the rock is released as flat as possible. More rigorously, the normal force of the rock should be parallel to the $\hat{z}$ axis.

Most importantly, we impose a constraint on spatial velocity with small error bound $\epsilon_{v}$ at the release point. This requires that our end-effector is moving at an input desired $\hat{x}$ velocity. Once again, this is highly important throwing because we want to make sure the rock is thrown at a velocity that is advantageous to allow for skipping.

We minimize the length and duration of the throwing trajectory to get a smooth and fast resulting trajectory. The optimization problem is solved using the SNOPT solver [2] implemented in PyDrake.

The dynamics model we employed to model skipping implies that skipping lift force is highly dependent on velocity. We ran our planning structure on a set of desired velocities to measure the effect that velocity has on skipping. The robotic system did not perfectly achieve the desired release velocities. This is likely due to imperfections on throw release. Our two-finger gripper is sub-optimal for throwing and cannot release the rock as directly as hoped. The rock may collide with the gripper on release and decelerate. Discrepancies between desired velocities and actual release velocities are tabulated in Table II.

Despite this fact, rock velocities were still quite high and able to skip across the water in simulation. The resulting trajectories for respective desired throwing velocities are graphed in Figure 8.

We can conclude a few things from these resulting trajectories. Firstly, it is clear that a higher velocity $\dot{x}$ leads to a longer trajectory length and greater number of skips. We can clearly see that the rock thrown with highest desired velocity achieves the most skips. The rock thrown at a lower velocity still skips, but does not skip as many times or go as far.

Furthermore, we can conclude that a rock must be thrown above a certain a velocity to actually skip. All other conditions withstanding, there is effectively a cut-off velocity which must be superseded in order to counteract the force of gravity. We see with the rock thrown at desired velocity of 15 m/s, it does not skip. This rock is thrown at an actual velocity of 9.9 m/s. So, we expect that the rock in our system must be thrown at at least 10 m/s to allow for skipping.

Many limitations were faced in attempt to get the IIWA arm to throw the rock at these high velocities. We only ran test trials below 25 m/s because any desired velocity over this led to slipping. When slipping occurred, the rock would preemptively fly out of the gripper before reaching the prescribed release point. We grasped the rock using the best possible anti-podal grasp on the flat sides of the cylinder, but this was not enough. We attempted a handful of preventative measures to solve this problem. First, we maximized the normal force of the grasp. Slipping still occurred. Second, we tried to increase the friction of the rock-gripper interaction. When that didn’t work, we tried to decrease the $\Delta t$ of our simulator. None of these methods allowed us to properly throw above 25 m/s. However, they did help us reach the 25 m/s maximum desired velocity that we observed. It is a lot to ask the simulator to work at velocities above this threshold. In the future, we would hope to further fine-tune the above mentioned parameters and likely design a new gripper. A customized multi-finger gripper that is more optimized for rock skipping would greatly increase our chances of throwing above 25 m/s and avoid premature slipping.

Our dynamics model tells us that a flatter velocity with minimum $v_{z}$ element is best for skipping. We hoped to test this hypothesis by running our implementation on a set of varying desired release heights. Our trajectory optimizer constrains that the rock must be released at nearly the exact release height. However, the solver had difficulty finding solutions for many different release heights. This meant we could not run as many trials with varying release heights as we had initially hoped to. We suspect that the reason the trajectory optimization did not work at different release points is because we had fine-tuned our setup to operate at a specific release height of 0.5m.

Despite difficulty, we still did get some successfully solved trajectories at a different release height. Results comparing these different release heights at a constant throwing velocity are plotted in Figure 9. It is clear that the higher release height only creates 3 skips. This is less than the lower release heights, which skips 4 times. This matches our assumption from the dynamic model of skipping we use.

Due to problems with solving for trajectories, we cannot fully prove that a lower release height is best for rock skipping. However, the two trajectories we have generated suggest that a lower release would likely lead to less skips. In the future, we would need to further tune our planning system in order to get more reliable data.