#include <stdio.h> /* puts, printf */
#include <time.h> /* time_t, struct tm, time, localtime */
struct Position{
double x;
double y;
double z;
};
struct Orientation{
double w;
double x;
double y;
double z;
};
struct pose{
Position position;
Orientation orientation;
};
//cur
//orientation: {w: 0.25, x: 0.0, y: 0.0, z: 0.97}
//position: {x: -4.95, y: -2.07, z: 0.0}
//origin
//-8.750273, -10.928155
int main ()
{
pose cur;
cur.position.x=-4.95;
cur.position.y=-2.07;
cur.position.z=0.0;
cur.orientation.w=0.25;
cur.orientation.x=0.0;
cur.orientation.y=0.0;
cur.orientation.z=0.97;
pose origin;
origin.position.x=-8.75;
origin.position.y=-10.93;
origin.position.z=0.0;
origin.orientation.w=0.0;
origin.orientation.x=0.0;
origin.orientation.y=0.0;
origin.orientation.z=0.0;
return 0;
}