Adds orientation constraints #55
There are no files selected for viewing
| Original file line number | Diff line number | Diff line change |
|---|---|---|
|
|
@@ -125,6 +125,7 @@ void KTOptPlanningContext::solve(planning_interface::MotionPlanResponse& res) | |
|
|
||
| // process path_constraints | ||
| addPathPositionConstraints(trajopt, plant, plant_context, params_.position_constraint_padding); | ||
| addPathOrientationConstraints(trajopt, plant, plant_context, params_.orientation_constraint_padding); | ||
|
|
||
| // solve the program | ||
| auto result = Solve(prog); | ||
|
|
@@ -252,6 +253,73 @@ void KTOptPlanningContext::addPathPositionConstraints(KinematicTrajectoryOptimiz | |
| } | ||
| } | ||
|
|
||
| void KTOptPlanningContext::addPathOrientationConstraints(KinematicTrajectoryOptimization& trajopt, | ||
| const MultibodyPlant<double>& plant, | ||
| Context<double>& plant_context, const double padding) | ||
| { | ||
| // retrieve the motion planning request | ||
| const auto& req = getMotionPlanRequest(); | ||
|
|
||
| // check for path position constraints | ||
| if (!req.path_constraints.orientation_constraints.empty()) | ||
| { | ||
| for (const auto& orientation_constraint : req.path_constraints.orientation_constraints) | ||
| { | ||
| // NOTE: Current thesis, when you apply a Drake Orientation Constraint it | ||
| // expects the following | ||
| // Frame A: The frame in which the orientation constraint is defined | ||
| // Frame B: The frame to which the orientation constraint is enforced | ||
|
There was a problem hiding this comment. In the code below, the roles of A and B are exactly swapped: There was a problem hiding this comment. Oooh nice catch. Thanks for that! However, I was adding it correctly into the constraints. I'll update the comments |
||
| // theta_bound: The angle difference between frame A's orientation and | ||
| // frame B's orientation | ||
|
|
||
| // Extract FrameA and FrameB for orientation constraint | ||
| // frame A | ||
| const auto link_ee_name = orientation_constraint.link_name; | ||
| if (!plant.HasBodyNamed(link_ee_name)) | ||
| { | ||
| RCLCPP_ERROR(getLogger(), | ||
| "The link specified in the PositionConstraint message, %s, does not exist in the Drake " | ||
sea-bass marked this conversation as resolved.
Show resolved
Hide resolved
|
||
| "plant.", | ||
| link_ee_name.c_str()); | ||
| continue; | ||
| } | ||
| const auto& link_ee_frame = plant.GetFrameByName(link_ee_name); | ||
|
|
||
| // frame B | ||
| const auto base_frame_name = orientation_constraint.header.frame_id; | ||
| if (!plant.HasBodyNamed(base_frame_name)) | ||
| { | ||
| RCLCPP_ERROR(getLogger(), | ||
| "The link specified in the PositionConstraint message, %s, does not exist in the Drake " | ||
sea-bass marked this conversation as resolved.
Show resolved
Hide resolved
|
||
| "plant.", | ||
| base_frame_name.c_str()); | ||
| continue; | ||
| } | ||
| const auto& base_frame = plant.GetFrameByName(base_frame_name); | ||
|
|
||
| // Extract the orientation of that Frame B needs to be constrained to w.r.t Frame A | ||
| const auto& constrained_orientation = orientation_constraint.orientation; | ||
| // convert to drake's RotationMatrix | ||
| const auto R_BbarB = RotationMatrixd(Eigen::Quaterniond(constrained_orientation.w, constrained_orientation.x, | ||
| constrained_orientation.y, constrained_orientation.z)); | ||
|
|
||
| // NOTE: There are 3 tolerances given for the orientation constraint in moveit | ||
| // message, rake only takes in a single theta bound. For now, we take any | ||
sea-bass marked this conversation as resolved.
Show resolved
Hide resolved
|
||
| // of the tolerances as the theta bound | ||
| const double theta_bound = orientation_constraint.absolute_x_axis_tolerance; | ||
|
There was a problem hiding this comment. Yeah, I think Probably just There was a problem hiding this comment. Also, since This is still not a proper calculation, so I think we anyhow have to be "creative" in how we interpret the tolerances in the MoveIt message vs. what translates to here. There was a problem hiding this comment. It is but only via the orientation padding parameter in the yaml file. There was a problem hiding this comment. no, the variable There was a problem hiding this comment. I agree with #55 (comment) to use the norm or sum of all angle tolerances. MoveIt's orientation constraint is much more flexible than Drake's one, allowing deviations to be specified for all rotation axes separately. It will only translate in a meaningful way if the MoveIt tolerances are all identical. There was a problem hiding this comment. So for this, a better approach would be to add an additional rotational transformation to There was a problem hiding this comment. This will not work. Drake's orientation constraint already fully constrains all three axes. Adding more orientation constraints doesn't make sense. |
||
|
|
||
| // Add position constraint to each knot point of the trajectory | ||
|
There was a problem hiding this comment. I was wondering in the last PR whether we should have used the same number of knot points for all position/orientation constraints. What do you think? There was a problem hiding this comment. It is a parameter, so the user has the option of adding more/less constraints along the path. There was a problem hiding this comment. Yes, I meant should there be 2 separate parameters for position/orientation constraints, or just use the same parameter for all constraints? There was a problem hiding this comment. Ah I meant, right now even though im reading theta bound as the tolerance, I am using the one @sea-bass added as a parameter. |
||
| for (int i = 0; i < params_.num_orientation_constraint_points; ++i) | ||
| { | ||
| trajopt.AddPathPositionConstraint( | ||
|
There was a problem hiding this comment. Strangely enough, this is correct... There was a problem hiding this comment. I believe the terminology reflects that it is indeed still a constraint on the generalized positions, which happens to be a vector in the configuration space. Therefore, you are adding a constraint on one coordinate (link end-effector) out of all the available coordinates. At least this is my understanding |
||
| std::make_shared<OrientationConstraint>(&plant, base_frame, RotationMatrixd::Identity(), link_ee_frame, | ||
| R_BbarB, padding, &plant_context), | ||
| static_cast<double>(i) / (params_.num_position_constraint_points - 1)); | ||
| } | ||
| } | ||
| } | ||
| } | ||
|
|
||
| bool KTOptPlanningContext::terminate() | ||
| { | ||
| RCLCPP_ERROR(getLogger(), "KTOptPlanningContext::terminate() is not implemented!"); | ||
|
|
||
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Remove?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Just talked to Aditya and I think we should keep and uncomment as these are the move-on-line and move-within-plane demos
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Sounds good. I do suspect that the rotated bounding box constraints may not work with the current implementation of position constraints -- there is another overload that accepts a rigid transform from a coordinate frame
AtoA_barwhich enables rotating the reference frame, and we may need to achieve this.Trying it out will tell.