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.

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