Trajectory Generation Testing

	A	B	C	D	E	F	G	H	I	J	K
1	Trajectory Generation Testing
2	Test #	Desired x pos	Desired y pos	Desired z pos	Actual x pos	Actual y pos	Actual z pos	Error x	Error y	Error z	Total Error
3	1	0.4	-0.3	0.4	0.399972	-0.299957	0.399988	0.000028	0.000043	0.000012	0.00005269724851
4	2	0.4	-0.4	0.4	0.39998	-0.400082	0.399914	0.00002	0.000082	0.000086	0.0001204989627
5	3	0.5	-0.2	0.4	0.499916	-0.199964	0.399931	0.000084	0.000036	0.000069	0.0001145120081
6	4	0.5	0	0.4	0.500068	0.0000275	0.399892	0.000068	0.0000275	0.000108	0.0001305536288
7	5	0.6	-0.5	0.4	0.59996	-0.50001	0.39974	0.00004	0.00001	0.00026	0.0002632489316
8	6	0.3	-0.5	0.4	0.299999	-0.500039	0.399822	0.000001	0.000039	0.000178	0.0001822251355
9	7	0	-0.6	0.4	0.0000272	-0.599903	0.399875	0.0000272	0.000097	0.000125	0.0001605423309
10	8	0	-0.6	0.6	0.0000249	-0.600046	0.599922	0.0000249	0.000046	0.000078	0.00009391490829
11	9	-0.1	-0.6	0.4	-0.0999774	-0.599964	0.399872	0.0000226	0.000036	0.000128	0.0001348731256
12	10	-0.2	-0.5	0.4	-0.200023	-0.499989	0.399838	0.000023	0.000011	0.000162	0.0001639939023
13
14
15										Average	0.0001417060182
16
17
18
19
20
21
22	Trajectory Generation Testing with Obstacle Avoidance
23	1 Obstacle
24	Obstacle Info				Desired x pos	Desired y pos	Desired z pos	Actual x pos	Actual y pos	Actual z pos
25	geometry_msgs::Pose box_pose;				-0.2	-0.5	0.4	-0.20009	-0.5002	0.3999
26	box_pose.position.x = 0.1;				0	-0.5	0.4	-0.200028	-0.500037	0.399963
27	box_pose.position.y = -0.3;
28	box_pose.position.z = 0.4;
29	primitive.dimensions[primitive.BOX_X] = 0.05;
30	primitive.dimensions[primitive.BOX_Y] = 0.5;
31	primitive.dimensions[primitive.BOX_Z] = 0.7;
32
33
34
35
36
37	2 Obstacles
38	Obstacle2 Info				Desired x pos	Desired y pos	Desired z pos	Actual x pos	Actual y pos	Actual z pos
39	geometry_msgs::Pose box_pose;				-0.2	-0.5	0.4	-0.199937	-499996	0.399935
40	obstacle_pose.position.x = 0;
41	obstacle_pose.position.y = -0.5;
42	obstacle_pose.position.z = 0.4;
43	primitive.dimensions[primitive.BOX_X] = 0.05;
44	primitive.dimensions[primitive.BOX_Y] = 0.05;
45	primitive.dimensions[primitive.BOX_Z] = 0.07;
46
47	obstacle 3 info
48	geometry_msgs::Pose box_pose2;
49	obstacle_pose2.position.x = 0.1;
50	obstacle_pose2.position.y = -0.4;
51	obstacle_pose2.position.z = 0.3;
52	primitive.dimensions[primitive.BOX_X] = 0.05;
53	primitive.dimensions[primitive.BOX_Y] = 0.05;
54	primitive.dimensions[primitive.BOX_Z] = 0.07;
55
56
57
58	3 Obstacles
59					Desired x pos	Desired y pos	Desired z pos	Actual x pos	Actual y pos	Actual z pos
60	obstacle_pose.position.x = 0;				-0.2	-0.5	0.4	-0.19993	-0.500041	0.399885
61	obstacle_pose.position.y = -0.5;
62	obstacle_pose.position.z = 0.4;
63
64
65
66	obstacle_pose2.position.x = 0.1;
67	obstacle_pose2.position.y = -0.4;
68	obstacle_pose2.position.z = 0.3;
69
70
71	obstacle_pose3.position.x = -0.1;
72	obstacle_pose3.position.y = -0.4;
73	obstacle_pose3.position.z = 0.3;
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100