We chose to adapt the code so the double pendulum duo follows a point that goes in a S shape. We developed an illusion of the two robots passing the green dot to one another as the green dot moves. The PD metrics were increased to track the green dot better but each arm is better in tracking the green dot in its own side because of the required torque to reach the arms out far.

The changes in the code cause the two arms to target the same point. They also change the path of the green dot with respect to time. 

class PendulumController(rcp.doublependulum.DoublePendulumController):
    def __init__(self):
        super().__init__()
        self.timestep = 0

        # set stiffer position and damping gains
        self.kp      = np.array((300.0, 300))
        self.kd      = np.array((16.0,  16))

        return
    
    #================================================================
    def setup(self):
        if self.identity == 0:
            self.write("""Pair of double-pendulum simulations.

One moves the endpoint along a circular path, while the other tries to follow.
""")
        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]``
        """

        t = t%12
        if(t <= 6):
            phase = 0.25 * np.pi * t
        else: 
            phase = np.pi/2 - t%6*np.pi/4


        # radius slowly cycles up and down again
        #radius = 0.1 + 0.5 * abs(math.sin(0.05 * t))
        radius = .4

        # 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)))
        if(t<=6):
            end = np.array((1 + radius * np.cos(phase), radius * np.sin(phase)))
        else:
            end = np.array((1 + radius * np.cos(phase),-2*radius + radius * np.sin(phase)))

        if self.identity == 0:
            # the first robot chooses a target pose in robot coordinates based
            # on the inverse kinematics solution for a spiral path

            # 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:
            # the other robot observes the first and tries to track the endpoint
            end0 = self.world.dblpend_endpoint(0)
            s1, s2 = self.model.endpointIK(end)
            pose = s2
            
        # 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        
        return