The goal for our pendulum gesture was to replicate a curl, where the arm would start in a straight, netural position, curl upwards at the elbow, hold the pose, and then relax back to neutral. The force of the elbow turning upwards had to be neutralized at the shoulder with friction dampening and slight counter force, allowing for the shoulder to stay as still as possible while the elbow rotated.

The setup for the curl was in a set of four keyframes, sketched out below:

These keyframes were modified in the application directly (.config file attached). A challenge we faced after setting the keyframes was that the force from the elbow relaxing was too strong on the shoulder, forcing it to swing out of control in the last keyframe. To counter this, we adjusted kd and kp such that these would be neutralized, as in the following code snippet:

class DoublePendulumController(object):
    """Prototype for a double-pendulum controller.  This class is typically subclassed to customize the control strategy and the subclass passed into the common application framework.

    :ivar initial_state: four-element numpy array used to initialize the simulator state: [q1, q2, qd1, qd2]
    :ivar kp: two-element numpy array with default proportional position gains 
    :ivar ki: two-element numpy array with default integral gains
    :ivar kd: two-element numpy array with default velocity damping gain
    """
    def __init__(self):

        # delegate object to which debugging and status messages can be printed; it must implement write()
        self.console = None

        # fixed controller parameters
        self.initial_state = np.array([0.0, 0.0, 0.0, 0.0])
        self.d_state       = np.array((1.0, 0.5*math.pi, 0.0, 0.0))
        self.kp      = np.array((16.0, 8.0))
        self.ki      = np.array((4.0, 2.0))
        self.kd      = np.array((50.0, 2.0))
        
        return

def compute_control(self, t, dt, state, tau):
        """Method called from simulator to calculate the next step of applied torques.

        :param t: time in seconds since simulation began
        :param state: four element numpy array with joint positions ``q`` and joint velocities ``qd`` as ``[q0, q1, qd0, qd1]``, expressed in radians and radians/sec
        :param tau: two element numpy array to fill in with the joint torques to apply ``[tau0, tau1]``
        """
        # select the current keyframe based on the time
        frame  = int(t // 1.5)
        if frame != self.last_frame:
            self.write("Starting frame %d" % frame)
        self.last_frame = frame

        # select the pose for the current keyframe, looping over the available poses
        pose = self.keyframes[frame % len(self.keyframes)]

        # create a target state by extending the pose to include velocity
        target = np.concatenate((pose, np.zeros(2)))

        # calculate position and velocity error as difference from reference state
        qerr = target - state

        # apply PD control to reach the pose (no integral term)
        if(frame % 4 == 3):
            self.kd[1] = 4.0
        if (frame % 4 == 0):
            self.kd[1] = 2.0
        tau[0] = (self.kp[0] * qerr[0]) + (self.kd[0] * qerr[2])
        tau[1] = (self.kp[1] * qerr[1]) + (self.kd[1] * qerr[3])

        return