Assignment #1 - Part 2
 
Simulate a 2-link manipulator and make it do a bunch of stuff.

HW1_Exec.zip
HW1_Code.zip

1.    Angle Controller

1a)    In order to get the arm to move to commanded angles, a PD controller was used.  The proportional term increases torque at large angle errors, and the derivative term penalizes angular velocities different from zero (since the commanded angular velocity is zero). Gains of the following values allowed the arm to controllably converge to a commanded angle.

	Kp[SHOULDER] = -300.0;
	Kd[SHOULDER] = -14.0;
	Kp[ELBOW] = -300.0;
	Kd[ELBOW] = -11.0;

1b)    To the get the arm to converge exactly to a commanded angle, gravity compensation was added.  This adds torque to each joint to exactly the counter the torque caused be gravity due to overhang in the x-direction.

	gravity[SHOULDER] = 9.81*(s->elbow[XX]/2
		+ s->elbow[XX] + (s->wrist[XX] - s->elbow[XX])/2);
    
	gravity[ELBOW] = 9.81*((s->wrist[XX] - s->elbow[XX])/2);3EF634CC-9397-4330-9B36-D45ECF6C933A_files/HW1_Exec-2.zip3EF634CC-9397-4330-9B36-D45ECF6C933A_files/HW1_Code.zipshapeimage_1_link_0shapeimage_1_link_1
Sunday, January 24, 2010
2.    Cartesian Controllers

2a)    To get the arm to move to a target in x-z space, an inverse kinematics solver was put in front of the PD angle controller.  This used the law of cosines and atan2() to solve for the 2 possible configurations of the arm that could reach each point.  The solver also checks configurations at +/- 2π in order to account for discontinuities in atan2() that cause incorrect behaviors when crossing the z-axis.  The configuration that gets passed to the angle controller is the one with the least amount combined angular displacement in both joints.   The relationship between displacements (∆x, ∆y) and (∆Ø1 , ∆Ø2) is controlled by the Jabobian (J).  In the case of this 2-link arm the Jacobian is:

 	s->J[XX][SHOULDER] = s->upper_arm_length*cos(s->angle[SHOULDER]) +
		s->forearm_length*cos(s->angle[SHOULDER] + s->angle[ELBOW]);

	s->J[XX][ELBOW] = s->forearm_length*cos(s->angle[SHOULDER] + s->angle[ELBOW]);

	s->J[ZZ][SHOULDER] = s->upper_arm_length*sin(s->angle[SHOULDER]) +
		s->forearm_length*sin(s->angle[SHOULDER] + s->angle[ELBOW]);

	s->J[ZZ][ELBOW] = s->forearm_length*sin(s->angle[SHOULDER] + s->angle[ELBOW]);

2b)    In order to calculate the angular velocities that correspond to a cartesian velocity at the wrist we use the inverse of the Jacobian, ∆X = J-1 ∆Ø.  Because the evaluation if the inverse involves scaling by the inverse of the determinant of the Jacobian, it becomes unstable at singularities.  To prevent the simulation from breaking values of the inverse Jacobian is capped, but problems still arise when the controller is run over large distances.

In the demonstration controller the (x,z) point marks a velocity vector from the arm’s current position to the point.  There is an experimental proportional and damping constant to the vector.  The controller is stable depending on the configuration of the arm.


2c)    In order to calculate the torques that correspond to a cartesian force at the wrist we use the transpose of the Jacobian, ∆X = JT ∆Ø.  This value is stable at all configurations, but approaches zero where the force aligns perfectly with the arm and thus know joint torque can produce force in that direction.

In the demonstration controller the (x,z) point marks a force vector from the arm’s current position to the point.  There is an experimental proportional and damping constant to the vector.