Our goal for the gesture was to make the shorter link complete several rotations as the larger link completed one rotation before coming to a rest. The speed and positioning of the smaller link suggests that the larger link’s rotation is partly driven by the smaller link’s momentum, leading both links to work together to complete a full rotation.

Code:

We combined snippets of code from both dblpend_swingup.py and dblpend.keyframes.py and manually set the specific values for the torque and friction damping applied at each joint to produce the desired movement. After the rotation is completed (hard coded based on time elapsed), the pendulum is programmed to go to rest. 

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

        # for this example, there are four fixed poses commanded a fixed intervals, expressed as a set of (q0, q1) pairs

        # Explicitly set default initial state
        self.initial_state = np.array([0.0, 0.0, 0.0, 0.0])
        self.friction_damping = 0.1
        self.velocity_gain = 2.0
        self.keyframes = np.zeros((6,2))
        self.last_frame = None
        return

    #================================================================
    def setup(self):
        self.write("""In class double pendelum demonstration.
        Applies a sequence of joint targets to a position controller at fixed intervals.
        """)
        return

    #================================================================
    def user_parameter_change(self, parameter, value):
        pose = parameter // 2
        joint = parameter % 2
        if pose <  len(self.keyframes):
            self.keyframes[pose][joint] = 2 * math.pi * (value - 0.5)
            self.write("Set pose %d joint %d to angle %f" % (pose, joint, self.keyframes[pose][joint]))

    def apply_configuration(self, config):
        if 'keyframes' in config:
            for i in range(len(self.keyframes)):
                q0 = config['keyframes'].getfloat('keyframe-%d-q0' % i, fallback = 0.0)
                q1 = config['keyframes'].getfloat('keyframe-%d-q1' % i, fallback = 0.0)
                self.keyframes[i,:] = [q0, q1]
        return

    def gather_configuration(self, config):
        if 'keyframes' not in config:
            config['keyframes'] = {}
        for i in range(len(self.keyframes)):
            config['keyframes']['keyframe-%d-q0' % i] = str(self.keyframes[i, 0])
            config['keyframes']['keyframe-%d-q1' % i] = str(self.keyframes[i, 1])
        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 t &gt; 1.8:
            target_state = np.array([0,0,0, 0])
            qerr = target_state - state
            tau[0] = (self.kp[0] * qerr[2]) + (self.kd[0] * qerr[2])
            tau[1] = (self.kp[1] * qerr[3]) + (self.kd[1] * qerr[3])
            return

            

        # convenience variables to notate the state variables (for reference)
        q0  = state[0]  # 'shoulder' angle in radians
        q1  = state[1]  # 'elbow' angle in radians
        qd0 = state[2]  # 'shoulder' velocity in radians/second
        qd1 = state[3]  # 'elbow' velocity in radians/second

        target_speed = np.array([0,0,4,9.1]) #[5,8.06] or [4, 9.1]

        qerr = target_speed - state

        # apply PD control to reach the pose (no integral term)
        tau[0] = (self.kp[0] * qerr[2])# + (self.kd[0] * qerr[2])
        tau[1] = (self.kp[1] * qerr[3])# + (self.kd[1] * qerr[3])
        

        return