@@ -84,7 +84,6 @@ JointDeltaResult jointDeltaFromJointJog(const JointJogCommand& command, const mo
84
84
const JointNameToMoveGroupIndexMap& joint_name_group_index_map)
85
85
{
86
86
// Find the target joint position based on the commanded joint velocity
87
- StatusCode status = StatusCode::NO_WARNING;
88
87
const auto & group_name =
89
88
servo_params.active_subgroup .empty () ? servo_params.move_group_name : servo_params.active_subgroup ;
90
89
const moveit::core::JointModelGroup* joint_model_group = robot_state->getJointModelGroup (group_name);
@@ -93,7 +92,14 @@ JointDeltaResult jointDeltaFromJointJog(const JointJogCommand& command, const mo
93
92
Eigen::VectorXd velocities (joint_names.size ());
94
93
95
94
velocities.setZero ();
96
- bool names_valid = true ;
95
+ if (command.velocities .size () != command.names .size ())
96
+ {
97
+ RCLCPP_WARN_STREAM (getLogger (), " Invalid joint jog command. Each joint name must have one corresponding "
98
+ " velocity command. Received "
99
+ << command.names .size () << " joints with " << command.velocities .size ()
100
+ << " commands." );
101
+ return std::make_pair (StatusCode::INVALID, joint_position_delta);
102
+ }
97
103
98
104
for (size_t i = 0 ; i < command.names .size (); ++i)
99
105
{
@@ -104,44 +110,34 @@ JointDeltaResult jointDeltaFromJointJog(const JointJogCommand& command, const mo
104
110
}
105
111
else
106
112
{
107
- RCLCPP_WARN_STREAM (getLogger (), " Invalid joint name: " << command.names [i]);
108
-
109
- names_valid = false ;
110
- break ;
113
+ RCLCPP_WARN_STREAM (getLogger (), " Invalid joint name: " << command.names [i]
114
+ << " Either you're sending commands for a joint "
115
+ " that is not part of the move group or certain joints "
116
+ " cannot be moved because a "
117
+ " subgroup is active and they are not part of it." );
118
+ return std::make_pair (StatusCode::INVALID, joint_position_delta);
111
119
}
112
120
}
113
- const bool velocity_valid = isValidCommand (velocities);
114
- if (names_valid && velocity_valid )
121
+
122
+ if (! isValidCommand (velocities) )
115
123
{
116
- joint_position_delta = velocities * servo_params.publish_period ;
117
- if (servo_params.command_in_type == " unitless" )
118
- {
119
- joint_position_delta *= servo_params.scale .joint ;
120
- }
124
+ RCLCPP_WARN_STREAM (getLogger (), " Invalid velocity values in joint jog command" );
125
+ return std::make_pair (StatusCode::INVALID, joint_position_delta);
121
126
}
122
- else
127
+
128
+ joint_position_delta = velocities * servo_params.publish_period ;
129
+ if (servo_params.command_in_type == " unitless" )
123
130
{
124
- status = StatusCode::INVALID;
125
- if (!names_valid)
126
- {
127
- RCLCPP_WARN_STREAM (getLogger (),
128
- " Invalid joint names in joint jog command. Either you're sending commands for a joint "
129
- " that is not part of the move group or certain joints cannot be moved because a "
130
- " subgroup is active and they are not part of it." );
131
- }
132
- if (!velocity_valid)
133
- {
134
- RCLCPP_WARN_STREAM (getLogger (), " Invalid velocity values in joint jog command" );
135
- }
131
+ joint_position_delta *= servo_params.scale .joint ;
136
132
}
137
133
138
134
if (!servo_params.active_subgroup .empty () && servo_params.active_subgroup != servo_params.move_group_name )
139
135
{
140
- return std::make_pair (status , createMoveGroupDelta (joint_position_delta, robot_state, servo_params,
141
- joint_name_group_index_map));
136
+ return std::make_pair (StatusCode::NO_WARNING , createMoveGroupDelta (joint_position_delta, robot_state, servo_params,
137
+ joint_name_group_index_map));
142
138
}
143
139
144
- return std::make_pair (status , joint_position_delta);
140
+ return std::make_pair (StatusCode::NO_WARNING , joint_position_delta);
145
141
}
146
142
147
143
JointDeltaResult jointDeltaFromTwist (const TwistCommand& command, const moveit::core::RobotStatePtr& robot_state,
@@ -154,10 +150,23 @@ JointDeltaResult jointDeltaFromTwist(const TwistCommand& command, const moveit::
154
150
Eigen::VectorXd joint_position_delta (num_joints);
155
151
Eigen::Vector<double , 6 > cartesian_position_delta;
156
152
157
- const bool valid_command = isValidCommand (command);
158
- const bool is_planning_frame = (command.frame_id == planning_frame);
159
- const bool is_zero = command.velocities .isZero ();
160
- if (!is_zero && is_planning_frame && valid_command)
153
+ if (command.frame_id != planning_frame)
154
+ {
155
+ RCLCPP_WARN_STREAM (getLogger (), " Command frame is: " << command.frame_id << " , expected: " << planning_frame);
156
+ return std::make_pair (StatusCode::INVALID, joint_position_delta);
157
+ }
158
+
159
+ if (!isValidCommand (command))
160
+ {
161
+ RCLCPP_WARN_STREAM (getLogger (), " Invalid twist command." );
162
+ return std::make_pair (StatusCode::INVALID, joint_position_delta);
163
+ }
164
+
165
+ if (command.velocities .isZero ())
166
+ {
167
+ joint_position_delta.setZero ();
168
+ }
169
+ else
161
170
{
162
171
// Compute the Cartesian position delta based on incoming twist command.
163
172
cartesian_position_delta = command.velocities * servo_params.publish_period ;
@@ -208,22 +217,7 @@ JointDeltaResult jointDeltaFromTwist(const TwistCommand& command, const moveit::
208
217
}
209
218
}
210
219
}
211
- else if (is_zero)
212
- {
213
- joint_position_delta.setZero ();
214
- }
215
- else
216
- {
217
- status = StatusCode::INVALID;
218
- if (!valid_command)
219
- {
220
- RCLCPP_ERROR_STREAM (getLogger (), " Invalid twist command." );
221
- }
222
- if (!is_planning_frame)
223
- {
224
- RCLCPP_ERROR_STREAM (getLogger (), " Command frame is: " << command.frame_id << " , expected: " << planning_frame);
225
- }
226
- }
220
+
227
221
return std::make_pair (status, joint_position_delta);
228
222
}
229
223
@@ -237,74 +231,68 @@ JointDeltaResult jointDeltaFromPose(const PoseCommand& command, const moveit::co
237
231
robot_state->getJointModelGroup (servo_params.move_group_name )->getActiveJointModelNames ().size ();
238
232
Eigen::VectorXd joint_position_delta (num_joints);
239
233
240
- const bool valid_command = isValidCommand (command);
241
- const bool is_planning_frame = (command.frame_id == planning_frame);
242
-
243
- if (valid_command && is_planning_frame)
234
+ if (!isValidCommand (command))
244
235
{
245
- Eigen::Vector<double , 6 > cartesian_position_delta;
236
+ RCLCPP_WARN_STREAM (getLogger (), " Invalid pose command." );
237
+ return std::make_pair (StatusCode::INVALID, joint_position_delta);
238
+ }
246
239
247
- // Compute linear and angular change needed.
248
- const Eigen::Isometry3d ee_pose{ robot_state->getGlobalLinkTransform (planning_frame).inverse () *
249
- robot_state->getGlobalLinkTransform (ee_frame) };
250
- const Eigen::Quaterniond q_current (ee_pose.rotation ());
251
- Eigen::Quaterniond q_target (command.pose .rotation ());
252
- Eigen::Vector3d translation_error = command.pose .translation () - ee_pose.translation ();
240
+ if (command.frame_id != planning_frame)
241
+ {
242
+ RCLCPP_WARN_STREAM (getLogger (), " Command frame is: " << command.frame_id << " expected: " << planning_frame);
243
+ return std::make_pair (StatusCode::INVALID, joint_position_delta);
244
+ }
253
245
254
- // Limit the commands by the maximum linear and angular speeds provided.
255
- if (servo_params.scale .linear > 0.0 )
256
- {
257
- const auto linear_speed_scale =
258
- (translation_error.norm () / servo_params.publish_period ) / servo_params.scale .linear ;
259
- if (linear_speed_scale > 1.0 )
260
- {
261
- translation_error /= linear_speed_scale;
262
- }
263
- }
264
- if (servo_params.scale .rotational > 0.0 )
265
- {
266
- const auto angular_speed_scale =
267
- (std::abs (q_target.angularDistance (q_current)) / servo_params.publish_period ) / servo_params.scale .rotational ;
268
- if (angular_speed_scale > 1.0 )
269
- {
270
- q_target = q_current.slerp (1.0 / angular_speed_scale, q_target);
271
- }
272
- }
246
+ Eigen::Vector<double , 6 > cartesian_position_delta;
273
247
274
- // Compute the Cartesian deltas from the velocity-scaled values.
275
- const auto angle_axis_error = Eigen::AngleAxisd (q_target * q_current.inverse ());
276
- cartesian_position_delta.head <3 >() = translation_error;
277
- cartesian_position_delta.tail <3 >() = angle_axis_error.axis () * angle_axis_error.angle ();
248
+ // Compute linear and angular change needed.
249
+ const Eigen::Isometry3d ee_pose{ robot_state->getGlobalLinkTransform (planning_frame).inverse () *
250
+ robot_state->getGlobalLinkTransform (ee_frame) };
251
+ const Eigen::Quaterniond q_current (ee_pose.rotation ());
252
+ Eigen::Quaterniond q_target (command.pose .rotation ());
253
+ Eigen::Vector3d translation_error = command.pose .translation () - ee_pose.translation ();
278
254
279
- // Compute the required change in joint angles.
280
- const auto delta_result =
281
- jointDeltaFromIK (cartesian_position_delta, robot_state, servo_params, joint_name_group_index_map);
282
- status = delta_result.first ;
283
- if (status != StatusCode::INVALID)
255
+ // Limit the commands by the maximum linear and angular speeds provided.
256
+ if (servo_params.scale .linear > 0.0 )
257
+ {
258
+ const auto linear_speed_scale =
259
+ (translation_error.norm () / servo_params.publish_period ) / servo_params.scale .linear ;
260
+ if (linear_speed_scale > 1.0 )
284
261
{
285
- joint_position_delta = delta_result.second ;
286
- // Get velocity scaling information for singularity.
287
- const auto singularity_scaling_info =
288
- velocityScalingFactorForSingularity (robot_state, cartesian_position_delta, servo_params);
289
- // Apply velocity scaling for singularity, if there was any scaling.
290
- if (singularity_scaling_info.second != StatusCode::NO_WARNING)
291
- {
292
- status = singularity_scaling_info.second ;
293
- RCLCPP_WARN_STREAM (getLogger (), SERVO_STATUS_CODE_MAP.at (status));
294
- joint_position_delta *= singularity_scaling_info.first ;
295
- }
262
+ translation_error /= linear_speed_scale;
296
263
}
297
264
}
298
- else
265
+ if (servo_params. scale . rotational > 0.0 )
299
266
{
300
- status = StatusCode::INVALID;
301
- if (!valid_command)
267
+ const auto angular_speed_scale =
268
+ (std::abs (q_target.angularDistance (q_current)) / servo_params.publish_period ) / servo_params.scale .rotational ;
269
+ if (angular_speed_scale > 1.0 )
302
270
{
303
- RCLCPP_WARN_STREAM ( getLogger (), " Invalid pose command. " );
271
+ q_target = q_current. slerp ( 1.0 / angular_speed_scale, q_target );
304
272
}
305
- if (!is_planning_frame)
273
+ }
274
+
275
+ // Compute the Cartesian deltas from the velocity-scaled values.
276
+ const auto angle_axis_error = Eigen::AngleAxisd (q_target * q_current.inverse ());
277
+ cartesian_position_delta.head <3 >() = translation_error;
278
+ cartesian_position_delta.tail <3 >() = angle_axis_error.axis () * angle_axis_error.angle ();
279
+
280
+ // Compute the required change in joint angles.
281
+ const auto delta_result =
282
+ jointDeltaFromIK (cartesian_position_delta, robot_state, servo_params, joint_name_group_index_map);
283
+ status = delta_result.first ;
284
+ if (status != StatusCode::INVALID)
285
+ {
286
+ joint_position_delta = delta_result.second ;
287
+ // Get velocity scaling information for singularity.
288
+ const auto singularity_scaling_info =
289
+ velocityScalingFactorForSingularity (robot_state, cartesian_position_delta, servo_params);
290
+ // Apply velocity scaling for singularity, if there was any scaling.
291
+ if (singularity_scaling_info.second != StatusCode::NO_WARNING)
306
292
{
307
- RCLCPP_WARN_STREAM (getLogger (), " Command frame is: " << command.frame_id << " expected: " << planning_frame);
293
+ status = singularity_scaling_info.second ;
294
+ RCLCPP_WARN_STREAM (getLogger (), SERVO_STATUS_CODE_MAP.at (status));
295
+ joint_position_delta *= singularity_scaling_info.first ;
308
296
}
309
297
}
310
298
return std::make_pair (status, joint_position_delta);
0 commit comments