From 7328c2ecf60c323bb5a3c6c968cafa269bb74fcd Mon Sep 17 00:00:00 2001 From: omid Date: Thu, 28 Nov 2019 16:32:10 -0700 Subject: [PATCH 1/2] some changes related to those from trajopt in moveit_planners --- doc/trajopt_planner/src/trajopt_example.cpp | 71 +++++++++++++++------ 1 file changed, 50 insertions(+), 21 deletions(-) diff --git a/doc/trajopt_planner/src/trajopt_example.cpp b/doc/trajopt_planner/src/trajopt_example.cpp index f026a7646..9b63d755d 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"; @@ -148,29 +182,24 @@ int main(int argc, char** argv) int const N_STEPS = 20; // number of steps int const N_DOF = 7; // number of degrees of freedom - // Calculate the increment value for each joint - std::vector dt_vector; - for (int joint_index = 0; joint_index < N_DOF; ++joint_index) - { - double dt = (goal_joint_values[joint_index] - current_joint_values[joint_index]) / N_STEPS; - dt_vector.push_back(dt); - } - + // We need one reference trajectory with one joint_trajectory req.reference_trajectories.resize(1); req.reference_trajectories[0].joint_trajectory.resize(1); // trajectory includes both the start and end points (N_STEPS + 1) - req.reference_trajectories[0].joint_trajectory[0].points.resize(N_STEPS + 1); - req.reference_trajectories[0].joint_trajectory[0].joint_names = joint_names; + req.reference_trajectories[0].joint_trajectory[0].points.resize(N_STEPS + 1); + req.reference_trajectories[0].joint_trajectory[0].joint_names = joint_names; req.reference_trajectories[0].joint_trajectory[0].points[0].positions = current_joint_values; - // Use the increment values (dt_vector) to caluclate the joint values at each step + + std::vector joint_values = current_joint_values; + // Increment joint values at each step for (std::size_t step_index = 1; step_index <= N_STEPS; ++step_index) { - std::vector joint_values; + double increment; + step_index <=10 ? (increment = 0.05) : (increment = 0.044); for (int dof_index = 0; dof_index < N_DOF; ++dof_index) - { - double joint_value = current_joint_values[dof_index] + step_index * dt_vector[dof_index]; - joint_values.push_back(joint_value); - } + { + joint_values[dof_index] = joint_values[dof_index] + increment; + } req.reference_trajectories[0].joint_trajectory[0].joint_names = joint_names; req.reference_trajectories[0].joint_trajectory[0].points[step_index].positions = joint_values; } From c7659533a9389de34035d95a6415c13bc5c079c1 Mon Sep 17 00:00:00 2001 From: Omid Date: Wed, 10 Mar 2021 19:39:05 -0700 Subject: [PATCH 2/2] clang format --- doc/trajopt_planner/src/trajopt_example.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/doc/trajopt_planner/src/trajopt_example.cpp b/doc/trajopt_planner/src/trajopt_example.cpp index 9b63d755d..1b071cf63 100644 --- a/doc/trajopt_planner/src/trajopt_example.cpp +++ b/doc/trajopt_planner/src/trajopt_example.cpp @@ -186,8 +186,8 @@ int main(int argc, char** argv) req.reference_trajectories.resize(1); req.reference_trajectories[0].joint_trajectory.resize(1); // trajectory includes both the start and end points (N_STEPS + 1) - req.reference_trajectories[0].joint_trajectory[0].points.resize(N_STEPS + 1); - req.reference_trajectories[0].joint_trajectory[0].joint_names = joint_names; + req.reference_trajectories[0].joint_trajectory[0].points.resize(N_STEPS + 1); + req.reference_trajectories[0].joint_trajectory[0].joint_names = joint_names; req.reference_trajectories[0].joint_trajectory[0].points[0].positions = current_joint_values; std::vector joint_values = current_joint_values; @@ -195,11 +195,11 @@ int main(int argc, char** argv) for (std::size_t step_index = 1; step_index <= N_STEPS; ++step_index) { double increment; - step_index <=10 ? (increment = 0.05) : (increment = 0.044); + step_index <= 10 ? (increment = 0.05) : (increment = 0.044); for (int dof_index = 0; dof_index < N_DOF; ++dof_index) - { - joint_values[dof_index] = joint_values[dof_index] + increment; - } + { + joint_values[dof_index] = joint_values[dof_index] + increment; + } req.reference_trajectories[0].joint_trajectory[0].joint_names = joint_names; req.reference_trajectories[0].joint_trajectory[0].points[step_index].positions = joint_values; }