Fixed licenses. Closes #428

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<std::string> joint_names,
-		   robot_state::RobotStatePtr& robot_state,
-		   const robot_state::JointModelGroup* joint_model_group,
-		   const std::vector<double>& start_joint_vals,
-		   const std::vector<double>& goal_joint_vals,
-		   trajectory_msgs::JointTrajectory& joint_trajectory);
+  void interpolate(const std::vector<std::string> joint_names, robot_state::RobotStatePtr& robot_state,
+                   const robot_state::JointModelGroup* joint_model_group, const std::vector<double>& start_joint_vals,
+                   const std::vector<double>& goal_joint_vals, trajectory_msgs::JointTrajectory& joint_trajectory);
 };
 }

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 <moveit/planning_interface/planning_interface.h>
-
 #include "lerp_interface/lerp_interface.h"
 
 namespace lerp_interface

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<std::string>& joint_names = joint_model_group->getActiveJointModelNames();
   const std::vector<std::string>& 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<double> 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<moveit_msgs::JointConstraint> 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<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true);
   moveit_msgs::DisplayTrajectory display_trajectory;

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<std::string> joint_names,
-				robot_state::RobotStatePtr& rob_state,
-				const robot_state::JointModelGroup* joint_model_group,
-				const std::vector<double>& start_joint_vals,
-				const std::vector<double>& goal_joint_vals,
-				trajectory_msgs::JointTrajectory& joint_trajectory)
+void LERPInterface::interpolate(const std::vector<std::string> joint_names, robot_state::RobotStatePtr& rob_state,
+                                const robot_state::JointModelGroup* joint_model_group,
+                                const std::vector<double>& start_joint_vals, const std::vector<double>& goal_joint_vals,
+                                trajectory_msgs::JointTrajectory& joint_trajectory)
 {
   joint_trajectory.points.resize(num_steps_ + 1);
-  
+
   std::vector<double> dt_vector;
   for (int joint_index = 0; joint_index < dof_; ++joint_index)
   {
@@ -134,5 +131,5 @@ void LERPInterface::interpolate(const std::vector<std::string> joint_names,
     joint_trajectory.points[step].positions = joint_values;
   }
 }
-  
+
 }  // namespace lerp_interface

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

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 <ros/ros.h>
 #include <ros/console.h>
 #include <moveit/planning_interface/planning_interface.h>
@@ -21,11 +60,6 @@
 #include <tf2_eigen/tf2_eigen.h>
 #include <tf2_ros/transform_listener.h>
 
-/* 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";