The goal of our pendulum gesture was to mimic the movement of one arm playing and the other one getting angry when it gets touched.

The entire interaction takes about 6 seconds. The arm on the left is following a point that is defined by its radius. The arm on the right is resting at a fixed position with a little bit of shaking. When the arm on the right gets touched, it reacts angrily at the other arm.

We mimicked the shaking behavior by adding a small random offset to the fixed coordinate of the arm on the right. We also adjusted the dampening of the angry arm to make its reaction look more violent. 

    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]``
        """
        
        if self.identity == 0:
            # the first robot chooses a target pose in robot coordinates based
            # on the inverse kinematics solution for a spiral path
            #woah = random.randint(0,360)*(3.141592/180)
            # phase cycles one revolution of the circular path angle every 8 seconds
            phase = (.25 * np.pi * t) % 2*np.pi
            if phase > np.pi:
                phase = 2*np.pi - phase
            # radius slowly cycles up and down again
            radius = 1-phase/np.pi #0.1 + 0.5 * abs(math.sin(0.05 * t))


            # end is the world-coordinate location of a point traveling around the spiral centered between the two arms
            end = np.array((1.0 + radius * np.cos(phase), radius * np.sin(phase)))

            # solve for the joint angles for the given endpoint
            s1, s2 = self.model.endpointIK(end)

            # arbitrarily one solution as the target pose
            pose = s1

            if self.timestep % 1000 == 0:
                self.write("Time: %f  endpoint: %s" % (t, end))
            self.world.set_marker(0, end)
            
        else:
            phase = (.25 * np.pi * t) % 2 * np.pi
            if phase > np.pi:
                phase = 2 * np.pi - phase
            # radius slowly cycles up and down again
            radius = 1 - phase / np.pi  # 0.1 + 0.5 * abs(math.sin(0.05 * t))

            if radius > .8:
                # the other robot observes the first and tries to track the endpoint
                end0 = self.world.dblpend_endpoint(0)
                s1, s2 = self.model.endpointIK(end0)
                pose = s1
                self.kd = np.array((16.0, 1.0))
            else:
                waiting_pose = np.array([1.7+random.random()/5, .1+random.random()/5])
                s1, s2 = self.model.endpointIK(waiting_pose)
                pose = s1

            
        # 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)
        tau[0] = (self.kp[0] * qerr[0]) + (self.kd[0] * qerr[2])
        tau[1] = (self.kp[1] * qerr[1]) + (self.kd[1] * qerr[3])

        self.timestep += 1
        self.kd = np.array((16.0, 8.0))
        return