diff --git a/doc/creating_moveit_plugins/lerp_motion_planner/include/lerp_interface/lerp_interface.h b/doc/creating_moveit_plugins/lerp_motion_planner/include/lerp_interface/lerp_interface.h index aecf7eacd..a0d544864 100644 --- a/doc/creating_moveit_plugins/lerp_motion_planner/include/lerp_interface/lerp_interface.h +++ b/doc/creating_moveit_plugins/lerp_motion_planner/include/lerp_interface/lerp_interface.h @@ -59,11 +59,8 @@ class LERPInterface int dof_; private: - void interpolate(const std::vector joint_names, - robot_state::RobotStatePtr& robot_state, - const robot_state::JointModelGroup* joint_model_group, - const std::vector& start_joint_vals, - const std::vector& goal_joint_vals, - trajectory_msgs::JointTrajectory& joint_trajectory); + void interpolate(const std::vector joint_names, robot_state::RobotStatePtr& robot_state, + const robot_state::JointModelGroup* joint_model_group, const std::vector& start_joint_vals, + const std::vector& goal_joint_vals, trajectory_msgs::JointTrajectory& joint_trajectory); }; } diff --git a/doc/creating_moveit_plugins/lerp_motion_planner/include/lerp_interface/lerp_planning_context.h b/doc/creating_moveit_plugins/lerp_motion_planner/include/lerp_interface/lerp_planning_context.h index 676519722..d0f11ffd1 100644 --- a/doc/creating_moveit_plugins/lerp_motion_planner/include/lerp_interface/lerp_planning_context.h +++ b/doc/creating_moveit_plugins/lerp_motion_planner/include/lerp_interface/lerp_planning_context.h @@ -1,8 +1,43 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, PickNik, LLC. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Omid Heidari */ + #ifndef LERP_PLANNING_CONTEXT_H #define LERP_PLANNING_CONTEXT_H #include - #include "lerp_interface/lerp_interface.h" namespace lerp_interface diff --git a/doc/creating_moveit_plugins/lerp_motion_planner/src/lerp_example.cpp b/doc/creating_moveit_plugins/lerp_motion_planner/src/lerp_example.cpp index 9dc885215..c575bc730 100644 --- a/doc/creating_moveit_plugins/lerp_motion_planner/src/lerp_example.cpp +++ b/doc/creating_moveit_plugins/lerp_motion_planner/src/lerp_example.cpp @@ -76,28 +76,28 @@ int main(int argc, char** argv) psm->startSceneMonitor(); psm->startWorldGeometryMonitor(); psm->startStateMonitor(); - + robot_model::RobotModelPtr robot_model = robot_model_loader->getModel(); - // Create a RobotState and to keep track of the current robot pose and planning group + // Create a RobotState and to keep track of the current robot pose and planning group robot_state::RobotStatePtr robot_state( new robot_state::RobotState(planning_scene_monitor::LockedPlanningSceneRO(psm)->getCurrentState())); robot_state->setToDefaultValues(); robot_state->update(); - // Create JointModelGroup + // Create JointModelGroup const robot_state::JointModelGroup* joint_model_group = robot_state->getJointModelGroup(PLANNING_GROUP); const std::vector& joint_names = joint_model_group->getActiveJointModelNames(); const std::vector& link_model_names = joint_model_group->getLinkModelNames(); ROS_INFO_NAMED(NODE_NAME, "end effector name %s\n", link_model_names.back().c_str()); - // Set the planner + // Set the planner std::string planner_plugin_name = "lerp_interface/LERPPlanner"; node_handle.setParam("planning_plugin", planner_plugin_name); - // Create pipeline + // Create pipeline planning_pipeline::PlanningPipelinePtr planning_pipeline( - new planning_pipeline::PlanningPipeline(robot_model, node_handle, "planning_plugin", "request_adapters")); + new planning_pipeline::PlanningPipeline(robot_model, node_handle, "planning_plugin", "request_adapters")); // ================================ Set the start and goal joint state planning_interface::MotionPlanRequest req; @@ -113,20 +113,22 @@ int main(int argc, char** argv) std::vector goal_joint_values = { 0.8, 0.7, 1, 1.3, 1.9, 2.2, 3 }; robot_state->setJointGroupPositions(joint_model_group, goal_joint_values); robot_state->update(); - moveit_msgs::Constraints joint_goal = kinematic_constraints::constructGoalConstraints(*robot_state, joint_model_group); + moveit_msgs::Constraints joint_goal = + kinematic_constraints::constructGoalConstraints(*robot_state, joint_model_group); req.goal_constraints.clear(); req.goal_constraints.push_back(joint_goal); req.goal_constraints[0].name = "goal_pos"; - // Set joint tolerance + // Set joint tolerance std::vector goal_joint_constraint = req.goal_constraints[0].joint_constraints; for (std::size_t x = 0; x < goal_joint_constraint.size(); ++x) { - ROS_INFO_STREAM_NAMED(NODE_NAME ," ======================================= joint position at goal: " << goal_joint_constraint[x].position); + ROS_INFO_STREAM_NAMED(NODE_NAME, " ======================================= joint position at goal: " + << goal_joint_constraint[x].position); req.goal_constraints[0].joint_constraints[x].tolerance_above = 0.001; req.goal_constraints[0].joint_constraints[x].tolerance_below = 0.001; } - + // ================================ Visualization tools namespace rvt = rviz_visual_tools; moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0", rviz_visual_tools::RVIZ_MARKER_TOPIC, psm); @@ -147,7 +149,6 @@ int main(int argc, char** argv) /* We can also use visual_tools to wait for user input */ visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo"); - // ================================ planning context { planning_scene_monitor::LockedPlanningSceneRO lscene(psm); @@ -162,12 +163,11 @@ int main(int argc, char** argv) } visual_tools.prompt("Press 'next' to visualzie the result"); - // ================================ Visualize the trajectory // visual_tools.publishRobotState(planning_scene->getCurrentStateNonConst(), rviz_visual_tools::GREEN); // visual_tools.trigger(); - + ros::Publisher display_publisher = node_handle.advertise("/move_group/display_planned_path", 1, true); moveit_msgs::DisplayTrajectory display_trajectory; diff --git a/doc/creating_moveit_plugins/lerp_motion_planner/src/lerp_interface.cpp b/doc/creating_moveit_plugins/lerp_motion_planner/src/lerp_interface.cpp index 0b44ba160..6237bbc6d 100644 --- a/doc/creating_moveit_plugins/lerp_motion_planner/src/lerp_interface.cpp +++ b/doc/creating_moveit_plugins/lerp_motion_planner/src/lerp_interface.cpp @@ -62,7 +62,6 @@ bool LERPInterface::solve(const planning_scene::PlanningSceneConstPtr& planning_ // Load the planner-specific parameters nh_.getParam("num_steps", num_steps_); - ros::WallTime start_time = ros::WallTime::now(); robot_model::RobotModelConstPtr robot_model = planning_scene->getRobotModel(); robot_state::RobotStatePtr start_state(new robot_state::RobotState(robot_model)); @@ -103,15 +102,13 @@ bool LERPInterface::solve(const planning_scene::PlanningSceneConstPtr& planning_ return true; } -void LERPInterface::interpolate(const std::vector joint_names, - robot_state::RobotStatePtr& rob_state, - const robot_state::JointModelGroup* joint_model_group, - const std::vector& start_joint_vals, - const std::vector& goal_joint_vals, - trajectory_msgs::JointTrajectory& joint_trajectory) +void LERPInterface::interpolate(const std::vector joint_names, robot_state::RobotStatePtr& rob_state, + const robot_state::JointModelGroup* joint_model_group, + const std::vector& start_joint_vals, const std::vector& goal_joint_vals, + trajectory_msgs::JointTrajectory& joint_trajectory) { joint_trajectory.points.resize(num_steps_ + 1); - + std::vector dt_vector; for (int joint_index = 0; joint_index < dof_; ++joint_index) { @@ -134,5 +131,5 @@ void LERPInterface::interpolate(const std::vector joint_names, joint_trajectory.points[step].positions = joint_values; } } - + } // namespace lerp_interface diff --git a/doc/creating_moveit_plugins/lerp_motion_planner/src/lerp_planning_context.cpp b/doc/creating_moveit_plugins/lerp_motion_planner/src/lerp_planning_context.cpp index 509ff2665..a0e5d8315 100644 --- a/doc/creating_moveit_plugins/lerp_motion_planner/src/lerp_planning_context.cpp +++ b/doc/creating_moveit_plugins/lerp_motion_planner/src/lerp_planning_context.cpp @@ -97,7 +97,7 @@ bool LERPPlanningContext::terminate() { return true; } - + void LERPPlanningContext::clear() { // This planner has no state, so has nothing to clear diff --git a/doc/trajopt_planner/src/trajopt_example.cpp b/doc/trajopt_planner/src/trajopt_example.cpp index e36dc0a3e..77ea7089d 100644 --- a/doc/trajopt_planner/src/trajopt_example.cpp +++ b/doc/trajopt_planner/src/trajopt_example.cpp @@ -1,3 +1,42 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2019, PickNik, LLC. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Omid Heidari + Desc: This file is a test for using trajopt in MoveIt. The goal is to make different types of constraints in + MotionPlanRequest and visualize the result calculated using the trajopt planner. +*/ + #include #include #include @@ -21,11 +60,6 @@ #include #include -/* Author: Omid Heidari - Desc: This file is a test for using trajopt in MoveIt. The goal is to make different types of constraints in - MotionPlanRequest and visualize the result calculated using the trajopt planner. -*/ - int main(int argc, char** argv) { const std::string NODE_NAME = "trajopt_example";