Goals: For this assignment, we are interested in creating a motion of one of the pendulum trying to bounce the ball while the other helps it when it loses control. 

Process and outcome: We worked off the ndblpend_spirals.py and started with changing the phase and radius to make ball to move in an up and down direction. Then, we modified the endpoints of both pendulums and added a small damping point for the first arm so the ball catching movement is more fluid, while keeping the other guiding arm less flexible.

Final Code:

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

        # set stiffer position and damping gains
        self.kp      = np.array((200.0, 100.0))
        self.kd      = np.array((32.0,  8.0))

        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]``
        """

        # phase cycles one revolution of the circular path angle every 8 seconds
        phase = 0.45 * np.pi * t

        # radius slowly cycles up and down again
        radius = 0

        if self.identity == 0:

            self.kd = np.array((2.0, 1.0))
            # the first robot chooses a target pose in robot coordinates based
            # on the inverse kinematics solution for a spiral path

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

            # 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:

            # end is the world-coordinate location of a point traveling around the spiral centered between the two arms
            end = np.array((1.25, -np.cos(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)
            
        # 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