Assignment 1

by Jon Boerner & Andrew Burks

Part 1: Implement a Joint Angle Controller

Parameters

Initial Observations Tuning PID Control

Code

All of the code for this portion of the assignment is in this zip file. The helper functions gotoJoint and gotoTask exist to help test the PID control. They both take the SIM block as their initial argument, then either theta1 and theta2 or x and z as desired positions. The PID always acts on the s->angle_d data, so manipulating that in any way will cause the robot to move towards those joint angles.

1a) Experimentally Determined PID Gains

Slow Response (1s):
Top - Joint Angles, Bottom - Joint Velocities; For (Shoulder,Elbow), Goals=(90d,45d), Colors=(Blue,Red)


Very Fast Response (.1s):
Top - Joint Angles, Bottom - Joint Velocities; For (Shoulder,Elbow), Goals=(90d,45d), Colors=(Blue,Red)

1b) Joint Control Algorithm

These examples all use an implementation of PID control:
 
double PID(SIM *s,int j)
{
  double T;
  double e = s->angle[j] - s->angle_d[j];
  s->gains[j][PID_E] += e;
  T = (-1*e*s->gains[j][PID_P]) 
    + (-1*s->gains[j][PID_E]*s->gains[j][PID_I]) 
    + (-1*(s->angled[j]-s->angled_d[j])*s->gains[j][PID_D]);
  return T;
}
Because of the ambiguity in angles (0=2PI=-2PI) and the difference in relative position compared to other angles, we needed to transform all angles into the proper frame. We used a 2PI wide frame centered around the current angle for all angle calculations. This allowed me to easily determine the shortest distance to angles, and later (x,z) coordinates. The following function transformed any goal angle into the reference zone centered at the base angle: 
 
double transformAngle(double baseAngle, double goalAngle){
  double b = baseAngle;
  double g = goalAngle;
 
  double high = b + M_PI;
  double low = b - M_PI;
 
  if(g > high){
    g-=(2*M_PI);
    g = transformAngle(b,g);
  }
  if(g < low){
    g+=(2*M_PI);
    g = transformAngle(b,g);
  }
 
  return g;
}

Part 2: Implement a task (Cartesian) controller

All code for this part is available here.

To move the arm to a location in task coordinates, we just used gemoetric relations between angles and cartesian coordinates to find the angles we needed to acheive the desired task coordinates. Since there are two solutions to that geometric problem, we picked the solution closer to our current position in joint space, so the arm would have to move the least total distance. We then used PID control to acheive these angles with the PID based on the difference between our current angle and the desired angle. This worked well, though the end effector's route to the final position was not a straight line.

To move the arm at a desired speed, we used PI control based on the difference between our current angular velocities and our desired angular velocities. The desired angular velocities were obtained by performing inverse kinematics on the desired x-y velocities. This PI control was enough to get the end effector to the right location if the desired velocity at any point in time pointed from the current x-y location to the final x-y location, and was reduced to 0 desired velocity at the final location. The integral term was necessary for the controller to acheive stability. Without it, the arm was very unstable.

The last step was getting the arm to acheive desired accelerations. This was done by taking the inverse kinematics one step further, producing a matrix equation including terms for angular acceleration, centripedal acceleration, and the coriollis acceleration. Using this matrix equation, we could derive angular accelerations from given cartesian accelerations. We used a P controller for this step, and included a feed-forward term as well. Again, as long as the desired acceleration always pointed towards the final destination from our current destination, the control loop moved the end effector to the correct final destination. This acceleration-based control loop in particular delivered the best results when it came to having the end effector move in a straight line in cartesian space. Unfortunately, when the arm acheived equilibrium there was a small error in the end effectors final position. We later acounted for this error by including a position-based proportional term that acted in parallel with the acceleration-based loop, leading to great results.

3: Implement Trajectory Controller

Our trajectory tests were all controlled by a modified PID controller, where the D term also accounts for a desired velocity. A function read in a .csv full of desired wrist positions at different times, and calculated a desired wrist velocity for each time step. The robot then moved using the x and z positions as waypoints. The waypoints were updated based on elapsed time in the simulation.

Code

The code for trajectory control is in this zip file. To use the program, you need a .csv file with three columns of data. The first column is time and the second and third are the desired x and z positions of the wrist at that time, respectively. The duration of the entire simulation is automatically adjusted, and .5s is added to the end of your last time step. To select the csv file, call simulate as "./simulate filename.csv ".

3a) Design a Trajectory to Follow

The trajectory chosen was a striaght line from (-.4,.1) to (.4,.1). in the initial test, the waypoints were spaced out by .005m at intervals of .01s. The robot started from rest all the way down, vertical.

After quickly reaching the start of the line, the robot settles in and completes the straight line very accurately. The largest angular velocity happened near the origin, as you might expect, and was around 5 radians/second. The following graph shows the results of this test:
TOP: blue=x position, red=z position; BOTTOM: blue=angular velocity of shoulder, red=angular velocity of elbow

3b) Follow That Same Path Faster

The second test with this straight line path used a .02m spacing (4 times the previous spacing) and the same .01s intervals. The same initial configuration of the robot was used.

While the z value takes longer to settle than expected, the robot was still able to follow the trajectory very accurately. A maximum angular velocity of 20 radians/second was acieved during this test. The following figure displays the results from this setup:
TOP: blue=x position, red=z position; BOTTOM: blue=angular velocity of shoulder, red=angular velocity of elbow

3c) Robot Signature

The program provided captured mouse input and recorded x and y values over time. The time values were then scaled to a 4 second duration while the x and y values were scaled to a size that could fit in the arm's reach.

In general, the robot did very well following this path. However, near the origin large changes in joint angles were necessary between individual time intervals, and so the robot had trouble keeping up. By comparing the scaled x and z data from the mouse capture to the x and z data from the simulation output, a direct visual comparison can be made:
Human Input, scaled to the task space

Computer Output, unscaled

Part 4: Implement Balancing

Same code as part II, available here.

We got the arm to balance straight up with only proportional gains of 8 for the shoulder and 3 for the elbow. It ALMOST fell over, but then managed to save itself. Any smaller gains, and the arm was unable to correct itself once it started falling over.

As for implementing balancing with a delay, we could not get the torques of the arm to remain reasonable. Basically, we ended up getting infinite torque no matter the gains we attempted to use. Perhaps an error with the method of integration is to blame. Also, the proximity of the vertical position to a singularity wreaks havoc on the usual mathematical methods.