From 22fe8d4f1dd26e129d0f8eebbfb9410b90c6d325 Mon Sep 17 00:00:00 2001 From: Thomas Herring Date: Mon, 30 Aug 2021 17:14:44 -0500 Subject: [PATCH 01/10] Adding initial --- .../include/robowflex_library/builder.h | 18 +++++++++++------- robowflex_library/src/builder.cpp | 11 +++++++++++ robowflex_library/src/tf.cpp | 14 ++++++++++++++ 3 files changed, 36 insertions(+), 7 deletions(-) diff --git a/robowflex_library/include/robowflex_library/builder.h b/robowflex_library/include/robowflex_library/builder.h index 75e59c1ae..94bf77ffb 100644 --- a/robowflex_library/include/robowflex_library/builder.h +++ b/robowflex_library/include/robowflex_library/builder.h @@ -178,13 +178,17 @@ namespace robowflex const GeometryConstPtr &geometry, const Eigen::Quaterniond &orientation, const Eigen::Vector3d &tolerances); - /** \brief Tiles some \a geometry around a \a pose in \a base_name for the end-effector \a ee_name. - * The \a geometry is placed at \a offset from \a pose, and \a n copies are placed evenly rotated - * about \a axis. The desired \a orientation is also rotated about the axis and set for each copy. - * \param[in] ee_name The name of the end-effector link. - * \param[in] base_name The name of the frame of reference of \a pose and \a orientation. - * \param[in] pose The pose of the frame to be rotated about. - * \param[in] geometry The geometry describing the position constraint. + void addJointConstraintToGoal(int idx, const std::string &joint_name, float position, + const Eigen::Vector2d &tolerances); + + void addJointConstraintToGoal(int idx, moveit_msgs::JointConstraintPtr joint_constraint); + + /** \brief Tiles some \a geometry around a \a pose in \a base_name for the end-effector \a + * ee_name. The \a geometry is placed at \a offset from \a pose, and \a n copies are placed evenly + * rotated about \a axis. The desired \a orientation is also rotated about the axis and set for + * each copy. \param[in] ee_name The name of the end-effector link. \param[in] base_name The name + * of the frame of reference of \a pose and \a orientation. \param[in] pose The pose of the frame + * to be rotated about. \param[in] geometry The geometry describing the position constraint. * \param[in] orientation The desired orientation. * \param[in] tolerances XYZ Euler angle tolerances about orientation. * \param[in] offset Offset of the goal \a geometry from \a pose. diff --git a/robowflex_library/src/builder.cpp b/robowflex_library/src/builder.cpp index fa2d5015a..e50e7afb1 100644 --- a/robowflex_library/src/builder.cpp +++ b/robowflex_library/src/builder.cpp @@ -332,6 +332,17 @@ void MotionRequestBuilder::addGoalRegion(const std::string &ee_name, const std:: request_.goal_constraints.push_back(constraints); } +void MotionRequestBuilder::addJointConstraintToGoal(int idx, const std::string &joint_name, float position, + const Eigen::Vector2d &tolerances) +{ + addJointConstraintToGoal(idx, TF::getJointConstraint(joint_name, position, tolerances)); +} + +void MotionRequestBuilder::addJointConstraintToGoal(int idx, moveit_msgs::JointConstraintPtr joint_constraint) +{ + request_.goal_constraints[idx].joint_constraint.push_back(joint_constraint); +} + void MotionRequestBuilder::addGoalRotaryTile(const std::string &ee_name, const std::string &base_name, const RobotPose &pose, const GeometryConstPtr &geometry, const Eigen::Quaterniond &orientation, diff --git a/robowflex_library/src/tf.cpp b/robowflex_library/src/tf.cpp index 31a70f86b..af65c8c1b 100644 --- a/robowflex_library/src/tf.cpp +++ b/robowflex_library/src/tf.cpp @@ -204,6 +204,20 @@ moveit_msgs::OrientationConstraint TF::getOrientationConstraint(const std::strin return constraint; } +moveit_msgs::JointConstraint TF::getJointConstraint(const std::string &joint_name, float position, + const Eigen::Vector2d &tolerances) +{ + moveit_msgs::JointConstraint constraint; + + constraint.joint_name = joint_name; + constraint.position = position; + constraint.tolerance_above = tolerances[0]; + constraint.tolerance_below = tolerances[1]; + constraint.weight = 1; + + return constraint +} + Eigen::Quaterniond TF::sampleOrientation(const Eigen::Quaterniond &orientation, const Eigen::Vector3d &tolerances) { From 59a93b1ebfd757085f9a212c558406ef8b8bbdb8 Mon Sep 17 00:00:00 2001 From: Thomas Herring Date: Mon, 30 Aug 2021 17:38:29 -0500 Subject: [PATCH 02/10] Added ignore cor cache --- .gitignore | 1 + 1 file changed, 1 insertion(+) diff --git a/.gitignore b/.gitignore index d7cbddf3c..c4062bfa4 100644 --- a/.gitignore +++ b/.gitignore @@ -37,6 +37,7 @@ core # Compile Commands *compile_commands.json +.cache/ # Python __pycache__ From 7d648c26be3a1371a58be9f6f31a712d7c94881c Mon Sep 17 00:00:00 2001 From: Thomas Herring Date: Mon, 30 Aug 2021 17:38:41 -0500 Subject: [PATCH 03/10] Fixed type --- robowflex_library/src/builder.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/robowflex_library/src/builder.cpp b/robowflex_library/src/builder.cpp index e50e7afb1..665cd9bc5 100644 --- a/robowflex_library/src/builder.cpp +++ b/robowflex_library/src/builder.cpp @@ -340,7 +340,7 @@ void MotionRequestBuilder::addJointConstraintToGoal(int idx, const std::string & void MotionRequestBuilder::addJointConstraintToGoal(int idx, moveit_msgs::JointConstraintPtr joint_constraint) { - request_.goal_constraints[idx].joint_constraint.push_back(joint_constraint); + request_.goal_constraints[idx].joint_constraints.push_back(joint_constraint); } void MotionRequestBuilder::addGoalRotaryTile(const std::string &ee_name, const std::string &base_name, From a893b986d2cdf126d150ddf35ba558dd9c662093 Mon Sep 17 00:00:00 2001 From: Thomas Herring Date: Wed, 1 Sep 2021 03:20:01 -0500 Subject: [PATCH 04/10] Fixed compilation errors in joint constraints --- .../include/robowflex_library/builder.h | 49 +++++++++---------- .../include/robowflex_library/tf.h | 10 ++++ robowflex_library/src/builder.cpp | 5 +- robowflex_library/src/tf.cpp | 28 +++++------ 4 files changed, 49 insertions(+), 43 deletions(-) diff --git a/robowflex_library/include/robowflex_library/builder.h b/robowflex_library/include/robowflex_library/builder.h index 94bf77ffb..435621df7 100644 --- a/robowflex_library/include/robowflex_library/builder.h +++ b/robowflex_library/include/robowflex_library/builder.h @@ -181,7 +181,7 @@ namespace robowflex void addJointConstraintToGoal(int idx, const std::string &joint_name, float position, const Eigen::Vector2d &tolerances); - void addJointConstraintToGoal(int idx, moveit_msgs::JointConstraintPtr joint_constraint); + void addJointConstraintToGoal(int idx, moveit_msgs::JointConstraint joint_constraint); /** \brief Tiles some \a geometry around a \a pose in \a base_name for the end-effector \a * ee_name. The \a geometry is placed at \a offset from \a pose, and \a n copies are placed evenly @@ -200,13 +200,11 @@ namespace robowflex const Eigen::Quaterniond &orientation, const Eigen::Vector3d &tolerances, const RobotPose &offset, const Eigen::Vector3d &axis, unsigned int n); - /** \brief Adds a set of regions to grasp a cylinder from the side. This function assumes the X-axis - * of the end-effector frame \a ee_name points "forward" for grasping. - * \param[in] ee_name The name of the end-effector link. - * \param[in] base_name The name of the frame of reference of \a pose. - * \param[in] pose The pose of the frame to be rotated about. - * \param[in] cylinder The cylinder to grasp. - * \param[in] distance The distance from the cylinder to place the regions. + /** \brief Adds a set of regions to grasp a cylinder from the side. This function assumes the + * X-axis of the end-effector frame \a ee_name points "forward" for grasping. \param[in] ee_name + * The name of the end-effector link. \param[in] base_name The name of the frame of reference of + * \a pose. \param[in] pose The pose of the frame to be rotated about. \param[in] cylinder The + * cylinder to grasp. \param[in] distance The distance from the cylinder to place the regions. * \param[in] depth The depth of boxes to create. * \param[in] n The number of regions to create. */ @@ -249,11 +247,10 @@ namespace robowflex double tolerance = 0.001); /** \brief Set a goal region for an end-effector \a ee_name. - * Sets the position constraint from \a geometry at a pose \a pose, and the orientation constraint - * from \a orientation and XYZ Euler angle tolerances \a tolerances. - * \param[in] ee_name The name of the end-effector link. - * \param[in] base_name The name of the frame of reference of \a pose and \a orientation. - * \param[in] pose The pose of \a geometry in \a base_frame. + * Sets the position constraint from \a geometry at a pose \a pose, and the orientation + * constraint from \a orientation and XYZ Euler angle tolerances \a tolerances. \param[in] ee_name + * The name of the end-effector link. \param[in] base_name The name of the frame of reference of + * \a pose and \a orientation. \param[in] pose The pose of \a geometry in \a base_frame. * \param[in] geometry The geometry describing the position constraint. * \param[in] orientation The desired orientation. * \param[in] tolerances XYZ Euler angle tolerances about orientation. @@ -272,11 +269,10 @@ namespace robowflex \{ */ /** \brief Set a pose constraint on the path. - * Sets the position constraint from \a geometry at a pose \a pose, and the orientation constraint - * from \a orientation and XYZ Euler angle tolerances \a tolerances. - * \param[in] ee_name The name of the end-effector link. - * \param[in] base_name The name of the frame of reference of \a pose and \a orientation. - * \param[in] pose The pose of \a geometry in \a base_frame. + * Sets the position constraint from \a geometry at a pose \a pose, and the orientation + * constraint from \a orientation and XYZ Euler angle tolerances \a tolerances. \param[in] ee_name + * The name of the end-effector link. \param[in] base_name The name of the frame of reference of + * \a pose and \a orientation. \param[in] pose The pose of \a geometry in \a base_frame. * \param[in] geometry The geometry describing the position constraint. * \param[in] orientation The desired orientation. * \param[in] tolerances XYZ Euler angle tolerances about orientation. @@ -296,10 +292,9 @@ namespace robowflex const RobotPose &pose, const GeometryConstPtr &geometry); /** \brief Set an orientation constraint on the path. - * Sets the orientation constraint from \a orientation and XYZ Euler angle tolerances \a tolerances. - * \param[in] ee_name The name of the end-effector link. - * \param[in] base_name The name of the frame of reference of \a orientation. - * \param[in] orientation The desired orientation. + * Sets the orientation constraint from \a orientation and XYZ Euler angle tolerances \a + * tolerances. \param[in] ee_name The name of the end-effector link. \param[in] base_name The name + * of the frame of reference of \a orientation. \param[in] orientation The desired orientation. * \param[in] tolerances XYZ Euler angle tolerances about orientation. */ void addPathOrientationConstraint(const std::string &ee_name, const std::string &base_name, @@ -313,11 +308,11 @@ namespace robowflex /** \brief Set the planning configuration to use for the motion planning request. * Attempts to match \a requested_config with the planner configuration offered by \a planner_ - * that is the shortest configuration that contains \a requested_config as a substring. For example, - * specifying `RRTConnect` will match `RRTConnectkConfigDefault`, and specifying `RRT` will match - * `RRTkConfigDefault` and not `RRTConnectkConfigDefault`. - * \param[in] requested_config The planner config to find and use. - * \return True if the \a requested_config is found, false otherwise. + * that is the shortest configuration that contains \a requested_config as a substring. For + * example, specifying `RRTConnect` will match `RRTConnectkConfigDefault`, and specifying `RRT` + * will match `RRTkConfigDefault` and not `RRTConnectkConfigDefault`. \param[in] requested_config + * The planner config to find and use. \return True if the \a requested_config is found, false + * otherwise. */ bool setConfig(const std::string &requested_config); diff --git a/robowflex_library/include/robowflex_library/tf.h b/robowflex_library/include/robowflex_library/tf.h index 58bd71b68..328235e48 100644 --- a/robowflex_library/include/robowflex_library/tf.h +++ b/robowflex_library/include/robowflex_library/tf.h @@ -14,6 +14,7 @@ #include #include #include +#include #include #include @@ -206,6 +207,15 @@ namespace robowflex Eigen::Quaterniond offsetOrientation(const Eigen::Quaterniond &orientation, const Eigen::Vector3d &axis, double value); + /** \brief Get a joint constraint message. + * \param[in] joint_name The name of the constrained joint. + * \param[in] position The value in radians to constrain to. + * \param[in] tolerances A vector of upper bound and lower bound offsets from position. + * \return The joint constraint as a MoveIt message. + */ + moveit_msgs::JointConstraint getJointConstraint(const std::string &joint_name, float position, + const Eigen::Vector2d &tolerances); + /** \brief Sample a position within the given \a bounds using a uniform distribution. * \param[in] bounds The desired mean orientation. * \return The sampled position. diff --git a/robowflex_library/src/builder.cpp b/robowflex_library/src/builder.cpp index 665cd9bc5..25a23df32 100644 --- a/robowflex_library/src/builder.cpp +++ b/robowflex_library/src/builder.cpp @@ -338,9 +338,10 @@ void MotionRequestBuilder::addJointConstraintToGoal(int idx, const std::string & addJointConstraintToGoal(idx, TF::getJointConstraint(joint_name, position, tolerances)); } -void MotionRequestBuilder::addJointConstraintToGoal(int idx, moveit_msgs::JointConstraintPtr joint_constraint) +void MotionRequestBuilder::addJointConstraintToGoal(int idx, moveit_msgs::JointConstraint joint_constraint) { - request_.goal_constraints[idx].joint_constraints.push_back(joint_constraint); + auto joint_constraints = request_.goal_constraints[idx].joint_constraints; + /* request_.goal_constraints.at(idx).joint_constraints.push_back(joint_constraint); */ } void MotionRequestBuilder::addGoalRotaryTile(const std::string &ee_name, const std::string &base_name, diff --git a/robowflex_library/src/tf.cpp b/robowflex_library/src/tf.cpp index af65c8c1b..19081f294 100644 --- a/robowflex_library/src/tf.cpp +++ b/robowflex_library/src/tf.cpp @@ -204,20 +204,6 @@ moveit_msgs::OrientationConstraint TF::getOrientationConstraint(const std::strin return constraint; } -moveit_msgs::JointConstraint TF::getJointConstraint(const std::string &joint_name, float position, - const Eigen::Vector2d &tolerances) -{ - moveit_msgs::JointConstraint constraint; - - constraint.joint_name = joint_name; - constraint.position = position; - constraint.tolerance_above = tolerances[0]; - constraint.tolerance_below = tolerances[1]; - constraint.weight = 1; - - return constraint -} - Eigen::Quaterniond TF::sampleOrientation(const Eigen::Quaterniond &orientation, const Eigen::Vector3d &tolerances) { @@ -245,6 +231,20 @@ Eigen::Quaterniond TF::offsetOrientation(const Eigen::Quaterniond &orientation, return Eigen::AngleAxisd(value, axis) * orientation; } +moveit_msgs::JointConstraint TF::getJointConstraint(const std::string &joint_name, float position, + const Eigen::Vector2d &tolerances) +{ + moveit_msgs::JointConstraint constraint; + + constraint.joint_name = joint_name; + constraint.position = position; + constraint.tolerance_above = tolerances[0]; + constraint.tolerance_below = tolerances[1]; + constraint.weight = 1; + + return constraint; +} + Eigen::Vector3d TF::samplePositionUniform(const Eigen::Vector3d &bounds) { return RNG::uniformVec(bounds); From c6c7fa168fffb8b1db2cae0c3d52cd1f6c63dece Mon Sep 17 00:00:00 2001 From: Thomas Herring Date: Sun, 12 Sep 2021 17:58:39 -0500 Subject: [PATCH 05/10] Changed method header --- .../include/robowflex_library/builder.h | 4 ++-- robowflex_library/src/builder.cpp | 13 +++++++------ 2 files changed, 9 insertions(+), 8 deletions(-) diff --git a/robowflex_library/include/robowflex_library/builder.h b/robowflex_library/include/robowflex_library/builder.h index 435621df7..61881d918 100644 --- a/robowflex_library/include/robowflex_library/builder.h +++ b/robowflex_library/include/robowflex_library/builder.h @@ -178,10 +178,10 @@ namespace robowflex const GeometryConstPtr &geometry, const Eigen::Quaterniond &orientation, const Eigen::Vector3d &tolerances); - void addJointConstraintToGoal(int idx, const std::string &joint_name, float position, + void addJointConstraintToGoal(int goal_idx, const std::string &joint_name, float position, const Eigen::Vector2d &tolerances); - void addJointConstraintToGoal(int idx, moveit_msgs::JointConstraint joint_constraint); + void addJointConstraintToGoal(int goal_idx, moveit_msgs::JointConstraint joint_constraint); /** \brief Tiles some \a geometry around a \a pose in \a base_name for the end-effector \a * ee_name. The \a geometry is placed at \a offset from \a pose, and \a n copies are placed evenly diff --git a/robowflex_library/src/builder.cpp b/robowflex_library/src/builder.cpp index 25a23df32..9b4d29ab5 100644 --- a/robowflex_library/src/builder.cpp +++ b/robowflex_library/src/builder.cpp @@ -332,16 +332,17 @@ void MotionRequestBuilder::addGoalRegion(const std::string &ee_name, const std:: request_.goal_constraints.push_back(constraints); } -void MotionRequestBuilder::addJointConstraintToGoal(int idx, const std::string &joint_name, float position, - const Eigen::Vector2d &tolerances) +void MotionRequestBuilder::addJointConstraintToGoal(int goal_idx, const std::string &joint_name, + float position, const Eigen::Vector2d &tolerances) { - addJointConstraintToGoal(idx, TF::getJointConstraint(joint_name, position, tolerances)); + addJointConstraintToGoal(goal_idx, TF::getJointConstraint(joint_name, position, tolerances)); } -void MotionRequestBuilder::addJointConstraintToGoal(int idx, moveit_msgs::JointConstraint joint_constraint) +void MotionRequestBuilder::addJointConstraintToGoal(int goal_idx, + moveit_msgs::JointConstraint joint_constraint) { - auto joint_constraints = request_.goal_constraints[idx].joint_constraints; - /* request_.goal_constraints.at(idx).joint_constraints.push_back(joint_constraint); */ + /* auto joint_constraints = request_.goal_constraints[goal_idx].joint_constraints; */ + request_.goal_constraints.at(goal_idx).joint_constraints.push_back(joint_constraint); } void MotionRequestBuilder::addGoalRotaryTile(const std::string &ee_name, const std::string &base_name, From 75ab34ea4aa6a9a483e718b5a1742fbf4e33dc1a Mon Sep 17 00:00:00 2001 From: Thomas Herring Date: Mon, 13 Sep 2021 15:28:59 -0500 Subject: [PATCH 06/10] Added comments --- .../include/robowflex_library/builder.h | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/robowflex_library/include/robowflex_library/builder.h b/robowflex_library/include/robowflex_library/builder.h index 61881d918..e7adf8ac2 100644 --- a/robowflex_library/include/robowflex_library/builder.h +++ b/robowflex_library/include/robowflex_library/builder.h @@ -178,9 +178,23 @@ namespace robowflex const GeometryConstPtr &geometry, const Eigen::Quaterniond &orientation, const Eigen::Vector3d &tolerances); + /** \brief Adds a joint constraint to a specific goal region. + * Creates a joint constraint for \a joint_name at \a position between the bounds specified by \a + * tolerances and adds it to the goal region indexed by \a goal_idx from \a orientation and XYZ Euler + * angle tolerances \a tolerances. + * \param[in] goal_idx The index of the goal region to add this joint constraint to. + * \param[in] joint_name The string name of the joint to constrain. + * \param[in] position The joint value to constrain to. + * \param[in] tolerances A 2D vector of the upper bound and lower bound from \a position. + */ void addJointConstraintToGoal(int goal_idx, const std::string &joint_name, float position, const Eigen::Vector2d &tolerances); + /** \brief Adds a MoveIt! joint constraint to a specific goal region. + * Adds a MoveIt! joint constraint to the goal region indexed by \a goal_idx. + * \param[in] goal_idx The index of the goal region to add this joint constraint to. + * \param[in] joint_constraint The MoveIt! joint constraint to add. + */ void addJointConstraintToGoal(int goal_idx, moveit_msgs::JointConstraint joint_constraint); /** \brief Tiles some \a geometry around a \a pose in \a base_name for the end-effector \a From 19ba2e693524708cbb93afb9984a02e884a43a87 Mon Sep 17 00:00:00 2001 From: Thomas Herring Date: Mon, 13 Sep 2021 15:34:29 -0500 Subject: [PATCH 07/10] Fixed formatting --- .../include/robowflex_library/builder.h | 60 ++++++++++--------- 1 file changed, 33 insertions(+), 27 deletions(-) diff --git a/robowflex_library/include/robowflex_library/builder.h b/robowflex_library/include/robowflex_library/builder.h index e7adf8ac2..14ac5725c 100644 --- a/robowflex_library/include/robowflex_library/builder.h +++ b/robowflex_library/include/robowflex_library/builder.h @@ -180,29 +180,29 @@ namespace robowflex /** \brief Adds a joint constraint to a specific goal region. * Creates a joint constraint for \a joint_name at \a position between the bounds specified by \a - * tolerances and adds it to the goal region indexed by \a goal_idx from \a orientation and XYZ Euler - * angle tolerances \a tolerances. - * \param[in] goal_idx The index of the goal region to add this joint constraint to. - * \param[in] joint_name The string name of the joint to constrain. - * \param[in] position The joint value to constrain to. - * \param[in] tolerances A 2D vector of the upper bound and lower bound from \a position. + * tolerances and adds it to the goal region indexed by \a goal_idx from \a orientation and XYZ Euler + * angle tolerances \a tolerances. + * \param[in] goal_idx The index of the goal region to add this joint constraint to. + * \param[in] joint_name The string name of the joint to constrain. + * \param[in] position The joint value to constrain to. + * \param[in] tolerances A 2D vector of the upper bound and lower bound from \a position. */ void addJointConstraintToGoal(int goal_idx, const std::string &joint_name, float position, const Eigen::Vector2d &tolerances); /** \brief Adds a MoveIt! joint constraint to a specific goal region. - * Adds a MoveIt! joint constraint to the goal region indexed by \a goal_idx. - * \param[in] goal_idx The index of the goal region to add this joint constraint to. - * \param[in] joint_constraint The MoveIt! joint constraint to add. + * Adds a MoveIt! joint constraint to the goal region indexed by \a goal_idx. + * \param[in] goal_idx The index of the goal region to add this joint constraint to. + * \param[in] joint_constraint The MoveIt! joint constraint to add. */ void addJointConstraintToGoal(int goal_idx, moveit_msgs::JointConstraint joint_constraint); /** \brief Tiles some \a geometry around a \a pose in \a base_name for the end-effector \a - * ee_name. The \a geometry is placed at \a offset from \a pose, and \a n copies are placed evenly - * rotated about \a axis. The desired \a orientation is also rotated about the axis and set for - * each copy. \param[in] ee_name The name of the end-effector link. \param[in] base_name The name - * of the frame of reference of \a pose and \a orientation. \param[in] pose The pose of the frame - * to be rotated about. \param[in] geometry The geometry describing the position constraint. + * ee_name. The \a geometry is placed at \a offset from \a pose, and \a n copies are placed evenly + * rotated about \a axis. The desired \a orientation is also rotated about the axis and set for + * each copy. \param[in] ee_name The name of the end-effector link. \param[in] base_name The name + * of the frame of reference of \a pose and \a orientation. \param[in] pose The pose of the frame + * to be rotated about. \param[in] geometry The geometry describing the position constraint. * \param[in] orientation The desired orientation. * \param[in] tolerances XYZ Euler angle tolerances about orientation. * \param[in] offset Offset of the goal \a geometry from \a pose. @@ -216,11 +216,13 @@ namespace robowflex /** \brief Adds a set of regions to grasp a cylinder from the side. This function assumes the * X-axis of the end-effector frame \a ee_name points "forward" for grasping. \param[in] ee_name - * The name of the end-effector link. \param[in] base_name The name of the frame of reference of - * \a pose. \param[in] pose The pose of the frame to be rotated about. \param[in] cylinder The - * cylinder to grasp. \param[in] distance The distance from the cylinder to place the regions. - * \param[in] depth The depth of boxes to create. - * \param[in] n The number of regions to create. + * The name of the end-effector link. + * \param[in] base_name The name of the frame of reference of \a pose. + * \param[in] pose The pose of the frame to be rotated about. + * \param[in] cylinder The cylinder to grasp. + * \param[in] distance The distance from the cylinder to place the regions. + * \param[in] depth The depth of boxes to create. + * \param[in] n The number of regions to create. */ void addCylinderSideGrasp(const std::string &ee_name, const std::string &base_name, const RobotPose &pose, const GeometryConstPtr &cylinder, double distance, @@ -262,9 +264,10 @@ namespace robowflex /** \brief Set a goal region for an end-effector \a ee_name. * Sets the position constraint from \a geometry at a pose \a pose, and the orientation - * constraint from \a orientation and XYZ Euler angle tolerances \a tolerances. \param[in] ee_name - * The name of the end-effector link. \param[in] base_name The name of the frame of reference of - * \a pose and \a orientation. \param[in] pose The pose of \a geometry in \a base_frame. + * constraint from \a orientation and XYZ Euler angle tolerances \a tolerances. + * \param[in] ee_name The name of the end-effector link. + * \param[in] base_name The name of the frame of reference of \a pose and \a orientation. + * \param[in] pose The pose of \a geometry in \a base_frame. * \param[in] geometry The geometry describing the position constraint. * \param[in] orientation The desired orientation. * \param[in] tolerances XYZ Euler angle tolerances about orientation. @@ -284,9 +287,10 @@ namespace robowflex /** \brief Set a pose constraint on the path. * Sets the position constraint from \a geometry at a pose \a pose, and the orientation - * constraint from \a orientation and XYZ Euler angle tolerances \a tolerances. \param[in] ee_name - * The name of the end-effector link. \param[in] base_name The name of the frame of reference of - * \a pose and \a orientation. \param[in] pose The pose of \a geometry in \a base_frame. + * constraint from \a orientation and XYZ Euler angle tolerances \a tolerances. + * \param[in] ee_name The name of the end-effector link. + * \param[in] base_name The name of the frame of reference of \a pose and \a orientation. + * \param[in] pose The pose of \a geometry in \a base_frame. * \param[in] geometry The geometry describing the position constraint. * \param[in] orientation The desired orientation. * \param[in] tolerances XYZ Euler angle tolerances about orientation. @@ -307,8 +311,10 @@ namespace robowflex /** \brief Set an orientation constraint on the path. * Sets the orientation constraint from \a orientation and XYZ Euler angle tolerances \a - * tolerances. \param[in] ee_name The name of the end-effector link. \param[in] base_name The name - * of the frame of reference of \a orientation. \param[in] orientation The desired orientation. + * tolerances. + * \param[in] ee_name The name of the end-effector link. + * \param[in] base_name The name of the frame of reference of \a orientation. + * \param[in] orientation The desired orientation. * \param[in] tolerances XYZ Euler angle tolerances about orientation. */ void addPathOrientationConstraint(const std::string &ee_name, const std::string &base_name, From ce6495af6d9d5b58bfdaf9d73494cf21feb6a76c Mon Sep 17 00:00:00 2001 From: Thomas Herring Date: Mon, 13 Sep 2021 15:38:08 -0500 Subject: [PATCH 08/10] Fixed last formatting issues --- robowflex_library/include/robowflex_library/builder.h | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/robowflex_library/include/robowflex_library/builder.h b/robowflex_library/include/robowflex_library/builder.h index 14ac5725c..b12b09ef8 100644 --- a/robowflex_library/include/robowflex_library/builder.h +++ b/robowflex_library/include/robowflex_library/builder.h @@ -310,8 +310,7 @@ namespace robowflex const RobotPose &pose, const GeometryConstPtr &geometry); /** \brief Set an orientation constraint on the path. - * Sets the orientation constraint from \a orientation and XYZ Euler angle tolerances \a - * tolerances. + * Sets the orientation constraint from \a orientation and XYZ Euler angle tolerances \a tolerances. * \param[in] ee_name The name of the end-effector link. * \param[in] base_name The name of the frame of reference of \a orientation. * \param[in] orientation The desired orientation. @@ -329,10 +328,10 @@ namespace robowflex /** \brief Set the planning configuration to use for the motion planning request. * Attempts to match \a requested_config with the planner configuration offered by \a planner_ * that is the shortest configuration that contains \a requested_config as a substring. For - * example, specifying `RRTConnect` will match `RRTConnectkConfigDefault`, and specifying `RRT` - * will match `RRTkConfigDefault` and not `RRTConnectkConfigDefault`. \param[in] requested_config - * The planner config to find and use. \return True if the \a requested_config is found, false - * otherwise. + * example, specifying `RRTConnect` will match `RRTConnectkConfigDefault`, and specifying `RRT` + * will match `RRTkConfigDefault` and not `RRTConnectkConfigDefault`. + * \param[in] requested_config The planner config to find and use. + * \return True if the \a requested_config is found, false otherwise. */ bool setConfig(const std::string &requested_config); From 0050d306587ba088348d9f1e3bfdb3d148103f22 Mon Sep 17 00:00:00 2001 From: Thomas Herring Date: Mon, 13 Sep 2021 15:39:10 -0500 Subject: [PATCH 09/10] More formatting issues --- robowflex_library/include/robowflex_library/builder.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/robowflex_library/include/robowflex_library/builder.h b/robowflex_library/include/robowflex_library/builder.h index b12b09ef8..3ba249747 100644 --- a/robowflex_library/include/robowflex_library/builder.h +++ b/robowflex_library/include/robowflex_library/builder.h @@ -215,8 +215,8 @@ namespace robowflex const RobotPose &offset, const Eigen::Vector3d &axis, unsigned int n); /** \brief Adds a set of regions to grasp a cylinder from the side. This function assumes the - * X-axis of the end-effector frame \a ee_name points "forward" for grasping. \param[in] ee_name - * The name of the end-effector link. + * X-axis of the end-effector frame \a ee_name points "forward" for grasping. + * \param[in] ee_name The name of the end-effector link. * \param[in] base_name The name of the frame of reference of \a pose. * \param[in] pose The pose of the frame to be rotated about. * \param[in] cylinder The cylinder to grasp. From 2e2a95a38e81aef04ce31ba17227583c386b4060 Mon Sep 17 00:00:00 2001 From: Thomas Herring Date: Mon, 13 Sep 2021 16:23:35 -0500 Subject: [PATCH 10/10] Removed old code --- robowflex_library/src/builder.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/robowflex_library/src/builder.cpp b/robowflex_library/src/builder.cpp index 9b4d29ab5..0255346b7 100644 --- a/robowflex_library/src/builder.cpp +++ b/robowflex_library/src/builder.cpp @@ -341,7 +341,6 @@ void MotionRequestBuilder::addJointConstraintToGoal(int goal_idx, const std::str void MotionRequestBuilder::addJointConstraintToGoal(int goal_idx, moveit_msgs::JointConstraint joint_constraint) { - /* auto joint_constraints = request_.goal_constraints[goal_idx].joint_constraints; */ request_.goal_constraints.at(goal_idx).joint_constraints.push_back(joint_constraint); }