This assignment explores 3D representation of orientation, 3D dynamics, and control.
The provided software can be used with the same support code used in assignment 1.
Simulation is done using programs created by SDFAST. You don't need to worry about that, and the SDFAST part is easy to replace if you want.
Quaternions in the high level programs and data structures start with the scalar term (cos() term). SDFAST quaternions have the scalar term at the end. You will see conversions in the code in dynamics.c
We need a way for you to report results to the TA that he can easily grade. That means we need to somewhat artificially define a specific fixed inertial "assignment" coordinate system. The coordinate system we will use is placed at the initial position and orientation of the lander. The initial marker_lander[][] values are in the assignment coordinate system, and future marker measurements must be offset by any subsequent lander movement to put them in the assignment coordinate system.
Variables in the SIM structure you should feel free to use:
time_step, time, duration (provided by on board clocks),
status (provided by safety systems),
lander_I (provided by the spacecraft builder and previous calibration),
lander_x, lander_xd, lander_q, lander_r, lander_w, lander_w_world (provided by inertial measurement system,
may need to convert to assignment coordinate system),
n_markers, markers_lander (provided by alien tracking system), and
lander_thrust, lander_torque (you command).
You can access and define any CONTROLLER variables you want.
Other variables should not be used by your code, although you should
feel free to use them in debugging your code.
Do not assume the centroid of the markers is the same as the center of mass. The alien artifact may not have a uniform density.
Part 1: Play with the initial angular velocity of the alien artifact. What initial angular velocity vectors make it tumble maximally? What initial angular velocity vectors result in rotation about a fixed axis in space?
Part 2: Estimate the future trajectory of the tumbling alien artifact,
given the initial conditions specified in the distributed program.
This can be done
by first estimating the moment of inertia matrix I, and then
using the dynamics equation
torque = I w_dot + w x I w
where w is the angular velocity and w_dot is the angular acceleration.
To estimate the inertia matrix and initialize the dynamics, estimates
of the alien artifact angular velocity and acceleration are needed.
These can be obtained by numerically differentiating: computing angular
velocity vectors from adjacent rotations, and computing angular
acceleration vectors from adjacent angular velocities.
Note that the dynamics equation and corresponding I, angular
velocities, and angular accelerations need to be represented either
in artifact body coordinates (in which case conversion to body
coordinates
of the angular velocities and accelerations is needed) or assignment
coordinates (in which case representation of I in assignment coordinates is
needed).
2A) What is the center of mass location of the artifact and orientation of the marker cloud (both in the assignment coordinate system) as a function of time sampled at 100Hz for the first 10 seconds? Assume the initial artifact pose has zero orientation q(t=0) = (1, 0, 0, 0). To make this easy to grade, turn in a text file with channels for cx, cy, cz, q_scaler, qx, qy, and qz. Separate the numbers with white space. Label this file problem_2_0.dat. How did you calculate this from the marker data?
2B) What is the angular velocity in the assignment coordinate system of the marker cloud as a function of time sampled at 100Hz for the first 10 seconds? To make this easy to grade, turn in a text file with channels for wx, wy, and wz. Separate the numbers with white space. Label this file problem_2_1.dat. How did you calculate this from the marker data?
2C) What is the angular acceleration in the assignment coordinate system of the marker cloud as a function of time sampled at 100Hz for the first 10 seconds? Turn in a text file with channels for wdx, wdy, and wdz, separated by white space. Label this file problem_2_2.dat. How did you calculate this from the marker data?
2D) What is the moment of inertia matrix in the assignment coordinate system for the alien object in the initial configuration? What are the principal moments of inertia in ascending order? What is the rotation from assignment coordinates to principal axis for the artifact in the given initial configuration? How did you calculate all of this from the marker data?
2E) What is the future trajectory of the object, expressed using quaternions in the world coordinate system, sampled at 100Hz for 10 seconds? Generate this by integating your dynamic model forward in time from the state of the simulation at 10 seconds. (Note: You can compare what you get to what our simulator generates.) Turn in a text file with channels for cx, cy, cz, q_scalar, qx, qy, and qz, separated by white space. Label this file problem_2_3.dat.
Note that all of part 2 can be done in Matlab using based on a datafile
from the simulation of the marker trajectory for the first 10 seconds.
Check out in useful/mrdplot/
Part 3: Now control the lander (see controller.c for an intial example which you should feel free to change) to "land" on the alien artifact as close to the center of mass as possible. Since we don't want to actually hit the alien artifact, this means you should continually thrust so as to fly just with the smallest surface of the lander parallel to the nearest surface and flush to the tumbling alien artifact surface as close to the center of mass as possible. For extra style points align the corresponding edges of the alien artifact and lander. Be sure to use feedback control to reduce the effect of any prediction errors. In addition to your writeup on how you do Part 3, and your code, turn in a data file with the rendezvous trajectory, starting from the initial conditions specified in the distributed program. The data file should be named problem_3.dat, and be in MRDPLOT format that can be passed to animate.c with no additional processing.
You can use any type of computer/OS/language you want. You can work in groups or alone.
> For Part3 of assignment2, do we start with the information the > position of the COM and the inertia matrix? In other words, we don't > have to do all the optimizations we did in Part2 in real time in Part > 3, do we?
You can use the COM location and inertia matrix you identified in Part 2 in Part 3.
It is also okay to keep refining your estimates. An alternate version of the assignment could have asked you to land as fast as possible subject to constraints. In which case doing online estimation as you approached the artifact would be useful.
Where are the matlab optimization examples you showed us in class last week?
It turned out that optimizing center of mass location, COM velocity, and object orientation all at once is less effective that doing them separately. This matlab code optimizes the COM location and velocity by keeping the distances to the markers the same in each frame. This matlab code optimizes the orientation of the marker cloud in axis-angle format, using the identified COM location and velocity.
What are principal axes, etc.?
When you talked about your thesis, you said you could get things in the form A*p = b, where A is a matrix of known values (from w_dot and w), b is a vector of torques, and p are the unknown 6 elements of the moment of inertia tensor. In our problem, b is zero since there are no external torques in the system. So every time I try to solve for p, I get p = 0; What gives?
Since there are no external torques, we can't uniquely identify I. We can only identify I up to a scaling factor. I is a 6 element vector in the null space of A. Do a singular value decomposition of A, and I is the 6D svd-vector (aka eigenvector) corresponding to the smallest singular value of A. Type "help svd" in Matlab.
So for part 1: What does 'tumble maximaly' mean? Are you looking for specifics or gerneral values? I assume you are looking for a rotation around a non-trivial (no one of the three axis). In this case, would using three different rotation matricies work? I have been trying to implement this using a rotation of some angle around the x, then the y then the z axis. By testing 90 degree rotations, on any of the three axis, I am getting understandible results; however when I try this on an arbitrary axis in space, some thing is not going right. Am I even approaching this the right way?
part 1: We just want you to eyeball the results, Basically, you can choose initial angular velocities, and report which causes the artifact to tumble the most. Professor Atkeson's lecture with the tennis racket should give you an idea of which axes are most promising, and why they would lead to the most tumbling. I am not sure what rotation matricies have to do with angular velocity.
On to part 2: I am assuming the 'I's in the following: torque = I w_dot + w x I w is the moment of intertia matrix for the alien. And the x is the cross-product of the angular velocity cross (Intertia * angular velocity)
Part 2: Your assumptions about the notation are correct.
If the lander never moves, then the assignment coordinates will always be related to the lander. So the offset is not needed until the lander starts moving. Is there a particular direction you would point me to figure out part A. I figure it will take a while to get it figure out, and any help would be greatly apprecaited.
Assume that you have a guess as to where the center of gravity of the artifact lies in relation to the geometry of the artifact ( i.e. 3 units marker 2 wards of marker 3). Thus, you can calculate where the center of gravity would be at any frame from the position of the markers. We know that there are no external forces acting on lander, which implies certain invariants in the motion of the center of gravity. We can then use numerical optimization (fmin search, brute force, etc) to find where the center of gravity would have to lie to minimize violation of the invariants.
On 2A, we are asked to find the orientation "q_scaler, qx, qy, and qz" for 10 seconds at 100Hz (1000 frames). By implementing the optimization code provided, we were able to find the COG initial position and it's translational velocities. It seems like using the other optimization code should be able to provide the desired orientation information for each of the frames but it takes an extremely long time to run: 5 seconds for 5 frames, 47 seconds for 10 frames and 447 seconds for 20 frames ... so each time we double the number of frames, the time required increase by an order of magnitude. We imagine that this is because the number of parameters to be optimized is equal to 4x the number of frames we use, rather than the constant 6 parameters used in the previous optimization code for COG and translational velocity. Obviously this won't be effective for 1000 frames. Are we approaching this correctly (using optimization) and if not, could you please suggest a better method? If we are supposed to use optimization, do you have any tips for making the code more efficient?
First of all, you can compute the orientation of the artifact without using optimization. Second, even if you were using optimization, there is no need to optimize the entire 1000 frames at the same time, you can handle them individually or in small groups.
One way to go is to directly estimate the rotation matrix that describes the orientation of the artifact. Remember that a rotation matrix describing the orientation of frame 2 with respect to frame 1 can be constructed by using the orthonormal basis vectors of frame 2, written in the coordinates of frame 1, as column vectors of the rotation matrix. Think about the shape of the artifact, and I'm pretty sure you can figure out how to extract basis vectors. (But this requires a slightly more complex method to identify coordinate systems (say principal component analysis) for more random marker clouds). You might want to see how this is done in "structure from motion" in computer vision.
So we are able to get the rotation matrices but I was a little unclear about the phrase "the orientation of frame 2 with respect to frame 1". I assume that means the R that would be used to transform a vector in frame 1 units to frame 2 units .... however using columns vectors, as you describe, would make an R that would transform a vector in frame 2 units to frame 1 units (I believe). Could you just clarify which is meant (from-1-to-2(row-wise) or from-2-to-1(column-wise))?
1) Just to be clear, you are turning in quaternions, not rotation matrices.
2) R_from_1_to_2 = transpose(R_from_2_to_1). A similar simple relationship holds for quaternions. If q_from_2_to_1 = ( q0, qx, qy, qz ) then q_from_1_to_2 = ( q0, -qx, -qy, -qz ).
3) It might be useful to distinguish between rotations as a way to represent orientation and as an operator. Your question is really about which operator represents the orientation. The answer is that they both can, but we need to pick a convention.
4) What does the word orientation mean? I checked Wikipedia, which says: "it is the imaginary rotation that is needed to move the object from a reference placement to its current placement." So we will use this as our convention.
Assume an object in a reference placement has a local coordinate system with the x axis along (1, 0, 0), the y axis along (0, 1, 0) and the z axis along (0, 0, 1). Take a coordinate system where x points to the right from the viewers point of view, y points up, and z points toward the viewer. Imagine the object centered at the origin. A 1 radian rotation around the z axis would move the right side of the object up and the left side down from the VIEWERS perspective, using the right hand rule. We would describe this orientation using a quaternion ( cos(1/2), 0, 0, sin(1/2) ) The key issue is the sign of the sin() term. Using a cryptic notation c = cos(1/2) and s = sin(1/2), this quaternion corresponds to a rotation matrix of
c^2 - s^2 -2cs 0 2cs c^2 - s^2 0 0 0 c^2 + s^2 = 0.5403 -0.8415 0 0.8415 0.5403 0 0 0 1This rotation matrix viewed as an operator maps a vector (1, 0, 0) to (0.5403, 0.8415, 0), so it is mapping from object coordinates (think about the object in the reference position) to viewer coordinates. The vector (0, 1, 0) in object coordinates is mapped to (-0.8415, 0.5403, 0), which rotates it to the left in viewer coordinates. A vector along the z axis is unchanged. So this is the quaternion and corresponding rotation matrix that moves the object from a reference placement to its current placement in viewer coordinates.
I was wondering if the mrdplot plots of a_w[...] are what we're supposed to be matching with our own angular velocities for part 2(b)? Basically, when we generate our file of angular velocities, is that what we should be comparing to?
THIS IS FIXED Mon Feb 14 08:25:50 EST 2011 a_w[...] is s->alien_w[...]; which is in alien body coordinates. See main.h double alien_w[N_Q]; // angular velocity in body coordinates double alien_w_world[N_Q]; // angular velocity in world coordinates The angular velocities you are turning in should be in assignment coordinates, which are not the same. So you can compare these angular velocities, but only if you change coordinates.
Hi, what exactly does tumbling and tumbling maximally mean?
See answer to earlier tumbling question as well.
Tumbling means that the axis of rotation is changing, and the artifact will appear to tumble, instead of spinning nicely about one axis. Tumbling maximally means find the initial angular velocities that will make the artifact appear to tumble the most
Can you give us some example data files.
Here are some examples. Each data file should be a white space seperated form. You can generate this from a matlab matrix named foo with the command
save -ascii 'filename' 'foo'
The data files are then placed in a folder named by the andrew id of the member of your group who submitted the homework (so I can match results to submissions easily). Running the unzip command in linux should result in a single folder containing all of your data files
To avoid any confusion, in problem 2E, your data file submission should only contain your predicted data, covering the 10 seconds from 10 seconds into the simulation to 20 seconds into the simulation.
My group is experiencing a problem with 2D. We took the I matrix from the code given and used the omegas provided with mrdplot, and the differences of omega divided by the time step for omega dot. Our assumption is that all of these are in the world frame. We then ran the torque equation given on the site, but got non-zero torques at every timestep. This makes us believe that perhaps our math for the SVD methodology is correct, but our notions of what the I matrix represents are incorrect. Do you have any advice?
For a constant I to work, w and w_dot have to be expressed in body
coordinates, which are rotating with respect to world coordinates.
Or
You can express I in world coordinates:
transpose(R_to_body_from_world)*I_body*R_to_body_from_world = I_world
I fixed a typo in the assignment, which now says:
"Note that the dynamics equation and corresponding I, angular
velocities, and angular accelerations need to be represented either in
artifact body coordinates (in which case conversion to body
coordinates of the angular velocities and accelerations is needed) or
assignment coordinates (in which case representation of I in
assignment coordinates is needed)."
For your check you would want to either convert everything to body
coordinates or world coordinates.
Trying to check whether 0 = I w_d + w x I w in the SDFAST dynamics, and it is not working?
Turns out that the dynamics specification file was set up only for diagonal I, so SDFAST was stripping out the off diagonal elements. So the I to compare to is actually:
1.57143 0 0 0 5.36264 0 0 0 7.06593
Here is how I checked this. I added:
double tmp[N_XYZ]; double tmp2[N_XYZ]; double tmp3[N_XYZ]; double error[N_XYZ]; double alien_I_body[N_XYZ][N_XYZ];to the beginning of integrate_one_time_step() in dynamics.c.
I added
s->alien_wd[XX] = s->alien_sdfast_stated[ALIEN_WX];
s->alien_wd[YY] = s->alien_sdfast_stated[ALIEN_WY];
s->alien_wd[ZZ] = s->alien_sdfast_stated[ALIEN_WZ];
alien_getiner( ALIEN_BODY, alien_I_body );
multiply_m3_v3( alien_I_body, s->alien_wd, tmp );
multiply_m3_v3( alien_I_body, s->alien_w, tmp2 );
cross_product_v3( s->alien_w, tmp2, tmp3 );
add_v3( tmp, tmp3, error );
printf( "I: %g %g %g\n",
alien_I_body[0][0], alien_I_body[0][1], alien_I_body[0][2] );
printf( "I: %g %g %g\n",
alien_I_body[1][0], alien_I_body[1][1], alien_I_body[1][2] );
printf( "I: %g %g %g\n",
alien_I_body[2][0], alien_I_body[2][1], alien_I_body[2][2] );
printf( "w: %g %g %g\n", s->alien_w[0], s->alien_w[1], s->alien_w[2] );
printf( "wd: %g %g %g\n", s->alien_wd[0], s->alien_wd[1], s->alien_wd[2] );
printf( "error: %g %g %g\n", error[0], error[1], error[2] );
to the end of integrate_one_time_step() in dynamics.c.
I added
double alien_wd[N_XYZ]; // angular acceleration in body coordinatesto main.h
I added an add_v3() to matrix3.c. Modify subtract_v3().
Here is what I got:
I: 1.57143 0 0 I: 0 5.36264 0 I: 0 0 7.06593 w: 0.0998799 0.333367 0.0331546 wd: -0.0119802 0.00339291 -0.0178653 error: -3.46945e-18 0 0 I: 1.57143 0 0 I: 0 5.36264 0 I: 0 0 7.06593 w: 0.0997604 0.333401 0.032976 wd: -0.0119168 0.0033706 -0.0178457 error: -3.46945e-18 0 2.77556e-17 I: 1.57143 0 0 I: 0 5.36264 0 I: 0 0 7.06593 w: 0.0996415 0.333435 0.0327977 wd: -0.0118536 0.00334837 -0.0178262 error: 0 3.46945e-18 0 ...
s->alien_w and s->alien_wd are in alien body coordinates. (See main.h).
If you are interested in how SDFAST works, download the software and extract the manual.
In the answer above, you're saying that the inertia matrix that we'd
compute will be diagonal?
I'm trying to figure out how to get p from the equation A p = 0, where
A is 3x6 and p is the 6x1 elements of the inertia matrix.
(( wd1 0 0 0 wd3 wd2 ) ( 0 -w3 w2 )( w1 0 0 0 w3 w2 ))( I11 ) ( 0 )
(( 0 wd2 0 wd3 0 wd1 ) + ( w3 0 -w1 )( 0 w2 0 w3 0 w1 ))( I22 ) = ( 0 )
(( 0 0 wd3 wd2 wd1 0 ) ( -w2 w1 0 )( 0 0 w3 w2 w1 0 ))( I33 ) ( 0 )
( I23 )
( I13 )
( I12 )
Using matlab's null, it returns the 3 bases for the null space. I'm
not sure what to do with this result. Also, there is an A for each
timestep. I'm not sure how to combine the results for all time? Do I
have to use least-squares or something?
You are on the right track. The answer should be diagonal, but you should try to identify the full I, only to find that the off diagonal elements are close to zero. You could just identify the diagonal elements to help debug your code.
With one sample, you end up with 3 equations for 6 unknowns. This leaves 3 variables unknown when you are done. You are going to need more equations. You are better off with too many equations (least squares).
To get more equations You need to stack up A's. In matlab it should be something like:
A_total = [ A1 A2 A3 ... ]where A_total is 3nx6 where n is the number of points you use. The right hand side will be a vector of zeros with the appropriate dimensions.
From a practical point of view, you will need angular velocity vectors that are sufficiently distinct to get an A_total that has only one singular value near zero. This means that data has to come from more than just a few adjacent samples.
By the way, did you try just using optimization to search for an I that works? You would have had to fix the missing degree of freedom by doing something like penalizing (sum_i [sum_(j>=i) (Iij)^2 ] - 1)^2.
I'll try optimization. But why penalize the top right triangle of entries? Shouldn't we penalize I13 - I31, I21 - I12, and I23 - I32 to get them to equal each other?
I am suggesting using as optimization variables
( I11, I12, I13, I22, I23, I33 ) only.
There is a free degree of freedom in this vector since
we can scale it and still get exactly the same motion.
If (s is a scale factor)
I * w_dot + w x I * w = 0
then
s * I * w_dot + w x (s * I) * w = 0.
The penalty I suggested:
(I11^2 + I12^2 + I13^2 + I22^2 + I23^2 + I33^2 - 1)^2
will avoid drift in the optimization parameters (all going to zero
or all blowing up to infinity).
How do I fix this random spinning when trying to track the artifact's orientation? Last time with the arm I had to keep track of whenever my joint angles wrapped around 2pi but I dunno if it's the same problem in 3D. I created an artifact coordinate system by subtracting marker points, then put this into a rotation matrix and used this as my lander's desired orientation. The rest is done by the default code. Do I need to switch around which markers I use to define the axes based on the current rotation? I noticed that my computed q from the rotation matrix I mentioned was suddenly flipping all its signs which caused my ship to want to flip around. I kept track of the sum of components of q and flipped signs everytime it made a huge change which fixed the ship flipping. This is kind of what I did last time to fix this issue. What exactly is this issue caused and what's a robust way of dealing with this?
It is the same problem in 3D. This is a problem with atan2(), acos(), and asin(). These routines have no way to pick the angle nearest to a previous answer if they don't know what that angle was. You could define something like my_atan2( y, x, a ) which would return an angle within +/-pi of a. This shows up any time you are using atan2() with full rotations, for example in q_to_rotvec(). You could define a q_to_rotvec( q, rotvec, a ) which would return an angle within +/-pi of a.