The goal of this double pendulum is to simulate the arm movements of a volleyball player, including spiking and getting ready for the next ball.
Outcome: We use the PD controller to send it to different poses. The ‘arm’ first spikes, and then gets ready for the next ball. Then we adjust the PD controller, so that it simulates the player getting tired, such that the height of the arm, and the position changes.
Here is our code: We started with the keyframe code and modified it for our volleyball player. Note that the 5 keyframes are set at the init, and as t increases the P constant decreases until the arm gets too tired and gives up after 60 seconds.
#!/usr/bin/env python3 """Prototype Python 3 script for the 16-375 double-pendulum control exercise.""" ################################################################ # Written in 2018-2019 by Garth Zeglin <garthz@cmu.edu> # To the extent possible under law, the author has dedicated all copyright # and related and neighboring rights to this software to the public domain # worldwide. This software is distributed without any warranty. # You should have received a copy of the CC0 Public Domain Dedication along with this software. # If not, see <http://creativecommons.org/publicdomain/zero/1.0/>. ################################################################ import math import numpy as np import rcp.doublependulum from rcp.ex.dblpend import main ################################################################ 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 self.keyframes = np.zeros((5,2)) self.keyframes[0,0] = np.pi self.keyframes[0,1] = np.pi self.keyframes[1,0] = 0 self.keyframes[1,1] = -1.5 self.keyframes[2,0] = 1.35 self.keyframes[2,1] = 2.2 self.keyframes[3,0] = 1.35 self.keyframes[3,1] = 2.2 self.keyframes[4,0] = 0 self.keyframes[4,1] = 0 self.last_frame = None return #================================================================ def setup(self): self.write("""Keyframed position control demonstration. Applies a sequence of joint targets to a position controller at fixed intervals. The parameter sliders 0 and 1 control the 'shoulder' and 'elbow' angles for the first keyframe, 2 and 3 are the second keyframe, etc. """) 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]`` """ # select the current keyframe based on the time frame = int(t // 1.5) if frame != self.last_frame: self.write("Starting frame %d" % frame) self.last_frame = frame # select the pose for the current keyframe, looping over the available poses pose = self.keyframes[frame % len(self.keyframes)] # create a target state by extending the pose to include velocity target = np.concatenate((pose, np.zeros(2))) # after 60 seconds, give up. if t > 60: target[0] = 0; target[1] = 0; # calculate position and velocity error as difference from reference state qerr = target - state # apply PD control to reach the pose (no integral term) # P constant decreases as the robot gets "tired" tau[0] = ((self.kp[0] * (1 - (t/60))) * qerr[0]) + (self.kd[0] * qerr[2]) tau[1] = ((self.kp[1] * (1 - (t/60))) * qerr[1]) + (self.kd[1] * qerr[3]) return ################################################################$ if __name__ == "__main__": main(PendulumController)
Comments are closed.