diff --git a/.github/CODEOWNERS.disabled b/.github/CODEOWNERS.disabled index 059134e4bd..960b877e64 100644 --- a/.github/CODEOWNERS.disabled +++ b/.github/CODEOWNERS.disabled @@ -93,6 +93,5 @@ /moveit_planners/chomp/chomp_interface/ @raghavendersahdev @knorth55 @bmagyar /moveit_planners/chomp/chomp_optimizer_adapter/ @raghavendersahdev @knorth55 @bmagyar /moveit_planners/chomp/chomp_motion_planner/ @raghavendersahdev @knorth55 @bmagyar -/moveit_planners/trajopt @ommmid @mamoll /moveit_planners/pilz_industrial_motion_planner @jschleicher @ct2034 /moveit_planners/pilz_industrial_motion_planner_testutils/ @jschleicher @ct2034 diff --git a/moveit_planners/trajopt/CATKIN_IGNORE b/moveit_planners/trajopt/CATKIN_IGNORE deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/moveit_planners/trajopt/CMakeLists.txt b/moveit_planners/trajopt/CMakeLists.txt deleted file mode 100644 index 53f006669e..0000000000 --- a/moveit_planners/trajopt/CMakeLists.txt +++ /dev/null @@ -1,108 +0,0 @@ -cmake_minimum_required(VERSION 3.22) -project(moveit_planners_trajopt LANGUAGES CXX) - -find_package(catkin REQUIRED - COMPONENTS - moveit_core - moveit_visual_tools - moveit_ros_planning - moveit_ros_planning_interface - pluginlib - roscpp - rosparam_shortcuts -) -find_package(trajopt REQUIRED) - -catkin_package( - INCLUDE_DIRS include - LIBRARIES - moveit_planners_trajopt - INCLUDE_DIRS - CATKIN_DEPENDS - roscpp - pluginlib - moveit_core - moveit_visual_tools - moveit_ros_planning_interface - rosparam_shortcuts -) - -# The following include_directory should have include folder of the new planner. -include_directories( - include - ${catkin_INCLUDE_DIRS} - SYSTEM - ${Boost_INCLUDE_DIRS} - ${catkin_INCLUDE_DIRS} -) - -add_library( - moveit_planners_trajopt - src/trajopt_interface.cpp - src/problem_description.cpp - src/kinematic_terms.cpp - src/trajopt_planning_context.cpp -) - -target_link_libraries( - moveit_planners_trajopt - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} - trajopt::trajopt -) - -set_target_properties( - moveit_planners_trajopt - PROPERTIES - VERSION - "${moveit_planners_trajopt_VERSION}" -) - -# TrajOpt planning plugin -add_library(moveit_trajopt_planner_plugin src/trajopt_planner_manager.cpp) -set_target_properties(moveit_trajopt_planner_plugin PROPERTIES VERSION "${moveit_planners_trajopt_VERSION}") -target_link_libraries(moveit_trajopt_planner_plugin moveit_planners_trajopt ${catkin_LIBRARIES}) - -############# -## Install ## -############# - -# Mark executables and/or libraries for installation -install( - TARGETS - moveit_trajopt_planner_plugin - ARCHIVE DESTINATION - ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION - ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION - ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -# Mark cpp header files for installation -install( - DIRECTORY - include/trajopt_interface/ - DESTINATION - ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) - -install(FILES trajopt_interface_plugin_description.xml - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) - -############# -## Testing ## -############# - -if(CATKIN_ENABLE_TESTING) - find_package(rostest REQUIRED) - - add_rostest_gtest(trajectory_test test/trajectory_test.test test/trajectory_test.cpp) - target_link_libraries( - trajectory_test - ${LIBRARY_NAME} - ${catkin_LIBRARIES} - moveit_planners_trajopt - ) - -endif() diff --git a/moveit_planners/trajopt/COLCON_IGNORE b/moveit_planners/trajopt/COLCON_IGNORE deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/moveit_planners/trajopt/README.md b/moveit_planners/trajopt/README.md deleted file mode 100644 index 84e0ce1d44..0000000000 --- a/moveit_planners/trajopt/README.md +++ /dev/null @@ -1 +0,0 @@ -As of August 2019, this is a work in progress towards adding trajopt motion planning algorithm to MoveIt as a planner plugin. diff --git a/moveit_planners/trajopt/include/trajopt_interface/kinematic_terms.h b/moveit_planners/trajopt/include/trajopt_interface/kinematic_terms.h deleted file mode 100644 index 96274cf9c7..0000000000 --- a/moveit_planners/trajopt/include/trajopt_interface/kinematic_terms.h +++ /dev/null @@ -1,93 +0,0 @@ -#pragma once - -#include - -#include - -namespace trajopt_interface -{ -/** - * @brief Extracts the vector part of quaternion - */ -inline Eigen::Vector3d quaternionRotationVector(const Eigen::Matrix3d& matrix) -{ - Eigen::Quaterniond quaternion; - quaternion = matrix; - return Eigen::Vector3d(quaternion.x(), quaternion.y(), quaternion.z()); -} - -/** - * @brief Appends b to a of type VectorXd - */ -inline Eigen::VectorXd concatVector(const Eigen::VectorXd& vector_a, const Eigen::VectorXd& vector_b) -{ - Eigen::VectorXd out(vector_a.size() + vector_b.size()); - out.topRows(vector_a.size()) = vector_a; - out.middleRows(vector_a.size(), vector_b.size()) = vector_b; - return out; -} - -/** - * @brief Appends b to a of type T - */ -template -inline std::vector concatVector(const std::vector& vector_a, const std::vector& vector_b) -{ - std::vector out; - out.insert(out.end(), vector_a.begin(), vector_a.end()); - out.insert(out.end(), vector_b.begin(), vector_b.end()); - return out; -} - -/** - * @brief Used to calculate the error for StaticCartPoseTermInfo - * This is converted to a cost or constraint using TrajOptCostFromErrFunc or TrajOptConstraintFromErrFunc - */ -struct CartPoseErrCalculator : public sco::VectorOfVector -{ - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - Eigen::Isometry3d target_pose_inv_; - planning_scene::PlanningSceneConstPtr planning_scene_; - std::string link_; - Eigen::Isometry3d tcp_; - - CartPoseErrCalculator(const Eigen::Isometry3d& pose, const planning_scene::PlanningSceneConstPtr planning_scene, - const std::string& link, Eigen::Isometry3d tcp = Eigen::Isometry3d::Identity()) - : target_pose_inv_(pose.inverse()), planning_scene_(planning_scene), link_(link), tcp_(tcp) - { - } - - Eigen::VectorXd operator()(const Eigen::VectorXd& dof_vals) const override; -}; - -// TODO(omid): The following should be added and adjusted from trajopt -// JointPosEqCost -// JointPosIneqCost -// JointPosEqConstraint -// JointPosIneqConstraint - -struct JointVelErrCalculator : sco::VectorOfVector -{ - /** @brief Velocity target */ - double target_; - /** @brief Upper tolerance */ - double upper_tol_; - /** @brief Lower tolerance */ - double lower_tol_; - JointVelErrCalculator() : target_(0.0), upper_tol_(0.0), lower_tol_(0.0) - { - } - JointVelErrCalculator(double target, double upper_tol, double lower_tol) - : target_(target), upper_tol_(upper_tol), lower_tol_(lower_tol) - { - } - - Eigen::VectorXd operator()(const Eigen::VectorXd& var_vals) const; -}; - -struct JointVelJacobianCalculator : sco::MatrixOfVector -{ - Eigen::MatrixXd operator()(const Eigen::VectorXd& var_vals) const; -}; - -} // namespace trajopt_interface diff --git a/moveit_planners/trajopt/include/trajopt_interface/problem_description.h b/moveit_planners/trajopt/include/trajopt_interface/problem_description.h deleted file mode 100644 index c8a8598856..0000000000 --- a/moveit_planners/trajopt/include/trajopt_interface/problem_description.h +++ /dev/null @@ -1,369 +0,0 @@ -#pragma once -#include -#include -#include - -#include -#include - -#include - -namespace trajopt_interface -{ -/** -@brief Used to apply cost/constraint to joint-space velocity - -Term is applied to every step between first_step and last_step. It applies two limits, upper_limits/lower_limits, -to the joint velocity subject to the following cases. - -* term_type = TT_COST -** upper_limit = lower_limit = 0 - Cost is applied with a SQUARED error scaled for each joint by coeffs -** upper_limit != lower_limit - 2 hinge costs are applied scaled for each joint by coeffs. If velocity < upper_limit and -velocity > lower_limit, no penalty. - -* term_type = TT_CNT -** upper_limit = lower_limit = 0 - Equality constraint is applied -** upper_limit != lower_limit - 2 Inequality constraints are applied. These are both satisfied when velocity < -upper_limit and velocity > lower_limit - -Note: coeffs, upper_limits, and lower_limits are optional. If a term is not given it will default to 0 for all joints. -If one value is given, this will be broadcast to all joints. - -Note: Velocity is calculated numerically using forward finite difference - -\f{align*}{ - cost = \sum_{t=0}^{T-2} \sum_j c_j (x_{t+1,j} - x_{t,j})^2 -\f} -where j indexes over DOF, and \f$c_j\f$ are the coeffs. -*/ - -struct TermInfo; -MOVEIT_CLASS_FORWARD(TermInfo); // Defines TermInfoPtr, ConstPtr, WeakPtr... etc - -class TrajOptProblem; -MOVEIT_CLASS_FORWARD(TrajOptProblem); // Defines TrajOptProblemPtr, ConstPtr, WeakPtr... etc - -struct JointPoseTermInfo; -MOVEIT_CLASS_FORWARD(JointPoseTermInfo); // Defines JointPoseTermInfoPtr, ConstPtr, WeakPtr... etc - -struct CartPoseTermInfo; -MOVEIT_CLASS_FORWARD(CartPoseTermInfo); // Defines CartPoseTermInfoPtr, ConstPtr, WeakPtr... etc - -struct JointVelTermInfo; -MOVEIT_CLASS_FORWARD(JointVelTermInfo); // Defines JointVelTermInfoPtr, ConstPtr, WeakPtr... etc - -struct ProblemInfo; -TrajOptProblemPtr ConstructProblem(const ProblemInfo&); - -enum TermType -{ - TT_COST = 0x1, // 0000 0001 - TT_CNT = 0x2, // 0000 0010 - TT_USE_TIME = 0x4, // 0000 0100 -}; - -struct BasicInfo -{ - /** @brief If true, first time step is fixed with a joint level constraint - If this is false, the starting point of the trajectory will - not be the current position of the robot. The use case example is: if we are trying to execute a process like - sanding the critical part which is the actual process path not how we get to the start of the process path. So we - plan the - process path first leaving the start free to hopefully get the most optimal and then we plan from the current - location with - start fixed to the start of the process path. It depends on what we want the default behavior to be - */ - bool start_fixed; - /** @brief Number of time steps (rows) in the optimization matrix */ - int n_steps; - sco::IntVec dofs_fixed; // optional - sco::ModelType convex_solver; // which convex solver to use - - /** @brief If true, the last column in the optimization matrix will be 1/dt */ - bool use_time = false; - /** @brief The upper limit of 1/dt values allowed in the optimization*/ - double dt_upper_lim = 1.0; - /** @brief The lower limit of 1/dt values allowed in the optimization*/ - double dt_lower_lim = 1.0; -}; - -/** -Initialization info read from json -*/ -struct InitInfo -{ - /** @brief Methods of initializing the optimization matrix. This defines how robot moves from the current - state to the start state - - STATIONARY: Initializes all joint values to the initial value (the current value in the env) - JOINT_INTERPOLATED: Linearly interpolates between initial value and the joint position specified in InitInfo.data - GIVEN_TRAJ: Initializes the matrix to a given trajectory - - In all cases the dt column (if present) is appended the selected method is defined. - */ - enum Type - { - STATIONARY, - JOINT_INTERPOLATED, - GIVEN_TRAJ, - }; - /** @brief Specifies the type of initialization to use */ - Type type; - /** @brief Data used during initialization. Use depends on the initialization selected. This data will be used - to create initialization matrix. We need to give the goal information to this init info - */ - trajopt::TrajArray data; - // Eigen::VectorXd data_vec; - // trajopt::TrajArray data_trajectory; - /** @brief Default value the final column of the optimization is initialized too if time is being used */ - double dt = 1.0; -}; - -/** -When cost or constraint element of JSON doc is read, one of these guys gets -constructed to hold the parameters. -Then it later gets converted to a Cost object by the addObjectiveTerms method -*/ -struct TermInfo -{ - std::string name; - int term_type; - int getSupportedTypes() - { - return supported_term_types_; - } - // virtual void fromJson(ProblemConstructionInfo& pci, const Json::Value& v) = 0; - virtual void addObjectiveTerms(TrajOptProblem& prob) = 0; - - static TermInfoPtr fromName(const std::string& type); - - /** - * Registers a user-defined TermInfo so you can use your own cost - * see function RegisterMakers.cpp - */ - using MakerFunc = TermInfoPtr (*)(void); - static void RegisterMaker(const std::string& type, MakerFunc); - - TermInfo() = default; - TermInfo(const TermInfo&) = default; - TermInfo(TermInfo&&) = default; - TermInfo& operator=(const TermInfo&) = default; - TermInfo& operator=(TermInfo&&) = default; - virtual ~TermInfo() = default; - -protected: - TermInfo(int supported_term_types) : supported_term_types_(supported_term_types) - { - } - -private: - static std::map name_to_maker_; - int supported_term_types_; -}; - -struct ProblemInfo -{ -public: - BasicInfo basic_info; - sco::BasicTrustRegionSQPParameters opt_info; - std::vector cost_infos; - std::vector cnt_infos; - InitInfo init_info; - - planning_scene::PlanningSceneConstPtr planning_scene; - std::string planning_group_name; - - ProblemInfo(planning_scene::PlanningSceneConstPtr ps, const std::string& pg) - : planning_scene(ps), planning_group_name(pg) - { - } -}; - -/** - * Holds all the data for a trajectory optimization problem - * so you can modify it programmatically, e.g. add your own costs - */ -class TrajOptProblem : public sco::OptProb -{ -public: - TrajOptProblem(); - TrajOptProblem(const ProblemInfo& problem_info); - virtual ~TrajOptProblem() = default; - /** @brief Returns the values of the specified joints (start_col to num_col) for the specified timestep i.*/ - sco::VarVector GetVarRow(int i, int start_col, int num_col) - { - return matrix_traj_vars.rblock(i, start_col, num_col); - } - /** @brief Returns the values of all joints for the specified timestep i.*/ - sco::VarVector GetVarRow(int i) - { - return matrix_traj_vars.row(i); - } - /** @brief Returns the value of the specified joint j for the specified timestep i.*/ - sco::Var& GetVar(int i, int j) - { - return matrix_traj_vars.at(i, j); - } - trajopt::VarArray& GetVars() - { - return matrix_traj_vars; - } - /** @brief Returns the number of steps in the problem. This is the number of rows in the optimization matrix.*/ - int GetNumSteps() - { - return matrix_traj_vars.rows(); - } - /** @brief Returns the problem DOF. This is the number of columns in the optization matrix. - * Note that this is not necessarily the same as the kinematic DOF.*/ - int GetNumDOF() - { - return matrix_traj_vars.cols(); - } - /** @brief Returns the kinematic DOF of the active joint model group - */ - int GetActiveGroupNumDOF() - { - return dof_; - } - planning_scene::PlanningSceneConstPtr GetPlanningScene() - { - return planning_scene_; - } - void SetInitTraj(const trajopt::TrajArray& x) - { - matrix_init_traj = x; - } - trajopt::TrajArray GetInitTraj() - { - return matrix_init_traj; - } - // friend TrajOptProbPtr ConstructProblem(const ProblemConstructionInfo&); - /** @brief Returns TrajOptProb.has_time */ - bool GetHasTime() - { - return has_time; - } - /** @brief Sets TrajOptProb.has_time */ - void SetHasTime(bool tmp) - { - has_time = tmp; - } - -private: - /** @brief If true, the last column in the optimization matrix will be 1/dt */ - bool has_time; - /** @brief is the matrix holding the joint values of the trajectory for all timesteps */ - trajopt::VarArray matrix_traj_vars; - planning_scene::PlanningSceneConstPtr planning_scene_; - std::string planning_group_; - /** @brief Kinematic DOF of the active joint model group */ - int dof_; - trajopt::TrajArray matrix_init_traj; -}; - -/** @brief This term is used when the goal frame is fixed in cartesian space - - Set term_type == TT_COST or TT_CNT for cost or constraint. -*/ -struct CartPoseTermInfo : public TermInfo -{ - // EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - /** @brief Timestep at which to apply term */ - int timestep; - /** @brief Cartesian position */ - Eigen::Vector3d xyz; - /** @brief Rotation quaternion */ - Eigen::Vector4d wxyz; - /** @brief coefficients for position and rotation */ - Eigen::Vector3d pos_coeffs, rot_coeffs; - /** @brief Link which should reach desired pose */ - std::string link; - /** @brief Static transform applied to the link */ - Eigen::Isometry3d tcp; - - CartPoseTermInfo(); - - /** @brief Used to add term to pci from json */ - // void fromJson(ProblemConstructionInfo& pci, const Json::Value& v) override; - /** @brief Converts term info into cost/constraint and adds it to trajopt problem */ - void addObjectiveTerms(TrajOptProblem& prob) override; - - static TermInfoPtr create() - { - return std::make_shared(); - } -}; - -/** - \brief Joint space position cost - Position operates on a single point (unlike velocity, etc). This is b/c the primary usecase is joint-space - position waypoints - - \f{align*}{ - \sum_i c_i (x_i - xtarg_i)^2 - \f} - where \f$i\f$ indexes over dof and \f$c_i\f$ are coeffs - */ -struct JointPoseTermInfo : public TermInfo -{ - /** @brief Vector of coefficients that scale the cost. Size should be the DOF of the system. Default: vector of 0's*/ - trajopt::DblVec coeffs; - /** @brief Vector of position targets. This is a required value. Size should be the DOF of the system */ - trajopt::DblVec targets; - /** @brief Vector of position upper limits. Size should be the DOF of the system. Default: vector of 0's*/ - trajopt::DblVec upper_tols; - /** @brief Vector of position lower limits. Size should be the DOF of the system. Default: vector of 0's*/ - trajopt::DblVec lower_tols; - /** @brief First time step to which the term is applied. Default: 0 */ - int first_step = 0; - /** @brief Last time step to which the term is applied. Default: prob.GetNumSteps() - 1*/ - int last_step = -1; - - /** @brief Initialize term with it's supported types */ - JointPoseTermInfo() : TermInfo(TT_COST | TT_CNT | TT_USE_TIME) - { - } - - /** @brief Converts term info into cost/constraint and adds it to trajopt problem */ - void addObjectiveTerms(TrajOptProblem& prob) override; - - static TermInfoPtr create() - { - return std::make_shared(); - } -}; - -struct JointVelTermInfo : public TermInfo -{ - /** @brief Vector of coefficients that scale the cost. Size should be the DOF of the system. Default: vector of 0's*/ - trajopt::DblVec coeffs; - /** @brief Vector of velocity targets. This is a required value. Size should be the DOF of the system */ - trajopt::DblVec targets; - /** @brief Vector of velocity upper limits. Size should be the DOF of the system. Default: vector of 0's*/ - trajopt::DblVec upper_tols; - /** @brief Vector of velocity lower limits. Size should be the DOF of the system. Default: vector of 0's*/ - trajopt::DblVec lower_tols; - /** @brief First time step to which the term is applied. Default: 0*/ - int first_step = 0; - /** @brief Last time step to which the term is applied. Default: prob.GetNumSteps() - 1*/ - int last_step = -1; - - /** @brief Initialize term with it's supported types */ - JointVelTermInfo() : TermInfo(TT_COST | TT_CNT | TT_USE_TIME) - { - } - - /** @brief Converts term info into cost/constraint and adds it to trajopt problem */ - void addObjectiveTerms(TrajOptProblem& prob) override; - - static TermInfoPtr create() - { - return std::make_shared(); - } -}; - -void generateInitialTrajectory(const ProblemInfo& pci, const std::vector& current_joint_values, - trajopt::TrajArray& init_traj); - -} // namespace trajopt_interface diff --git a/moveit_planners/trajopt/include/trajopt_interface/trajopt_interface.h b/moveit_planners/trajopt/include/trajopt_interface/trajopt_interface.h deleted file mode 100644 index 706fffd6f9..0000000000 --- a/moveit_planners/trajopt/include/trajopt_interface/trajopt_interface.h +++ /dev/null @@ -1,77 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2019, PickNik Inc. - * 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 Inc. 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 */ -#pragma once - -#include -#include -#include -#include "problem_description.h" - -namespace trajopt_interface -{ -MOVEIT_CLASS_FORWARD(TrajOptInterface); // Defines TrajOptInterfacePtr, ConstPtr, WeakPtr... etc - -class TrajOptInterface -{ -public: - TrajOptInterface(const ros::NodeHandle& nh = ros::NodeHandle("~")); - - const sco::BasicTrustRegionSQPParameters& getParams() const - { - return params_; - } - - bool solve(const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, moveit_msgs::MotionPlanDetailedResponse& res); - -protected: - /** @brief Configure everything using the param server */ - void setTrajOptParams(sco::BasicTrustRegionSQPParameters& param); - void setDefaultTrajOPtParams(); - void setProblemInfoParam(ProblemInfo& problem_info); - void setJointPoseTermInfoParams(JointPoseTermInfoPtr& jp, std::string name); - trajopt::DblVec extractStartJointValues(const planning_interface::MotionPlanRequest& req, - const std::vector& group_joint_names); - - ros::NodeHandle nh_; /// The ROS node handle - sco::BasicTrustRegionSQPParameters params_; - std::vector optimizer_callbacks_; - TrajOptProblemPtr trajopt_problem_; - std::string name_; -}; - -void callBackFunc(sco::OptProb* opt_prob, sco::OptResults& opt_res); -} // namespace trajopt_interface diff --git a/moveit_planners/trajopt/include/trajopt_interface/trajopt_planning_context.h b/moveit_planners/trajopt/include/trajopt_interface/trajopt_planning_context.h deleted file mode 100644 index 9c06eb0f5e..0000000000 --- a/moveit_planners/trajopt/include/trajopt_interface/trajopt_planning_context.h +++ /dev/null @@ -1,32 +0,0 @@ -#pragma once - -#include - -#include -#include - -namespace trajopt_interface -{ -MOVEIT_CLASS_FORWARD(TrajOptPlanningContext); // Defines TrajOptPlanningContextPtr, ConstPtr, WeakPtr... etc - -class TrajOptPlanningContext : public planning_interface::PlanningContext -{ -public: - TrajOptPlanningContext(const std::string& name, const std::string& group, - const moveit::core::RobotModelConstPtr& model); - ~TrajOptPlanningContext() override - { - } - - bool solve(planning_interface::MotionPlanResponse& res) override; - bool solve(planning_interface::MotionPlanDetailedResponse& res) override; - - bool terminate() override; - void clear() override; - -private: - moveit::core::RobotModelConstPtr robot_model_; - - TrajOptInterfacePtr trajopt_interface_; -}; -} // namespace trajopt_interface diff --git a/moveit_planners/trajopt/package.xml b/moveit_planners/trajopt/package.xml deleted file mode 100644 index 7cb48c34d4..0000000000 --- a/moveit_planners/trajopt/package.xml +++ /dev/null @@ -1,34 +0,0 @@ - - - - moveit_planners_trajopt - 1.1.0 - TrajOpt planning plugin, an optimization based motion planner - - Omid Heidari - MoveIt Release Team - - BSD-3-Clause - - http://moveit.ros.org - https://github.com/ros-planning/moveit2/issues - https://github.com/ros-planning/moveit2 - - Omid Heidari - - catkin - - pluginlib - moveit_core - moveit_ros_planning - moveit_ros_planning_interface - moveit_visual_tools - roscpp - rosparam_shortcuts - trajopt - - - - - - diff --git a/moveit_planners/trajopt/src/kinematic_terms.cpp b/moveit_planners/trajopt/src/kinematic_terms.cpp deleted file mode 100644 index 617ab0bbd9..0000000000 --- a/moveit_planners/trajopt/src/kinematic_terms.cpp +++ /dev/null @@ -1,64 +0,0 @@ -#include -#include - -#include -#include - -#include - -using namespace std; -using namespace sco; -using namespace Eigen; - -namespace trajopt_interface -{ -VectorXd CartPoseErrCalculator::operator()(const VectorXd& dof_vals) const -{ - // TODO: create the actual error function from information in planning scene - VectorXd err; - return err; -} - -VectorXd JointVelErrCalculator::operator()(const VectorXd& var_vals) const -{ - assert(var_vals.rows() % 2 == 0); - // var_vals = (theta_t1, theta_t2, theta_t3 ... 1/dt_1, 1/dt_2, 1/dt_3 ...) - int half = static_cast(var_vals.rows() / 2); - int num_vels = half - 1; - // (x1-x0)*(1/dt) - VectorXd vel = (var_vals.segment(1, num_vels) - var_vals.segment(0, num_vels)).array() * - var_vals.segment(half + 1, num_vels).array(); - - // Note that for equality terms tols are 0, so error is effectively doubled - VectorXd result(vel.rows() * 2); - result.topRows(vel.rows()) = -(upper_tol_ - (vel.array() - target_)); - result.bottomRows(vel.rows()) = lower_tol_ - (vel.array() - target_); - return result; -} - -MatrixXd JointVelJacobianCalculator::operator()(const VectorXd& var_vals) const -{ - // var_vals = (theta_t1, theta_t2, theta_t3 ... 1/dt_1, 1/dt_2, 1/dt_3 ...) - int num_vals = static_cast(var_vals.rows()); - int half = num_vals / 2; - int num_vels = half - 1; - MatrixXd jac = MatrixXd::Zero(num_vels * 2, num_vals); - - for (int i = 0; i < num_vels; ++i) - { - // v = (j_i+1 - j_i)*(1/dt) - // We calculate v with the dt from the second pt - int time_index = i + half + 1; - jac(i, i) = -1.0 * var_vals(time_index); - jac(i, i + 1) = 1.0 * var_vals(time_index); - jac(i, time_index) = var_vals(i + 1) - var_vals(i); - // All others are 0 - } - - // bottom half is negative velocities - jac.bottomRows(num_vels) = -jac.topRows(num_vels); - - return jac; -} - -} // namespace trajopt_interface diff --git a/moveit_planners/trajopt/src/problem_description.cpp b/moveit_planners/trajopt/src/problem_description.cpp deleted file mode 100644 index 9fd3803ad0..0000000000 --- a/moveit_planners/trajopt/src/problem_description.cpp +++ /dev/null @@ -1,601 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2013, John Schulman - * 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. - * - * 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 HOLDER 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. - * - * http://opensource.org/licenses/BSD-2-Clause - *********************************************************************/ - -#include - -#include - -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -#include -#include - -/** - * @brief Checks the size of the parameter given and throws if incorrect - * @param parameter The vector whose size is getting checked - * @param expected_size The expected size of the vector - * @param name The name to use when printing an error or warning - * @param apply_first If true and only one value is given, broadcast value to length of expected_size - */ -void checkParameterSize(trajopt::DblVec& parameter, unsigned int expected_size, const std::string& name, - bool apply_first = true) -{ - if (apply_first == true && parameter.size() == 1) - { - parameter = trajopt::DblVec(expected_size, parameter[0]); - ROS_INFO("1 %s given. Applying to all %i joints", name.c_str(), expected_size); - } - else if (parameter.size() != expected_size) - { - PRINT_AND_THROW(boost::format("wrong number of %s. expected %i got %i") % name % expected_size % parameter.size()); - } -} - -namespace trajopt_interface -{ -TrajOptProblem::TrajOptProblem() -{ -} - -TrajOptProblem::TrajOptProblem(const ProblemInfo& problem_info) - : OptProb(problem_info.basic_info.convex_solver) - , planning_scene_(problem_info.planning_scene) - , planning_group_(problem_info.planning_group_name) -{ - moveit::core::RobotModelConstPtr robot_model = planning_scene_->getRobotModel(); - moveit::core::RobotState current_state = planning_scene_->getCurrentState(); - const moveit::core::JointModelGroup* joint_model_group = current_state.getJointModelGroup(planning_group_); - - moveit::core::JointBoundsVector bounds = joint_model_group->getActiveJointModelsBounds(); - dof_ = joint_model_group->getActiveJointModelNames().size(); // or bounds.size(); - - int n_steps = problem_info.basic_info.n_steps; - - ROS_INFO(" ======================================= problem_description: limits"); - Eigen::MatrixX2d limits(dof_, 2); - for (int k = 0; k < limits.size() / 2; ++k) - { - moveit::core::JointModel::Bounds bound = *bounds[k]; - // In MoveIt, joints are considered to have multiple dofs but we only have single dof joints: - moveit::core::VariableBounds joint_bound = bound.front(); - - limits(k, 0) = joint_bound.min_position_; - limits(k, 1) = joint_bound.max_position_; - - ROS_INFO("joint %i with lower bound: %f, upper bound: %f", k, joint_bound.min_position_, joint_bound.max_position_); - } - - Eigen::VectorXd lower, upper; - lower = limits.col(0); - upper = limits.col(1); - - trajopt::DblVec vlower, vupper; - std::vector names; - for (int i = 0; i < n_steps; ++i) - { - for (int j = 0; j < dof_; ++j) - { - names.push_back((boost::format("j_%i_%i") % i % j).str()); - } - vlower.insert(vlower.end(), lower.data(), lower.data() + lower.size()); - vupper.insert(vupper.end(), upper.data(), upper.data() + upper.size()); - - if (problem_info.basic_info.use_time == true) - { - vlower.insert(vlower.end(), problem_info.basic_info.dt_lower_lim); - vupper.insert(vupper.end(), problem_info.basic_info.dt_upper_lim); - names.push_back((boost::format("dt_%i") % i).str()); - } - } - - sco::VarVector trajvarvec = createVariables(names, vlower, vupper); - matrix_traj_vars = trajopt::VarArray(n_steps, dof_ + (problem_info.basic_info.use_time ? 1 : 0), trajvarvec.data()); - // matrix_traj_vars is essentially a matrix of elements like: - // j_0_0, j_0_1 ... - // j_1_0, j_1_1 ... - // its size is n_steps by n_dof -} - -TrajOptProblemPtr ConstructProblem(const ProblemInfo& pci) -{ - const BasicInfo& bi = pci.basic_info; - int n_steps = bi.n_steps; - - bool use_time = false; - // Check that all costs and constraints support the types that are specified in pci - for (TermInfoPtr cost : pci.cost_infos) - { - if (cost->term_type & TT_CNT) - ROS_WARN("%s is listed as a type TT_CNT but was added to cost_infos", (cost->name).c_str()); - if (!(cost->getSupportedTypes() & TT_COST)) - PRINT_AND_THROW(boost::format("%s is only a constraint, but you listed it as a cost") % cost->name); - if (cost->term_type & TT_USE_TIME) - { - use_time = true; - if (!(cost->getSupportedTypes() & TT_USE_TIME)) - PRINT_AND_THROW(boost::format("%s does not support time, but you listed it as a using time") % cost->name); - } - } - for (TermInfoPtr cnt : pci.cnt_infos) - { - if (cnt->term_type & TT_COST) - ROS_WARN("%s is listed as a type TT_COST but was added to cnt_infos", (cnt->name).c_str()); - if (!(cnt->getSupportedTypes() & TT_CNT)) - PRINT_AND_THROW(boost::format("%s is only a cost, but you listed it as a constraint") % cnt->name); - if (cnt->term_type & TT_USE_TIME) - { - use_time = true; - if (!(cnt->getSupportedTypes() & TT_USE_TIME)) - PRINT_AND_THROW(boost::format("%s does not support time, but you listed it as a using time") % cnt->name); - } - } - - // Check that if a cost or constraint uses time, basic_info is set accordingly - if ((use_time == true) && (pci.basic_info.use_time == false)) - PRINT_AND_THROW("A term is using time and basic_info is not set correctly. Try basic_info.use_time = true"); - - // This could be removed in the future once we are sure that all costs are - if ((use_time == false) && (pci.basic_info.use_time == true)) - PRINT_AND_THROW("No terms use time and basic_info is not set correctly. Try basic_info.use_time = false"); - - auto prob = std::make_shared(pci); - - // Generate initial trajectory and check its size - moveit::core::RobotModelConstPtr robot_model = pci.planning_scene->getRobotModel(); - moveit::core::RobotState current_state = pci.planning_scene->getCurrentState(); - - const moveit::core::JointModelGroup* joint_model_group = current_state.getJointModelGroup(pci.planning_group_name); - int n_dof = prob->GetNumDOF(); - - std::vector current_joint_values; - current_state.copyJointGroupPositions(joint_model_group, current_joint_values); - - trajopt::TrajArray init_traj; - generateInitialTrajectory(pci, current_joint_values, init_traj); - - if (pci.basic_info.use_time == true) - { - prob->SetHasTime(true); - if (init_traj.rows() != n_steps || init_traj.cols() != n_dof + 1) - { - PRINT_AND_THROW(boost::format("Initial trajectory is not the right size matrix\n" - "Expected %i rows (time steps) x %i columns (%i dof + 1 time column)\n" - "Got %i rows and %i columns") % - n_steps % (n_dof + 1) % n_dof % init_traj.rows() % init_traj.cols()); - } - } - else - { - prob->SetHasTime(false); - if (init_traj.rows() != n_steps || init_traj.cols() != n_dof) - { - PRINT_AND_THROW(boost::format("Initial trajectory is not the right size matrix\n" - "Expected %i rows (time steps) x %i columns\n" - "Got %i rows and %i columns") % - n_steps % n_dof % init_traj.rows() % init_traj.cols()); - } - } - prob->SetInitTraj(init_traj); - - trajopt::VarArray matrix_traj_vars_temp; - // If start_fixed, constrain the joint values for the first time step to be their initialized values - if (bi.start_fixed) - { - if (init_traj.rows() < 1) - { - PRINT_AND_THROW("Initial trajectory must contain at least the start state."); - } - - if (init_traj.cols() != (n_dof + (use_time ? 1 : 0))) - { - PRINT_AND_THROW("robot dof values don't match initialization. I don't " - "know what you want me to use for the dof values"); - } - - for (int j = 0; j < static_cast(n_dof); ++j) - { - matrix_traj_vars_temp = prob->GetVars(); - prob->addLinearConstraint(sco::exprSub(sco::AffExpr(matrix_traj_vars_temp(0, j)), init_traj(0, j)), sco::EQ); - } - } - - // Apply constraint to each fixed dof to its initial value for all timesteps (freeze that column) - if (!bi.dofs_fixed.empty()) - { - for (int dof_ind : bi.dofs_fixed) - { - for (int i = 1; i < prob->GetNumSteps(); ++i) - { - matrix_traj_vars_temp = prob->GetVars(); - prob->addLinearConstraint(sco::exprSub(sco::AffExpr(matrix_traj_vars_temp(i, dof_ind)), - sco::AffExpr(init_traj(0, dof_ind))), - sco::EQ); - } - } - } - - for (const TermInfoPtr& ci : pci.cost_infos) - { - ci->addObjectiveTerms(*prob); - } - - for (const TermInfoPtr& ci : pci.cnt_infos) - { - ci->addObjectiveTerms(*prob); - } - return prob; -} - -CartPoseTermInfo::CartPoseTermInfo() : TermInfo(TT_COST | TT_CNT) -{ - pos_coeffs = Eigen::Vector3d::Ones(); - rot_coeffs = Eigen::Vector3d::Ones(); - tcp.setIdentity(); -} - -void CartPoseTermInfo::addObjectiveTerms(TrajOptProblem& prob) -{ - unsigned int n_dof = prob.GetActiveGroupNumDOF(); - - Eigen::Isometry3d input_pose; - Eigen::Quaterniond q(wxyz(0), wxyz(1), wxyz(2), wxyz(3)); - input_pose.linear() = q.matrix(); - input_pose.translation() = xyz; - - if (term_type == (TT_COST | TT_USE_TIME)) - { - ROS_ERROR("Use time version of this term has not been defined."); - } - else if (term_type == (TT_CNT | TT_USE_TIME)) - { - ROS_ERROR("Use time version of this term has not been defined."); - } - else if ((term_type & TT_COST) && ~(term_type | ~TT_USE_TIME)) - { - sco::VectorOfVector::Ptr f = - std::make_shared(input_pose, prob.GetPlanningScene(), link, tcp); - prob.addCost(std::make_shared(f, prob.GetVarRow(timestep, 0, n_dof), - concatVector(rot_coeffs, pos_coeffs), sco::ABS, name)); - } - else if ((term_type & TT_CNT) && ~(term_type | ~TT_USE_TIME)) - { - sco::VectorOfVector::Ptr f = - std::make_shared(input_pose, prob.GetPlanningScene(), link, tcp); - prob.addConstraint(std::make_shared( - f, prob.GetVarRow(timestep, 0, n_dof), concatVector(rot_coeffs, pos_coeffs), sco::EQ, name)); - } - else - { - ROS_WARN("CartPoseTermInfo does not have a valid term_type defined. No cost/constraint applied"); - } -} - -void JointPoseTermInfo::addObjectiveTerms(TrajOptProblem& prob) -{ - unsigned int n_dof = prob.GetActiveGroupNumDOF(); - - // If optional parameter not given, set to default - if (coeffs.empty()) - coeffs = trajopt::DblVec(n_dof, 1); - if (upper_tols.empty()) - upper_tols = trajopt::DblVec(n_dof, 0); - if (lower_tols.empty()) - lower_tols = trajopt::DblVec(n_dof, 0); - if (last_step <= -1) - last_step = prob.GetNumSteps() - 1; - - // Check time step is valid - if ((prob.GetNumSteps() - 1) <= first_step) - first_step = prob.GetNumSteps() - 1; - if ((prob.GetNumSteps() - 1) <= last_step) - last_step = prob.GetNumSteps() - 1; - // if (last_step == first_step) - // last_step += 1; - if (last_step < first_step) - { - int tmp = first_step; - first_step = last_step; - last_step = tmp; - ROS_WARN("Last time step for JointPosTerm comes before first step. Reversing them."); - } - if (last_step == -1) // last_step not set - last_step = first_step; - - // Check if parameters are the correct size. - checkParameterSize(coeffs, n_dof, "JointPoseTermInfo coeffs", true); - checkParameterSize(targets, n_dof, "JointPoseTermInfo targets", true); - checkParameterSize(upper_tols, n_dof, "JointPoseTermInfo upper_tols", true); - checkParameterSize(lower_tols, n_dof, "JointPoseTermInfo lower_tols", true); - - // Check if tolerances are all zeros - bool is_upper_zeros = - std::all_of(upper_tols.begin(), upper_tols.end(), [](double i) { return util::doubleEquals(i, 0.); }); - bool is_lower_zeros = - std::all_of(lower_tols.begin(), lower_tols.end(), [](double i) { return util::doubleEquals(i, 0.); }); - - // Get vars associated with joints - trajopt::VarArray vars = prob.GetVars(); - trajopt::VarArray joint_vars = vars.block(0, 0, vars.rows(), static_cast(n_dof)); - if (prob.GetHasTime()) - ROS_INFO("JointPoseTermInfo does not differ based on setting of TT_USE_TIME"); - - if (term_type & TT_COST) - { - // If the tolerances are 0, an equality cost is set. Otherwise it's a hinged "inequality" cost - if (is_upper_zeros && is_lower_zeros) - { - prob.addCost(std::make_shared(joint_vars, util::toVectorXd(coeffs), - util::toVectorXd(targets), first_step, last_step)); - prob.getCosts().back()->setName(name); - } - else - { - prob.addCost(std::make_shared(joint_vars, util::toVectorXd(coeffs), - util::toVectorXd(targets), util::toVectorXd(upper_tols), - util::toVectorXd(lower_tols), first_step, last_step)); - prob.getCosts().back()->setName(name); - } - } - else if (term_type & TT_CNT) - { - // If the tolerances are 0, an equality cnt is set. Otherwise it's an inequality constraint - if (is_upper_zeros && is_lower_zeros) - { - prob.addConstraint(std::make_shared( - joint_vars, util::toVectorXd(coeffs), util::toVectorXd(targets), first_step, last_step)); - prob.getConstraints().back()->setName(name); - } - else - { - prob.addConstraint(std::make_shared( - joint_vars, util::toVectorXd(coeffs), util::toVectorXd(targets), util::toVectorXd(upper_tols), - util::toVectorXd(lower_tols), first_step, last_step)); - prob.getConstraints().back()->setName(name); - } - } - else - { - ROS_WARN("JointPosTermInfo does not have a valid term_type defined. No cost/constraint applied"); - } -} - -void JointVelTermInfo::addObjectiveTerms(TrajOptProblem& prob) -{ - unsigned int n_dof = prob.GetNumDOF(); - - // If optional parameter not given, set to default - if (coeffs.empty()) - coeffs = trajopt::DblVec(n_dof, 1); - if (upper_tols.empty()) - upper_tols = trajopt::DblVec(n_dof, 0); - if (lower_tols.empty()) - lower_tols = trajopt::DblVec(n_dof, 0); - if (last_step <= -1) - last_step = prob.GetNumSteps() - 1; - - // If only one time step is desired, calculate velocity with next step (2 steps are needed for 1 velocity calculation) - if ((prob.GetNumSteps() - 2) <= first_step) - first_step = prob.GetNumSteps() - 2; - if ((prob.GetNumSteps() - 1) <= last_step) - last_step = prob.GetNumSteps() - 1; - if (last_step == first_step) - last_step += 1; - if (last_step < first_step) - { - int tmp = first_step; - first_step = last_step; - last_step = tmp; - ROS_WARN("Last time step for JointVelTerm comes before first step. Reversing them."); - } - - // Check if parameters are the correct size. - checkParameterSize(coeffs, n_dof, "JointVelTermInfo coeffs", true); - checkParameterSize(targets, n_dof, "JointVelTermInfo targets", true); - checkParameterSize(upper_tols, n_dof, "JointVelTermInfo upper_tols", true); - checkParameterSize(lower_tols, n_dof, "JointVelTermInfo lower_tols", true); - assert(last_step > first_step); - assert(first_step >= 0); - - // Check if tolerances are all zeros - bool is_upper_zeros = - std::all_of(upper_tols.begin(), upper_tols.end(), [](double i) { return util::doubleEquals(i, 0.); }); - bool is_lower_zeros = - std::all_of(lower_tols.begin(), lower_tols.end(), [](double i) { return util::doubleEquals(i, 0.); }); - - // Get vars associated with joints - trajopt::VarArray vars = prob.GetVars(); - trajopt::VarArray joint_vars = vars.block(0, 0, vars.rows(), static_cast(n_dof)); - - if (term_type == (TT_COST | TT_USE_TIME)) - { - unsigned num_vels = last_step - first_step; - - // Apply separate cost to each joint b/c that is how the error function is currently written - for (size_t j = 0; j < n_dof; ++j) - { - // Get a vector of a single column of vars - sco::VarVector joint_vars_vec = joint_vars.cblock(first_step, j, last_step - first_step + 1); - sco::VarVector time_vars_vec = vars.cblock(first_step, vars.cols() - 1, last_step - first_step + 1); - - // If the tolerances are 0, an equality cost is set - if (is_upper_zeros && is_lower_zeros) - { - trajopt::DblVec single_jnt_coeffs = trajopt::DblVec(num_vels * 2, coeffs[j]); - prob.addCost(std::make_shared( - std::make_shared(targets[j], upper_tols[j], lower_tols[j]), - std::make_shared(), concatVector(joint_vars_vec, time_vars_vec), - util::toVectorXd(single_jnt_coeffs), sco::SQUARED, name + "_j" + std::to_string(j))); - } - // Otherwise it's a hinged "inequality" cost - else - { - trajopt::DblVec single_jnt_coeffs = trajopt::DblVec(num_vels * 2, coeffs[j]); - prob.addCost(std::make_shared( - std::make_shared(targets[j], upper_tols[j], lower_tols[j]), - std::make_shared(), concatVector(joint_vars_vec, time_vars_vec), - util::toVectorXd(single_jnt_coeffs), sco::HINGE, name + "_j" + std::to_string(j))); - } - } - } - else if (term_type == (TT_CNT | TT_USE_TIME)) - { - unsigned num_vels = last_step - first_step; - - // Apply separate cnt to each joint b/c that is how the error function is currently written - for (size_t j = 0; j < n_dof; ++j) - { - // Get a vector of a single column of vars - sco::VarVector joint_vars_vec = joint_vars.cblock(first_step, j, last_step - first_step + 1); - sco::VarVector time_vars_vec = vars.cblock(first_step, vars.cols() - 1, last_step - first_step + 1); - - // If the tolerances are 0, an equality cnt is set - if (is_upper_zeros && is_lower_zeros) - { - trajopt::DblVec single_jnt_coeffs = trajopt::DblVec(num_vels * 2, coeffs[j]); - prob.addConstraint(std::make_shared( - std::make_shared(targets[j], upper_tols[j], lower_tols[j]), - std::make_shared(), concatVector(joint_vars_vec, time_vars_vec), - util::toVectorXd(single_jnt_coeffs), sco::EQ, name + "_j" + std::to_string(j))); - } - // Otherwise it's a hinged "inequality" constraint - else - { - trajopt::DblVec single_jnt_coeffs = trajopt::DblVec(num_vels * 2, coeffs[j]); - prob.addConstraint(std::make_shared( - std::make_shared(targets[j], upper_tols[j], lower_tols[j]), - std::make_shared(), concatVector(joint_vars_vec, time_vars_vec), - util::toVectorXd(single_jnt_coeffs), sco::INEQ, name + "_j" + std::to_string(j)))); - } - } - } - else if ((term_type & TT_COST) && ~(term_type | ~TT_USE_TIME)) - { - // If the tolerances are 0, an equality cost is set. Otherwise it's a hinged "inequality" cost - if (is_upper_zeros && is_lower_zeros) - { - prob.addCost(std::make_shared(joint_vars, util::toVectorXd(coeffs), - util::toVectorXd(targets), first_step, last_step)); - prob.getCosts().back()->setName(name); - } - else - { - prob.addCost(std::make_shared(joint_vars, util::toVectorXd(coeffs), - util::toVectorXd(targets), util::toVectorXd(upper_tols), - util::toVectorXd(lower_tols), first_step, last_step)); - prob.getCosts().back()->setName(name); - } - } - else if ((term_type & TT_CNT) && ~(term_type | ~TT_USE_TIME)) - { - // If the tolerances are 0, an equality cnt is set. Otherwise it's an inequality constraint - if (is_upper_zeros && is_lower_zeros) - { - prob.addConstraint(std::make_shared( - joint_vars, util::toVectorXd(coeffs), util::toVectorXd(targets), first_step, last_step)); - prob.getConstraints().back()->setName(name); - } - else - { - prob.addConstraint(std::make_shared( - joint_vars, util::toVectorXd(coeffs), util::toVectorXd(targets), util::toVectorXd(upper_tols), - util::toVectorXd(lower_tols), first_step, last_step)); - prob.getConstraints().back()->setName(name); - } - } - else - { - ROS_WARN("JointVelTermInfo does not have a valid term_type defined. No cost/constraint applied"); - } -} - -void generateInitialTrajectory(const ProblemInfo& pci, const std::vector& current_joint_values, - trajopt::TrajArray& init_traj) -{ - Eigen::VectorXd current_pos(current_joint_values.size()); - for (int joint_index = 0; joint_index < current_joint_values.size(); ++joint_index) - { - current_pos(joint_index) = current_joint_values[joint_index]; - } - - InitInfo init_info = pci.init_info; - - // initialize based on type specified - if (init_info.type == InitInfo::STATIONARY) - { - // Initializes all joint values to the initial value (the current value in scene) - init_traj = current_pos.transpose().replicate(pci.basic_info.n_steps, 1); - } - else if (init_info.type == InitInfo::JOINT_INTERPOLATED) - { - // Linearly interpolates between initial value (current values in the scene) and the joint position specified in - // InitInfo.data - Eigen::VectorXd end_pos = init_info.data; - init_traj.resize(pci.basic_info.n_steps, end_pos.rows()); - for (int dof_index = 0; dof_index < current_pos.rows(); ++dof_index) - { - init_traj.col(dof_index) = - Eigen::VectorXd::LinSpaced(pci.basic_info.n_steps, current_pos(dof_index), end_pos(dof_index)); - } - } - else if (init_info.type == InitInfo::GIVEN_TRAJ) - { - // Initializes the matrix to a given trajectory - init_traj = init_info.data; - } - else - { - PRINT_AND_THROW("Init Info did not have a valid type. Valid types are " - "STATIONARY, JOINT_INTERPOLATED, or GIVEN_TRAJ"); - } - - // Currently all trajectories are generated without time then appended here - if (pci.basic_info.use_time) - { - // add on time (default to 1 sec) - init_traj.conservativeResize(Eigen::NoChange_t(), init_traj.cols() + 1); - - init_traj.block(0, init_traj.cols() - 1, init_traj.rows(), 1) = - Eigen::VectorXd::Constant(init_traj.rows(), init_info.dt); - } -} -} // namespace trajopt_interface diff --git a/moveit_planners/trajopt/src/trajopt_interface.cpp b/moveit_planners/trajopt/src/trajopt_interface.cpp deleted file mode 100644 index bf7e89387a..0000000000 --- a/moveit_planners/trajopt/src/trajopt_interface.cpp +++ /dev/null @@ -1,463 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2019, PickNik Inc. - * 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 Inc. 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 */ - -#include -#include - -#include - -#include -#include -#include - -#include - -#include -#include - -#include -#include -#include -#include - -#include -#include - -namespace trajopt_interface -{ -TrajOptInterface::TrajOptInterface(const ros::NodeHandle& nh) : nh_(nh), name_("TrajOptInterface") -{ - trajopt_problem_ = std::make_shared(); - setDefaultTrajOPtParams(); - - // TODO: callbacks should be defined by the user - optimizer_callbacks_.push_back(callBackFunc); -} - -bool TrajOptInterface::solve(const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, - moveit_msgs::MotionPlanDetailedResponse& res) -{ - ROS_INFO(" ======================================= From trajopt_interface, solve is called"); - setTrajOptParams(params_); - - if (!planning_scene) - { - ROS_ERROR_STREAM_NAMED(name_, "No planning scene initialized."); - res.error_code.val = moveit_msgs::MoveItErrorCodes::FAILURE; - return false; - } - - ROS_INFO(" ======================================= Extract current state information"); - ros::WallTime start_time = ros::WallTime::now(); - moveit::core::RobotModelConstPtr robot_model = planning_scene->getRobotModel(); - bool robot_model_ok = static_cast(robot_model); - if (!robot_model_ok) - ROS_ERROR_STREAM_NAMED(name_, "robot model is not loaded properly"); - auto current_state = std::make_shared(robot_model); - *current_state = planning_scene->getCurrentState(); - const moveit::core::JointModelGroup* joint_model_group = current_state->getJointModelGroup(req.group_name); - if (joint_model_group == nullptr) - ROS_ERROR_STREAM_NAMED(name_, "joint model group is empty"); - std::vector group_joint_names = joint_model_group->getActiveJointModelNames(); - int dof = group_joint_names.size(); - std::vector current_joint_values; - current_state->copyJointGroupPositions(joint_model_group, current_joint_values); - - // Current state is different from star state in general - ROS_INFO(" ======================================= Extract start state information"); - trajopt::DblVec start_joint_values = extractStartJointValues(req, group_joint_names); - - // check the start state for being empty or joint limit violiation - if (start_joint_values.empty()) - { - ROS_ERROR_STREAM_NAMED(name_, "Start_state is empty"); - res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE; - return false; - } - - if (not joint_model_group->satisfiesPositionBounds(start_joint_values.data())) - { - ROS_ERROR_STREAM_NAMED(name_, "Start state violates joint limits"); - res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE; - return false; - } - - ROS_INFO(" ======================================= Create ProblemInfo"); - ProblemInfo problem_info(planning_scene, req.group_name); - - setProblemInfoParam(problem_info); - - ROS_INFO(" ======================================= Populate init info, hard-coded"); - // TODO: init info should be defined by user. To this end, we need to add seed trajectories to MotionPlanRequest. - // JOINT_INTERPOLATED: data is the current joint values - // GIVEN_TRAJ: data is the joint values of the current state copied to all timesteps - Eigen::VectorXd current_joint_values_eigen(dof); - for (int joint_index = 0; joint_index < dof; ++joint_index) - { - current_joint_values_eigen(joint_index) = current_joint_values[joint_index]; - } - - if (problem_info.init_info.type == InitInfo::JOINT_INTERPOLATED) - { - problem_info.init_info.data = current_joint_values_eigen; - } - else if (problem_info.init_info.type == InitInfo::GIVEN_TRAJ) - { - problem_info.init_info.data = current_joint_values_eigen.transpose().replicate(problem_info.basic_info.n_steps, 1); - } - - ROS_INFO(" ======================================= Create Constraints"); - if (req.goal_constraints.empty()) - { - ROS_ERROR_STREAM_NAMED("trajopt_planner", "No goal constraints specified!"); - res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS; - return false; - } - - ROS_INFO(" ======================================= Cartesian Constraints"); - if (!req.goal_constraints[0].position_constraints.empty() && !req.goal_constraints[0].orientation_constraints.empty()) - { - CartPoseTermInfoPtr cart_goal_pos = std::make_shared(); - - // TODO: Feed cart_goal_pos with request information and the needed param to the setup.yaml file - // TODO: Multiple Cartesian constraints - - // Add the constraint to problem_info - problem_info.cnt_infos.push_back(cart_goal_pos); - } - else if (req.goal_constraints[0].position_constraints.empty() && - !req.goal_constraints[0].orientation_constraints.empty()) - { - ROS_ERROR_STREAM_NAMED("trajopt_planner", "position constraint is not defined"); - res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS; - return false; - } - else if (!req.goal_constraints[0].orientation_constraints.empty() && - req.goal_constraints[0].orientation_constraints.empty()) - { - ROS_ERROR_STREAM_NAMED("trajopt_planner", "orientation constraint is not defined"); - res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS; - return false; - } - - ROS_INFO(" ======================================= Constraints from request goal_constraints"); - for (auto goal_cnt : req.goal_constraints) - { - JointPoseTermInfoPtr joint_pos_term = std::make_shared(); - // When using MotionPlanning Display in RViz, the created request has no name for the constraint - setJointPoseTermInfoParams(joint_pos_term, (goal_cnt.name != "") ? goal_cnt.name : "goal_tmp"); - - trajopt::DblVec joint_goal_constraints; - for (const moveit_msgs::JointConstraint& joint_goal_constraint : goal_cnt.joint_constraints) - { - joint_goal_constraints.push_back(joint_goal_constraint.position); - } - joint_pos_term->targets = joint_goal_constraints; - problem_info.cnt_infos.push_back(joint_pos_term); - } - - ROS_INFO(" ======================================= Constraints from request start_state"); - // add the start pos from request as a constraint - auto joint_start_pos = std::make_shared(); - - joint_start_pos->targets = start_joint_values; - setJointPoseTermInfoParams(joint_start_pos, "start_pos"); - problem_info.cnt_infos.push_back(joint_start_pos); - - ROS_INFO(" ======================================= Velocity Constraints, hard-coded"); - // TODO: should be defined by user, its parameters should be added to setup.yaml - auto joint_vel = std::make_shared(); - - joint_vel->coeffs = std::vector(dof, 5.0); - joint_vel->targets = std::vector(dof, 0.0); - joint_vel->first_step = 0; - joint_vel->last_step = problem_info.basic_info.n_steps - 1; - joint_vel->name = "joint_vel"; - joint_vel->term_type = trajopt_interface::TT_COST; - problem_info.cost_infos.push_back(joint_vel); - - ROS_INFO(" ======================================= Visibility Constraints"); - if (!req.goal_constraints[0].visibility_constraints.empty()) - { - // TODO: Add visibility constraint - } - - ROS_DEBUG_STREAM_NAMED(name_, "trajopt_param.improve_ratio_threshold: " << params_.improve_ratio_threshold); - ROS_DEBUG_STREAM_NAMED(name_, "trajopt_param.min_trust_box_size: " << params_.min_trust_box_size); - ROS_DEBUG_STREAM_NAMED(name_, "trajopt_param.min_approx_improve: " << params_.min_approx_improve); - ROS_DEBUG_STREAM_NAMED(name_, "trajopt_param.params_.min_approx_improve_frac: " << params_.min_approx_improve_frac); - ROS_DEBUG_STREAM_NAMED(name_, "trajopt_param.max_iter: " << params_.max_iter); - ROS_DEBUG_STREAM_NAMED(name_, "trajopt_param.trust_shrink_ratio: " << params_.trust_shrink_ratio); - ROS_DEBUG_STREAM_NAMED(name_, "trajopt_param.trust_expand_ratio: " << params_.trust_expand_ratio); - ROS_DEBUG_STREAM_NAMED(name_, "trajopt_param.cnt_tolerance: " << params_.cnt_tolerance); - ROS_DEBUG_STREAM_NAMED(name_, "trajopt_param.max_merit_coeff_increases: " << params_.max_merit_coeff_increases); - ROS_DEBUG_STREAM_NAMED(name_, "trajopt_param.merit_coeff_increase_ratio: " << params_.merit_coeff_increase_ratio); - ROS_DEBUG_STREAM_NAMED(name_, "trajopt_param.max_time: " << params_.max_time); - ROS_DEBUG_STREAM_NAMED(name_, "trajopt_param.initial_merit_error_coeff: " << params_.initial_merit_error_coeff); - ROS_DEBUG_STREAM_NAMED(name_, "trajopt_param.trust_box_size: " << params_.trust_box_size); - ROS_DEBUG_STREAM_NAMED(name_, "problem_info.basic_info.n_steps: " << problem_info.basic_info.n_steps); - ROS_DEBUG_STREAM_NAMED(name_, "problem_info.basic_info.dt_upper_lim: " << problem_info.basic_info.dt_upper_lim); - ROS_DEBUG_STREAM_NAMED(name_, "problem_info.basic_info.dt_lower_lim: " << problem_info.basic_info.dt_lower_lim); - ROS_DEBUG_STREAM_NAMED(name_, "problem_info.basic_info.start_fixed: " << problem_info.basic_info.start_fixed); - ROS_DEBUG_STREAM_NAMED(name_, "problem_info.basic_info.use_time: " << problem_info.basic_info.use_time); - ROS_DEBUG_STREAM_NAMED(name_, "problem_info.basic_info.convex_solver: " << problem_info.basic_info.convex_solver); - - std::string problem_info_type; - switch (problem_info.init_info.type) - { - case InitInfo::STATIONARY: - problem_info_type = "STATIONARY"; - break; - case InitInfo::JOINT_INTERPOLATED: - problem_info_type = "JOINT_INTERPOLATED"; - break; - case InitInfo::GIVEN_TRAJ: - problem_info_type = "GIVEN_TRAJ"; - break; - } - ROS_DEBUG_STREAM_NAMED(name_, "problem_info.basic_info.type: " << problem_info_type); - ROS_DEBUG_STREAM_NAMED(name_, "problem_info.basic_info.dt: " << problem_info.init_info.dt); - - ROS_INFO(" ======================================= Construct problem"); - trajopt_problem_ = ConstructProblem(problem_info); - - ROS_INFO_STREAM_NAMED("num_cost", trajopt_problem_->getNumCosts()); - ROS_INFO_STREAM_NAMED("num_constraints", trajopt_problem_->getNumConstraints()); - - ROS_INFO(" ======================================= TrajOpt Optimization"); - sco::BasicTrustRegionSQP opt(trajopt_problem_); - - opt.setParameters(params_); - opt.initialize(trajopt::trajToDblVec(trajopt_problem_->GetInitTraj())); - - // Add all callbacks - for (const sco::Optimizer::Callback& callback : optimizer_callbacks_) - { - opt.addCallback(callback); - } - - // Optimize - ros::WallTime create_time = ros::WallTime::now(); - opt.optimize(); - - ROS_INFO(" ======================================= TrajOpt Solution"); - trajopt::TrajArray opt_solution = trajopt::getTraj(opt.x(), trajopt_problem_->GetVars()); - - // assume that the trajectory is now optimized, fill in the output structure: - ROS_INFO_STREAM_NAMED("num_rows", opt_solution.rows()); - ROS_INFO_STREAM_NAMED("num_cols", opt_solution.cols()); - - res.trajectory.resize(1); - res.trajectory[0].joint_trajectory.joint_names = group_joint_names; - res.trajectory[0].joint_trajectory.header = req.start_state.joint_state.header; - - // fill in the entire trajectory - res.trajectory[0].joint_trajectory.points.resize(opt_solution.rows()); - for (int i = 0; i < opt_solution.rows(); ++i) - { - res.trajectory[0].joint_trajectory.points[i].positions.resize(opt_solution.cols()); - for (size_t j = 0; j < opt_solution.cols(); ++j) - { - res.trajectory[0].joint_trajectory.points[i].positions[j] = opt_solution(i, j); - } - // Further filtering is required to set valid timestamps accounting for velocity and acceleration constraints. - res.trajectory[0].joint_trajectory.points[i].time_from_start = ros::Duration(0.0); - } - - res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS; - res.processing_time.push_back((ros::WallTime::now() - start_time).toSec()); - - ROS_INFO(" ======================================= check if final state is within goal tolerances"); - kinematic_constraints::JointConstraint joint_cnt(planning_scene->getRobotModel()); - moveit::core::RobotState last_state(*current_state); - last_state.setJointGroupPositions(req.group_name, res.trajectory[0].joint_trajectory.points.back().positions); - - for (int jn = 0; jn < res.trajectory[0].joint_trajectory.points.back().positions.size(); ++jn) - { - ROS_INFO_STREAM_NAMED("joint_value", res.trajectory[0].joint_trajectory.points.back().positions[jn] - << " " << req.goal_constraints.back().joint_constraints[jn].position); - } - - bool constraints_are_ok = true; - int joint_cnt_index = 0; - for (const moveit_msgs::JointConstraint& constraint : req.goal_constraints.back().joint_constraints) - { - ROS_INFO("index %i: jc.configure(constraint)=> %d, jc.decide(last_state).satisfied=> %d, tolerance %f", - joint_cnt_index, joint_cnt.configure(constraint), joint_cnt.decide(last_state).satisfied, - constraint.tolerance_above); - constraints_are_ok = constraints_are_ok and joint_cnt.configure(constraint); - constraints_are_ok = constraints_are_ok and joint_cnt.decide(last_state).satisfied; - if (not constraints_are_ok) - { - ROS_ERROR_STREAM_NAMED("trajopt_planner", "Goal constraints are violated: " << constraint.joint_name); - res.error_code.val = moveit_msgs::MoveItErrorCodes::GOAL_CONSTRAINTS_VIOLATED; - return false; - } - joint_cnt_index = joint_cnt_index + 1; - } - - ROS_INFO(" ==================================== Response"); - res.trajectory_start = req.start_state; - - ROS_INFO(" ==================================== Debug Response"); - ROS_INFO_STREAM_NAMED("group_name", res.group_name); - ROS_INFO_STREAM_NAMED("start_traj_name_size", res.trajectory_start.joint_state.name.size()); - ROS_INFO_STREAM_NAMED("start_traj_position_size", res.trajectory_start.joint_state.position.size()); - ROS_INFO_STREAM_NAMED("traj_name_size", res.trajectory[0].joint_trajectory.joint_names.size()); - ROS_INFO_STREAM_NAMED("traj_point_size", res.trajectory[0].joint_trajectory.points.size()); - return true; -} - -void TrajOptInterface::setDefaultTrajOPtParams() -{ - sco::BasicTrustRegionSQPParameters params; - params_ = params; -} - -void TrajOptInterface::setTrajOptParams(sco::BasicTrustRegionSQPParameters& params) -{ - nh_.param("trajopt_param/improve_ratio_threshold", params.improve_ratio_threshold, 0.25); - nh_.param("trajopt_param/min_trust_box_size", params.min_trust_box_size, 1e-4); - nh_.param("trajopt_param/min_approx_improve", params.min_approx_improve, 1e-4); - nh_.param("trajopt_param/min_approx_improve_frac", params.min_approx_improve_frac, -static_cast(INFINITY)); - nh_.param("trajopt_param/max_iter", params.max_iter, 100.0); - nh_.param("trajopt_param/trust_shrink_ratio", params.trust_shrink_ratio, 0.1); - - nh_.param("trajopt_param/trust_expand_ratio", params.trust_expand_ratio, 1.5); - nh_.param("trajopt_param/cnt_tolerance", params.cnt_tolerance, 1e-4); - nh_.param("trajopt_param/max_merit_coeff_increases", params.max_merit_coeff_increases, 5.0); - nh_.param("trajopt_param/merit_coeff_increase_ratio", params.merit_coeff_increase_ratio, 10.0); - nh_.param("trajopt_param/max_time", params.max_time, static_cast(INFINITY)); - nh_.param("trajopt_param/merit_error_coeff", params.initial_merit_error_coeff, 10.0); - nh_.param("trajopt_param/trust_box_size", params.trust_box_size, 1e-1); -} - -void TrajOptInterface::setProblemInfoParam(ProblemInfo& problem_info) -{ - nh_.param("problem_info/basic_info/n_steps", problem_info.basic_info.n_steps, 20); - nh_.param("problem_info/basic_info/dt_upper_lim", problem_info.basic_info.dt_upper_lim, 2.0); - nh_.param("problem_info/basic_info/dt_lower_lim", problem_info.basic_info.dt_lower_lim, 100.0); - nh_.param("problem_info/basic_info/start_fixed", problem_info.basic_info.start_fixed, true); - nh_.param("problem_info/basic_info/use_time", problem_info.basic_info.use_time, false); - int convex_solver_index; - nh_.param("problem_info/basic_info/convex_solver", convex_solver_index, 1); - switch (convex_solver_index) - { - case 1: - problem_info.basic_info.convex_solver = sco::ModelType::AUTO_SOLVER; - break; - case 2: - problem_info.basic_info.convex_solver = sco::ModelType::BPMPD; - break; - case 3: - problem_info.basic_info.convex_solver = sco::ModelType::OSQP; - break; - case 4: - problem_info.basic_info.convex_solver = sco::ModelType::QPOASES; - break; - case 5: - problem_info.basic_info.convex_solver = sco::ModelType::GUROBI; - break; - } - - nh_.param("problem_info/init_info/dt", problem_info.init_info.dt, 0.5); - int type_index; - nh_.param("problem_info/init_info/type", type_index, 1); - switch (type_index) - { - case 1: - problem_info.init_info.type = InitInfo::STATIONARY; - break; - case 2: - problem_info.init_info.type = InitInfo::JOINT_INTERPOLATED; - break; - case 3: - problem_info.init_info.type = InitInfo::GIVEN_TRAJ; - break; - } -} - -void TrajOptInterface::setJointPoseTermInfoParams(JointPoseTermInfoPtr& jp, std::string name) -{ - int term_type_index; - std::string term_type_address = "joint_pos_term_info/" + name + "/term_type"; - nh_.param(term_type_address, term_type_index, 1); - - switch (term_type_index) - { - case 1: - jp->term_type = TT_COST; - break; - case 2: - jp->term_type = TT_CNT; - break; - case 3: - jp->term_type = TT_USE_TIME; - break; - } - - nh_.getParam("joint_pos_term_info/" + name + "/first_timestep", jp->first_step); - nh_.getParam("joint_pos_term_info/" + name + "/last_timestep", jp->last_step); - nh_.getParam("joint_pos_term_info/" + name + "/name", jp->name); -} - -trajopt::DblVec TrajOptInterface::extractStartJointValues(const planning_interface::MotionPlanRequest& req, - const std::vector& group_joint_names) -{ - std::unordered_map all_joints; - trajopt::DblVec start_joint_vals; - - for (int joint_index = 0; joint_index < req.start_state.joint_state.position.size(); ++joint_index) - { - all_joints[req.start_state.joint_state.name[joint_index]] = req.start_state.joint_state.position[joint_index]; - } - - for (auto joint_name : group_joint_names) - { - ROS_INFO(" joint position from start state, name: %s, value: %f", joint_name.c_str(), all_joints[joint_name]); - start_joint_vals.push_back(all_joints[joint_name]); - } - - return start_joint_vals; -} - -void callBackFunc(sco::OptProb* opt_prob, sco::OptResults& opt_res) -{ - // TODO: Create the actual implementation -} - -} // namespace trajopt_interface diff --git a/moveit_planners/trajopt/src/trajopt_planner_manager.cpp b/moveit_planners/trajopt/src/trajopt_planner_manager.cpp deleted file mode 100644 index 055e093ebd..0000000000 --- a/moveit_planners/trajopt/src/trajopt_planner_manager.cpp +++ /dev/null @@ -1,140 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2019, PickNik Inc. - * 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 Inc. 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: TrajOpt planning plugin -*/ - -#include - -#include - -#include - -#include - -namespace trajopt_interface -{ -class TrajOptPlannerManager : public planning_interface::PlannerManager -{ -public: - TrajOptPlannerManager() : planning_interface::PlannerManager() - { - } - - bool initialize(const moveit::core::RobotModelConstPtr& model, const std::string& ns) override - { - ROS_INFO(" ======================================= initialize gets called"); - - if (!ns.empty()) - nh_ = ros::NodeHandle(ns); - std::string trajopt_ns = ns.empty() ? "trajopt" : ns + "/trajopt"; - - for (const std::string& gpName : model->getJointModelGroupNames()) - { - ROS_INFO(" ======================================= group name: %s, robot model: %s", gpName.c_str(), - model->getName().c_str()); - planning_contexts_[gpName] = std::make_shared("trajopt_planning_context", gpName, model); - } - - return true; - } - - bool canServiceRequest(const moveit_msgs::MotionPlanRequest& req) const override - { - return req.trajectory_constraints.constraints.empty(); - } - - std::string getDescription() const override - { - return "TrajOpt"; - } - - void getPlanningAlgorithms(std::vector& algs) const override - { - algs.clear(); - algs.push_back("trajopt"); - } - - planning_interface::PlanningContextPtr getPlanningContext(const planning_scene::PlanningSceneConstPtr& planning_scene, - const planning_interface::MotionPlanRequest& req, - moveit_msgs::MoveItErrorCodes& error_code) const override - { - ROS_INFO(" ======================================= getPlanningContext() is called "); - error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS; - - if (req.group_name.empty()) - { - ROS_ERROR("No group specified to plan for"); - error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME; - return planning_interface::PlanningContextPtr(); - } - - if (!planning_scene) - { - ROS_ERROR("No planning scene supplied as input"); - error_code.val = moveit_msgs::MoveItErrorCodes::FAILURE; - return planning_interface::PlanningContextPtr(); - } - - // create PlanningScene using hybrid collision detector - planning_scene::PlanningScenePtr ps = planning_scene->diff(); - - // set FCL for the collision - ps->allocateCollisionDetector(collision_detection::CollisionDetectorAllocatorFCL::create()); - - // retrieve and configure existing context - const TrajOptPlanningContextPtr& context = planning_contexts_.at(req.group_name); - - ROS_INFO(" ======================================= context is made "); - - context->setPlanningScene(ps); - context->setMotionPlanRequest(req); - - error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS; - - return context; - } - -private: - ros::NodeHandle nh_; - -protected: - std::map planning_contexts_; -}; - -} // namespace trajopt_interface - -// register the TrajOptPlannerManager class as a plugin -CLASS_LOADER_REGISTER_CLASS(trajopt_interface::TrajOptPlannerManager, planning_interface::PlannerManager); diff --git a/moveit_planners/trajopt/src/trajopt_planning_context.cpp b/moveit_planners/trajopt/src/trajopt_planning_context.cpp deleted file mode 100644 index 4a7405642e..0000000000 --- a/moveit_planners/trajopt/src/trajopt_planning_context.cpp +++ /dev/null @@ -1,75 +0,0 @@ -#include -#include -#include -#include - -#include - -#include - -#include -#include - -namespace trajopt_interface -{ -TrajOptPlanningContext::TrajOptPlanningContext(const std::string& context_name, const std::string& group_name, - const moveit::core::RobotModelConstPtr& model) - : planning_interface::PlanningContext(context_name, group_name), robot_model_(model) -{ - ROS_INFO(" ======================================= TrajOptPlanningContext is constructed"); - trajopt_interface_ = std::make_shared(); -} - -bool TrajOptPlanningContext::solve(planning_interface::MotionPlanDetailedResponse& res) -{ - moveit_msgs::MotionPlanDetailedResponse res_msg; - bool trajopt_solved = trajopt_interface_->solve(planning_scene_, request_, res_msg); - - if (trajopt_solved) - { - res.trajectory.resize(1); - res.trajectory[0] = std::make_shared(robot_model_, getGroupName()); - - moveit::core::RobotState start_state(robot_model_); - moveit::core::robotStateMsgToRobotState(res_msg.trajectory_start, start_state); - res.trajectory[0]->setRobotTrajectoryMsg(start_state, res_msg.trajectory[0]); - - res.description.push_back("plan"); - // TODO: Add the initial trajectory to res (MotionPlanDetailedResponse) - res.processing_time = res_msg.processing_time; - res.error_code = res_msg.error_code; - return true; - } - else - { - res.error_code = res_msg.error_code; - return false; - } -} - -bool TrajOptPlanningContext::solve(planning_interface::MotionPlanResponse& res) -{ - planning_interface::MotionPlanDetailedResponse res_detailed; - bool planning_success = solve(res_detailed); - - res.error_code = res_detailed.error_code; - - if (planning_success) - { - res.trajectory = res_detailed.trajectory[0]; - res.planning_time = res_detailed.processing_time[0]; - } - - return planning_success; -} - -bool TrajOptPlanningContext::terminate() -{ - ROS_ERROR_STREAM_NAMED("trajopt_planning_context", "TrajOpt is not interruptible yet"); - return false; -} -void TrajOptPlanningContext::clear() -{ -} - -} // namespace trajopt_interface diff --git a/moveit_planners/trajopt/test/trajectory_test.cpp b/moveit_planners/trajopt/test/trajectory_test.cpp deleted file mode 100644 index 53a44b052b..0000000000 --- a/moveit_planners/trajopt/test/trajectory_test.cpp +++ /dev/null @@ -1,212 +0,0 @@ -/********************************************************************* - * Software License Agreement - * - * Copyright (c) 2019, PickNik Inc. - * All rights reserved. - * - *********************************************************************/ - -/* Author: Omid Heidari - Desc: Test the trajectory planned by trajopt - */ - -// C++ -#include -#include -#include - -// ROS -#include - -// Testing -#include - -#include - -#include -#include -#include -#include -#include - -#include -#include - -#include -#include -#include -#include -#include - -class TrajectoryTest : public ::testing::Test -{ -public: - TrajectoryTest() - { - } - -protected: - void SetUp() override - { - node_handle_ = ros::NodeHandle("~"); - robot_model_ = moveit::core::loadTestingRobotModel("panda"); - bool robot_model_ok_ = static_cast(robot_model_); - if (!robot_model_ok_) - ROS_ERROR_STREAM_NAMED("trajectory_test", "robot model is not loaded correctly"); - } - -protected: - moveit::core::RobotModelPtr robot_model_; - std::vector group_joint_names_; - const std::string PLANNING_GROUP = "panda_arm"; - const double GOAL_TOLERANCE = 0.1; - ros::NodeHandle node_handle_; -}; // class TrajectoryTest - -TEST_F(TrajectoryTest, concatVectorValidation) -{ - std::vector vec_a = { 1, 2, 3, 4, 5 }; - std::vector vec_b = { 6, 7, 8, 9, 10 }; - std::vector vec_c = trajopt_interface::concatVector(vec_a, vec_b); - EXPECT_EQ(vec_c.size(), vec_a.size() + vec_b.size()); - - // Check if the output of concatVector is correct. - // Loop over the output and the input vectors to see if they match - std::size_t length_ab = vec_a.size() + vec_b.size(); - for (std::size_t index = 0; index < length_ab; ++index) - { - if (index < vec_a.size()) - { - EXPECT_EQ(vec_c[index], vec_a[index]); - } - else - { - EXPECT_EQ(vec_c[index], vec_b[index - vec_a.size()]); - } - } -} - -TEST_F(TrajectoryTest, goalTolerance) -{ - const std::string NODE_NAME = "trajectory_test"; - - // Create a RobotState and JointModelGroup to keep track of the current robot pose and planning group - auto current_state = std::make_shared(robot_model_); - current_state->setToDefaultValues(); - - const moveit::core::JointModelGroup* joint_model_group = current_state->getJointModelGroup(PLANNING_GROUP); - EXPECT_NE(joint_model_group, nullptr); - const std::vector& joint_names = joint_model_group->getActiveJointModelNames(); - EXPECT_EQ(joint_names.size(), 7); - - auto planning_scene = std::make_shared(robot_model_); - - // Create response and request - planning_interface::MotionPlanRequest req; - planning_interface::MotionPlanResponse res; - - // Set start state - // ====================================================================================== - std::vector start_joint_values = { 0.4, 0.3, 0.5, -0.55, 0.88, 1.0, -0.075 }; - auto start_state = std::make_shared(robot_model_); - start_state->setJointGroupPositions(joint_model_group, start_joint_values); - start_state->update(); - - req.start_state.joint_state.name = joint_names; - req.start_state.joint_state.position = start_joint_values; - req.goal_constraints.clear(); - req.group_name = PLANNING_GROUP; - - // Set the goal state and joints tolerance - // ======================================================================================== - auto goal_state = std::make_shared(robot_model_); - std::vector goal_joint_values = { 0.8, 0.7, 1, -1.3, 1.9, 2.2, -0.1 }; - goal_state->setJointGroupPositions(joint_model_group, goal_joint_values); - goal_state->update(); - moveit_msgs::Constraints joint_goal = kinematic_constraints::constructGoalConstraints(*goal_state, joint_model_group); - req.goal_constraints.push_back(joint_goal); - req.goal_constraints[0].name = "goal_pos"; - // 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) - { - req.goal_constraints[0].joint_constraints[x].tolerance_above = 0.001; - req.goal_constraints[0].joint_constraints[x].tolerance_below = 0.001; - } - - // Load planner - // ====================================================================================== - // We will now construct a loader to load a planner, by name. - // Note that we are using the ROS pluginlib library here. - std::unique_ptr> planner_plugin_loader; - planning_interface::PlannerManagerPtr planner_instance; - - std::string planner_plugin_name = "trajopt_interface/TrajOptPlanner"; - node_handle_.setParam("planning_plugin", planner_plugin_name); - - // Make sure the planner plugin is loaded - EXPECT_TRUE(node_handle_.getParam("planning_plugin", planner_plugin_name)); - try - { - planner_plugin_loader = std::make_shared>( - "moveit_core", "planning_interface::PlannerManager"); - } - catch (pluginlib::PluginlibException& ex) - { - ROS_FATAL_STREAM_NAMED(NODE_NAME, "Exception while creating planning plugin loader " << ex.what()); - } - try - { - planner_instance.reset(planner_plugin_loader->createUnmanagedInstance(planner_plugin_name)); - if (!planner_instance->initialize(robot_model_, node_handle_.getNamespace())) - ROS_FATAL_STREAM_NAMED(NODE_NAME, "Could not initialize planner instance"); - ROS_INFO_STREAM_NAMED(NODE_NAME, "Using planning interface '" << planner_instance->getDescription() << '\''); - } - catch (pluginlib::PluginlibException& ex) - { - const std::vector& classes = planner_plugin_loader->getDeclaredClasses(); - std::stringstream ss; - for (std::size_t i = 0; i < classes.size(); ++i) - ss << classes[i] << ' '; - ROS_ERROR_STREAM_NAMED(NODE_NAME, "Exception while loading planner '" << planner_plugin_name << "': " << ex.what() - << '\n' - << "Available plugins: " << ss.str()); - } - - // Create planning context - // ======================================================================================== - planning_interface::PlanningContextPtr context = - planner_instance->getPlanningContext(planning_scene, req, res.error_code); - - context->solve(res); - EXPECT_EQ(res.error_code.val, res.error_code.SUCCESS); - - moveit_msgs::MotionPlanResponse response; - res.getMessage(response); - - // Check the difference between the last step in the solution and the goal - // ======================================================================================== - std::vector joints_values_last_step = response.trajectory.joint_trajectory.points.back().positions; - - for (std::size_t joint_index = 0; joint_index < joints_values_last_step.size(); ++joint_index) - { - double goal_error = - abs(joints_values_last_step[joint_index] - req.goal_constraints[0].joint_constraints[joint_index].position); - std::cerr << "goal_error: " << goal_error << '\n'; - EXPECT_LT(goal_error, GOAL_TOLERANCE); - } -} - -int main(int argc, char** argv) -{ - testing::InitGoogleTest(&argc, argv); - ros::init(argc, argv, "trajectory_test"); - - ros::AsyncSpinner spinner(1); - spinner.start(); - - int result = RUN_ALL_TESTS(); - ros::shutdown(); - - return result; -} diff --git a/moveit_planners/trajopt/test/trajectory_test.test b/moveit_planners/trajopt/test/trajectory_test.test deleted file mode 100644 index bec08fda3d..0000000000 --- a/moveit_planners/trajopt/test/trajectory_test.test +++ /dev/null @@ -1,10 +0,0 @@ - - - - - - - - - - diff --git a/moveit_planners/trajopt/trajopt_interface_plugin_description.xml b/moveit_planners/trajopt/trajopt_interface_plugin_description.xml deleted file mode 100644 index 051864285a..0000000000 --- a/moveit_planners/trajopt/trajopt_interface_plugin_description.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - The trajopt motion planner plugin which implements sequential convex optimization to solve motion planning problem. - - - diff --git a/moveit_setup_assistant/unported_templates/README.md b/moveit_setup_assistant/unported_templates/README.md index 8bb545d5d8..33dc6ec378 100644 --- a/moveit_setup_assistant/unported_templates/README.md +++ b/moveit_setup_assistant/unported_templates/README.md @@ -2,4 +2,3 @@ * `joystick_control.launch` - Control the Rviz Motion Planning Plugin with a joystick * `ompl-chomp_planning_pipeline.launch.xml` - * `run_benchmark_ompl.launch` - Launch file for benchmarking OMPL planners - * `run_benchmark_trajopt.launch` diff --git a/moveit_setup_assistant/unported_templates/run_benchmark_trajopt.launch b/moveit_setup_assistant/unported_templates/run_benchmark_trajopt.launch deleted file mode 100644 index 50631869af..0000000000 --- a/moveit_setup_assistant/unported_templates/run_benchmark_trajopt.launch +++ /dev/null @@ -1,22 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - -