Frames Overview What are frames? What is the frame class? The test_frame example.

Frames Overview

• What are frames?

• What is the frame class?

• The test_frame example.

What are Frames?

• Frames are named locations

• Frames have a parent frame

• Frames may have multiple child frames

• Transformations between frames are built in

What is the Frame class?

• Constructors

Frame(const char * name = "baseframe", Frame * parent = NULL, Location l = 0);Frame(const char * name, Frame_h &parent, Location l = 0);Frame(const char * name, Frame * parent,

double x, double y = 0, double z = 0, double roll = 0, double pitch = 0 ,double yaw = 0);

What is the Frame class?

• Public Interfaceconst char * get_name() Frame & get_parent() Frame & get_base()bool is_base() Frame& get_common(Frame& other_frame);int change_parent(Frame & new_parentBool is_child_of(const Frame & pop) Location location_of(const Frame & other_frame, const Location & loc) const;Location location_of(const Frame & other_frame) const;int set_location(double x, double y, double z,

double roll, double pitch, double yaw);int set_location(Location l);int set_location(double x, double y, double z,

double roll, double pitch, double yaw, Frame & wrt_here);

int set_location(Location l, Frame & wrt_here);void show();void show_tree();void show_children();Frame& operator = (const Location & ht);Frame& operator = (const Frame & f);

// First we'll test the default constructor

cout <<endl<<"Testing Frame class"<<endl<<endl<<endl; cout << "Create a default frame."<<endl;

Frame baseframe; // Create a default Frame

cout <<"Here's what it looks like:"<<endl<<baseframe<<endl;

The test_frame example

Testing Frame class

Create a default frame.Here's what it looks like:baseframe (0, 0, 0) (0, 0, 0)

cout <<"Now let's move the base frame using " "set_location(1, 2, 3, 0.1, 0.2, 0.3)"<<endl;

baseframe.set_location(1, 2, 3, 0.1, 0.2, 0.3); // Does it make sense to be able to move a base frame? - RDP

cout <<"Here's what it looks like now:"<<endl<<baseframe<<endl <<"And here's what it looks like cast as a Location:"<<endl << (Location)baseframe << endl


Now let's move the base frame using set_location(1, 2, 3, 0.1, 0.2, 0.3)Here's what it looks like now:baseframe (1, 2, 3) (0.1, 0.2, 0.3)

And here's what it looks like cast as a Location:HTrans:[x: 1 y: 2 z: 3 roll(rx): 0.1 pitch(ry): 0.2 yaw(rz): 0.3 ]

<<"What happens when we try to get the parent and use the returned value?"<<endl;

cout << " parent = "<< baseframe.get_parent().get_name() << endl;

cout <<"The baseframe is its own parent. This makes the following"<<endl <<"line safe. Returning NULL would cause a seg fault..."<<endl <<"cout << \" parent = \"<< baseframe.get_parent().get_name() << endl;"<<endl;


What happens when we try to get the parent and use the returned value? parent = baseframeThe baseframe is its own parent. This makes the followingline safe. Returning NULL would cause a seg fault...cout << " parent = "<< baseframe.get_parent().get_name() << endl;

// We'll build a frame tree and manipulate it

cout<<endl <<"Let's create a world with landers and rovers and rocks"<<endl <<"Create the world frame with the default parent (none)"<<endl <<"and default Location (0,0,0,0,0,0)"<<endl <<"Let's assume a traditional world frame with X axis facing north"<<endl <<"Y axis facing east and the Z axis down along the gravity vector"<<endl;

Frame world_frame("World"); // Create a World Frame


Let's create a world with landers and rovers and rocksCreate the world frame with the default parent (none)and default Location (0,0,0,0,0,0)Let's assume a traditional world frame with X axis facing northY axis facing east and the Z axis down along the gravity vector

cout<<endl <<"Now we'll create a couple of landers. The first is aligned"<<endl <<"with the world frame, the second is 10 meters North and 10 meters"<<endl <<"west. Each lander has a rover on it that starts 1 meter above"<<endl <<"and 1 meter in the x direction in the lander frame. We will"<<endl <<"move the rovers, discover rocks, and change reference frames"<<endl <<"of these objects to test this code."<<endl; Frame lander_frame("lander", // Create a lander Frame

& world_frame); // aligned with the world frame

Frame lander2_frame("lander2", // Create a nearby lander Frame & world_frame, Location(10,-10,0));// use the location constructor

Frame rover_frame("rover", // Create a rover Frame & lander_frame, Location(1,0,-1)); // use the location constructor

Frame rover2_frame("rover2", // Create a second rover Frame & lander2_frame, 1,0,-1,0,0,0); // use the point and rotation

// constructor


cout<<endl <<"Here's what the world looks like (displayed with show_tree):"<<endl; world_frame.show_tree(); cout<<endl


Now we'll create a couple of landers. The first is alignedwith the world frame, the second is 10 meters North and 10 meterswest. Each lander has a rover on it that starts 1 meter aboveand 1 meter in the x direction in the lander frame. We willmove the rovers, discover rocks, and change reference framesof these objects to test this code.

Here's what the world looks like (displayed with show_tree):World (0, 0, 0) (0, 0, 0) lander (0, 0, 0) (0, 0, 0) rover (1, 0, -1) (0, 0, 0) lander2 (10, -10, 0) (0, 0, 0) rover2 (1, 0, -1) (0, 0, 0)

cout<<endl <<"We see a long line of rocks north of the lander and a short"<<endl <<"line south of the lander 5 meters away in both cases. There"<<endl <<"is also a short line of rocks north of lander2. Here are the"<<endl <<"children of lander (using show_children):"<<endl;

Frame rock1_frame("rock1", // Create a named rock Frame & lander_frame, 5,-4,0,0,0,0); // use the point and rotation

// constructor Frame rock2_frame("rock2", // Create a named rock Frame

& lander_frame, 5,-3,0,0,0,0); // use the point and rotation






& lander_frame, 5,0,0,0,0,0); // use the point and rotation

// constructor


Frame rock6_frame("rock6", // Create a named rock Frame & lander_frame, 5,1,0,0,0,0); // use the point and rotation








& lander_frame, -5,-1,0,0,0,0); // use the point and rotation


& lander_frame, -5,0,0,0,0,0); // use the point and rotation


& lander_frame, -5,1,0,0,0,0); // use the point and rotation

// constructor


Frame rock13_frame("rock13", // Create a named rock Frame & lander2_frame, 5,-1,0,0,0,0); // use the point and rotation


& lander2_frame, 5,0,0,0,0,0); // use the point and rotation


& lander2_frame, 5,1,0,0,0,0); // use the point and rotation

// constructor lander_frame.show_children(); cout<<endl


lander (0, 0, 0) (0, 0, 0) rover (1, 0, -1) (0, 0, 0) rock1 (5, -4, 0) (0, 0, 0) rock2 (5, -3, 0) (0, 0, 0) rock3 (5, -2, 0) (0, 0, 0) rock4 (5, -1, 0) (0, 0, 0) rock5 (5, 0, 0) (0, 0, 0) rock6 (5, 1, 0) (0, 0, 0) rock7 (5, 2, 0) (0, 0, 0) rock8 (5, 3, 0) (0, 0, 0) rock9 (5, 4, 0) (0, 0, 0) rock10 (-5, -1, 0) (0, 0, 0) rock11 (-5, 0, 0) (0, 0, 0) rock12 (-5, 1, 0) (0, 0, 0)

<<"Now here is the lander2 frame:"<<endl; lander2_frame.show_children();


Now here is the lander2 frame:lander2 (10, -10, 0) (0, 0, 0) rover2 (1, 0, -1) (0, 0, 0) rock13 (5, -1, 0) (0, 0, 0) rock14 (5, 0, 0) (0, 0, 0) rock15 (5, 1, 0) (0, 0, 0)

cout<<endl <<"Now let's move the rover off of the lander. We will use the"<<endl <<"HTrans trans and rotate functions to move. Let's move down to"<<endl <<"ground level and forward 8 meters. Here's what that looks like:"<<endl;

rover_frame = rover_frame.trans_z(1).trans_x(8); rover_frame.show();

cout<<endl <<"Now turn 90 degrees and move 7 meters:"<<endl;

rover_frame = rover_frame.r_z(M_PI/2).trans_x(7); rover_frame.show();


Now let's move the rover off of the lander. We will use theHTrans trans and rotate functions to move. Let's move down toground level and forward 8 meters. Here's what that looks like:rover (9, 0, 0) (0, 0, 0)

Now turn 90 degrees and move 7 meters:rover (9, 7, 0) (0, 0, 1.5708)

cout<<endl <<"We'll take pictures here, so create a site frame. It will have the"<<endl <<"lander as the parent frame. The position is the rovers, but the"<<endl <<"rotation is aligned with the lander (or the world). "<<endl;

Frame site1_frame("site1", // Create a site Frame & lander_frame, rover_frame.get_x(), rover_frame.get_y(), rover_frame.get_z(), world_frame.get_roll(), world_frame.get_pitch(), world_frame.get_yaw()); // use the point and rotation

// constructor

site1_frame.show();


We'll take pictures here, so create a site frame. It will have thelander as the parent frame. The position is the rovers, but therotation is aligned with the lander (or the world). site1 (9, 7, 0) (0, 0, 0)

cout<<endl <<"In the pictures we see a small line of rocks 5 meters ahead of the"<<endl <<"rover. The rocks are created in the rover frame. Like this:"<<endl;

Frame rock16_frame("rock16", // Create a named rock Frame & rover_frame, 5,-1,0,0,0,0); // use the point and rotation


& rover_frame, 5,0,0,0,0,0); // use the point and rotation


& rover_frame, 5,1,0,0,0,0); // use the point and rotation

// constructor rover_frame.show_children();


In the pictures we see a small line of rocks 5 meters ahead of therover. The rocks are created in the rover frame. Like this:rover (9, 7, 0) (0, 0, 1.5708) rock16 (5, -1, 0) (0, 0, 0) rock17 (5, 0, 0) (0, 0, 0) rock18 (5, 1, 0) (0, 0, 0)

cout<<endl <<"Now that we have the panorama, we'll want to put the rover in the"<<endl <<"site frame. This is simple with the change_parent function. The"<<endl <<"site frame now looks like this:"<<endl;

rover_frame.change_parent(site1_frame); site1_frame.show_children();


Now that we have the panorama, we'll want to put the rover in thesite frame. This is simple with the change_parent function. Thesite frame now looks like this:site1 (9, 7, 0) (0, 0, 0) rover (0, 0, 0) (0, 0, 1.5708) rock16 (5, -1, 0) (0, 0, 0) rock17 (5, 0, 0) (0, 0, 0) rock18 (5, 1, 0) (0, 0, 0)

cout<<endl <<"These rocks don't really belong in the rover frame. They need to"<<endl <<"be transfered to the site frame. Notice how the positions are"<<endl <<"automatically updated."<<endl;

rock16_frame.change_parent(site1_frame); rock17_frame.change_parent(site1_frame); rock18_frame.change_parent(site1_frame); site1_frame.show_children();


These rocks don't really belong in the rover frame. They need tobe transfered to the site frame. Notice how the positions areautomatically updated.site1 (9, 7, 0) (0, 0, 0) rover (0, 0, 0) (0, 0, 1.5708) rock16 (1, 5, 0) (0, 0, 1.5708) rock17 (3.06152e-16, 5, 0) (0, 0, 1.5708) rock18 (-1, 5, 0) (0, 0, 1.5708)

cout<<endl <<"Rover2 has also been moving. The lander 2 sees it at (-3, 3) and"<<endl <<"rotated -90 degrees. Let's just update the position using"<<endl <<"set_location."<<endl;

rover2_frame.set_location(-3,3,0,0,0,-M_PI/2); rover2_frame.show();


Rover2 has also been moving. The lander 2 sees it at (-3, 3) androtated -90 degrees. Let's just update the position usingset_location.rover2 (-3, 3, 0) (0, 0, -1.5708)

cout<<endl <<"Rover 2 also sees rocks. Let's add them."<<endl;

Frame rock19_frame("rock19", // Create a named rock Frame & rover2_frame, 5,-1,0,0,0,0); // use the point and rotation


& rover2_frame, 5,0,0,0,0,0); // use the point and rotation


& rover2_frame, 5,1,0,0,0,0); // use the point and rotation

// constructor

lander2_frame.show_children();


Rover 2 also sees rocks. Let's add them.lander2 (10, -10, 0) (0, 0, 0) rover2 (-3, 3, 0) (0, 0, -1.5708) rock19 (5, -1, 0) (0, 0, 0) rock20 (5, 0, 0) (0, 0, 0) rock21 (5, 1, 0) (0, 0, 0) rock13 (5, -1, 0) (0, 0, 0) rock14 (5, 0, 0) (0, 0, 0) rock15 (5, 1, 0) (0, 0, 0)

cout<<endl <<"Here is the world we created. We can use show_tree with any frame"<<endl <<"to see it."<<endl;

rover2_frame.show_tree();


Here is the world we created. We can use show_tree with any frameto see it.World (0, 0, 0) (0, 0, 0) lander (0, 0, 0) (0, 0, 0) rock1 (5, -4, 0) (0, 0, 0) rock2 (5, -3, 0) (0, 0, 0) rock3 (5, -2, 0) (0, 0, 0) rock4 (5, -1, 0) (0, 0, 0) rock5 (5, 0, 0) (0, 0, 0) rock6 (5, 1, 0) (0, 0, 0) rock7 (5, 2, 0) (0, 0, 0) rock8 (5, 3, 0) (0, 0, 0) rock9 (5, 4, 0) (0, 0, 0) rock10 (-5, -1, 0) (0, 0, 0) rock11 (-5, 0, 0) (0, 0, 0) rock12 (-5, 1, 0) (0, 0, 0) site1 (9, 7, 0) (0, 0, 0) rover (0, 0, 0) (0, 0, 1.5708) rock16 (1, 5, 0) (0, 0, 1.5708) rock17 (3.06152e-16, 5, 0) (0, 0, 1.5708) rock18 (-1, 5, 0) (0, 0, 1.5708) lander2 (10, -10, 0) (0, 0, 0) rover2 (-3, 3, 0) (0, 0, -1.5708) rock19 (5, -1, 0) (0, 0, 0) rock20 (5, 0, 0) (0, 0, 0) rock21 (5, 1, 0) (0, 0, 0) rock13 (5, -1, 0) (0, 0, 0) rock14 (5, 0, 0) (0, 0, 0) rock15 (5, 1, 0) (0, 0, 0)

cout<<endl <<"We can get the name of any frame. Here is the name of the lander"<<endl <<"frame:"<<endl << lander_frame.get_name() <<endl <<endl <<"We can also just use iostream to output the lander frame"<<endl << lander_frame << endl <<endl <<"We can find the parent of any frame. Here is the lander frame's"<<endl <<" parent:"<<endl << lander_frame.get_parent().get_name() <<endl <<endl <<"We can get the base frame of any frame. Here is the base frame of"<<endl <<"rover:"<<endl << rover_frame.get_base().get_name() <<endl <<endl <<"We can find the common parent of two frames:"<<endl <<"The common parent of Rock 1 and rock 16 is " << rock1_frame.get_common(rock16_frame).get_name()<<endl <<"The common parent of Rock 13 and rock 16 is " << rock13_frame.get_common(rock16_frame).get_name()<<endl <<"The common parent of Rock 1 and rock 1 is " << rock1_frame.get_common(rock1_frame).get_name()<<endl<<endl;



We can get the name of any frame. Here is the name of the landerframe:lander

We can also just use iostream to output the lander framelander (0, 0, 0) (0, 0, 0)

We can find the parent of any frame. Here is the lander frame's parent:World

We can get the base frame of any frame. Here is the base frame ofrover:World

We can find the common parent of two frames:The common parent of Rock 1 and rock 16 is landerThe common parent of Rock 13 and rock 16 is WorldThe common parent of Rock 1 and rock 1 is rock1

cout <<"The common parent of Rock 1 and baseframe is " << rock1_frame.get_common(baseframe).get_name()<<endl;

cout<<endl <<"This last one shows what happens when you compare"<<endl <<"two unrelated trees. It prints an error message, but"<<endl <<"returns the base frame (a safe value). Note: could raise"<<endl <<"an exception here"<<endl;


No common parent. Ouch!The common parent of Rock 1 and baseframe is baseframe

This last one shows what happens when you comparetwo unrelated trees. It prints an error message, butreturns the base frame (a safe value). Note: could raisean exception here

cout<<endl <<"Since the Frame class has HTrans as a base class, we can"<<endl <<"use matrix multiplication to convert from one frame to"<<endl <<"another using something like:"<<endl <<"cout << (inverse(lander2_frame) * site1_frame * rock16_frame) <<endl;" <<endl <<"to yield:"<<endl; cout << (inverse(lander2_frame) * site1_frame * rock16_frame) <<endl;


Since the Frame class has HTrans as a base class, we canuse matrix multiplication to convert from one frame toanother using something like:cout << (inverse(lander2_frame) * site1_frame * rock16_frame) <<endl;to yield:HTrans:[x:2.22e-16 y: 22 z: 0 roll(rx): 0 pitch(ry): 0 yaw(rz): 1.571 ]

cout<<endl <<"However, the location_of function makes this much simpler. It"<<endl <<"automatically traverses the frame tree to do the transformation."<<endl <<"Here is rock 16 in the lander 2 frame(same as above):"<<endl <<lander2_frame.location_of(rock16_frame) <<endl <<"Here is rover in the lander 2 frame:"<<endl <<lander2_frame.location_of(rover_frame) <<endl; Location loc1(1,1); cout <<"Now here is the location (1,1) in the rover frame"<<endl <<" from the lander2 frame: "<<endl <<lander2_frame.location_of(rover_frame, loc1) <<endl <<"Let's show that as a point."<<endl; Point<double> p1 = lander2_frame.location_of(rover_frame, loc1).get_translation_point(); cout <<p1<<endl;


However, the location_of function makes this much simpler. Itautomatically traverses the frame tree to do the transformation.Here is rock 16 in the lander 2 frame(same as above):HTrans:[x:2.22e-16 y: 22 z: 0 roll(rx): 0 pitch(ry): 0 yaw(rz): 1.571 ]Here is rover in the lander 2 frame:HTrans:[x: -1 y: 17 z: 0 roll(rx): 0 pitch(ry): 0 yaw(rz): 1.571 ]Now here is the location (1,1) in the rover frame from the lander2 frame: HTrans:[x: -2 y: 18 z: 0 roll(rx): 0 pitch(ry): 0 yaw(rz): 1.571 ]Let's show that as a point.Point: [x: -2 y: 18 z: 0]

cout<<endl <<"Finally let's test to see if one frame is a child of another"<<endl <<"Is rock 16 child of lander? "<<rock16_frame.is_child_of(lander_frame)<<endl <<"Is rock 13 child of lander? "<<rock13_frame.is_child_of(lander_frame)<<endl <<"Is rock 13 child of rock13? "<<rock13_frame.is_child_of(rock13_frame)<<endl;


Finally let's test to see if one frame is a child of anotherIs rock 16 child of lander? 1Is rock 13 child of lander? 0Is rock 13 child of rock13? 1

Frames Overview What are frames? What is the frame class? The test_frame example.

Documents

Transcript of Frames Overview What are frames? What is the frame class? The test_frame example.