For the pendulum duet, we decided to try to express a dialogue and imply a relationship between the two arms. In the simulator, one arm is holding the ball just out of reach from the second arm. Although the initial movements are slow, it rapidly builds up to almost a violent scuffle between the two arms before the first arm regains control of the ball and the cycle repeats.

The code is based on the example ndblpend_spirals.py, and the main change is the trajectory of the leading arm. The arm’s trajectory is controlled by one of three possible algorithms, which is decided by the time in the simulation. The “following” arm is still controlled by the same code as the example.

import math
import numpy as np

import rcp.doublependulum
from rcp.ex.ndblpend import main

################################################################
class PendulumController(rcp.doublependulum.DoublePendulumController):
    def __init__(self):
        super().__init__()
        self.timestep = 0
        
        
        # set stiffer position and damping gains
        self.kp      = np.array((100.0, 100.0))
        self.kd      = np.array((16.0,  8.0))

        return
    
    #================================================================
    def setup(self):
        if self.identity == 0:
            self.write("""Pair of double-pendulum simulations. What do you think?
""")
        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]``
        """

        if self.identity == 0:
            if (t % 15) < 5:
                end = (0.5-(t % 5)/10,0)
            else:
                if ((t % 15) &gt; 5) and ((t % 15) < 10):
                    end = (2-(t % 5)/10,0)
                else:
                    end = (0.5,np.cos(t * np.pi))
            # 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(end0)
            pose = s2
            if self.timestep % 1000 == 0:
                self.write("Time: %f  endpoint: %s" % (t, end0))
        # 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

################################################################$
if __name__ == "__main__":
    main(PendulumController)