Overview
==========================
We selected energy of the body as the evaluation metric because it quantifies both the change in position and the change in orientation of a body in a single value. The same metric is compatible with all the described experiments. The energy metric is designed in such a way as to measure the amount of kinetic energy required to return the body to its initial configuration.
double energy( double mass,
Matrix3 inertial_tensor,
double dt,
Pose current_pose,
Pose desired_pose,
Vector3 current_linvel,
Vector3 desired_linvel,
Vector3 current_angvel,
Vector3 desired_angvel )
{
// The current position of the body w.r.t. global frame
Vector3 x = current_pose.pos;
// The desired position of the body w.r.t. global frame
Vector3 x_star = desired_pose.pos;
// The current linear velocity of the body w.r.t. global frame
Vector3 xd = current_linvel;
// The desired linear velocity of the body w.r.t. global frame
Vector3 xd_star = desired_linvel;
// The current orientation of the body w.r.t. global frame
Quaternion q = current_pose.rot;
// The desired orientation of the body w.r.t. global frame
Quaternion q_star = desired_pose.rot;
// The current angular velocity of the body w.r.t. global frame
Vector3 thetad = current_angvel;
// The desired angular velocity of the body w.r.t. global frame
Vector3 thetad_star = desired_angvel;
// The inertial tensor of the body w.r.t. global frame
Matrix3 I = inertial_tensor;
// The current rate of change in orientation w.r.t. to desired orientation
Quaternion qd = (q_star - q) * (1.0 / dt);
// The linear velocity of the body w.r.t. the desired position and the desired linear velocity
Vector3 v = (x_star - x) / dt + (xd_star - xd);
// The angular velocity of the body w.r.t. the desired orientation and desired angular velocity
Vector3 omega = to_omega( q, qd ) + (thetad_star - thetad);
// energy = 1/2 * m * v^T * v + 1/2 * omega^T * I * omega
// linear component + angular component
double KE = (0.5 * v.Dot( v ) * mass) + (0.5 * omega.Dot( I * omega ));
return KE;
}
The function to_omega converts a Quaternion based change in orientation to a 3-tuple vector derivative.
Vector3 to_omega( Quaternion q, Quaternion qd ) {
Vector3 omega;
omega.x = 2 * (-q.x * qd.w + q.w * qd.x - q.z * qd.y + q.y * qd.z);
omega.y = 2 * (-q.y * qd.w + q.z * qd.x + q.w * qd.y - q.x * qd.z);
omega.z = 2 * (-q.z * qd.w - q.y * qd.x + q.x * qd.y + q.w * qd.z);
return omega;
}
The desired position, desired linear velocity, desired orientation, and desired angular velocity of the body is the position, linear velocity, orientation, and angular velocity respectively of the body with respect to either the left or right gripper in the initial configuration. Each iteration, the current energy is computed again using the body's updated position, linear velocity, orientation, and angular velocity with respect to both the left and right grippers. The two energies, w.r.t. the left gripper and w.r.t. the right gripper, are averaged to compute a single energy value for the body.
==========================
We present two scenarios to evaluate the performance of dynamics engines, an industrial arm grasping scenario and a multiple block grasping scenario. Within each scenario, we formulated a number of experiments to compare the strengths and weaknesses of each dynamics engine and to determine where those engines break down.
Industrial Arm Grasping
Multiple Block Grasping
For the Industrial Arm Grasping scenario, we combined a model of the UR10 robotic arm with a model of the Schunk MPG-80 hand to produce a 6-DOF arm with parallel gripper and we modeled a cube for the grasped object. We placed the object in the gripper such that the gripper was in contact with the object from the beginning of simulation and only simulated holding the object rather than the entire grasping process. From the beginning of simulation, a force sufficient to maintain a hold on a dense object is applied to the gripper such that the object should be held fast in the gripper throughout the simulation. During the simulation, the torques are applied to each joint based on sinusoidal functions such that the arm is driven through a wide variety of configurations so the force at the object is varied in both magnitude and direction. When the metric was exceeded or if the simulation ran to completion (100s), the simulation was terminated. Please refer to the Industrial Arm Grasping page for more information on the models, experiments and results for this scenario.
For the Multiple Block Grasping scenario, we developed a fixed base parallel gripper composed of two primitive blocks. Between the gripper blocks, we placed a set number of cubes ranging from one to eleven in such a way that the grasp objects were in contact with one another and the gripper blocks at the beginning of simulation. Over the course of the simulation, the gripper blocks are actuated toward closure with a force relative to the number objects within the grasp. When the metric was exceeded or if the simulation ran to completion (100s), the simulation was terminated. Please refer to the Multiple Block Grasping page for information on the models, experiments and results for this scenario.
==================