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.
![](wp-content/uploads/2019/09/Slap1.gif)
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
Comments are closed.