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:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
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