error_block_tests2.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2015 Fetch Robotics Inc.
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  */
16 
17 #include <urdf/model.h>
18 #include <robot_calibration/ceres/optimizer.h>
19 #include <gtest/gtest.h>
20 
21 std::string robot_description =
22 "<?xml version='1.0' ?>"
23 "<robot name='maxwell'>"
24 " <link name='base_link'/>"
25 " <joint name='torso_joint' type='fixed'>"
26 " <origin rpy='0 0 0' xyz='-0.00635 0 0.7914'/>"
27 " <parent link='base_link'/>"
28 " <child link='torso_link'/>"
29 " </joint>"
30 " <link name='torso_link'/>"
31 " <joint name='torso_actuator_joint' type='fixed'>"
32 " <origin rpy='0 0 0' xyz='0 0 0'/>"
33 " <parent link='torso_link'/>"
34 " <child link='torso_actuator_link'/>"
35 " </joint>"
36 " <link name='torso_actuator_link'/>"
37 " <joint name='arm_lift_joint' type='prismatic'>"
38 " <axis xyz='0 0 1'/>"
39 " <limit effort='30' lower='-0.464' upper='0' velocity='0.0508'/>"
40 " <origin rpy='0 0 0' xyz='0 0 0'/>"
41 " <parent link='torso_link'/>"
42 " <child link='arm_lift_link'/>"
43 " </joint>"
44 " <link name='arm_lift_link'/>"
45 " <joint name='arm_base_joint' type='fixed'>"
46 " <origin rpy='0 0 0' xyz='0.0611 0 0'/>"
47 " <parent link='arm_lift_link'/>"
48 " <child link='arm_link'/>"
49 " </joint>"
50 " <link name='arm_link'/>"
51 " <joint name='arm_shoulder_pan_servo_joint' type='fixed'>"
52 " <origin rpy='0 0 0' xyz='0 0 0'/>"
53 " <parent link='arm_link'/>"
54 " <child link='arm_shoulder_pan_servo_link'/>"
55 " </joint>"
56 " <link name='arm_shoulder_pan_servo_link'/>"
57 " <joint name='arm_shoulder_pan_joint' type='revolute'>"
58 " <origin rpy='0 0 0' xyz='0 0 0.0235'/>"
59 " <axis xyz='0 0 1'/>"
60 " <limit effort='30' lower='-1.57' upper='1.57' velocity='0.524'/>"
61 " <parent link='arm_shoulder_pan_servo_link'/>"
62 " <child link='arm_shoulder_pan_link'/>"
63 " </joint>"
64 " <link name='arm_shoulder_pan_link'/>"
65 " <joint name='arm_shoulder_lift_servo_joint' type='fixed'>"
66 " <origin rpy='0 -1.57 0' xyz='0 0 0.0526'/>"
67 " <parent link='arm_shoulder_pan_link'/>"
68 " <child link='arm_shoulder_lift_servo_link'/>"
69 " </joint>"
70 " <link name='arm_shoulder_lift_servo_link'/>"
71 " <joint name='arm_shoulder_lift_joint' type='revolute'>"
72 " <origin rpy='0 1.57 0' xyz='0 0 0'/>"
73 " <axis xyz='0 1 0'/>"
74 " <limit effort='30' lower='-1.77' upper='1.317' velocity='0.524'/>"
75 " <parent link='arm_shoulder_lift_servo_link'/>"
76 " <child link='arm_shoulder_lift_link'/>"
77 " </joint>"
78 " <link name='arm_shoulder_lift_link'/>"
79 " <joint name='arm_upperarm_roll_servo_joint' type='fixed'>"
80 " <origin rpy='1.57 1.57 0' xyz='0.0712978 0 0'/>"
81 " <parent link='arm_shoulder_lift_link'/>"
82 " <child link='arm_upperarm_roll_servo_link'/>"
83 " </joint>"
84 " <link name='arm_upperarm_roll_servo_link'/>"
85 " <joint name='arm_upperarm_roll_joint' type='revolute'>"
86 " <origin rpy='-1.57 0 1.57' xyz='0 0 0'/>"
87 " <axis xyz='1 0 0'/>"
88 " <limit effort='30' lower='-2' upper='2' velocity='0.524'/>"
89 " <parent link='arm_upperarm_roll_servo_link'/>"
90 " <child link='arm_upperarm_roll_link'/>"
91 " </joint>"
92 " <link name='arm_upperarm_roll_link'/>"
93 " <joint name='arm_elbow_flex_joint' type='revolute'>"
94 " <origin rpy='0 0 0' xyz='0.0869955 0 0'/>"
95 " <axis xyz='0 1 0'/>"
96 " <limit effort='30' lower='-1.57' upper='2.617' velocity='0.524'/>"
97 " <parent link='arm_upperarm_roll_link'/>"
98 " <child link='arm_elbow_flex_link'/>"
99 " </joint>"
100 " <link name='arm_elbow_flex_link'/>"
101 " <joint name='arm_forearm_fixed_joint' type='fixed'>"
102 " <origin rpy='0 0 0' xyz='0 0 0'/>"
103 " <parent link='arm_elbow_flex_link'/>"
104 " <child link='arm_forearm_link'/>"
105 " </joint>"
106 " <link name='arm_forearm_link'/>"
107 " <joint name='arm_wrist_flex_servo_joint' type='fixed'>"
108 " <origin rpy='0 0 0' xyz='0.125 0 0'/>"
109 " <parent link='arm_forearm_link'/>"
110 " <child link='arm_wrist_flex_servo_link'/>"
111 " </joint>"
112 " <link name='arm_wrist_flex_servo_link'/>"
113 " <joint name='arm_wrist_flex_joint' type='revolute'>"
114 " <origin rpy='0 0 0' xyz='0 0 0.0'/>"
115 " <axis xyz='0 1 0'/>"
116 " <limit effort='30' lower='-1.57' upper='1.57' velocity='0.785'/>"
117 " <parent link='arm_wrist_flex_servo_link'/>"
118 " <child link='arm_wrist_flex_link'/>"
119 " </joint>"
120 " <link name='arm_wrist_flex_link'/>"
121 " <joint name='arm_wrist_roll_joint' type='revolute'>"
122 " <axis xyz='1 0 0'/>"
123 " <limit effort='30' lower='-2.617' upper='2.617' velocity='0.785'/>"
124 " <origin rpy='0 0 0' xyz='0.031 0 0'/>"
125 " <parent link='arm_wrist_flex_link'/>"
126 " <child link='arm_wrist_roll_link'/>"
127 " </joint>"
128 " <link name='arm_wrist_roll_link'/>"
129 " <joint name='gripper_joint' type='fixed'>"
130 " <axis xyz='0 0 1'/>"
131 " <origin rpy='0 0 0' xyz='0.15 0 -0.015'/>"
132 " <parent link='arm_wrist_roll_link'/>"
133 " <child link='gripper_led_frame'/>"
134 " </joint>"
135 " <link name='gripper_led_frame'/>"
136 " <joint name='head_joint' type='fixed'>"
137 " <origin rpy='0 0 0' xyz='0 0 0.512375'/>"
138 " <parent link='torso_link'/>"
139 " <child link='head_link'/>"
140 " </joint>"
141 " <link name='head_link'/>"
142 " <joint name='head_pan_servo_joint' type='fixed'>"
143 " <origin rpy='0 0 0' xyz='0.010 0 0.0254'/>"
144 " <parent link='head_link'/>"
145 " <child link='head_pan_servo_link'/>"
146 " </joint>"
147 " <link name='head_pan_servo_link'/>"
148 " <joint name='head_pan_joint' type='revolute'>"
149 " <origin rpy='0 0 0' xyz='0 0 0.019'/>"
150 " <axis xyz='0 0 1'/>"
151 " <limit effort='30' lower='-2.617' upper='2.617' velocity='1.0'/>"
152 " <parent link='head_pan_servo_link'/>"
153 " <child link='head_pan_link'/>"
154 " </joint>"
155 " <link name='head_pan_link'/>"
156 " <joint name='head_tilt_servo_joint' type='fixed'>"
157 " <origin rpy='0 0 0' xyz='0 0 0.0415'/>"
158 " <parent link='head_pan_link'/>"
159 " <child link='head_tilt_servo_link'/>"
160 " </joint>"
161 " <link name='head_tilt_servo_link'/>"
162 " <joint name='head_tilt_joint' type='revolute'>"
163 " <origin rpy='0 0 0' xyz='0 0 0'/>"
164 " <axis xyz='0 1 0'/>"
165 " <limit effort='30' lower='-1.57' upper='1.57' velocity='1.0'/>"
166 " <parent link='head_tilt_servo_link'/>"
167 " <child link='head_tilt_link'/>"
168 " </joint>"
169 " <link name='head_tilt_link'/>"
170 " <joint name='head_camera_frame_joint' type='fixed'>"
171 " <origin rpy='0 0 0' xyz='0 0 0.026'/>"
172 " <parent link='head_tilt_link'/>"
173 " <child link='head_camera_frame'/>"
174 " </joint>"
175 " <link name='head_camera_frame'/>"
176 " <joint name='head_camera_ir_joint' type='fixed'>"
177 " <origin rpy='0 0 0' xyz='0.01905 -0.0269875 0.032075'/>"
178 " <parent link='head_camera_frame'/>"
179 " <child link='head_camera_ir_link'/>"
180 " </joint>"
181 " <link name='head_camera_ir_link'/>"
182 " <joint name='head_camera_ir_optical_frame_joint' type='fixed'>"
183 " <origin rpy='-1.57 0.0 -1.57' xyz='0 0 0'/>"
184 " <parent link='head_camera_ir_link'/>"
185 " <child link='head_camera_ir_optical_frame'/>"
186 " </joint>"
187 " <link name='head_camera_ir_optical_frame'/>"
188 " <joint name='head_camera_rgb_joint' type='fixed'>"
189 " <origin rpy='0 -0.08 0' xyz='0 0.0552875 0'/>"
190 " <parent link='head_camera_ir_link'/>"
191 " <child link='head_camera_rgb_link'/>"
192 " </joint>"
193 " <link name='head_camera_rgb_link'/>"
194 " <joint name='head_camera_rgb_optical_frame_joint' type='fixed'>"
195 " <origin rpy='-1.57 0.0 -1.57' xyz='0 0 0'/>"
196 " <parent link='head_camera_rgb_link'/>"
197 " <child link='head_camera_rgb_optical_frame'/>"
198 " </joint>"
199 " <link name='head_camera_rgb_optical_frame'/>"
200 "</robot>";
201 
202 TEST(ErrorBlockTests, error_blocks_maxwell)
203 {
204  ros::NodeHandle nh("~");
205 
206  robot_calibration::Optimizer opt(robot_description);
207 
208  std::vector<robot_calibration_msgs::CalibrationData> data;
209  robot_calibration_msgs::CalibrationData msg;
210 
211  // Match expected output from chain manager
212  msg.joint_states.name.resize(10);
213  msg.joint_states.name[0] = "arm_lift_joint";
214  msg.joint_states.name[1] = "arm_shoulder_pan_joint";
215  msg.joint_states.name[2] = "arm_shoulder_lift_joint";
216  msg.joint_states.name[3] = "arm_upperarm_roll_joint";
217  msg.joint_states.name[4] = "arm_elbow_flex_joint";
218  msg.joint_states.name[5] = "arm_wrist_flex_joint";
219  msg.joint_states.name[6] = "arm_wrist_roll_joint";
220  msg.joint_states.name[7] = "head_pan_joint";
221  msg.joint_states.name[8] = "head_tilt_joint";
222  msg.joint_states.name[9] = "arm_lift_joint";
223  msg.joint_states.position.resize(10);
224  msg.joint_states.position[0] = -0.05; // Add some error
225  msg.joint_states.position[1] = -0.814830;
226  msg.joint_states.position[2] = -0.00022290000000002586;
227  msg.joint_states.position[3] = 0.0;
228  msg.joint_states.position[4] = -0.7087341;
229  msg.joint_states.position[5] = 0.0;
230  msg.joint_states.position[6] = 0.0;
231  msg.joint_states.position[7] = -0.8280187999999999;
232  msg.joint_states.position[8] = 0.6358500000000002;
233  msg.joint_states.position[9] = 0.0;
234 
235  // Expectect output from led finder
236  msg.observations.resize(2);
237  msg.observations[0].sensor_name = "camera";
238  msg.observations[1].sensor_name = "arm";
239 
240  msg.observations[0].features.resize(1);
241  msg.observations[0].features[0].header.frame_id = "head_camera_rgb_optical_frame";
242  msg.observations[0].features[0].point.x = -0.0143163670728;
243  msg.observations[0].features[0].point.y = 0.111304592065;
244  msg.observations[0].features[0].point.z = 0.522079317365;
245 
246  msg.observations[0].ext_camera_info.camera_info.P[0] = 100.0; // fx
247  msg.observations[0].ext_camera_info.camera_info.P[5] = 100.0; // fy
248  msg.observations[0].ext_camera_info.camera_info.P[2] = 320.0; // cx
249  msg.observations[0].ext_camera_info.camera_info.P[6] = 240.0; // cy
250  msg.observations[0].ext_camera_info.parameters.resize(2);
251  msg.observations[0].ext_camera_info.parameters[0].name = "z_offset";
252  msg.observations[0].ext_camera_info.parameters[0].value = 0.0;
253  msg.observations[0].ext_camera_info.parameters[1].name = "z_scaling";
254  msg.observations[0].ext_camera_info.parameters[1].value = 1.0;
255 
256  msg.observations[1].features.resize(1);
257  msg.observations[1].features[0].header.frame_id = "gripper_led_frame";
258  msg.observations[1].features[0].point.x = 0.0;
259  msg.observations[1].features[0].point.y = 0.0;
260  msg.observations[1].features[0].point.z = 0.0;
261 
262  // Add first data point
263  data.push_back(msg);
264 
265  // Add a second data point that is just a little different
266  msg.joint_states.position[1] = -0.019781999999999966;
267  msg.joint_states.position[7] = 0.0;
268  msg.observations[0].features[0].point.x = 0.0365330705881;
269  msg.observations[0].features[0].point.y = 0.102609552493;
270  msg.observations[0].features[0].point.z = 0.536061220027;
271  data.push_back(msg);
272 
273  // And a third data point
274  msg.joint_states.position[1] = 0.883596;
275  msg.joint_states.position[7] = 0.9442135999999999;
276  msg.observations[0].features[0].point.x = 0.0942445346646;
277  msg.observations[0].features[0].point.y = 0.11409172323;
278  msg.observations[0].features[0].point.z = 0.517497963716;
279  data.push_back(msg);
280 
281  // Setup params
282  robot_calibration::OptimizationParams params;
283  params.LoadFromROS(nh);
284 
285  // Optimize
286  opt.optimize(params, data, false);
287  EXPECT_GT(opt.summary()->initial_cost, 0.001);
288  EXPECT_LT(opt.summary()->final_cost, 1e-18);
289  EXPECT_GT(opt.summary()->iterations.size(), static_cast<size_t>(1)); // expect more than 1 iteration
290  // The -0.05 we added above should be calibrated off
291  EXPECT_LT(fabs(0.05 - opt.getOffsets()->get("arm_lift_joint")), 0.001);
292  // 1 joint
293  EXPECT_EQ(1, opt.getNumParameters());
294  // 3 CalibrationData, each with chain3d with a single observed point (3 residuals)
295  EXPECT_EQ(9, opt.getNumResiduals());
296 }
297 
298 int main(int argc, char** argv)
299 {
300  ros::init(argc, argv, "error_block_tests");
301  testing::InitGoogleTest(&argc, argv);
302  return RUN_ALL_TESTS();
303 }
ros::init
ROSCPP_DECL void init(const M_string &remappings, const std::string &name, uint32_t options=0)
robot_calibration::Optimizer::getNumResiduals
int getNumResiduals()
Definition: optimizer.h:78
robot_calibration::Optimizer::summary
boost::shared_ptr< ceres::Solver::Summary > summary()
Returns the summary of the optimization last run.
Definition: optimizer.h:63
main
int main(int argc, char **argv)
Definition: error_block_tests2.cpp:298
TEST
TEST(ErrorBlockTests, error_blocks_maxwell)
Definition: error_block_tests2.cpp:202
robot_calibration::Optimizer::optimize
int optimize(OptimizationParams &params, std::vector< robot_calibration_msgs::CalibrationData > data, bool progress_to_stdout=false)
Run optimization.
Definition: optimizer.cpp:63
robot_calibration::Optimizer::getOffsets
boost::shared_ptr< CalibrationOffsetParser > getOffsets()
Definition: optimizer.h:68
robot_calibration::Optimizer
Class to do optimization.
Definition: optimizer.h:42
robot_calibration::OptimizationParams::LoadFromROS
bool LoadFromROS(ros::NodeHandle &nh)
Definition: optimization_params.cpp:30
model.h
robot_calibration::OptimizationParams
Class to hold parameters for optimization.
Definition: optimization_params.h:29
optimizer.h
robot_calibration::Optimizer::getNumParameters
int getNumParameters()
Definition: optimizer.h:73
EXPECT_EQ
#define EXPECT_EQ(a, b)
ros::NodeHandle
robot_description
std::string robot_description
Definition: error_block_tests2.cpp:21

robot_calibration
Author(s): Michael Ferguson
autogenerated on Fri Sep 1 2023 02:52:01