diff --git a/trajopt_ext/osqp_eigen/CMakeLists.txt b/trajopt_ext/osqp_eigen/CMakeLists.txt index 1b97d296..888185f2 100644 --- a/trajopt_ext/osqp_eigen/CMakeLists.txt +++ b/trajopt_ext/osqp_eigen/CMakeLists.txt @@ -28,10 +28,11 @@ if(NOT ${OsqpEigen_FOUND} AND NOT TRAJOPT_OSQP_DISABLED) SOURCE_DIR ${CMAKE_BINARY_DIR}-src BINARY_DIR ${CMAKE_BINARY_DIR}-build CMAKE_CACHE_ARGS - -DCMAKE_INSTALL_PREFIX:STRING=${CMAKE_INSTALL_PREFIX} -DCMAKE_BUILD_TYPE:STRING=Release + -DCMAKE_INSTALL_PREFIX:STRING=${CMAKE_INSTALL_PREFIX} -DCMAKE_TOOLCHAIN_FILE:STRING=${CMAKE_TOOLCHAIN_FILE} - -DVCPKG_TARGET_TRIPLET:STRING=${VCPKG_TARGET_TRIPLET}) + -DVCPKG_TARGET_TRIPLET:STRING=${VCPKG_TARGET_TRIPLET} + -DOSQP_EIGEN_DEBUG_OUTPUT=OFF) if(TRAJOPT_PACKAGE) find_package(ros_industrial_cmake_boilerplate REQUIRED) diff --git a/trajopt_ifopt/CMakeLists.txt b/trajopt_ifopt/CMakeLists.txt index 0ae6127d..ec20778d 100644 --- a/trajopt_ifopt/CMakeLists.txt +++ b/trajopt_ifopt/CMakeLists.txt @@ -43,7 +43,10 @@ set(TRAJOPT_IFOPT_SOURCE_FILES src/utils/ifopt_utils.cpp src/utils/numeric_differentiation.cpp src/utils/trajopt_utils.cpp - src/variable_sets/joint_position_variable.cpp) + src/variable_sets/nodes_variables.cpp + src/variable_sets/nodes_observer.cpp + src/variable_sets/node.cpp + src/variable_sets/var.cpp) add_library(${PROJECT_NAME} ${TRAJOPT_IFOPT_SOURCE_FILES}) target_link_libraries( diff --git a/trajopt_ifopt/include/trajopt_ifopt/constraints/cartesian_line_constraint.h b/trajopt_ifopt/include/trajopt_ifopt/constraints/cartesian_line_constraint.h index 620ae7d0..0d451ecf 100644 --- a/trajopt_ifopt/include/trajopt_ifopt/constraints/cartesian_line_constraint.h +++ b/trajopt_ifopt/include/trajopt_ifopt/constraints/cartesian_line_constraint.h @@ -36,7 +36,7 @@ TRAJOPT_IGNORE_WARNINGS_POP namespace trajopt_ifopt { -class JointPosition; +class Var; /** * @brief Contains kinematic information for the cartesian position cost; include cart point .h & remove? @@ -101,7 +101,7 @@ class CartLineConstraint : public ifopt::ConstraintSet tesseract_common::TransformMap&)>; CartLineConstraint(CartLineInfo info, - std::shared_ptr position_var, + std::shared_ptr position_var, const Eigen::VectorXd& coeffs, const std::string& name = "CartLine"); @@ -182,11 +182,8 @@ class CartLineConstraint : public ifopt::ConstraintSet /** @brief Bounds on the positions of each joint */ std::vector bounds_; - /** @brief Pointers to the vars used by this constraint. - * - * Do not access them directly. Instead use this->GetVariables()->GetComponent(position_var->GetName())->GetValues() - */ - std::shared_ptr position_var_; + /** @brief Pointers to the vars used by this constraint. */ + std::shared_ptr position_var_; /** @brief The cartesian line information used when calculating error */ CartLineInfo info_; diff --git a/trajopt_ifopt/include/trajopt_ifopt/constraints/cartesian_position_constraint.h b/trajopt_ifopt/include/trajopt_ifopt/constraints/cartesian_position_constraint.h index 82d5f380..83d1390d 100644 --- a/trajopt_ifopt/include/trajopt_ifopt/constraints/cartesian_position_constraint.h +++ b/trajopt_ifopt/include/trajopt_ifopt/constraints/cartesian_position_constraint.h @@ -35,7 +35,7 @@ TRAJOPT_IGNORE_WARNINGS_POP namespace trajopt_ifopt { -class JointPosition; +class Var; /** @brief Contains Cartesian pose constraint information */ struct CartPosInfo @@ -101,11 +101,11 @@ class CartPosConstraint : public ifopt::ConstraintSet tesseract_common::TransformMap&)>; CartPosConstraint(const CartPosInfo& info, - std::shared_ptr position_var, + std::shared_ptr position_var, const std::string& name = "CartPos"); CartPosConstraint(CartPosInfo info, - std::shared_ptr position_var, + std::shared_ptr position_var, const Eigen::VectorXd& coeffs, const std::string& name = "CartPos"); @@ -185,11 +185,8 @@ class CartPosConstraint : public ifopt::ConstraintSet /** @brief Bounds on the positions of each joint */ std::vector bounds_; - /** - * @brief Pointers to the vars used by this constraint. - * Do not access them directly. Instead use this->GetVariables()->GetComponent(position_var->GetName())->GetValues() - */ - std::shared_ptr position_var_; + /** @brief Pointers to the vars used by this constraint. */ + std::shared_ptr position_var_; /** @brief The kinematic information used when calculating error */ CartPosInfo info_; diff --git a/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/continuous_collision_constraint.h b/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/continuous_collision_constraint.h index 19b7475c..f29c4436 100644 --- a/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/continuous_collision_constraint.h +++ b/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/continuous_collision_constraint.h @@ -29,11 +29,12 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include #include +#include TRAJOPT_IGNORE_WARNINGS_POP namespace trajopt_ifopt { -class JointPosition; +class Var; class ContinuousCollisionEvaluator; class ContinuousCollisionConstraint : public ifopt::ConstraintSet @@ -52,7 +53,7 @@ class ContinuousCollisionConstraint : public ifopt::ConstraintSet * @param name */ ContinuousCollisionConstraint(std::shared_ptr collision_evaluator, - std::array, 2> position_vars, + std::array, 2> position_vars, bool vars0_fixed, bool vars1_fixed, int max_num_cnt = 1, @@ -103,18 +104,19 @@ class ContinuousCollisionConstraint : public ifopt::ConstraintSet /** @brief Bounds on the constraint value. Default: std::vector(1, ifopt::BoundSmallerZero) */ std::vector bounds_; - /** - * @brief Pointers to the vars used by this constraint. - * Do not access them directly. Instead use this->GetVariables()->GetComponent(position_var->GetName())->GetValues() - */ - std::array, 2> position_vars_; + /** @brief Pointers to the vars used by this constraint. */ + std::array, 2> position_vars_; bool vars0_fixed_{ false }; bool vars1_fixed_{ false }; + std::shared_ptr collision_evaluator_; + /** @brief Used to initialize jacobian because snopt sparsity cannot change */ - std::vector> triplet_list_; + bool fixed_sparsity_{ false }; + mutable std::once_flag init_flag_; + mutable std::vector> triplet_list_; - std::shared_ptr collision_evaluator_; + void initSparsity() const; }; } // namespace trajopt_ifopt diff --git a/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/continuous_collision_numerical_constraint.h b/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/continuous_collision_numerical_constraint.h index 4937ced1..20e2c73f 100644 --- a/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/continuous_collision_numerical_constraint.h +++ b/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/continuous_collision_numerical_constraint.h @@ -32,7 +32,7 @@ TRAJOPT_IGNORE_WARNINGS_POP namespace trajopt_ifopt { -class JointPosition; +class Var; class ContinuousCollisionEvaluator; class ContinuousCollisionNumericalConstraint : public ifopt::ConstraintSet @@ -51,7 +51,7 @@ class ContinuousCollisionNumericalConstraint : public ifopt::ConstraintSet * @param name */ ContinuousCollisionNumericalConstraint(std::shared_ptr collision_evaluator, - std::array, 2> position_vars, + std::array, 2> position_vars, bool vars0_fixed, bool vars1_fixed, int max_num_cnt = 1, @@ -102,11 +102,8 @@ class ContinuousCollisionNumericalConstraint : public ifopt::ConstraintSet /** @brief Bounds on the constraint value. Default: std::vector(1, ifopt::BoundSmallerZero) */ std::vector bounds_; - /** - * @brief Pointers to the vars used by this constraint. - * Do not access them directly. Instead use this->GetVariables()->GetComponent(position_var->GetName())->GetValues() - */ - std::array, 2> position_vars_; + /** @brief Pointers to the vars used by this constraint. */ + std::array, 2> position_vars_; bool vars0_fixed_{ false }; bool vars1_fixed_{ false }; diff --git a/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/discrete_collision_constraint.h b/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/discrete_collision_constraint.h index e8680569..f49927fd 100644 --- a/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/discrete_collision_constraint.h +++ b/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/discrete_collision_constraint.h @@ -29,11 +29,12 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include #include +#include TRAJOPT_IGNORE_WARNINGS_POP namespace trajopt_ifopt { -class JointPosition; +class Var; class DiscreteCollisionEvaluator; class DiscreteCollisionConstraint : public ifopt::ConstraintSet @@ -43,7 +44,7 @@ class DiscreteCollisionConstraint : public ifopt::ConstraintSet using ConstPtr = std::shared_ptr; DiscreteCollisionConstraint(std::shared_ptr collision_evaluator, - std::shared_ptr position_var, + std::shared_ptr position_var, int max_num_cnt = 1, bool fixed_sparsity = false, const std::string& name = "DiscreteCollisionV3"); @@ -102,16 +103,18 @@ class DiscreteCollisionConstraint : public ifopt::ConstraintSet /** @brief Bounds on the constraint value. Default: std::vector(1, ifopt::BoundSmallerZero) */ std::vector bounds_; - /** - * @brief Pointers to the vars used by this constraint. - * Do not access them directly. Instead use this->GetVariables()->GetComponent(position_var->GetName())->GetValues() - */ - std::shared_ptr position_var_; + /** @brief Pointers to the vars used by this constraint. */ + std::shared_ptr position_var_; + + std::shared_ptr collision_evaluator_; /** @brief Used to initialize jacobian because snopt sparsity cannot change */ - std::vector> triplet_list_; + bool fixed_sparsity_{ false }; + mutable std::once_flag init_flag_; + mutable std::vector> triplet_list_; - std::shared_ptr collision_evaluator_; + void initSparsity() const; }; + } // namespace trajopt_ifopt #endif diff --git a/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/discrete_collision_numerical_constraint.h b/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/discrete_collision_numerical_constraint.h index bf64c4e3..e6f5068b 100644 --- a/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/discrete_collision_numerical_constraint.h +++ b/trajopt_ifopt/include/trajopt_ifopt/constraints/collision/discrete_collision_numerical_constraint.h @@ -28,11 +28,12 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include #include +#include TRAJOPT_IGNORE_WARNINGS_POP namespace trajopt_ifopt { -class JointPosition; +class Var; class DiscreteCollisionEvaluator; class DiscreteCollisionNumericalConstraint : public ifopt::ConstraintSet @@ -42,7 +43,7 @@ class DiscreteCollisionNumericalConstraint : public ifopt::ConstraintSet using ConstPtr = std::shared_ptr; DiscreteCollisionNumericalConstraint(std::shared_ptr collision_evaluator, - std::shared_ptr position_var, + std::shared_ptr position_var, int max_num_cnt = 1, bool fixed_sparsity = false, const std::string& name = "DiscreteCollisionNumerical"); @@ -101,16 +102,17 @@ class DiscreteCollisionNumericalConstraint : public ifopt::ConstraintSet /** @brief Bounds on the constraint value. Default: std::vector(1, ifopt::BoundSmallerZero) */ std::vector bounds_; - /** - * @brief Pointers to the vars used by this constraint. - * Do not access them directly. Instead use this->GetVariables()->GetComponent(position_var->GetName())->GetValues() - */ - std::shared_ptr position_var_; + /** @brief Pointers to the vars used by this constraint. */ + std::shared_ptr position_var_; + + std::shared_ptr collision_evaluator_; /** @brief Used to initialize jacobian because snopt sparsity cannot change */ - std::vector> triplet_list_; + bool fixed_sparsity_{ false }; + mutable std::once_flag init_flag_; + mutable std::vector> triplet_list_; - std::shared_ptr collision_evaluator_; + void initSparsity() const; }; } // namespace trajopt_ifopt #endif // TRAJOPT_IFOPT_DISCRETE_COLLISION_NUMERICAL_CONSTRAINT_H diff --git a/trajopt_ifopt/include/trajopt_ifopt/constraints/inverse_kinematics_constraint.h b/trajopt_ifopt/include/trajopt_ifopt/constraints/inverse_kinematics_constraint.h index ed8df5bd..beb79171 100644 --- a/trajopt_ifopt/include/trajopt_ifopt/constraints/inverse_kinematics_constraint.h +++ b/trajopt_ifopt/include/trajopt_ifopt/constraints/inverse_kinematics_constraint.h @@ -33,7 +33,7 @@ TRAJOPT_IGNORE_WARNINGS_POP namespace trajopt_ifopt { -class JointPosition; +class Var; /** * @brief Contains kinematic information for the inverse kinematics constraint @@ -81,8 +81,8 @@ class InverseKinematicsConstraint : public ifopt::ConstraintSet InverseKinematicsConstraint(const Eigen::Isometry3d& target_pose, InverseKinematicsInfo::ConstPtr kinematic_info, - std::shared_ptr constraint_var, - std::shared_ptr seed_var, + std::shared_ptr constraint_var, + std::shared_ptr seed_var, const std::string& name = "InverseKinematics"); /** @@ -143,13 +143,11 @@ class InverseKinematicsConstraint : public ifopt::ConstraintSet /** @brief Bounds on the joint distance the constraint_var may vary from the IK solution */ std::vector bounds_; - /** @brief Pointer to the var used by this constraint. - * - * Do not access them directly. Instead use this->GetVariables()->GetComponent(position_var->GetName())->GetValues()*/ - std::shared_ptr constraint_var_; + /** @brief Pointers to the vars used by this constraint. */ + std::shared_ptr constraint_var_; /** @brief Pointer to the var used as a seed when calculating IK. This will usually be a adjacent point in the * trajectory*/ - std::shared_ptr seed_var_; + std::shared_ptr seed_var_; /** @brief Target pose for the TCP. Currently in robot frame since world_to_base_ has not been implemented */ Eigen::Isometry3d target_pose_; /** @brief The kinematic info used to create this constraint */ diff --git a/trajopt_ifopt/include/trajopt_ifopt/constraints/joint_acceleration_constraint.h b/trajopt_ifopt/include/trajopt_ifopt/constraints/joint_acceleration_constraint.h index bd5267d4..6b4c9f25 100644 --- a/trajopt_ifopt/include/trajopt_ifopt/constraints/joint_acceleration_constraint.h +++ b/trajopt_ifopt/include/trajopt_ifopt/constraints/joint_acceleration_constraint.h @@ -32,7 +32,7 @@ TRAJOPT_IGNORE_WARNINGS_POP namespace trajopt_ifopt { -class JointPosition; +class Var; /** * @brief This creates a joint acceleration constraint and allows bounds to be set on a joint position @@ -54,7 +54,7 @@ class JointAccelConstraint : public ifopt::ConstraintSet * @param name Name of the constraint */ JointAccelConstraint(const Eigen::VectorXd& targets, - const std::vector>& position_vars, + const std::vector>& position_vars, const Eigen::VectorXd& coeffs, const std::string& name = "JointAccel"); @@ -67,7 +67,7 @@ class JointAccelConstraint : public ifopt::ConstraintSet * @param name Name of the constraint */ JointAccelConstraint(const std::vector& bounds, - const std::vector>& position_vars, + const std::vector>& position_vars, const Eigen::VectorXd& coeffs, const std::string& name = "JointAccel"); @@ -93,6 +93,7 @@ class JointAccelConstraint : public ifopt::ConstraintSet private: /** @brief The number of joints in a single JointPosition */ long n_dof_; + /** @brief The number of JointPositions passed in */ long n_vars_; @@ -102,11 +103,8 @@ class JointAccelConstraint : public ifopt::ConstraintSet /** @brief Bounds on the velocities of each joint */ std::vector bounds_; - /** @brief Pointers to the vars used by this constraint. - * - * Do not access them directly. Instead use this->GetVariables()->GetComponent(position_var->GetName())->GetValues()*/ - std::vector> position_vars_; - std::unordered_map index_map_; + /** @brief Pointers to the vars used by this constraint. */ + std::vector> position_vars_; }; } // namespace trajopt_ifopt #endif diff --git a/trajopt_ifopt/include/trajopt_ifopt/constraints/joint_jerk_constraint.h b/trajopt_ifopt/include/trajopt_ifopt/constraints/joint_jerk_constraint.h index 84ce1d03..367376cb 100644 --- a/trajopt_ifopt/include/trajopt_ifopt/constraints/joint_jerk_constraint.h +++ b/trajopt_ifopt/include/trajopt_ifopt/constraints/joint_jerk_constraint.h @@ -32,7 +32,7 @@ TRAJOPT_IGNORE_WARNINGS_POP namespace trajopt_ifopt { -class JointPosition; +class Var; /** * @brief This creates a joint acceleration constraint and allows bounds to be set on a joint position @@ -54,7 +54,7 @@ class JointJerkConstraint : public ifopt::ConstraintSet * @param name Name of the constraint */ JointJerkConstraint(const Eigen::VectorXd& targets, - const std::vector>& position_vars, + const std::vector>& position_vars, const Eigen::VectorXd& coeffs, const std::string& name = "JointJerk"); @@ -67,7 +67,7 @@ class JointJerkConstraint : public ifopt::ConstraintSet * @param name Name of the constraint */ JointJerkConstraint(const std::vector& bounds, - const std::vector>& position_vars, + const std::vector>& position_vars, const Eigen::VectorXd& coeffs, const std::string& name = "JointJerk"); @@ -93,6 +93,7 @@ class JointJerkConstraint : public ifopt::ConstraintSet private: /** @brief The number of joints in a single JointPosition */ long n_dof_; + /** @brief The number of JointPositions passed in */ long n_vars_; @@ -102,11 +103,8 @@ class JointJerkConstraint : public ifopt::ConstraintSet /** @brief Bounds on the velocities of each joint */ std::vector bounds_; - /** @brief Pointers to the vars used by this constraint. - * - * Do not access them directly. Instead use this->GetVariables()->GetComponent(position_var->GetName())->GetValues()*/ - std::vector> position_vars_; - std::unordered_map index_map_; + /** @brief Pointers to the vars used by this constraint. */ + std::vector> position_vars_; }; } // namespace trajopt_ifopt #endif diff --git a/trajopt_ifopt/include/trajopt_ifopt/constraints/joint_position_constraint.h b/trajopt_ifopt/include/trajopt_ifopt/constraints/joint_position_constraint.h index 025e6bac..98ceed6a 100644 --- a/trajopt_ifopt/include/trajopt_ifopt/constraints/joint_position_constraint.h +++ b/trajopt_ifopt/include/trajopt_ifopt/constraints/joint_position_constraint.h @@ -32,7 +32,7 @@ TRAJOPT_IGNORE_WARNINGS_POP namespace trajopt_ifopt { -class JointPosition; +class Var; /** * @brief This creates a joint position constraint. Allows bounds to be set on a joint position @@ -51,8 +51,8 @@ class JointPosConstraint : public ifopt::ConstraintSet * @param coeffs The joint coefficients to use as weights. If size of 1 then the values is replicated for each joint. * @param name Name of the constraint */ - JointPosConstraint(const Eigen::VectorXd& targets, - const std::vector>& position_vars, + JointPosConstraint(const Eigen::VectorXd& target, + const std::shared_ptr& position_var, const Eigen::VectorXd& coeffs, const std::string& name = "JointPos"); @@ -64,7 +64,7 @@ class JointPosConstraint : public ifopt::ConstraintSet * @param name Name of the constraint */ JointPosConstraint(const std::vector& bounds, - const std::vector>& position_vars, + const std::shared_ptr& position_vars, const Eigen::VectorXd& coeffs, const std::string& name = "JointPos"); @@ -91,8 +91,6 @@ class JointPosConstraint : public ifopt::ConstraintSet private: /** @brief The number of joints in a single JointPosition */ long n_dof_; - /** @brief The number of JointPositions passed in */ - long n_vars_; /** @brief The coeff to apply to error and gradient */ Eigen::VectorXd coeffs_; @@ -100,10 +98,8 @@ class JointPosConstraint : public ifopt::ConstraintSet /** @brief Bounds on the positions of each joint */ std::vector bounds_; - /** @brief Pointers to the vars used by this constraint. - * - * Do not access them directly. Instead use this->GetVariables()->GetComponent(position_var->GetName())->GetValues()*/ - std::vector> position_vars_; + /** @brief Pointers to the vars used by this constraint. */ + std::shared_ptr position_var_; }; } // namespace trajopt_ifopt #endif diff --git a/trajopt_ifopt/include/trajopt_ifopt/constraints/joint_velocity_constraint.h b/trajopt_ifopt/include/trajopt_ifopt/constraints/joint_velocity_constraint.h index 22991b05..129dc16a 100644 --- a/trajopt_ifopt/include/trajopt_ifopt/constraints/joint_velocity_constraint.h +++ b/trajopt_ifopt/include/trajopt_ifopt/constraints/joint_velocity_constraint.h @@ -32,7 +32,7 @@ TRAJOPT_IGNORE_WARNINGS_POP namespace trajopt_ifopt { -class JointPosition; +class Var; /** * @brief This creates a joint velocity constraint and allows bounds to be set on a joint position @@ -54,7 +54,7 @@ class JointVelConstraint : public ifopt::ConstraintSet * @param name Name of the constraint */ JointVelConstraint(const Eigen::VectorXd& targets, - const std::vector>& position_vars, + const std::vector>& position_vars, const Eigen::VectorXd& coeffs, const std::string& name = "JointVel"); @@ -67,7 +67,7 @@ class JointVelConstraint : public ifopt::ConstraintSet * @param name Name of the constraint */ JointVelConstraint(const std::vector& bounds, - const std::vector>& position_vars, + const std::vector>& position_vars, const Eigen::VectorXd& coeffs, const std::string& name = "JointVel"); @@ -93,6 +93,7 @@ class JointVelConstraint : public ifopt::ConstraintSet private: /** @brief The number of joints in a single JointPosition */ long n_dof_; + /** @brief The number of JointPositions passed in */ long n_vars_; @@ -102,11 +103,8 @@ class JointVelConstraint : public ifopt::ConstraintSet /** @brief Bounds on the velocities of each joint */ std::vector bounds_; - /** @brief Pointers to the vars used by this constraint. - * - * Do not access them directly. Instead use this->GetVariables()->GetComponent(position_var->GetName())->GetValues()*/ - std::vector> position_vars_; - std::unordered_map index_map_; + /** @brief Pointers to the vars used by this constraint. */ + std::vector> position_vars_; }; } // namespace trajopt_ifopt #endif diff --git a/trajopt_ifopt/include/trajopt_ifopt/fwd.h b/trajopt_ifopt/include/trajopt_ifopt/fwd.h index 5137178a..cb73edea 100644 --- a/trajopt_ifopt/include/trajopt_ifopt/fwd.h +++ b/trajopt_ifopt/include/trajopt_ifopt/fwd.h @@ -18,8 +18,10 @@ namespace trajopt_ifopt { -// joint_position_variable.h -class JointPosition; +// variable_sets +class NodesVariables; +class Node; +class Var; // cartesian_line_constraint.h struct CartLineInfo; diff --git a/trajopt_ifopt/include/trajopt_ifopt/utils/trajopt_utils.h b/trajopt_ifopt/include/trajopt_ifopt/utils/trajopt_utils.h index 2a78a793..853814a2 100644 --- a/trajopt_ifopt/include/trajopt_ifopt/utils/trajopt_utils.h +++ b/trajopt_ifopt/include/trajopt_ifopt/utils/trajopt_utils.h @@ -33,22 +33,21 @@ TRAJOPT_IGNORE_WARNINGS_POP namespace trajopt_ifopt { -class JointPosition; +class Var; /** * @brief Converts a vector of trajopt variables into the legacy TrajArray format * @param joint_positions Vector of joint positions. Must be in order and all the same length * @return TrajArray - size = [joint_positions.size(), joint_positions.n_dof] */ -tesseract_common::TrajArray toTrajArray(const std::vector>& joint_positions); +tesseract_common::TrajArray toTrajArray(const std::vector>& joint_positions); /** * @brief Converts a vector of trajopt variables into tesseract_common JointTrajectory * @param joint_positions Vector of joint positions. Must be in order and all the same length * @return JointTrajectory */ -tesseract_common::JointTrajectory -toJointTrajectory(const std::vector>& joint_positions); +tesseract_common::JointTrajectory toJointTrajectory(const std::vector>& joint_positions); } // namespace trajopt_ifopt #endif diff --git a/trajopt_ifopt/include/trajopt_ifopt/variable_sets/joint_position_variable.h b/trajopt_ifopt/include/trajopt_ifopt/variable_sets/joint_position_variable.h deleted file mode 100644 index f3a9eb48..00000000 --- a/trajopt_ifopt/include/trajopt_ifopt/variable_sets/joint_position_variable.h +++ /dev/null @@ -1,103 +0,0 @@ -/** - * @file joint_position_variable.h - * @brief Contains the joint position variable - * - * @author Matthew Powelson - * @date May 18, 2020 - * - * @copyright Copyright (c) 2020, Southwest Research Institute - * - * @par License - * Software License Agreement (Apache License) - * @par - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * http://www.apache.org/licenses/LICENSE-2.0 - * @par - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ -#ifndef TRAJOPT_IFOPT_JOINT_POSITION_VARIABLE_H -#define TRAJOPT_IFOPT_JOINT_POSITION_VARIABLE_H - -#include -TRAJOPT_IGNORE_WARNINGS_PUSH -#include -#include -#include -#include -TRAJOPT_IGNORE_WARNINGS_POP - -namespace trajopt_ifopt -{ -/** @brief Represents a single joint position in the optimization. Values are of dimension 1 x n_dof */ -class JointPosition : public ifopt::VariableSet -{ -public: - using Ptr = std::shared_ptr; - using ConstPtr = std::shared_ptr; - - JointPosition(const Eigen::Ref& init_value, - std::vector joint_names, - const std::string& name = "Joint_Position"); - - JointPosition(const Eigen::Ref& init_value, - std::vector joint_names, - const ifopt::Bounds& bounds, - const std::string& name = "Joint_Position"); - - JointPosition(const Eigen::Ref& init_value, - std::vector joint_names, - const tesseract_common::KinematicLimits& bounds, - const std::string& name = "Joint_Position"); - - /** - * @brief Sets this variable to the given joint position - * @param x Joint Position to which this variable will be set. - */ - void SetVariables(const Eigen::VectorXd& x) override; - - /** - * @brief Gets the joint position associated with this variable - * @return This variable's joint position - */ - Eigen::VectorXd GetValues() const override; - - /** - * @brief Gets the bounds on this variable - * @return Bounds on this variable - */ - VecBound GetBounds() const override; - - /** - * @brief Sets the bounds for the joints in this variable. - * @param new_bounds New bounds for the joints - */ - void SetBounds(VecBound& new_bounds); - - /** - * @brief Sets the bounds for the joints in this variable from a MatrixX2d with the first column being lower bound and - * second column being upper bound - * @param bounds Columns 1/2 are lower/upper bounds. You probably will get this from forward_kinematics->getLimits() - */ - void SetBounds(const Eigen::Ref& bounds); - - /** - * @brief Get the joint names associated with this variable set - * @return The joint names - */ - std::vector GetJointNames() const; - -private: - VecBound bounds_; - Eigen::VectorXd values_; - std::vector joint_names_; -}; - -} // namespace trajopt_ifopt - -#endif diff --git a/trajopt_ifopt/include/trajopt_ifopt/variable_sets/node.h b/trajopt_ifopt/include/trajopt_ifopt/variable_sets/node.h new file mode 100644 index 00000000..c1d5102d --- /dev/null +++ b/trajopt_ifopt/include/trajopt_ifopt/variable_sets/node.h @@ -0,0 +1,226 @@ +/** + * @author Levi Armstrong + * @date Jan 7, 2025 + * + * @copyright Copyright (c) 2025, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#ifndef TRAJOPT_IFOPT_NODE_H +#define TRAJOPT_IFOPT_NODE_H + +#include + +#include +#include +#include +#include + +namespace trajopt_ifopt +{ +class NodesVariables; +class Var; + +/** + * @brief Group of related variables in the trajopt_ifopt problem. + * + * A Node encapsulates one or more @ref Var instances that together form a + * contiguous segment (or set of segments) of the global decision vector. + * Each variable is identified by a string name and may itself be scalar or + * vector-valued. + * + * Typical usage: + * - Create a Node with a descriptive name (e.g. "manipulator_states"). + * - Add variables via @ref addVar(). + * - Query all values and bounds via @ref getValues() and @ref getBounds(). + * - Let @ref NodesVariables manage indexing and variable layout. + */ +class Node +{ +public: + /** + * @brief Construct a Node with an optional name. + * + * @param node_name Descriptive name for this node (for logging/debugging). + */ + explicit Node(std::string node_name = "Node"); + ~Node(); + Node(const Node&) = delete; + Node& operator=(const Node&) = delete; + Node(Node&&) = default; + Node& operator=(Node&&) = default; + + /** + * @brief Get the name of this node. + * + * @return Constant reference to the node name. + */ + const std::string& getName() const; + + /** + * @brief Get the parent NodesVariables container. + * + * The parent is responsible for managing the layout of this Node's variables + * in the global decision vector. + * + * @return Pointer to the parent NodesVariables, or nullptr if not attached. + */ + const NodesVariables* getParent() const; + + /** + * @brief Check whether this node already has a variable by the given name. + * + * @param name Variable identifier (as passed to @ref addVar()). + * @return True if a variable with the given name exists, false otherwise. + */ + bool hasVar(const std::string& name) const; + + /** + * @brief Add a scalar variable to this node. + * + * @param name Variable name (must be unique within this node). + * @param value Initial scalar value. + * @param bounds Bounds associated with the variable (default: unbounded). + * + * @return Shared pointer to the newly created @ref Var. The returned pointer + * is shared so it can be held by constraints or other components. + * + * @note The global index of the variable (within the full decision vector) + * is managed by @ref NodesVariables and will be updated via + * @ref incrementIndex() when nodes are assembled. + */ + std::shared_ptr addVar(const std::string& name, double value, ifopt::Bounds bounds = ifopt::NoBound); + + /** + * @brief Add a vector-valued variable to this node. + * + * @param name Variable name (identifier) for this block. + * @param child_names Per-element names (size must match @p values.size()). + * @param values Initial values for each element. + * @param bounds Per-element bounds (size must match @p values.size()). + * + * @return Shared pointer to the newly created @ref Var. + * + * This is used for multi-DOF or grouped variables (e.g., a joint group). + * The @p name identifies the block, while @p child_names provides more + * detailed labels per element. + */ + std::shared_ptr addVar(const std::string& name, + const std::vector& child_names, + const Eigen::VectorXd& values, + const std::vector& bounds); + + /** + * @brief Get a variable by name. + * + * @param name Variable name as passed to @ref addVar(). + * @return Shared pointer to the corresponding @ref Var, or nullptr if not found. + */ + std::shared_ptr getVar(const std::string& name) const; + + /** + * @brief Get all variable values for this node as a single vector. + * + * The returned vector is the concatenation of the values of all variables + * owned by this node, in the internal order of @ref vars_. + * + * @return Concatenated vector of all variable values in this node. + */ + Eigen::VectorXd getValues() const; + + /** + * @brief Get all variable bounds for this node as a single vector. + * + * The returned vector is the concatenation of the bounds for all variables + * owned by this node, in the same order as @ref getValues(). + * + * @return Concatenated vector of all bounds in this node. + */ + std::vector getBounds() const; + + /** + * @brief Get the total number of scalar decision variables in this node. + * + * @return The sum of the sizes of all @ref Var instances owned by this node. + */ + Eigen::Index size() const; + + /** + * @brief Update all variable values from the full decision vector. + * + * This function extracts the segments of @p x corresponding to this node's + * variables (based on their indices and sizes) and updates each @ref Var's + * internal value vector. + * + * @param x Full decision vector for the optimization problem. + * + * @note It is assumed that all variable segments managed by this node have + * already been assigned valid indices by @ref NodesVariables. + */ + void setVariables(const Eigen::Ref& x); + +protected: + friend class NodesVariables; + + /** + * @brief Descriptive name for this node. + */ + std::string name_; + + /** + * @brief Variables owned by this node. + * + * Each Var may represent a scalar or vector-valued block of the global + * decision vector. The order in this vector defines the order in + * @ref getValues() and @ref getBounds(). + */ + std::vector> vars_; + + /** + * @brief Lookup table from variable name to Var pointer. + * + * This allows fast access to variables by their string name. + */ + std::unordered_map> vars_by_name_; + + /** + * @brief Total number of scalar variables owned by this node. + * + * This is the sum of the sizes of all variables in @ref vars_. It is + * typically maintained by @ref NodesVariables when variables are added or + * when indices are updated. + */ + Eigen::Index length_{ 0 }; + + /** + * @brief Pointer to the parent NodesVariables container. + */ + NodesVariables* parent_{ nullptr }; + + /** + * @brief Increment the starting index of all variables in this node. + * + * This is used by @ref NodesVariables when constructing the global layout + * of the decision vector: after placing previous nodes, each subsequent + * node's variable indices must be shifted by the number of already-placed + * variables. + * + * @param value Offset by which to increment each variable's starting index. + */ + void incrementIndex(Eigen::Index value); +}; + +} // namespace trajopt_ifopt +#endif // TRAJOPT_IFOPT_NODE_H diff --git a/trajopt_ifopt/include/trajopt_ifopt/variable_sets/nodes_observer.h b/trajopt_ifopt/include/trajopt_ifopt/variable_sets/nodes_observer.h new file mode 100644 index 00000000..074b9818 --- /dev/null +++ b/trajopt_ifopt/include/trajopt_ifopt/variable_sets/nodes_observer.h @@ -0,0 +1,69 @@ +/****************************************************************************** +Copyright (c) 2018, Alexander W. Winkler. 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 the copyright holder 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 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. +******************************************************************************/ + +#ifndef TRAJOPT_IFOPT_NODES_OBSERVER_H +#define TRAJOPT_IFOPT_NODES_OBSERVER_H + +#include + +namespace trajopt_ifopt +{ +class NodesVariables; + +/** + * @brief Base class to receive up-to-date values of the NodeVariables. + * + * This class registers with the node variables and everytime a node changes, + * the subject updates this class by calling the UpdatePolynomials() method. + * + * Used by spline.h + * + * This class implements the observer pattern: + * https://sourcemaking.com/design_patterns/observer + */ +class NodesObserver : public std::enable_shared_from_this +{ +public: + /** + * @brief Registers this observer with the subject class to receive updates. + * @param subject The subject holding the Hermite node values. + */ + NodesObserver(std::weak_ptr subject); + virtual ~NodesObserver() = default; + + /** + * @brief Callback method called every time the subject changes. + */ + virtual void UpdateNodes() = 0; + +protected: + std::weak_ptr subject_; +}; +} // namespace trajopt_ifopt +#endif // TRAJOPT_IFOPT_NODES_OBSERVER_H diff --git a/trajopt_ifopt/include/trajopt_ifopt/variable_sets/nodes_variables.h b/trajopt_ifopt/include/trajopt_ifopt/variable_sets/nodes_variables.h new file mode 100644 index 00000000..5ff9fe64 --- /dev/null +++ b/trajopt_ifopt/include/trajopt_ifopt/variable_sets/nodes_variables.h @@ -0,0 +1,126 @@ +/****************************************************************************** +Copyright (c) 2018, Alexander W. Winkler. 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 the copyright holder 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 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. +******************************************************************************/ + +#ifndef TRAJOPT_IFOPT_NODES_VARIABLES_H +#define TRAJOPT_IFOPT_NODES_VARIABLES_H + +#include +TRAJOPT_IGNORE_WARNINGS_PUSH +#include +#include +#include +TRAJOPT_IGNORE_WARNINGS_POP + +#include + +namespace trajopt_ifopt +{ +class NodesObserver; + +class NodesVariables : public ifopt::VariableSet +{ +public: + using Ptr = std::shared_ptr; + + /** + * @param variable_name The name of the variables in the optimization problem. + */ + NodesVariables(const std::string& variable_name, std::vector> nodes); + ~NodesVariables() override = default; + + /** + * @brief Get node based on index + * @param opt_idx The node index + * @return The node + */ + std::shared_ptr GetNode(std::size_t opt_idx) const; + + /** + * @brief Pure optimization variables that define the nodes. + * + * Not all node position and velocities are independent or optimized over, so + * usually the number of optimization variables is less than all nodes' pos/vel. + * + * @sa GetNodeInfoAtOptIndex() + */ + VectorXd GetValues() const override; + + /** + * @brief Sets some node positions and velocity from the optimization variables. + * @param x The optimization variables. + * + * Not all node position and velocities are independent or optimized over, so + * usually the number of optimization variables is less than + * all nodes pos/vel. + * + * @sa GetNodeValuesInfo() + */ + void SetVariables(const VectorXd& x) override; + + /** + * @returns the bounds on position and velocity of each node and dimension. + */ + VecBound GetBounds() const override; + + /** + * @returns All the nodes that can be used to reconstruct the spline. + */ + std::vector> GetNodes() const; + + /** + * @brief Adds a dependent observer that gets notified when the nodes change. + * @param spline Usually a pointer to a spline which uses the node values. + */ + void AddObserver(std::shared_ptr observer); + + /** + * @returns The dimensions (x,y,z) of every node. + */ + Eigen::Index GetDim() const; + +protected: + Eigen::VectorXd values_; + VecBound bounds_; ///< the bounds on the node values. + std::vector> nodes_; + Eigen::Index n_dim_{ 0 }; + std::vector> observers_; + + /** + * @brief Add node to the variable set + * @param node The node to append + */ + void AddNode(std::unique_ptr node); + + /** @brief Notifies the subscribed observers that the node values changes. */ + void UpdateObservers(); +}; + +} // namespace trajopt_ifopt + +#endif // TRAJOPT_IFOPT_NODES_VARIABLES_H diff --git a/trajopt_ifopt/include/trajopt_ifopt/variable_sets/var.h b/trajopt_ifopt/include/trajopt_ifopt/variable_sets/var.h new file mode 100644 index 00000000..24b9221f --- /dev/null +++ b/trajopt_ifopt/include/trajopt_ifopt/variable_sets/var.h @@ -0,0 +1,247 @@ +/** + * @author Levi Armstrong + * @date Jan 7, 2025 + * + * @copyright Copyright (c) 2025, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#ifndef TRAJOPT_IFOPT_VAR_H +#define TRAJOPT_IFOPT_VAR_H + +#include +#include +#include + +namespace trajopt_ifopt +{ +class Node; +/** + * @brief Variable segment used by trajopt_ifopt. + * + * This class represents a *contiguous* block of decision variables belonging to + * a single @ref Node. It stores: + * + * - The starting index of its block within the full optimization vector + * - A stable identifier string for logging / debugging + * - Per-element names (for debugging / introspection) + * - The current values of the variables + * - Per-element bounds (ifopt::Bounds) + * - A pointer back to the owning Node + * + * Constraints should store @ref Var instances (or pointers/references to them) + * and use @ref getIndex(), @ref size(), and @ref value() to correctly fill + * Jacobian and residual entries. + * + * The class supports both scalar and vector-valued variables: + * - Scalar: use the single-value constructor + * - Vector: use the multi-value constructor + */ +class Var +{ +public: + Var() = default; + ~Var() = default; + + /** + * @brief Construct a scalar variable. + * + * @param index Starting index of this variable in the full decision vector. + * @param name Identifier for this variable (also used as its element name). + * @param value Initial value of the scalar variable. + * @param bounds Bounds associated with this scalar (default: unbounded). + * @param parent Owning node (may be nullptr if not attached yet). + * + * This constructor creates a variable segment of size 1. The @p name is used + * both as the identifier and as the single element name in @ref names_. + */ + Var(Eigen::Index index, + std::string name, + double value, + ifopt::Bounds bounds = ifopt::NoBound, + Node* parent = nullptr); + + /** + * @brief Construct a vector-valued variable. + * + * @param index Starting index of the first element in the full decision vector. + * @param identifier Stable identifier for this variable segment (e.g. joint group name). + * @param names Per-element names (size must match @p values.size()). + * @param values Initial values for each element of the variable segment. + * @param bounds Per-element bounds (size must match @p values.size()). + * @param parent Owning node (may be nullptr if not attached yet). + * + * This constructor creates a variable segment of size @c values.size(). The + * @p identifier is stored in @ref identifier_ and is intended for debugging + * and logging; the human-readable per-element names are stored in + * @ref names_. + */ + Var(Eigen::Index index, + std::string identifier, + std::vector names, + const Eigen::VectorXd& values, + const std::vector& bounds, + Node* parent = nullptr); + + Var(const Var& other) = default; + Var& operator=(const Var&) = default; + Var(Var&&) = default; + Var& operator=(Var&&) = default; + + /** + * @brief Get the identifier for the variable segment. + * + * The identifier is a stable label for this block of variables, typically used + * for logging, debugging, or grouping (e.g., a joint group name or task name). + * + * @return Constant reference to the identifier string. + */ + const std::string& getIdentifier() const; + + /** + * @brief Get the parent node. + * + * The parent node is the object that owns this @ref Var and is responsible + * for interpreting its values (e.g., mapping them to robot states). + * + * @return Pointer to the parent node, or nullptr if not attached. + */ + const Node* getParent() const; + + /** + * @brief Set the variable values from the full decision vector. + * + * This function extracts the subsegment of @p x that corresponds to this + * variable block, based on @ref index_ and @ref size(), and stores it in + * @ref values_. + * + * @param x The full decision vector for the optimization problem. + * + * @note It is the caller's responsibility to ensure that + * @c index_ + size() does not exceed @c x.size(). + */ + void setVariables(const Eigen::Ref& x); + + /** + * @brief Get the starting index of this variable segment. + * + * This is the index in the full decision vector where the first element of + * this variable block resides. + * + * @return The starting index within the full decision vector. + */ + Eigen::Index getIndex() const; + + /** + * @brief Get the number of elements in this variable segment. + * + * @return The number of scalar entries (1 for a scalar, N for a vector of length N). + */ + Eigen::Index size() const; + + /** + * @brief Get the current variable values. + * + * @return Constant reference to the internal value vector. + * + * The returned vector has length @ref size(). The i-th element corresponds to + * the decision variable at global index @c getIndex() + i. + */ + const Eigen::VectorXd& value() const; + + /** + * @brief Get the variable names. + * + * @return Constant reference to the per-element names. + * + * The size of the returned vector matches @ref size(). Each entry is a + * human-readable name associated with the corresponding element of + * @ref value(). + */ + const std::vector& name() const; + + /** + * @brief Get the variable bounds. + * + * @return Constant reference to the per-element bounds. + * + * The size of the returned vector matches @ref size(). Each entry is an + * ifopt::Bounds object describing lower and upper bounds for the + * corresponding element. + */ + const std::vector& bounds() const; + +private: + friend class Node; + + /** + * @brief Starting index of this segment in the full decision vector. + * + * A value of -1 indicates an uninitialized / invalid index. + */ + Eigen::Index index_{ -1 }; + + /** + * @brief Identifier for this variable segment. + * + * Intended primarily for logging and debugging. This does not need to be + * unique globally, but should be meaningful within the context of the parent + * node or problem. + */ + std::string identifier_; + + /** + * @brief Per-element variable names. + * + * This vector has the same length as @ref values_ and @ref bounds_. It + * provides human-readable labels for debugging, logging, and introspection. + */ + std::vector names_; + + /** + * @brief Current values of the variable segment. + * + * The size of this vector defines the dimension of the variable block. + */ + Eigen::VectorXd values_; + + /** + * @brief Per-element bounds for the variable segment. + * + * This vector has the same length as @ref values_. Each element is an + * ifopt::Bounds describing the lower and upper bounds of the corresponding + * variable. + */ + std::vector bounds_; + + /** + * @brief Pointer to the parent node that owns this variable. + */ + Node* parent_{ nullptr }; + + /** + * @brief Increment the starting index by the provided offset. + * + * This is typically used when variables are laid out sequentially in the full + * decision vector and a new block is appended before this one. + * + * @param value The offset by which to increment @ref index_. + */ + void incrementIndex(Eigen::Index value); +}; + +} // namespace trajopt_ifopt + +#endif // TRAJOPT_IFOPT_VAR_H diff --git a/trajopt_ifopt/src/constraints/cartesian_line_constraint.cpp b/trajopt_ifopt/src/constraints/cartesian_line_constraint.cpp index d0a6eb66..b2f12298 100644 --- a/trajopt_ifopt/src/constraints/cartesian_line_constraint.cpp +++ b/trajopt_ifopt/src/constraints/cartesian_line_constraint.cpp @@ -25,7 +25,9 @@ */ #include -#include +#include +#include +#include #include #include #include @@ -72,7 +74,7 @@ CartLineInfo::CartLineInfo(std::shared_ptr position_var, + std::shared_ptr position_var, const Eigen::VectorXd& coeffs, // NOLINT const std::string& name) : ifopt::ConstraintSet(static_cast(info.indices.rows()), name) @@ -134,11 +136,7 @@ Eigen::VectorXd CartLineConstraint::CalcValues(const Eigen::RefGetVariables()->GetComponent(position_var_->GetName())->GetValues(); - return CalcValues(joint_vals); -} +Eigen::VectorXd CartLineConstraint::GetValues() const { return CalcValues(position_var_->value()); } // Set the limits on the constraint values std::vector CartLineConstraint::GetBounds() const { return bounds_; } @@ -184,7 +182,7 @@ void CartLineConstraint::CalcJacobianBlock(const Eigen::RefgetIndex() + j, coeffs_(i) * jac0.coeffRef(info_.indices[i], j)); } } } @@ -242,7 +240,7 @@ void CartLineConstraint::CalcJacobianBlock(const Eigen::RefgetIndex() + j, coeffs_(i) * jac0(info_.indices[i], j)); } } } @@ -252,12 +250,11 @@ void CartLineConstraint::CalcJacobianBlock(const Eigen::RefGetName()) // NOLINT + if (var_set != position_var_->getParent()->getParent()->GetName()) // NOLINT return; // Get current joint values and calculate jacobian - const Eigen::VectorXd joint_vals = this->GetVariables()->GetComponent(position_var_->GetName())->GetValues(); - CalcJacobianBlock(joint_vals, jac_block); // NOLINT + CalcJacobianBlock(position_var_->value(), jac_block); // NOLINT } std::pair CartLineConstraint::GetLine() const diff --git a/trajopt_ifopt/src/constraints/cartesian_position_constraint.cpp b/trajopt_ifopt/src/constraints/cartesian_position_constraint.cpp index 53c14ba6..d08273c5 100644 --- a/trajopt_ifopt/src/constraints/cartesian_position_constraint.cpp +++ b/trajopt_ifopt/src/constraints/cartesian_position_constraint.cpp @@ -23,7 +23,10 @@ * limitations under the License. */ #include -#include +#include +#include +#include + #include #include #include @@ -76,7 +79,7 @@ CartPosInfo::CartPosInfo(std::shared_ptr thread_local tesseract_common::TransformMap CartPosConstraint::transforms_cache; // NOLINT CartPosConstraint::CartPosConstraint(CartPosInfo info, - std::shared_ptr position_var, + std::shared_ptr position_var, const Eigen::VectorXd& coeffs, // NOLINT const std::string& name) : ifopt::ConstraintSet(static_cast(info.indices.rows()), name) @@ -197,7 +200,7 @@ CartPosConstraint::CartPosConstraint(CartPosInfo info, } CartPosConstraint::CartPosConstraint(const CartPosInfo& info, - std::shared_ptr position_var, + std::shared_ptr position_var, const std::string& name) : CartPosConstraint(info, std::move(position_var), Eigen::VectorXd::Ones(info.indices.rows()), name) { @@ -215,11 +218,7 @@ Eigen::VectorXd CartPosConstraint::CalcValues(const Eigen::RefGetVariables()->GetComponent(position_var_->GetName())->GetValues(); - return CalcValues(joint_vals); -} +Eigen::VectorXd CartPosConstraint::GetValues() const { return CalcValues(position_var_->value()); } // Set the limits on the constraint values std::vector CartPosConstraint::GetBounds() const { return bounds_; } @@ -260,7 +259,7 @@ void CartPosConstraint::CalcJacobianBlock(const Eigen::RefgetIndex() + j, coeffs_(i) * jac0(i, j)); } } } @@ -321,7 +320,7 @@ void CartPosConstraint::CalcJacobianBlock(const Eigen::RefgetIndex() + j, coeffs_(i) * jac0(info_.indices[i], j)); } } } @@ -331,12 +330,11 @@ void CartPosConstraint::CalcJacobianBlock(const Eigen::RefGetName()) // NOLINT + if (var_set != position_var_->getParent()->getParent()->GetName()) // NOLINT return; // Get current joint values and calculate jacobian - const VectorXd joint_vals = this->GetVariables()->GetComponent(position_var_->GetName())->GetValues(); - CalcJacobianBlock(joint_vals, jac_block); // NOLINT + CalcJacobianBlock(position_var_->value(), jac_block); // NOLINT } const CartPosInfo& CartPosConstraint::GetInfo() const { return info_; } @@ -352,8 +350,7 @@ Eigen::Isometry3d CartPosConstraint::GetTargetPose() const { return info_.target Eigen::Isometry3d CartPosConstraint::GetCurrentPose() const { transforms_cache.clear(); - const VectorXd joint_vals = this->GetVariables()->GetComponent(position_var_->GetName())->GetValues(); - info_.manip->calcFwdKin(transforms_cache, joint_vals); + info_.manip->calcFwdKin(transforms_cache, position_var_->value()); return transforms_cache[info_.source_frame] * info_.source_frame_offset; } diff --git a/trajopt_ifopt/src/constraints/collision/continuous_collision_constraint.cpp b/trajopt_ifopt/src/constraints/collision/continuous_collision_constraint.cpp index e2673836..5c4799c6 100644 --- a/trajopt_ifopt/src/constraints/collision/continuous_collision_constraint.cpp +++ b/trajopt_ifopt/src/constraints/collision/continuous_collision_constraint.cpp @@ -31,13 +31,15 @@ TRAJOPT_IGNORE_WARNINGS_POP #include #include #include -#include +#include +#include +#include namespace trajopt_ifopt { ContinuousCollisionConstraint::ContinuousCollisionConstraint( std::shared_ptr collision_evaluator, - std::array, 2> position_vars, + std::array, 2> position_vars, bool vars0_fixed, bool vars1_fixed, int max_num_cnt, @@ -48,16 +50,17 @@ ContinuousCollisionConstraint::ContinuousCollisionConstraint( , vars0_fixed_(vars0_fixed) , vars1_fixed_(vars1_fixed) , collision_evaluator_(std::move(collision_evaluator)) + , fixed_sparsity_(fixed_sparsity) { if (position_vars_[0] == nullptr && position_vars_[1] == nullptr) throw std::runtime_error("position_vars contains a nullptr!"); // Set n_dof_ for convenience - n_dof_ = position_vars_[0]->GetRows(); + n_dof_ = position_vars_[0]->size(); if (!(n_dof_ > 0)) throw std::runtime_error("position_vars[0] is empty!"); - if (position_vars_[0]->GetRows() != position_vars_[1]->GetRows()) + if (position_vars_[0]->size() != position_vars_[1]->size()) throw std::runtime_error("position_vars are not the same size!"); if (vars0_fixed_ && vars1_fixed_) @@ -67,29 +70,15 @@ ContinuousCollisionConstraint::ContinuousCollisionConstraint( throw std::runtime_error("max_num_cnt must be greater than zero!"); bounds_ = std::vector(static_cast(max_num_cnt), ifopt::BoundSmallerZero); - - if (fixed_sparsity) - { - // Setting to zeros because snopt sparsity cannot change - triplet_list_.reserve(static_cast(bounds_.size()) * - static_cast(position_vars_[0]->GetRows())); - - for (Eigen::Index i = 0; i < static_cast(bounds_.size()); i++) // NOLINT - for (Eigen::Index j = 0; j < n_dof_; j++) - triplet_list_.emplace_back(i, j, 0); - } } Eigen::VectorXd ContinuousCollisionConstraint::GetValues() const { - // Get current joint values - const Eigen::VectorXd joint_vals0 = this->GetVariables()->GetComponent(position_vars_[0]->GetName())->GetValues(); - const Eigen::VectorXd joint_vals1 = this->GetVariables()->GetComponent(position_vars_[1]->GetName())->GetValues(); const double margin_buffer = collision_evaluator_->GetCollisionMarginBuffer(); Eigen::VectorXd values = Eigen::VectorXd::Constant(static_cast(bounds_.size()), -margin_buffer); - auto collision_data = - collision_evaluator_->CalcCollisionData(joint_vals0, joint_vals1, vars0_fixed_, vars1_fixed_, bounds_.size()); + auto collision_data = collision_evaluator_->CalcCollisionData( + position_vars_[0]->value(), position_vars_[1]->value(), vars0_fixed_, vars1_fixed_, bounds_.size()); if (collision_data->gradient_results_sets.empty()) return values; @@ -128,29 +117,47 @@ Eigen::VectorXd ContinuousCollisionConstraint::GetValues() const // Set the limits on the constraint values std::vector ContinuousCollisionConstraint::GetBounds() const { return bounds_; } +void ContinuousCollisionConstraint::initSparsity() const +{ + if (!fixed_sparsity_) + return; + + // Setting to zeros because snopt sparsity cannot change + triplet_list_.reserve(static_cast(bounds_.size()) * static_cast(position_vars_[0]->size())); + + for (Eigen::Index i = 0; i < static_cast(bounds_.size()); i++) // NOLINT + { + for (Eigen::Index j = 0; j < n_dof_; j++) + { + triplet_list_.emplace_back(i, position_vars_[0]->getIndex() + j, 0); + triplet_list_.emplace_back(i, position_vars_[1]->getIndex() + j, 0); + } + } +} + void ContinuousCollisionConstraint::FillJacobianBlock(std::string var_set, Jacobian& jac_block) const { // Only modify the jacobian if this constraint uses var_set - if (var_set != position_vars_[0]->GetName() && var_set != position_vars_[1]->GetName()) // NOLINT + if (var_set != position_vars_.front()->getParent()->getParent()->GetName()) // NOLINT return; + // Setting to zeros because snopt sparsity cannot change + std::call_once(init_flag_, &ContinuousCollisionConstraint::initSparsity, this); + // Setting to zeros because snopt sparsity cannot change if (!triplet_list_.empty()) // NOLINT jac_block.setFromTriplets(triplet_list_.begin(), triplet_list_.end()); // NOLINT // Calculate collisions - const Eigen::VectorXd joint_vals0 = this->GetVariables()->GetComponent(position_vars_[0]->GetName())->GetValues(); - const Eigen::VectorXd joint_vals1 = this->GetVariables()->GetComponent(position_vars_[1]->GetName())->GetValues(); - - auto collision_data = - collision_evaluator_->CalcCollisionData(joint_vals0, joint_vals1, vars0_fixed_, vars1_fixed_, bounds_.size()); + auto collision_data = collision_evaluator_->CalcCollisionData( + position_vars_[0]->value(), position_vars_[1]->value(), vars0_fixed_, vars1_fixed_, bounds_.size()); if (collision_data->gradient_results_sets.empty()) return; /** @todo Should use triplet list and setFromTriplets */ const std::size_t cnt = std::min(collision_data->gradient_results_sets.size(), bounds_.size()); - if (var_set == position_vars_[0]->GetName() && !vars0_fixed_) + if (!vars0_fixed_) { for (std::size_t i = 0; i < cnt; ++i) { @@ -161,15 +168,16 @@ void ContinuousCollisionConstraint::FillJacobianBlock(std::string var_set, Jacob if (!vars1_fixed_) max_error_with_buffer = r.getMaxErrorWithBuffer(); - Eigen::VectorXd grad_vec = getWeightedAvgGradientT0(r, max_error_with_buffer, position_vars_[0]->GetRows()); + Eigen::VectorXd grad_vec = getWeightedAvgGradientT0(r, max_error_with_buffer, position_vars_[0]->size()); // Collision is 1 x n_dof for (int j = 0; j < n_dof_; j++) - jac_block.coeffRef(static_cast(i), j) = -1.0 * grad_vec[j]; + jac_block.coeffRef(static_cast(i), position_vars_[0]->getIndex() + j) = -1.0 * grad_vec[j]; } } } - else if (var_set == position_vars_[1]->GetName() && !vars1_fixed_) + + if (!vars1_fixed_) { for (std::size_t i = 0; i < cnt; ++i) { @@ -180,11 +188,11 @@ void ContinuousCollisionConstraint::FillJacobianBlock(std::string var_set, Jacob if (!vars0_fixed_) max_error_with_buffer = r.getMaxErrorWithBuffer(); - Eigen::VectorXd grad_vec = getWeightedAvgGradientT1(r, max_error_with_buffer, position_vars_[1]->GetRows()); + Eigen::VectorXd grad_vec = getWeightedAvgGradientT1(r, max_error_with_buffer, position_vars_[1]->size()); // Collision is 1 x n_dof for (int j = 0; j < n_dof_; j++) - jac_block.coeffRef(static_cast(i), j) = -1.0 * grad_vec[j]; + jac_block.coeffRef(static_cast(i), position_vars_[1]->getIndex() + j) = -1.0 * grad_vec[j]; } } } diff --git a/trajopt_ifopt/src/constraints/collision/continuous_collision_numerical_constraint.cpp b/trajopt_ifopt/src/constraints/collision/continuous_collision_numerical_constraint.cpp index 1cda5e59..d7106045 100644 --- a/trajopt_ifopt/src/constraints/collision/continuous_collision_numerical_constraint.cpp +++ b/trajopt_ifopt/src/constraints/collision/continuous_collision_numerical_constraint.cpp @@ -30,14 +30,16 @@ TRAJOPT_IGNORE_WARNINGS_POP #include #include -#include +#include +#include +#include #include namespace trajopt_ifopt { ContinuousCollisionNumericalConstraint::ContinuousCollisionNumericalConstraint( std::shared_ptr collision_evaluator, - std::array, 2> position_vars, + std::array, 2> position_vars, bool vars0_fixed, bool vars1_fixed, int max_num_cnt, @@ -53,11 +55,11 @@ ContinuousCollisionNumericalConstraint::ContinuousCollisionNumericalConstraint( throw std::runtime_error("position_vars contains a nullptr!"); // Set n_dof_ for convenience - n_dof_ = position_vars_[0]->GetRows(); + n_dof_ = position_vars_[0]->size(); if (!(n_dof_ > 0)) throw std::runtime_error("position_vars[0] is empty!"); - if (position_vars_[0]->GetRows() != position_vars_[1]->GetRows()) + if (position_vars_[0]->size() != position_vars_[1]->size()) throw std::runtime_error("position_vars are not the same size!"); if (vars0_fixed_ && vars1_fixed_) @@ -72,24 +74,25 @@ ContinuousCollisionNumericalConstraint::ContinuousCollisionNumericalConstraint( { // Setting to zeros because snopt sparsity cannot change triplet_list_.reserve(static_cast(bounds_.size()) * - static_cast(position_vars_[0]->GetRows())); + static_cast(position_vars_[0]->size())); for (Eigen::Index i = 0; i < static_cast(bounds_.size()); i++) // NOLINT for (Eigen::Index j = 0; j < n_dof_; j++) - triplet_list_.emplace_back(i, j, 0); + { + triplet_list_.emplace_back(i, position_vars_[0]->getIndex() + j, 0); + triplet_list_.emplace_back(i, position_vars_[1]->getIndex() + j, 0); + } } } Eigen::VectorXd ContinuousCollisionNumericalConstraint::GetValues() const { // Get current joint values - const Eigen::VectorXd joint_vals0 = this->GetVariables()->GetComponent(position_vars_[0]->GetName())->GetValues(); - const Eigen::VectorXd joint_vals1 = this->GetVariables()->GetComponent(position_vars_[1]->GetName())->GetValues(); const double margin_buffer = collision_evaluator_->GetCollisionMarginBuffer(); Eigen::VectorXd values = Eigen::VectorXd::Constant(static_cast(bounds_.size()), -margin_buffer); - auto collision_data = - collision_evaluator_->CalcCollisionData(joint_vals0, joint_vals1, vars0_fixed_, vars1_fixed_, bounds_.size()); + auto collision_data = collision_evaluator_->CalcCollisionData( + position_vars_[0]->value(), position_vars_[1]->value(), vars0_fixed_, vars1_fixed_, bounds_.size()); if (collision_data->gradient_results_sets.empty()) return values; @@ -130,7 +133,7 @@ std::vector ContinuousCollisionNumericalConstraint::GetBounds() c void ContinuousCollisionNumericalConstraint::FillJacobianBlock(std::string var_set, Jacobian& jac_block) const { // Only modify the jacobian if this constraint uses var_set - if (var_set != position_vars_[0]->GetName() && var_set != position_vars_[1]->GetName()) // NOLINT + if (var_set != position_vars_[0]->getParent()->getParent()->GetName()) // NOLINT return; // Setting to zeros because snopt sparsity cannot change @@ -140,8 +143,8 @@ void ContinuousCollisionNumericalConstraint::FillJacobianBlock(std::string var_s const double margin_buffer = collision_evaluator_->GetCollisionMarginBuffer(); // Calculate collisions - Eigen::VectorXd joint_vals0 = this->GetVariables()->GetComponent(position_vars_[0]->GetName())->GetValues(); - const Eigen::VectorXd joint_vals1 = this->GetVariables()->GetComponent(position_vars_[1]->GetName())->GetValues(); + Eigen::VectorXd joint_vals0 = position_vars_[0]->value(); + const Eigen::VectorXd joint_vals1 = position_vars_[1]->value(); auto collision_data = collision_evaluator_->CalcCollisionData(joint_vals0, joint_vals1, vars0_fixed_, vars1_fixed_, bounds_.size()); @@ -149,49 +152,51 @@ void ContinuousCollisionNumericalConstraint::FillJacobianBlock(std::string var_s return; const std::size_t cnt = std::min(bounds_.size(), collision_data->gradient_results_sets.size()); - - Eigen::VectorXd jv = joint_vals0; const double delta = 1e-8; - for (int j = 0; j < n_dof_; j++) + for (std::size_t p = 0; p < 2; p++) { - jv(j) = joint_vals0(j) + delta; - auto collision_data_delta = - collision_evaluator_->CalcCollisionData(jv, joint_vals1, vars0_fixed_, vars1_fixed_, bounds_.size()); - - for (int i = 0; i < static_cast(cnt); ++i) + Eigen::VectorXd jv = position_vars_[p]->value(); + for (int j = 0; j < n_dof_; j++) { - const auto& baseline = collision_data->gradient_results_sets[static_cast(i)]; - auto fn = [&baseline](const trajopt_common::GradientResultsSet& cr) { - return (cr.key == baseline.key && cr.shape_key == baseline.shape_key); - }; - auto it = std::find_if( - collision_data_delta->gradient_results_sets.begin(), collision_data_delta->gradient_results_sets.end(), fn); - if (it != collision_data_delta->gradient_results_sets.end()) - { - double dist_delta{ 0 }; - if (!vars0_fixed_ && !vars1_fixed_) - dist_delta = it->coeff * (it->getMaxError() - baseline.getMaxError()); - else if (!vars0_fixed_) - dist_delta = it->coeff * (it->getMaxErrorT0() - baseline.getMaxErrorT0()); - else - dist_delta = it->coeff * (it->getMaxErrorT1() - baseline.getMaxErrorT1()); + jv(j) += delta; + auto collision_data_delta = + collision_evaluator_->CalcCollisionData(jv, joint_vals1, vars0_fixed_, vars1_fixed_, bounds_.size()); - jac_block.coeffRef(i, j) = dist_delta / delta; - } - else + for (int i = 0; i < static_cast(cnt); ++i) { - double dist_delta{ 0 }; - if (!vars0_fixed_ && !vars1_fixed_) - dist_delta = baseline.coeff * ((-1.0 * margin_buffer) - baseline.getMaxError()); - else if (!vars0_fixed_) - dist_delta = baseline.coeff * ((-1.0 * margin_buffer) - baseline.getMaxErrorT0()); + const auto& baseline = collision_data->gradient_results_sets[static_cast(i)]; + auto fn = [&baseline](const trajopt_common::GradientResultsSet& cr) { + return (cr.key == baseline.key && cr.shape_key == baseline.shape_key); + }; + auto it = std::find_if( + collision_data_delta->gradient_results_sets.begin(), collision_data_delta->gradient_results_sets.end(), fn); + if (it != collision_data_delta->gradient_results_sets.end()) + { + double dist_delta{ 0 }; + if (!vars0_fixed_ && !vars1_fixed_) + dist_delta = it->coeff * (it->getMaxError() - baseline.getMaxError()); + else if (!vars0_fixed_) + dist_delta = it->coeff * (it->getMaxErrorT0() - baseline.getMaxErrorT0()); + else + dist_delta = it->coeff * (it->getMaxErrorT1() - baseline.getMaxErrorT1()); + + jac_block.coeffRef(i, position_vars_[p]->getIndex() + j) = dist_delta / delta; + } else - dist_delta = baseline.coeff * ((-1.0 * margin_buffer) - baseline.getMaxErrorT1()); - - jac_block.coeffRef(i, j) = dist_delta / delta; + { + double dist_delta{ 0 }; + if (!vars0_fixed_ && !vars1_fixed_) + dist_delta = baseline.coeff * ((-1.0 * margin_buffer) - baseline.getMaxError()); + else if (!vars0_fixed_) + dist_delta = baseline.coeff * ((-1.0 * margin_buffer) - baseline.getMaxErrorT0()); + else + dist_delta = baseline.coeff * ((-1.0 * margin_buffer) - baseline.getMaxErrorT1()); + + jac_block.coeffRef(i, position_vars_[p]->getIndex() + j) = dist_delta / delta; + } } + jv = position_vars_[p]->value(); } - jv(j) = joint_vals0(j); } } diff --git a/trajopt_ifopt/src/constraints/collision/discrete_collision_constraint.cpp b/trajopt_ifopt/src/constraints/collision/discrete_collision_constraint.cpp index c311e51a..c14cee61 100644 --- a/trajopt_ifopt/src/constraints/collision/discrete_collision_constraint.cpp +++ b/trajopt_ifopt/src/constraints/collision/discrete_collision_constraint.cpp @@ -31,48 +31,36 @@ TRAJOPT_IGNORE_WARNINGS_POP #include #include #include -#include +#include +#include +#include + #include namespace trajopt_ifopt { DiscreteCollisionConstraint::DiscreteCollisionConstraint( std::shared_ptr collision_evaluator, - std::shared_ptr position_var, + std::shared_ptr position_var, int max_num_cnt, bool fixed_sparsity, const std::string& name) : ifopt::ConstraintSet(max_num_cnt, name) , position_var_(std::move(position_var)) , collision_evaluator_(std::move(collision_evaluator)) + , fixed_sparsity_(fixed_sparsity) { // Set n_dof_ for convenience - n_dof_ = position_var_->GetRows(); + n_dof_ = position_var_->size(); assert(n_dof_ > 0); if (max_num_cnt < 1) throw std::runtime_error("max_num_cnt must be greater than zero!"); bounds_ = std::vector(static_cast(max_num_cnt), ifopt::BoundSmallerZero); - - // Setting to zeros because snopt sparsity cannot change - if (fixed_sparsity) - { - triplet_list_.reserve(static_cast(bounds_.size()) * - static_cast(position_var_->GetRows())); - for (Eigen::Index i = 0; i < static_cast(bounds_.size()); i++) - for (Eigen::Index j = 0; j < n_dof_; j++) - triplet_list_.emplace_back(i, j, 0); - } } -Eigen::VectorXd DiscreteCollisionConstraint::GetValues() const -{ - // Get current joint values - const Eigen::VectorXd joint_vals = this->GetVariables()->GetComponent(position_var_->GetName())->GetValues(); - - return CalcValues(joint_vals); -} +Eigen::VectorXd DiscreteCollisionConstraint::GetValues() const { return CalcValues(position_var_->value()); } // Set the limits on the constraint values std::vector DiscreteCollisionConstraint::GetBounds() const { return bounds_; } @@ -80,13 +68,10 @@ std::vector DiscreteCollisionConstraint::GetBounds() const { retu void DiscreteCollisionConstraint::FillJacobianBlock(std::string var_set, Jacobian& jac_block) const { // Only modify the jacobian if this constraint uses var_set - if (var_set != position_var_->GetName()) // NOLINT + if (var_set != position_var_->getParent()->getParent()->GetName()) // NOLINT return; - // Get current joint values - const VectorXd joint_vals = this->GetVariables()->GetComponent(position_var_->GetName())->GetValues(); - - CalcJacobianBlock(joint_vals, jac_block); // NOLINT + CalcJacobianBlock(position_var_->value(), jac_block); // NOLINT } Eigen::VectorXd DiscreteCollisionConstraint::CalcValues(const Eigen::Ref& joint_vals) const @@ -116,9 +101,23 @@ void DiscreteCollisionConstraint::SetBounds(const std::vector& bo bounds_ = bounds; } +void DiscreteCollisionConstraint::initSparsity() const +{ + if (!fixed_sparsity_) + return; + + triplet_list_.reserve(static_cast(bounds_.size()) * static_cast(position_var_->size())); + for (Eigen::Index i = 0; i < static_cast(bounds_.size()); i++) + for (Eigen::Index j = 0; j < n_dof_; j++) + triplet_list_.emplace_back(i, position_var_->getIndex() + j, 0); +} + void DiscreteCollisionConstraint::CalcJacobianBlock(const Eigen::Ref& joint_vals, Jacobian& jac_block) const { + // Setting to zeros because snopt sparsity cannot change + std::call_once(init_flag_, &DiscreteCollisionConstraint::initSparsity, this); + // Setting to zeros because snopt sparsity cannot change if (!triplet_list_.empty()) // NOLINT jac_block.setFromTriplets(triplet_list_.begin(), triplet_list_.end()); // NOLINT @@ -133,11 +132,11 @@ void DiscreteCollisionConstraint::CalcJacobianBlock(const Eigen::Refgradient_results_sets[i]; - Eigen::VectorXd grad_vec = getWeightedAvgGradientT0(r, r.getMaxErrorWithBufferT0(), position_var_->GetRows()); + Eigen::VectorXd grad_vec = getWeightedAvgGradientT0(r, r.getMaxErrorWithBufferT0(), position_var_->size()); // Collision is 1 x n_dof for (int j = 0; j < n_dof_; j++) - jac_block.coeffRef(static_cast(i), j) = -1.0 * grad_vec[j]; + jac_block.coeffRef(static_cast(i), position_var_->getIndex() + j) = -1.0 * grad_vec[j]; } } diff --git a/trajopt_ifopt/src/constraints/collision/discrete_collision_numerical_constraint.cpp b/trajopt_ifopt/src/constraints/collision/discrete_collision_numerical_constraint.cpp index 2ecb40c2..1e7daaf3 100644 --- a/trajopt_ifopt/src/constraints/collision/discrete_collision_numerical_constraint.cpp +++ b/trajopt_ifopt/src/constraints/collision/discrete_collision_numerical_constraint.cpp @@ -31,62 +31,60 @@ TRAJOPT_IGNORE_WARNINGS_POP #include #include #include -#include +#include +#include +#include #include namespace trajopt_ifopt { DiscreteCollisionNumericalConstraint::DiscreteCollisionNumericalConstraint( std::shared_ptr collision_evaluator, - std::shared_ptr position_var, + std::shared_ptr position_var, int max_num_cnt, bool fixed_sparsity, const std::string& name) : ifopt::ConstraintSet(max_num_cnt, name) , position_var_(std::move(position_var)) , collision_evaluator_(std::move(collision_evaluator)) + , fixed_sparsity_(fixed_sparsity) { // Set n_dof_ for convenience - n_dof_ = position_var_->GetRows(); + n_dof_ = position_var_->size(); assert(n_dof_ > 0); if (max_num_cnt < 1) throw std::runtime_error("max_num_cnt must be greater than zero!"); bounds_ = std::vector(static_cast(max_num_cnt), ifopt::BoundSmallerZero); - - // Setting to zeros because snopt sparsity cannot change - if (fixed_sparsity) - { - triplet_list_.reserve(static_cast(bounds_.size()) * - static_cast(position_var_->GetRows())); - for (Eigen::Index i = 0; i < static_cast(bounds_.size()); i++) - for (Eigen::Index j = 0; j < n_dof_; j++) - triplet_list_.emplace_back(i, j, 0); - } } -Eigen::VectorXd DiscreteCollisionNumericalConstraint::GetValues() const -{ - // Get current joint values - const Eigen::VectorXd joint_vals = this->GetVariables()->GetComponent(position_var_->GetName())->GetValues(); - - return CalcValues(joint_vals); -} +Eigen::VectorXd DiscreteCollisionNumericalConstraint::GetValues() const { return CalcValues(position_var_->value()); } // Set the limits on the constraint values std::vector DiscreteCollisionNumericalConstraint::GetBounds() const { return bounds_; } +void DiscreteCollisionNumericalConstraint::initSparsity() const +{ + if (!fixed_sparsity_) + return; + + triplet_list_.reserve(static_cast(bounds_.size()) * static_cast(position_var_->size())); + for (Eigen::Index i = 0; i < static_cast(bounds_.size()); i++) + for (Eigen::Index j = 0; j < n_dof_; j++) + triplet_list_.emplace_back(i, position_var_->getIndex() + j, 0); +} + void DiscreteCollisionNumericalConstraint::FillJacobianBlock(std::string var_set, Jacobian& jac_block) const { // Only modify the jacobian if this constraint uses var_set - if (var_set != position_var_->GetName()) // NOLINT + if (var_set != position_var_->getParent()->getParent()->GetName()) // NOLINT return; - // Get current joint values - const VectorXd joint_vals = this->GetVariables()->GetComponent(position_var_->GetName())->GetValues(); + // Setting to zeros because snopt sparsity cannot change + std::call_once(init_flag_, &DiscreteCollisionNumericalConstraint::initSparsity, this); - CalcJacobianBlock(joint_vals, jac_block); // NOLINT + CalcJacobianBlock(position_var_->value(), jac_block); // NOLINT } Eigen::VectorXd @@ -149,12 +147,12 @@ void DiscreteCollisionNumericalConstraint::CalcJacobianBlock(const Eigen::Refgradient_results_sets.end()) { const double dist_delta = baseline.coeff * (it->getMaxErrorT0() - baseline.getMaxErrorT0()); - jac_block.coeffRef(i, j) = dist_delta / delta; + jac_block.coeffRef(i, position_var_->getIndex() + j) = dist_delta / delta; } else { const double dist_delta = baseline.coeff * ((-1.0 * margin_buffer) - baseline.getMaxErrorT0()); - jac_block.coeffRef(i, j) = dist_delta / delta; + jac_block.coeffRef(i, position_var_->getIndex() + j) = dist_delta / delta; } } jv(j) = joint_vals(j); @@ -168,7 +166,7 @@ void DiscreteCollisionNumericalConstraint::CalcJacobianBlock(const Eigen::RefGetRows()); // for (int j = 0; j < n_dof_; j++) - // jac_block_debug.coeffRef(static_cast(i), j) = -1.0 * grad_vec[j]; + // jac_block_debug.coeffRef(static_cast(i), position_var_->getIndex() + j) = -1.0 * grad_vec[j]; // ++i; // } // std::cout << "Numerical Jacobian:" << '\n' << jac_block << '\n'; diff --git a/trajopt_ifopt/src/constraints/inverse_kinematics_constraint.cpp b/trajopt_ifopt/src/constraints/inverse_kinematics_constraint.cpp index 60d33dec..3d69f666 100644 --- a/trajopt_ifopt/src/constraints/inverse_kinematics_constraint.cpp +++ b/trajopt_ifopt/src/constraints/inverse_kinematics_constraint.cpp @@ -22,7 +22,9 @@ * limitations under the License. */ #include -#include +#include +#include +#include TRAJOPT_IGNORE_WARNINGS_PUSH #include @@ -47,19 +49,19 @@ InverseKinematicsInfo::InverseKinematicsInfo(std::shared_ptr constraint_var, - std::shared_ptr seed_var, + std::shared_ptr constraint_var, + std::shared_ptr seed_var, const std::string& name) - : ifopt::ConstraintSet(constraint_var->GetRows(), name) + : ifopt::ConstraintSet(static_cast(constraint_var->size()), name) , constraint_var_(std::move(constraint_var)) , seed_var_(std::move(seed_var)) , target_pose_(target_pose) , kinematic_info_(std::move(kinematic_info)) { // Set the n_dof and n_vars for convenience - n_dof_ = constraint_var_->GetRows(); + n_dof_ = constraint_var_->size(); assert(n_dof_ > 0); - if (constraint_var_->GetRows() != kinematic_info_->manip->numJoints()) + if (constraint_var_->size() != kinematic_info_->manip->numJoints()) CONSOLE_BRIDGE_logError("Inverse kinematics has a different number of joints than the given variable set"); bounds_ = std::vector(static_cast(n_dof_), ifopt::BoundZero); @@ -95,11 +97,7 @@ InverseKinematicsConstraint::CalcValues(const Eigen::Ref& Eigen::VectorXd InverseKinematicsConstraint::GetValues() const { - // Get the two variables - const Eigen::VectorXd seed_joint_position = this->GetVariables()->GetComponent(seed_var_->GetName())->GetValues(); - const Eigen::VectorXd joint_vals = this->GetVariables()->GetComponent(constraint_var_->GetName())->GetValues(); - - return CalcValues(joint_vals, seed_joint_position); + return CalcValues(constraint_var_->value(), seed_var_->value()); } // Set the limits on the constraint values @@ -116,7 +114,7 @@ void InverseKinematicsConstraint::SetBounds(const std::vector& bo void InverseKinematicsConstraint::FillJacobianBlock(std::string var_set, Jacobian& jac_block) const { // Only modify the jacobian if this constraint uses var_set - if (var_set != constraint_var_->GetName()) // NOLINT + if (var_set != constraint_var_->getParent()->getParent()->GetName()) // NOLINT return; std::vector > triplet_list; @@ -124,7 +122,7 @@ void InverseKinematicsConstraint::FillJacobianBlock(std::string var_set, Jacobia // err = target - x =? derr/dx = -1 for (int j = 0; j < n_dof_; j++) // NOLINT - triplet_list.emplace_back(j, j, -1); + triplet_list.emplace_back(j, constraint_var_->getIndex() + j, -1); jac_block.setFromTriplets(triplet_list.begin(), triplet_list.end()); // NOLINT } diff --git a/trajopt_ifopt/src/constraints/joint_acceleration_constraint.cpp b/trajopt_ifopt/src/constraints/joint_acceleration_constraint.cpp index fd5dbb85..cda80533 100644 --- a/trajopt_ifopt/src/constraints/joint_acceleration_constraint.cpp +++ b/trajopt_ifopt/src/constraints/joint_acceleration_constraint.cpp @@ -22,7 +22,9 @@ * limitations under the License. */ #include -#include +#include +#include +#include TRAJOPT_IGNORE_WARNINGS_PUSH #include @@ -31,7 +33,7 @@ TRAJOPT_IGNORE_WARNINGS_POP namespace trajopt_ifopt { JointAccelConstraint::JointAccelConstraint(const Eigen::VectorXd& targets, - const std::vector >& position_vars, + const std::vector >& position_vars, const Eigen::VectorXd& coeffs, const std::string& name) : ifopt::ConstraintSet(static_cast(targets.size()) * static_cast(position_vars.size()), name) @@ -46,14 +48,13 @@ JointAccelConstraint::JointAccelConstraint(const Eigen::VectorXd& targets, // Check and make sure the targets size aligns with the vars passed in for (const auto& position_var : position_vars_) { - if (targets.size() != position_var->GetRows()) + if (targets.size() != position_var->size()) CONSOLE_BRIDGE_logError("Targets size does not align with variables provided"); } // Set n_dof and n_vars assert(n_dof_ > 0); assert(n_vars_ > 0); - // assert(n_vars_ == 2); if (!(coeffs_.array() > 0).all()) throw std::runtime_error("JointAccelConstraint, coeff must be greater than zero."); @@ -65,15 +66,12 @@ JointAccelConstraint::JointAccelConstraint(const Eigen::VectorXd& targets, throw std::runtime_error("JointAccelConstraint, coeff must be the same size of the joint postion."); // Set the bounds to the input targets - std::vector bounds(static_cast(GetRows())); // All of the positions should be exactly at their targets + std::vector bounds(static_cast(GetRows())); for (long j = 0; j < n_vars_; j++) { - index_map_[position_vars_[static_cast(j)]->GetName()] = j; for (long i = 0; i < n_dof_; i++) - { - bounds[static_cast(i + j * n_dof_)] = ifopt::Bounds(targets[i], targets[i]); - } + bounds[static_cast(i + (j * n_dof_))] = ifopt::Bounds(targets[i], targets[i]); } bounds_ = bounds; } @@ -81,12 +79,13 @@ JointAccelConstraint::JointAccelConstraint(const Eigen::VectorXd& targets, Eigen::VectorXd JointAccelConstraint::GetValues() const { Eigen::VectorXd acceleration(static_cast(n_dof_) * position_vars_.size()); + // Forward Diff for (std::size_t ind = 0; ind < position_vars_.size() - 2; ind++) { - auto vals1 = GetVariables()->GetComponent(position_vars_[ind]->GetName())->GetValues(); - auto vals2 = GetVariables()->GetComponent(position_vars_[ind + 1]->GetName())->GetValues(); - auto vals3 = GetVariables()->GetComponent(position_vars_[ind + 2]->GetName())->GetValues(); + auto vals1 = position_vars_[ind]->value(); + auto vals2 = position_vars_[ind + 1]->value(); + auto vals3 = position_vars_[ind + 2]->value(); const Eigen::VectorXd single_step = (vals3 - 2 * vals2 + vals1); acceleration.block(n_dof_ * static_cast(ind), 0, n_dof_, 1) = coeffs_.cwiseProduct(single_step); } @@ -94,9 +93,9 @@ Eigen::VectorXd JointAccelConstraint::GetValues() const // Backward Diff for (std::size_t ind = position_vars_.size() - 2; ind < position_vars_.size(); ind++) { - auto vals1 = GetVariables()->GetComponent(position_vars_[ind]->GetName())->GetValues(); - auto vals2 = GetVariables()->GetComponent(position_vars_[ind - 1]->GetName())->GetValues(); - auto vals3 = GetVariables()->GetComponent(position_vars_[ind - 2]->GetName())->GetValues(); + auto vals1 = position_vars_[ind]->value(); + auto vals2 = position_vars_[ind - 1]->value(); + auto vals3 = position_vars_[ind - 2]->value(); const Eigen::VectorXd single_step = (vals3 - 2 * vals2 + vals1); acceleration.block(n_dof_ * static_cast(ind), 0, n_dof_, 1) = coeffs_.cwiseProduct(single_step); } @@ -111,37 +110,54 @@ void JointAccelConstraint::FillJacobianBlock(std::string var_set, Jacobian& jac_ { // Check if this constraint use the var_set // Only modify the jacobian if this constraint uses var_set - auto it = index_map_.find(var_set); - if (it == index_map_.end()) // NOLINT + if (var_set != position_vars_.front()->getParent()->getParent()->GetName()) return; - const Eigen::Index i = it->second; - std::vector > triplet_list; - triplet_list.reserve(static_cast(n_dof_ * 3)); + triplet_list.reserve(static_cast(n_dof_ * 3 * n_vars_)); // jac block will be (n_vars-1)*n_dof x n_dof - for (int j = 0; j < n_dof_; j++) + + Eigen::Index prev_idx2 = -1; + Eigen::Index prev_idx1 = -1; + Eigen::Index idx = -1; + Eigen::Index post_idx1 = -1; + Eigen::Index post_idx2 = -1; + for (std::size_t i = 0; i < n_vars_; i++) { - // The last two variable are special and only effect the last two constraints. Everything else - // effects 3 - if (i < n_vars_ - 1) - triplet_list.emplace_back(i * n_dof_ + j, j, 1.0 * coeffs_[j]); + idx = position_vars_[i]->getIndex(); - if (i > 0 && i < n_vars_ - 1) - triplet_list.emplace_back((i - 1) * n_dof_ + j, j, -2.0 * coeffs_[j]); + if (i > 0) + prev_idx1 = position_vars_[i - 1]->getIndex(); if (i > 1) - triplet_list.emplace_back((i - 2) * n_dof_ + j, j, 1.0 * coeffs_[j]); + prev_idx2 = position_vars_[i - 2]->getIndex(); - if (i == (n_vars_ - 1)) - triplet_list.emplace_back((i * n_dof_) + j, j, 1.0 * coeffs_[j]); + if (i < n_vars_ - 1) + post_idx1 = position_vars_[i + 1]->getIndex(); + + if (i < n_vars_ - 2) + post_idx2 = position_vars_[i + 2]->getIndex(); + + for (Eigen::Index j = 0; j < n_dof_; j++) + { + // The last two variable are special and only effect the last two constraints. + // Everything else effects 3 - if (i >= (n_vars_ - 3) && i <= (n_vars_ - 2)) - triplet_list.emplace_back(((i + 1) * n_dof_) + j, j, -2.0 * coeffs_[j]); + triplet_list.emplace_back(idx + j, idx + j, 1.0 * coeffs_[j]); - if (i >= (n_vars_ - 4) && i <= (n_vars_ - 3)) - triplet_list.emplace_back(((i + 2) * n_dof_) + j, j, 1.0 * coeffs_[j]); + if (i > 0 && i < n_vars_ - 1) + triplet_list.emplace_back(prev_idx1 + j, idx + j, -2.0 * coeffs_[j]); + + if (i > 1) + triplet_list.emplace_back(prev_idx2 + j, idx + j, 1.0 * coeffs_[j]); + + if (i >= (n_vars_ - 3) && i <= (n_vars_ - 2)) + triplet_list.emplace_back(post_idx1 + j, idx + j, -2.0 * coeffs_[j]); + + if (i >= (n_vars_ - 4) && i <= (n_vars_ - 3)) + triplet_list.emplace_back(post_idx2 + j, idx + j, 1.0 * coeffs_[j]); + } } jac_block.setFromTriplets(triplet_list.begin(), triplet_list.end()); // NOLINT diff --git a/trajopt_ifopt/src/constraints/joint_jerk_constraint.cpp b/trajopt_ifopt/src/constraints/joint_jerk_constraint.cpp index 99398690..74a0209c 100644 --- a/trajopt_ifopt/src/constraints/joint_jerk_constraint.cpp +++ b/trajopt_ifopt/src/constraints/joint_jerk_constraint.cpp @@ -22,7 +22,9 @@ * limitations under the License. */ #include -#include +#include +#include +#include TRAJOPT_IGNORE_WARNINGS_PUSH #include @@ -31,7 +33,7 @@ TRAJOPT_IGNORE_WARNINGS_POP namespace trajopt_ifopt { JointJerkConstraint::JointJerkConstraint(const Eigen::VectorXd& targets, - const std::vector>& position_vars, + const std::vector>& position_vars, const Eigen::VectorXd& coeffs, const std::string& name) : ifopt::ConstraintSet(static_cast(targets.size()) * static_cast(position_vars.size()), name) @@ -46,14 +48,13 @@ JointJerkConstraint::JointJerkConstraint(const Eigen::VectorXd& targets, // Check and make sure the targets size aligns with the vars passed in for (const auto& position_var : position_vars) { - if (targets.size() != position_var->GetRows()) + if (targets.size() != position_var->size()) CONSOLE_BRIDGE_logError("Targets size does not align with variables provided"); } // Set n_dof and n_vars assert(n_dof_ > 0); assert(n_vars_ > 0); - // assert(n_vars_ == 2); if (!(coeffs_.array() > 0).all()) throw std::runtime_error("JointJerkConstraint, coeff must be greater than zero."); @@ -69,11 +70,8 @@ JointJerkConstraint::JointJerkConstraint(const Eigen::VectorXd& targets, // All of the positions should be exactly at their targets for (long j = 0; j < n_vars_; j++) { - index_map_[position_vars_[static_cast(j)]->GetName()] = j; for (long i = 0; i < n_dof_; i++) - { - bounds[static_cast(i + j * n_dof_)] = ifopt::Bounds(targets[i], targets[i]); - } + bounds[static_cast(i + (j * n_dof_))] = ifopt::Bounds(targets[i], targets[i]); } bounds_ = bounds; } @@ -84,10 +82,10 @@ Eigen::VectorXd JointJerkConstraint::GetValues() const // Forward Diff for (std::size_t ind = 0; ind < position_vars_.size() - 3; ind++) { - auto vals1 = GetVariables()->GetComponent(position_vars_[ind]->GetName())->GetValues(); - auto vals2 = GetVariables()->GetComponent(position_vars_[ind + 1]->GetName())->GetValues(); - auto vals3 = GetVariables()->GetComponent(position_vars_[ind + 2]->GetName())->GetValues(); - auto vals4 = GetVariables()->GetComponent(position_vars_[ind + 3]->GetName())->GetValues(); + auto vals1 = position_vars_[ind]->value(); + auto vals2 = position_vars_[ind + 1]->value(); + auto vals3 = position_vars_[ind + 2]->value(); + auto vals4 = position_vars_[ind + 3]->value(); const Eigen::VectorXd single_step = (3.0 * vals2) - (3.0 * vals3) - vals1 + vals4; acceleration.block(n_dof_ * static_cast(ind), 0, n_dof_, 1) = coeffs_.cwiseProduct(single_step); } @@ -95,10 +93,10 @@ Eigen::VectorXd JointJerkConstraint::GetValues() const // Backward Diff for (std::size_t ind = position_vars_.size() - 3; ind < position_vars_.size(); ind++) { - auto vals1 = GetVariables()->GetComponent(position_vars_[ind]->GetName())->GetValues(); - auto vals2 = GetVariables()->GetComponent(position_vars_[ind - 1]->GetName())->GetValues(); - auto vals3 = GetVariables()->GetComponent(position_vars_[ind - 2]->GetName())->GetValues(); - auto vals4 = GetVariables()->GetComponent(position_vars_[ind - 3]->GetName())->GetValues(); + auto vals1 = position_vars_[ind]->value(); + auto vals2 = position_vars_[ind - 1]->value(); + auto vals3 = position_vars_[ind - 2]->value(); + auto vals4 = position_vars_[ind - 3]->value(); const Eigen::VectorXd single_step = vals1 - (3.0 * vals2) + (3.0 * vals3) - vals4; acceleration.block(n_dof_ * static_cast(ind), 0, n_dof_, 1) = coeffs_.cwiseProduct(single_step); } @@ -113,44 +111,72 @@ void JointJerkConstraint::FillJacobianBlock(std::string var_set, Jacobian& jac_b { // Check if this constraint use the var_set // Only modify the jacobian if this constraint uses var_set - auto it = index_map_.find(var_set); - if (it == index_map_.end()) // NOLINT + if (var_set != position_vars_.front()->getParent()->getParent()->GetName()) return; - const Eigen::Index i = it->second; - // Reserve enough room in the sparse matrix std::vector> triplet_list; triplet_list.reserve(static_cast(n_dof_ * 4)); // jac block will be (n_vars-1)*n_dof x n_dof - for (int j = 0; j < n_dof_; j++) + Eigen::Index prev_idx3 = -1; + Eigen::Index prev_idx2 = -1; + Eigen::Index prev_idx1 = -1; + Eigen::Index idx = -1; + Eigen::Index post_idx1 = -1; + Eigen::Index post_idx2 = -1; + Eigen::Index post_idx3 = -1; + + for (std::size_t i = 0; i < n_vars_; i++) { - // The last two variable are special and only effect the last two constraints. Everything else - // effects 3 - if (i < n_vars_ - 3) - triplet_list.emplace_back(i * n_dof_ + j, j, -1.0 * coeffs_[j]); + idx = position_vars_[i]->getIndex(); - if (i > 0 && i < n_vars_ - 2) - triplet_list.emplace_back((i - 1) * n_dof_ + j, j, 3.0 * coeffs_[j]); + if (i > 0) + prev_idx1 = position_vars_[i - 1]->getIndex(); - if (i > 1 && i < n_vars_ - 1) - triplet_list.emplace_back((i - 2) * n_dof_ + j, j, -3.0 * coeffs_[j]); + if (i > 1) + prev_idx2 = position_vars_[i - 2]->getIndex(); if (i > 2) - triplet_list.emplace_back((i - 3) * n_dof_ + j, j, 1.0 * coeffs_[j]); + prev_idx3 = position_vars_[i - 3]->getIndex(); + + if (i < n_vars_ - 1) + post_idx1 = position_vars_[i + 1]->getIndex(); + + if (i < n_vars_ - 2) + post_idx2 = position_vars_[i + 2]->getIndex(); + + if (i < n_vars_ - 3) + post_idx3 = position_vars_[i + 3]->getIndex(); + + for (int j = 0; j < n_dof_; j++) + { + // The last two variable are special and only effect the last two constraints. + // Everything else effects 3 + if (i < n_vars_ - 3) + triplet_list.emplace_back(idx + j, idx + j, -1.0 * coeffs_[j]); + + if (i > 0 && i < n_vars_ - 2) + triplet_list.emplace_back(prev_idx1 + j, idx + j, 3.0 * coeffs_[j]); - if (i >= (n_vars_ - 3) && i <= (n_vars_ - 1)) - triplet_list.emplace_back((i * n_dof_) + j, j, 1.0 * coeffs_[j]); + if (i > 1 && i < n_vars_ - 1) + triplet_list.emplace_back(prev_idx2 + j, idx + j, -3.0 * coeffs_[j]); - if (i >= (n_vars_ - 4) && i <= (n_vars_ - 2)) - triplet_list.emplace_back(((i + 1) * n_dof_) + j, j, -3.0 * coeffs_[j]); + if (i > 2) + triplet_list.emplace_back(prev_idx3 + j, idx + j, 1.0 * coeffs_[j]); - if (i >= (n_vars_ - 5) && i <= (n_vars_ - 3)) - triplet_list.emplace_back(((i + 2) * n_dof_) + j, j, 3.0 * coeffs_[j]); + if (i >= (n_vars_ - 3) && i <= (n_vars_ - 1)) + triplet_list.emplace_back(idx + j, idx + j, 1.0 * coeffs_[j]); - if (i >= (n_vars_ - 6) && i <= (n_vars_ - 4)) - triplet_list.emplace_back(((i + 3) * n_dof_) + j, j, -1.0 * coeffs_[j]); + if (i >= (n_vars_ - 4) && i <= (n_vars_ - 2)) + triplet_list.emplace_back(post_idx1 + j, idx + j, -3.0 * coeffs_[j]); + + if (i >= (n_vars_ - 5) && i <= (n_vars_ - 3)) + triplet_list.emplace_back(post_idx2 + j, idx + j, 3.0 * coeffs_[j]); + + if (i >= (n_vars_ - 6) && i <= (n_vars_ - 4)) + triplet_list.emplace_back(post_idx3 + j, idx + j, -1.0 * coeffs_[j]); + } } jac_block.setFromTriplets(triplet_list.begin(), triplet_list.end()); // NOLINT } diff --git a/trajopt_ifopt/src/constraints/joint_position_constraint.cpp b/trajopt_ifopt/src/constraints/joint_position_constraint.cpp index 4bc3930a..8fb0ce26 100644 --- a/trajopt_ifopt/src/constraints/joint_position_constraint.cpp +++ b/trajopt_ifopt/src/constraints/joint_position_constraint.cpp @@ -22,7 +22,9 @@ * limitations under the License. */ #include -#include +#include +#include +#include TRAJOPT_IGNORE_WARNINGS_PUSH #include @@ -30,19 +32,17 @@ TRAJOPT_IGNORE_WARNINGS_POP namespace trajopt_ifopt { -JointPosConstraint::JointPosConstraint(const Eigen::VectorXd& targets, - const std::vector>& position_vars, +JointPosConstraint::JointPosConstraint(const Eigen::VectorXd& target, + const std::shared_ptr& position_var, const Eigen::VectorXd& coeffs, const std::string& name) - : ifopt::ConstraintSet(static_cast(targets.size()) * static_cast(position_vars.size()), name) - , n_dof_(targets.size()) - , n_vars_(static_cast(position_vars.size())) + : ifopt::ConstraintSet(static_cast(target.size()), name) + , n_dof_(target.size()) , coeffs_(coeffs) - , position_vars_(position_vars) + , position_var_(position_var) { // Set the n_dof and n_vars for convenience assert(n_dof_ > 0); - assert(n_vars_ > 0); if (!(coeffs_.array() > 0).all()) throw std::runtime_error("JointPosConstraint, coeff must be greater than zero."); @@ -54,40 +54,35 @@ JointPosConstraint::JointPosConstraint(const Eigen::VectorXd& targets, throw std::runtime_error("JointPosConstraint, coeff must be the same size of the joint postion."); // Check and make sure the targets size aligns with the vars passed in - for (const auto& position_var : position_vars) - { - if (targets.size() != position_var->GetRows()) - CONSOLE_BRIDGE_logError("Targets size does not align with variables provided"); - } + if (target.size() != position_var->size()) + CONSOLE_BRIDGE_logError("Targets size does not align with variables provided"); // Set the bounds to the input targets std::vector bounds(static_cast(GetRows())); // All of the positions should be exactly at their targets - for (long j = 0; j < n_vars_; j++) + + for (long i = 0; i < n_dof_; i++) { - for (long i = 0; i < n_dof_; i++) - { - const double w_target = coeffs_[i] * targets[i]; - bounds[static_cast(i + j * n_dof_)] = ifopt::Bounds(w_target, w_target); - } + const double w_target = coeffs_[i] * target[i]; + bounds[static_cast(i)] = ifopt::Bounds(w_target, w_target); } + bounds_ = bounds; } JointPosConstraint::JointPosConstraint(const std::vector& bounds, - const std::vector>& position_vars, + const std::shared_ptr& position_var, const Eigen::VectorXd& coeffs, const std::string& name) - : ifopt::ConstraintSet(static_cast(bounds.size()) * static_cast(position_vars.size()), name) + : ifopt::ConstraintSet(static_cast(bounds.size()), name) , coeffs_(coeffs) , bounds_(bounds) - , position_vars_(position_vars) + , position_var_(position_var) { // Set the n_dof and n_vars for convenience n_dof_ = static_cast(bounds_.size()); - n_vars_ = static_cast(position_vars_.size()); + assert(n_dof_ > 0); - assert(n_vars_ > 0); if (!(coeffs_.array() > 0).all()) throw std::runtime_error("JointPosConstraint, coeff must be greater than zero."); @@ -99,45 +94,30 @@ JointPosConstraint::JointPosConstraint(const std::vector& bounds, throw std::runtime_error("JointPosConstraint, coeff must be the same size of the joint postion."); // Check and make sure the targets size aligns with the vars passed in - for (auto& position_var : position_vars_) - { - if (static_cast(bounds_.size()) != position_var->GetRows()) - CONSOLE_BRIDGE_logError("Bounds size does not align with variables provided"); - } + if (static_cast(bounds_.size()) != position_var_->size()) + CONSOLE_BRIDGE_logError("Bounds size does not align with variables provided"); } -Eigen::VectorXd JointPosConstraint::GetValues() const -{ - // Get the correct variables - Eigen::VectorXd values(static_cast(n_dof_ * n_vars_)); - for (const auto& position_var : position_vars_) - values << coeffs_.cwiseProduct(this->GetVariables()->GetComponent(position_var->GetName())->GetValues()); - - return values; -} +Eigen::VectorXd JointPosConstraint::GetValues() const { return coeffs_.cwiseProduct(position_var_->value()); } // Set the limits on the constraint values std::vector JointPosConstraint::GetBounds() const { return bounds_; } void JointPosConstraint::FillJacobianBlock(std::string var_set, Jacobian& jac_block) const { + // Check if this constraint use the var_set + // Only modify the jacobian if this constraint uses var_set + if (var_set != position_var_->getParent()->getParent()->GetName()) + return; + + // Reserve enough room in the sparse matrix + std::vector> triplet_list; + triplet_list.reserve(static_cast(n_dof_)); + // Loop over all of the variables this constraint uses - for (long i = 0; i < n_vars_; i++) // NOLINT - { - // Only modify the jacobian if this constraint uses var_set - if (var_set == position_vars_[static_cast(i)]->GetName()) // NOLINT - { - // Reserve enough room in the sparse matrix - std::vector> triplet_list; - triplet_list.reserve(static_cast(n_dof_)); - - // Each jac_block will be for a single variable but for all timesteps. Therefore we must index down to the - // correct timestep for this variable - for (int j = 0; j < n_dof_; j++) // NOLINT - triplet_list.emplace_back(i * n_dof_ * 0 + j, j, coeffs_[j] * 1.0); - - jac_block.setFromTriplets(triplet_list.begin(), triplet_list.end()); // NOLINT - } - } + for (int j = 0; j < n_dof_; j++) // NOLINT + triplet_list.emplace_back(j, position_var_->getIndex() + j, coeffs_[j] * 1.0); + + jac_block.setFromTriplets(triplet_list.begin(), triplet_list.end()); // NOLINT } } // namespace trajopt_ifopt diff --git a/trajopt_ifopt/src/constraints/joint_velocity_constraint.cpp b/trajopt_ifopt/src/constraints/joint_velocity_constraint.cpp index de3316a6..bbe5ea32 100644 --- a/trajopt_ifopt/src/constraints/joint_velocity_constraint.cpp +++ b/trajopt_ifopt/src/constraints/joint_velocity_constraint.cpp @@ -22,7 +22,9 @@ * limitations under the License. */ #include -#include +#include +#include +#include TRAJOPT_IGNORE_WARNINGS_PUSH #include @@ -31,7 +33,7 @@ TRAJOPT_IGNORE_WARNINGS_POP namespace trajopt_ifopt { JointVelConstraint::JointVelConstraint(const Eigen::VectorXd& targets, - const std::vector>& position_vars, + const std::vector>& position_vars, const Eigen::VectorXd& coeffs, const std::string& name) : ifopt::ConstraintSet(static_cast(targets.size()) * static_cast(position_vars.size() - 1), name) @@ -46,14 +48,13 @@ JointVelConstraint::JointVelConstraint(const Eigen::VectorXd& targets, // Check and make sure the targets size aligns with the vars passed in for (const auto& position_var : position_vars_) { - if (targets.size() != position_var->GetRows()) + if (targets.size() != position_var->size()) CONSOLE_BRIDGE_logError("Targets size does not align with variables provided"); } // Set n_dof and n_vars assert(n_dof_ > 0); assert(n_vars_ > 0); - // assert(n_vars_ == 2); if (!(coeffs_.array() > 0).all()) throw std::runtime_error("JointVelConstraint, coeff must be greater than zero."); @@ -69,13 +70,9 @@ JointVelConstraint::JointVelConstraint(const Eigen::VectorXd& targets, // All of the positions should be exactly at their targets for (long j = 0; j < n_vars_ - 1; j++) { - index_map_[position_vars_[static_cast(j)]->GetName()] = j; for (long i = 0; i < n_dof_; i++) - { - bounds[static_cast(i + j * n_dof_)] = ifopt::Bounds(targets[i], targets[i]); - } + bounds[static_cast(i + (j * n_dof_))] = ifopt::Bounds(targets[i], targets[i]); } - index_map_[position_vars_.back()->GetName()] = (n_vars_ - 1); bounds_ = bounds; } @@ -90,11 +87,13 @@ Eigen::VectorXd JointVelConstraint::GetValues() const // Velocity V = vel(var[0, 0]), vel(var[0, 1]), vel(var[0, 2]), vel(var[1, 0]), vel(var[1, 1]), vel(var[1, 2]), etc Eigen::VectorXd velocity(static_cast(n_dof_) * (static_cast(n_vars_) - 1)); + // Eigen::VectorXd val = this->GetVariables()->GetComponent(var_set_)->GetValues(); + // Forward differentiation for the first point for (std::size_t ind = 0; ind < position_vars_.size() - 1; ind++) { - auto vals1 = this->GetVariables()->GetComponent(position_vars_[ind]->GetName())->GetValues(); - auto vals2 = this->GetVariables()->GetComponent(position_vars_[ind + 1]->GetName())->GetValues(); + auto vals1 = position_vars_[ind]->value(); + auto vals2 = position_vars_[ind + 1]->value(); const Eigen::VectorXd single_step = (vals2 - vals1); velocity.block(n_dof_ * static_cast(ind), 0, n_dof_, 1) = coeffs_.cwiseProduct(single_step); } @@ -107,30 +106,37 @@ std::vector JointVelConstraint::GetBounds() const { return bounds void JointVelConstraint::FillJacobianBlock(std::string var_set, Jacobian& jac_block) const { - // FillJacobianBlockNumerical(var_set, jac_block); - // Check if this constraint use the var_set // Only modify the jacobian if this constraint uses var_set - auto it = index_map_.find(var_set); - if (it == index_map_.end()) // NOLINT + if (var_set != position_vars_.front()->getParent()->getParent()->GetName()) return; - const Eigen::Index i = it->second; - // Reserve enough room in the sparse matrix std::vector> triplet_list; - triplet_list.reserve(static_cast(3 * n_dof_)); + triplet_list.reserve(static_cast(n_vars_ * n_dof_)); + // The first and last variable are special and only effect the first and last constraint. Everything else effects 2 + Eigen::Index prev_idx = -1; + Eigen::Index idx = position_vars_[0]->getIndex(); for (int j = 0; j < n_dof_; j++) - { - // The first and last variable are special and only effect the first and last constraint. Everything else - // effects 2 - if (i < n_vars_ - 1) - triplet_list.emplace_back((i * n_dof_) + j, j, -1.0 * coeffs_[j]); + triplet_list.emplace_back(idx + j, idx + j, -1.0 * coeffs_[j]); - if (i > 0) - triplet_list.emplace_back(((i - 1) * n_dof_) + j, j, 1.0 * coeffs_[j]); + for (std::size_t i = 1; i < (n_vars_ - 1); i++) + { + prev_idx = position_vars_[i - 1]->getIndex(); + idx = position_vars_[i]->getIndex(); + for (int j = 0; j < n_dof_; j++) + { + triplet_list.emplace_back(idx + j, idx + j, -1.0 * coeffs_[j]); + triplet_list.emplace_back(prev_idx + j, idx + j, 1.0 * coeffs_[j]); + } } + + prev_idx = position_vars_[static_cast(n_vars_ - 2)]->getIndex(); + idx = position_vars_[static_cast(n_vars_ - 1)]->getIndex(); + for (int j = 0; j < n_dof_; j++) + triplet_list.emplace_back(prev_idx + j, idx + j, 1.0 * coeffs_[j]); + jac_block.setFromTriplets(triplet_list.begin(), triplet_list.end()); // NOLINT } diff --git a/trajopt_ifopt/src/utils/trajopt_utils.cpp b/trajopt_ifopt/src/utils/trajopt_utils.cpp index e9c8c3fc..05136b3d 100644 --- a/trajopt_ifopt/src/utils/trajopt_utils.cpp +++ b/trajopt_ifopt/src/utils/trajopt_utils.cpp @@ -23,7 +23,7 @@ */ #include -#include +#include TRAJOPT_IGNORE_WARNINGS_PUSH #include @@ -31,18 +31,18 @@ TRAJOPT_IGNORE_WARNINGS_POP namespace trajopt_ifopt { -tesseract_common::TrajArray toTrajArray(const std::vector& joint_positions) +tesseract_common::TrajArray toTrajArray(const std::vector>& joint_positions) { tesseract_common::TrajArray traj_array; if (!joint_positions.empty()) - traj_array.resize(static_cast(joint_positions.size()), joint_positions.front()->GetRows()); + traj_array.resize(static_cast(joint_positions.size()), joint_positions.front()->size()); for (Eigen::Index i = 0; i < traj_array.rows(); i++) - traj_array.row(i) = joint_positions[static_cast(i)]->GetValues().transpose(); + traj_array.row(i) = joint_positions[static_cast(i)]->value().transpose(); return traj_array; } tesseract_common::JointTrajectory -toJointTrajectory(const std::vector& joint_positions) +toJointTrajectory(const std::vector>& joint_positions) { tesseract_common::JointTrajectory joint_trajectory; @@ -50,7 +50,7 @@ toJointTrajectory(const std::vector& joi joint_trajectory.reserve(joint_positions.size()); for (const auto& jp : joint_positions) - joint_trajectory.states.emplace_back(jp->GetJointNames(), jp->GetValues()); + joint_trajectory.states.emplace_back(jp->name(), jp->value()); return joint_trajectory; } diff --git a/trajopt_ifopt/src/variable_sets/joint_position_variable.cpp b/trajopt_ifopt/src/variable_sets/joint_position_variable.cpp deleted file mode 100644 index ce84f555..00000000 --- a/trajopt_ifopt/src/variable_sets/joint_position_variable.cpp +++ /dev/null @@ -1,95 +0,0 @@ -/** - * @file joint_position_variable.cpp - * @brief Contains the joint position variable - * - * @author Matthew Powelson - * @date May 18, 2020 - * - * @copyright Copyright (c) 2020, Southwest Research Institute - * - * @par License - * Software License Agreement (Apache License) - * @par - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * http://www.apache.org/licenses/LICENSE-2.0 - * @par - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - */ -#include -TRAJOPT_IGNORE_WARNINGS_PUSH -#include -#include -TRAJOPT_IGNORE_WARNINGS_POP - -#include -#include - -namespace trajopt_ifopt -{ -JointPosition::JointPosition(const Eigen::Ref& init_value, - std::vector joint_names, - const std::string& name) - : ifopt::VariableSet(static_cast(init_value.size()), name) - , bounds_(std::vector(static_cast(init_value.size()), ifopt::NoBound)) - , values_(init_value) - , joint_names_(std::move(joint_names)) -{ -} - -JointPosition::JointPosition(const Eigen::Ref& init_value, - std::vector joint_names, - const ifopt::Bounds& bounds, - const std::string& name) - : ifopt::VariableSet(static_cast(init_value.size()), name) - , bounds_(std::vector(static_cast(init_value.size()), bounds)) - , joint_names_(std::move(joint_names)) -{ - /** @todo Print warning if init value is not within bounds */ - values_ = trajopt_ifopt::getClosestValidPoint(init_value, bounds_); - - if (!values_.isApprox(init_value, 1e-10)) - { - CONSOLE_BRIDGE_logWarn("The initial values are not within the provided bounds. Adjusting to be within the " - "bounds."); - } -} - -JointPosition::JointPosition(const Eigen::Ref& init_value, - std::vector joint_names, - const tesseract_common::KinematicLimits& bounds, - const std::string& name) - : ifopt::VariableSet(static_cast(init_value.size()), name) - , bounds_(std::vector(static_cast(init_value.size()), ifopt::NoBound)) - , joint_names_(std::move(joint_names)) -{ - /** @todo Print warning if init value is not within bounds */ - for (Eigen::Index i = 0; i < init_value.size(); ++i) - bounds_[static_cast(i)] = ifopt::Bounds(bounds.joint_limits(i, 0), bounds.joint_limits(i, 1)); - - values_ = trajopt_ifopt::getClosestValidPoint(init_value, bounds_); - - if (!values_.isApprox(init_value, 1e-10)) - { - CONSOLE_BRIDGE_logWarn("The initial values are not within the provided bounds. Adjusting to be within the " - "bounds."); - } -} - -void JointPosition::SetVariables(const Eigen::VectorXd& x) { values_ = x; } - -Eigen::VectorXd JointPosition::GetValues() const { return values_; } - -JointPosition::VecBound JointPosition::GetBounds() const { return bounds_; } - -void JointPosition::SetBounds(VecBound& new_bounds) { bounds_ = new_bounds; } - -void JointPosition::SetBounds(const Eigen::Ref& bounds) { bounds_ = toBounds(bounds); } - -std::vector JointPosition::GetJointNames() const { return joint_names_; } -} // namespace trajopt_ifopt diff --git a/trajopt_ifopt/src/variable_sets/node.cpp b/trajopt_ifopt/src/variable_sets/node.cpp new file mode 100644 index 00000000..e372e0ab --- /dev/null +++ b/trajopt_ifopt/src/variable_sets/node.cpp @@ -0,0 +1,93 @@ +/** + * @author Levi Armstrong + * @date Jan 7, 2025 + * + * @copyright Copyright (c) 2025, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#include +#include +#include +#include + +namespace trajopt_ifopt +{ +Node::Node(std::string node_name) : name_(std::move(node_name)) {} +Node::~Node() = default; + +const std::string& Node::getName() const { return name_; } +const NodesVariables* Node::getParent() const { return parent_; } + +bool Node::hasVar(const std::string& name) const { return (vars_by_name_.find(name) != vars_by_name_.end()); } + +std::shared_ptr Node::addVar(const std::string& name, double value, ifopt::Bounds bounds) +{ + vars_.emplace_back(std::make_shared(length_, name, value, bounds, this)); + vars_by_name_[name] = vars_.back(); + length_ += 1; + return vars_.back(); +} + +std::shared_ptr Node::addVar(const std::string& name, + const std::vector& child_names, + const Eigen::VectorXd& values, + const std::vector& bounds) +{ + vars_.emplace_back(std::make_shared(length_, name, child_names, values, bounds, this)); + vars_by_name_[name] = vars_.back(); + length_ += static_cast(child_names.size()); + return vars_.back(); +} + +std::shared_ptr Node::getVar(const std::string& name) const { return vars_by_name_.at(name); } + +Eigen::VectorXd Node::getValues() const +{ + std::vector values; + values.reserve(static_cast(length_)); + for (const auto& var : vars_) + { + const Eigen::VectorXd& value = var->value(); + values.insert(values.end(), value.data(), value.data() + value.size()); + } + return Eigen::Map(values.data(), static_cast(values.size())); +} + +std::vector Node::getBounds() const +{ + std::vector bounds; + for (const auto& var : vars_) + { + const std::vector& var_bounds = var->bounds(); + bounds.insert(bounds.end(), var_bounds.begin(), var_bounds.end()); + } + return bounds; +} + +Eigen::Index Node::size() const { return length_; } + +void Node::setVariables(const Eigen::Ref& x) +{ + for (auto& var : vars_) + var->setVariables(x); +} + +void Node::incrementIndex(Eigen::Index value) +{ + for (auto& var : vars_) + var->incrementIndex(value); +} +} // namespace trajopt_ifopt diff --git a/trajopt_ifopt/src/variable_sets/nodes_observer.cpp b/trajopt_ifopt/src/variable_sets/nodes_observer.cpp new file mode 100644 index 00000000..67cc41ca --- /dev/null +++ b/trajopt_ifopt/src/variable_sets/nodes_observer.cpp @@ -0,0 +1,43 @@ +/****************************************************************************** +Copyright (c) 2018, Alexander W. Winkler. 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 the copyright holder 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 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. +******************************************************************************/ + +#include +#include + +namespace trajopt_ifopt +{ +NodesObserver::NodesObserver(std::weak_ptr subject) : subject_(std::move(subject)) +{ + auto subject_shared = subject_.lock(); // Convert weak_ptr to shared_ptr + if (subject_shared) + subject_shared->AddObserver(shared_from_this()); // register observer to subject so this class always up-to-date + else // Handle the case where the weak_ptr is expired + throw std::runtime_error("Failed to lock weak_ptr; the object might have been destroyed."); +} +} // namespace trajopt_ifopt diff --git a/trajopt_ifopt/src/variable_sets/nodes_variables.cpp b/trajopt_ifopt/src/variable_sets/nodes_variables.cpp new file mode 100644 index 00000000..3e13c750 --- /dev/null +++ b/trajopt_ifopt/src/variable_sets/nodes_variables.cpp @@ -0,0 +1,107 @@ +/****************************************************************************** +Copyright (c) 2018, Alexander W. Winkler. 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 the copyright holder 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 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. +******************************************************************************/ + +#include +#include +#include + +namespace trajopt_ifopt +{ +NodesVariables::NodesVariables(const std::string& name, std::vector> nodes) + : VariableSet(kSpecifyLater, name) +{ + nodes_.reserve(nodes.size()); + for (auto& node : nodes) + AddNode(std::move(node)); + + // Set the size + SetRows(static_cast(n_dim_)); + + // Get the initial values + std::vector values; + values.reserve(static_cast(n_dim_)); + + for (const auto& node : nodes_) + { + Eigen::VectorXd node_values = node->getValues(); + values.insert(values.end(), node_values.data(), node_values.data() + node_values.size()); + } + + values_ = Eigen::Map(values.data(), static_cast(values.size())); + assert(values_.size() == bounds_.size()); +} + +void NodesVariables::AddNode(std::unique_ptr node) +{ + node->incrementIndex(n_dim_); + node->parent_ = this; + + Eigen::Index length = node->size(); + std::vector bounds = node->getBounds(); + + nodes_.emplace_back(std::move(node)); + bounds_.insert(bounds_.end(), bounds.begin(), bounds.end()); + n_dim_ += length; +} + +std::shared_ptr NodesVariables::GetNode(std::size_t opt_idx) const { return nodes_.at(opt_idx); } + +void NodesVariables::SetVariables(const VectorXd& x) +{ + for (auto& node : nodes_) + node->setVariables(x); + + values_ = x; + + UpdateObservers(); +} + +Eigen::VectorXd NodesVariables::GetValues() const { return values_; } + +void NodesVariables::UpdateObservers() +{ + for (auto& o : observers_) + o->UpdateNodes(); +} + +void NodesVariables::AddObserver(std::shared_ptr observer) { observers_.push_back(std::move(observer)); } + +Eigen::Index NodesVariables::GetDim() const { return n_dim_; } + +NodesVariables::VecBound NodesVariables::GetBounds() const { return bounds_; } + +std::vector> NodesVariables::GetNodes() const +{ + std::vector> nodes; + nodes.reserve(nodes_.size()); + std::copy(nodes_.begin(), nodes_.end(), std::back_inserter(nodes)); + return nodes; +} + +} // namespace trajopt_ifopt diff --git a/trajopt_ifopt/src/variable_sets/var.cpp b/trajopt_ifopt/src/variable_sets/var.cpp new file mode 100644 index 00000000..787db722 --- /dev/null +++ b/trajopt_ifopt/src/variable_sets/var.cpp @@ -0,0 +1,88 @@ +/** + * @author Levi Armstrong + * @date Jan 7, 2025 + * + * @copyright Copyright (c) 2025, Southwest Research Institute + * + * @par License + * Software License Agreement (Apache License) + * @par + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * http://www.apache.org/licenses/LICENSE-2.0 + * @par + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + */ +#include +#include +#include + +namespace trajopt_ifopt +{ +Var::Var(Eigen::Index index, std::string name, double value, ifopt::Bounds bounds, Node* parent) + : index_(index) + , identifier_(std::move(name)) + , names_({ identifier_ }) + , values_(Eigen::VectorXd::Constant(1, value)) + , bounds_({ bounds }) + , parent_(parent) +{ + values_ = trajopt_ifopt::getClosestValidPoint(Eigen::VectorXd::Constant(1, value), bounds_); + + if (!values_.isApprox(Eigen::VectorXd::Constant(1, value), 1e-10)) + { + CONSOLE_BRIDGE_logWarn("The initial values are not within the provided bounds. Adjusting to be within the " + "bounds."); + } +} + +Var::Var(Eigen::Index index, + std::string identifier, + std::vector names, + const Eigen::VectorXd& values, + const std::vector& bounds, + Node* parent) + : index_(index) + , identifier_(std::move(identifier)) + , names_(std::move(names)) + , values_(values) + , bounds_(bounds) + , parent_(parent) +{ + if (names_.size() != bounds_.size()) + throw std::runtime_error("Varaible: '" + identifier_ + "' has miss matched size for names and values"); + + if (names_.size() != values_.size()) + throw std::runtime_error("Varaible: '" + identifier_ + "' has miss matched size for names and values"); + + values_ = trajopt_ifopt::getClosestValidPoint(values, bounds_); + + if (!values_.isApprox(values, 1e-10)) + { + CONSOLE_BRIDGE_logWarn("The initial values are not within the provided bounds. Adjusting to be within the " + "bounds."); + } +} + +const std::string& Var::getIdentifier() const { return identifier_; } +const Node* Var::getParent() const { return parent_; } +Eigen::Index Var::getIndex() const { return index_; } +Eigen::Index Var::size() const { return values_.rows(); } +const Eigen::VectorXd& Var::value() const { return values_; } +const std::vector& Var::name() const { return names_; } +const std::vector& Var::bounds() const { return bounds_; } + +void Var::incrementIndex(Eigen::Index value) { index_ += value; } +void Var::setVariables(const Eigen::Ref& x) +{ + assert(index_ > -1 && index_ < x.size()); + assert(values_.rows() > 0 && (index_ + (values_.rows() - 1)) < x.size()); + values_ = x.segment(index_, values_.rows()); +} + +} // namespace trajopt_ifopt diff --git a/trajopt_ifopt/test/cartesian_line_constraint_unit.cpp b/trajopt_ifopt/test/cartesian_line_constraint_unit.cpp index 12be0f95..7504e761 100644 --- a/trajopt_ifopt/test/cartesian_line_constraint_unit.cpp +++ b/trajopt_ifopt/test/cartesian_line_constraint_unit.cpp @@ -12,8 +12,11 @@ TRAJOPT_IGNORE_WARNINGS_PUSH TRAJOPT_IGNORE_WARNINGS_POP #include -#include +#include +#include +#include #include +#include using namespace trajopt_ifopt; using namespace std; @@ -31,7 +34,7 @@ class CartesianLineConstraintUnit : public testing::TestWithParam tesseract_kinematics::JointGroup::ConstPtr manip; CartLineInfo info; - trajopt_ifopt::JointPosition::Ptr var; + std::shared_ptr var; Eigen::Isometry3d source_tf; Eigen::Isometry3d line_start_pose; @@ -52,9 +55,13 @@ class CartesianLineConstraintUnit : public testing::TestWithParam manip = env->getJointGroup("right_arm"); n_dof = manip->numJoints(); + std::vector bounds(static_cast(manip->numJoints()), ifopt::NoBound); + auto node = std::make_unique("Joint_Position_0"); auto pos = Eigen::VectorXd::Ones(n_dof); - var = std::make_shared(pos, manip->getJointNames(), "Joint_Position_0"); - variables->AddComponent(var); + var = node->addVar("position", manip->getJointNames(), pos, bounds); + std::vector> nodes; + nodes.push_back(std::move(node)); + variables->AddComponent(std::make_shared("joint_trajectory", std::move(nodes))); // Add constraints const Eigen::VectorXd joint_position = Eigen::VectorXd::Ones(n_dof); @@ -159,7 +166,7 @@ TEST_F(CartesianLineConstraintUnit, FillJacobian) // NOLINT } { ifopt::ConstraintSet::Jacobian jac_block(num_jac_block.rows(), num_jac_block.cols()); - constraint->FillJacobianBlock("Joint_Position_0", jac_block); + constraint->FillJacobianBlock("joint_trajectory", jac_block); EXPECT_TRUE(jac_block.toDense().isApprox(num_jac_block.toDense(), 1e-3)); } } diff --git a/trajopt_ifopt/test/cartesian_position_constraint_unit.cpp b/trajopt_ifopt/test/cartesian_position_constraint_unit.cpp index 6d79d21c..3ee04866 100644 --- a/trajopt_ifopt/test/cartesian_position_constraint_unit.cpp +++ b/trajopt_ifopt/test/cartesian_position_constraint_unit.cpp @@ -37,7 +37,10 @@ TRAJOPT_IGNORE_WARNINGS_POP #include #include -#include +#include +#include +#include +#include #include using namespace trajopt_ifopt; @@ -75,9 +78,14 @@ class CartesianPositionConstraintUnit : public testing::TestWithParamgetJointGroup("right_arm"); n_dof = kin_group->numJoints(); - auto pos = Eigen::VectorXd::Ones(kin_group->numJoints()); - auto var0 = std::make_shared(pos, kin_group->getJointNames(), "Joint_Position_0"); - nlp.AddVariableSet(var0); + const std::vector bounds(static_cast(n_dof), ifopt::NoBound); + auto pos = Eigen::VectorXd::Ones(n_dof); + auto node = std::make_unique("Joint_Position_0"); + auto var0 = node->addVar("position", kin_group->getJointNames(), pos, bounds); + + std::vector> nodes; + nodes.push_back(std::move(node)); + nlp.AddVariableSet(std::make_shared("joint_trajectory", std::move(nodes))); // 4) Add constraints const CartPosInfo cart_info(kin_group, "r_gripper_tool_frame", "base_footprint"); @@ -170,7 +178,7 @@ TEST_F(CartesianPositionConstraintUnit, FillJacobian) // NOLINT } { trajopt_ifopt::SparseMatrix jac_block(num_jac_block.rows(), num_jac_block.cols()); - constraint->FillJacobianBlock("Joint_Position_0", jac_block); + constraint->FillJacobianBlock("joint_trajectory", jac_block); EXPECT_TRUE(jac_block.toDense().isApprox(num_jac_block.toDense(), 1e-3)); // std::cout << "Numeric:\n" << num_jac_block.toDense() << '\n'; // std::cout << "Analytic:\n" << jac_block.toDense() << '\n'; @@ -187,14 +195,16 @@ TEST_F(CartesianPositionConstraintUnit, GetSetBounds) // NOLINT // Check that setting bounds works { + std::vector bounds_vec(static_cast(n_dof), ifopt::NoBound); + auto node = std::make_unique("Joint_Position_0"); const Eigen::VectorXd pos = Eigen::VectorXd::Ones(kin_group->numJoints()); - auto var0 = std::make_shared(pos, kin_group->getJointNames(), "Joint_Position_0"); + auto var0 = node->addVar("position", kin_group->getJointNames(), pos, bounds_vec); const CartPosInfo cart_info(kin_group, "r_gripper_tool_frame", "base_footprint"); auto constraint_2 = std::make_shared(cart_info, var0); const ifopt::Bounds bounds(-0.1234, 0.5678); - std::vector bounds_vec = std::vector(6, bounds); + bounds_vec = std::vector(6, bounds); constraint_2->SetBounds(bounds_vec); std::vector results_vec = constraint_2->GetBounds(); diff --git a/trajopt_ifopt/test/cast_cost_unit.cpp b/trajopt_ifopt/test/cast_cost_unit.cpp index 7fc08859..1bc7ed97 100644 --- a/trajopt_ifopt/test/cast_cost_unit.cpp +++ b/trajopt_ifopt/test/cast_cost_unit.cpp @@ -45,8 +45,11 @@ TRAJOPT_IGNORE_WARNINGS_POP #include #include #include -#include +#include +#include +#include #include +#include using namespace trajopt_ifopt; using namespace std; @@ -87,6 +90,7 @@ TEST_F(CastTest, boxes) // NOLINT const tesseract_scene_graph::StateSolver::Ptr state_solver = env->getStateSolver(); const ContinuousContactManager::Ptr manager = env->getContinuousContactManager(); const tesseract_kinematics::JointGroup::ConstPtr manip = env->getJointGroup("manipulator"); + const std::vector bounds = trajopt_ifopt::toBounds(manip->getLimits().joint_limits); manager->setActiveCollisionObjects(manip->getActiveLinkNames()); manager->setDefaultCollisionMargin(0); @@ -97,33 +101,35 @@ TEST_F(CastTest, boxes) // NOLINT ifopt::Problem nlp; // 3) Add Variables - std::vector vars; + std::vector> nodes; + std::vector> vars; std::vector positions; { + nodes.push_back(std::make_unique("Joint_Position_0")); Eigen::VectorXd pos(2); pos << -1.9, 0; positions.push_back(pos); - auto var = std::make_shared(pos, manip->getJointNames(), "Joint_Position_0"); - vars.push_back(var); - nlp.AddVariableSet(var); + vars.push_back(nodes.back()->addVar("position", manip->getJointNames(), pos, bounds)); } + { + nodes.push_back(std::make_unique("Joint_Position_1")); Eigen::VectorXd pos(2); pos << 0, 1.9; positions.push_back(pos); - auto var = std::make_shared(pos, manip->getJointNames(), "Joint_Position_1"); - vars.push_back(var); - nlp.AddVariableSet(var); + vars.push_back(nodes.back()->addVar("position", manip->getJointNames(), pos, bounds)); } + { + nodes.push_back(std::make_unique("Joint_Position_2")); Eigen::VectorXd pos(2); pos << 1.9, 3.8; positions.push_back(pos); - auto var = std::make_shared(pos, manip->getJointNames(), "Joint_Position_2"); - vars.push_back(var); - nlp.AddVariableSet(var); + vars.push_back(nodes.back()->addVar("position", manip->getJointNames(), pos, bounds)); } + nlp.AddVariableSet(std::make_shared("joint_trajectory", std::move(nodes))); + // Step 3: Setup collision const double margin_coeff = 1; const double margin = 0.02; @@ -133,16 +139,14 @@ TEST_F(CastTest, boxes) // NOLINT // 4) Add constraints { // Fix start position - const std::vector fixed_vars = { vars[0] }; const Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(manip->numJoints(), 1); - auto cnt = std::make_shared(positions[0], fixed_vars, coeffs); + auto cnt = std::make_shared(positions[0], vars[0], coeffs); nlp.AddConstraintSet(cnt); } { // Fix end position - const std::vector fixed_vars = { vars[2] }; const Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(manip->numJoints(), 1); - auto cnt = std::make_shared(positions[2], fixed_vars, coeffs); + auto cnt = std::make_shared(positions[2], vars[2], coeffs); nlp.AddConstraintSet(cnt); } @@ -153,7 +157,7 @@ TEST_F(CastTest, boxes) // NOLINT auto collision_evaluator = std::make_shared( collision_cache, manip, env, trajopt_collision_config); - const std::array position_vars{ vars[i - 1], vars[i] }; + const std::array, 2> position_vars{ vars[i - 1], vars[i] }; auto cnt = std::make_shared( collision_evaluator, position_vars, vars_fixed[0], vars_fixed[1], 1, true); nlp.AddConstraintSet(cnt); diff --git a/trajopt_ifopt/test/collision_unit.cpp b/trajopt_ifopt/test/collision_unit.cpp index 3087a351..6b7d49f2 100644 --- a/trajopt_ifopt/test/collision_unit.cpp +++ b/trajopt_ifopt/test/collision_unit.cpp @@ -38,7 +38,10 @@ TRAJOPT_IGNORE_WARNINGS_POP #include #include -#include +#include +#include +#include +#include using namespace trajopt_ifopt; using namespace tesseract_environment; @@ -73,10 +76,15 @@ class CollisionUnit : public testing::TestWithParam std::make_shared(collision_cache, kin, env, config); // 3) Add Variables + const std::vector bounds(static_cast(kin->numJoints()), ifopt::NoBound); Eigen::VectorXd pos(2); pos << -1.9, 0; - auto var0 = std::make_shared(pos, kin->getJointNames(), "Joint_Position_0"); - nlp.AddVariableSet(var0); + auto node = std::make_unique("Joint_Position_0"); + auto var0 = node->addVar("position", kin->getJointNames(), pos, bounds); + + std::vector> nodes; + nodes.push_back(std::move(node)); + nlp.AddVariableSet(std::make_shared("joint_trajectory", std::move(nodes))); constraint = std::make_shared(collision_evaluator, var0, 1); nlp.AddConstraintSet(constraint); @@ -101,7 +109,7 @@ TEST_F(CollisionUnit, GetValueFillJacobian) // NOLINT ifopt::ConstraintSet::Jacobian jac_block; jac_block.resize(1, 2); - constraint->FillJacobianBlock("Joint_Position_0", jac_block); + constraint->FillJacobianBlock("joint_trajectory", jac_block); const double dx = jac_block.coeff(0, 0); const double dy = jac_block.coeff(0, 1); EXPECT_NEAR(dx, 0.0, 1e-6); @@ -118,7 +126,7 @@ TEST_F(CollisionUnit, GetValueFillJacobian) // NOLINT ifopt::ConstraintSet::Jacobian jac_block; jac_block.resize(1, 2); - constraint->FillJacobianBlock("Joint_Position_0", jac_block); + constraint->FillJacobianBlock("joint_trajectory", jac_block); const double dx = jac_block.coeff(0, 0); const double dy = jac_block.coeff(0, 1); EXPECT_NEAR(dx, 1.0, 1e-6); @@ -133,7 +141,7 @@ TEST_F(CollisionUnit, GetValueFillJacobian) // NOLINT ifopt::ConstraintSet::Jacobian jac_block; jac_block.resize(1, 2); - constraint->FillJacobianBlock("Joint_Position_0", jac_block); + constraint->FillJacobianBlock("joint_trajectory", jac_block); const double dx = jac_block.coeff(0, 0); const double dy = jac_block.coeff(0, 1); EXPECT_NEAR(dx, 0.0, 1e-6); @@ -150,7 +158,7 @@ TEST_F(CollisionUnit, GetValueFillJacobian) // NOLINT ifopt::ConstraintSet::Jacobian jac_block; jac_block.resize(1, 2); - constraint->FillJacobianBlock("Joint_Position_0", jac_block); + constraint->FillJacobianBlock("joint_trajectory", jac_block); const double dx = jac_block.coeff(0, 0); const double dy = jac_block.coeff(0, 1); EXPECT_NEAR(dx, 1.0, 1e-6); @@ -167,14 +175,16 @@ TEST_F(CollisionUnit, GetSetBounds) // NOLINT // Check that setting bounds works { + std::vector bounds_vec{ ifopt::NoBound, ifopt::NoBound }; Eigen::VectorXd pos(2); pos << -1.9, 0; const std::vector joint_names(2, "names"); - auto var0 = std::make_shared(pos, joint_names, "Joint_Position_0"); + auto node = std::make_unique("Joint_Position_0"); + auto var0 = node->addVar("position", joint_names, pos, bounds_vec); auto constraint_2 = std::make_shared(collision_evaluator, var0, 3); const ifopt::Bounds bounds(-0.1234, 0.5678); - std::vector bounds_vec = std::vector(1, bounds); + bounds_vec = std::vector(1, bounds); constraint_2->SetBounds(bounds_vec); std::vector results_vec = constraint_2->GetBounds(); for (std::size_t i = 0; i < bounds_vec.size(); i++) diff --git a/trajopt_ifopt/test/continuous_collision_gradient_unit.cpp b/trajopt_ifopt/test/continuous_collision_gradient_unit.cpp index 81812188..b68b1598 100644 --- a/trajopt_ifopt/test/continuous_collision_gradient_unit.cpp +++ b/trajopt_ifopt/test/continuous_collision_gradient_unit.cpp @@ -45,7 +45,9 @@ TRAJOPT_IGNORE_WARNINGS_POP #include #include #include -#include +#include +#include +#include #include #include @@ -87,6 +89,7 @@ void runContinuousGradientTest(const Environment::Ptr& env, double coeff) const tesseract_scene_graph::StateSolver::Ptr state_solver = env->getStateSolver(); const ContinuousContactManager::Ptr manager = env->getContinuousContactManager(); const tesseract_kinematics::JointGroup::ConstPtr manip = env->getJointGroup("manipulator"); + const std::vector bounds = trajopt_ifopt::toBounds(manip->getLimits().joint_limits); manager->setActiveCollisionObjects(manip->getActiveLinkNames()); manager->setDefaultCollisionMargin(0); @@ -97,33 +100,35 @@ void runContinuousGradientTest(const Environment::Ptr& env, double coeff) ifopt::Problem nlp; // 3) Add Variables - std::vector vars; + std::vector> nodes; + std::vector> vars; std::vector positions; { + nodes.push_back(std::make_unique("Joint_Position_0")); Eigen::VectorXd pos(2); pos << -1.9, 0; positions.push_back(pos); - auto var = std::make_shared(pos, manip->getJointNames(), "Joint_Position_0"); - vars.push_back(var); - nlp.AddVariableSet(var); + vars.push_back(nodes.back()->addVar("position", manip->getJointNames(), pos, bounds)); } + { + nodes.push_back(std::make_unique("Joint_Position_1")); Eigen::VectorXd pos(2); pos << 0, 1.9; positions.push_back(pos); - auto var = std::make_shared(pos, manip->getJointNames(), "Joint_Position_1"); - vars.push_back(var); - nlp.AddVariableSet(var); + vars.push_back(nodes.back()->addVar("position", manip->getJointNames(), pos, bounds)); } + { + nodes.push_back(std::make_unique("Joint_Position_2")); Eigen::VectorXd pos(2); pos << 1.9, 3.8; positions.push_back(pos); - auto var = std::make_shared(pos, manip->getJointNames(), "Joint_Position_2"); - vars.push_back(var); - nlp.AddVariableSet(var); + vars.push_back(nodes.back()->addVar("position", manip->getJointNames(), pos, bounds)); } + nlp.AddVariableSet(std::make_shared("joint_trajectory", std::move(nodes))); + // Step 3: Setup collision const double margin_coeff = coeff; const double margin = 0.02; @@ -137,7 +142,7 @@ void runContinuousGradientTest(const Environment::Ptr& env, double coeff) auto collision_evaluator = std::make_shared( collision_cache, manip, env, trajopt_collision_config); - const std::array position_vars{ vars[i - 1], vars[i] }; + const std::array, 2> position_vars{ vars[i - 1], vars[i] }; auto cnt = std::make_shared( collision_evaluator, position_vars, false, false, 3); nlp.AddConstraintSet(cnt); diff --git a/trajopt_ifopt/test/cost_wrappers_unit.cpp b/trajopt_ifopt/test/cost_wrappers_unit.cpp index 2534b35b..26bc2720 100644 --- a/trajopt_ifopt/test/cost_wrappers_unit.cpp +++ b/trajopt_ifopt/test/cost_wrappers_unit.cpp @@ -30,7 +30,9 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include TRAJOPT_IGNORE_WARNINGS_POP -#include +#include +#include +#include #include #include #include @@ -46,7 +48,7 @@ class SimpleTestConstraint : public ifopt::ConstraintSet public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW - SimpleTestConstraint(trajopt_ifopt::JointPosition::ConstPtr position_var, + SimpleTestConstraint(std::shared_ptr position_var, const std::string& name = "SimpleTestConstraint") : ifopt::ConstraintSet(1, name), position_var_(std::move(position_var)) { @@ -56,8 +58,8 @@ class SimpleTestConstraint : public ifopt::ConstraintSet Eigen::VectorXd GetValues() const final { Eigen::VectorXd output(1); - Eigen::VectorXd joint_vals = this->GetVariables()->GetComponent(position_var_->GetName())->GetValues(); - output(0) = std::pow(joint_vals(0), 2) + 4 * joint_vals(0) + 3; + double v = position_var_->value()(0); + output(0) = std::pow(v, 2) + (4 * v) + 3; return output; } @@ -66,15 +68,13 @@ class SimpleTestConstraint : public ifopt::ConstraintSet void FillJacobianBlock(std::string var_set, Jacobian& jac_block) const final { // Only modify the jacobian if this constraint uses var_set - if (var_set != position_var_->GetName()) // NOLINT + if (var_set != position_var_->getParent()->getParent()->GetName()) // NOLINT return; // dy = 2x + 4; - Eigen::VectorXd joint_vals = this->GetVariables()->GetComponent(position_var_->GetName())->GetValues(); - // Reserve enough room in the sparse matrix jac_block.reserve(1); - jac_block.coeffRef(0, 0) = 2 * joint_vals(0) + 4; + jac_block.coeffRef(0, position_var_->getIndex()) = (2 * position_var_->value()(0)) + 4; } private: @@ -84,7 +84,7 @@ class SimpleTestConstraint : public ifopt::ConstraintSet /** @brief Pointers to the vars used by this constraint. * * Do not access them directly. Instead use this->GetVariables()->GetComponent(position_var->GetName())->GetValues()*/ - trajopt_ifopt::JointPosition::ConstPtr position_var_; + std::shared_ptr position_var_; }; /** @brief Tests that GetValues and FillJacobianBlock return the correct values */ @@ -94,33 +94,33 @@ TEST(CostWrapperUnit, SquaredCost) // NOLINT ifopt::Problem nlp; // 3) Add Variables - std::vector vars; - std::vector positions; + std::vector> nodes; + std::shared_ptr var; { + auto node = std::make_unique("Node"); Eigen::VectorXd pos(1); pos << -4; - positions.push_back(pos); const std::vector var_names = { "x" }; - auto var = std::make_shared(pos, var_names); - vars.push_back(var); - nlp.AddVariableSet(var); + var = node->addVar("position", { "x" }, pos, { ifopt::NoBound }); + nodes.push_back(std::move(node)); } + nlp.AddVariableSet(std::make_shared("joint-trajectory", std::move(nodes))); // 4) add cost - auto cnt = std::make_shared(vars[0]); + auto cnt = std::make_shared(var); cnt->LinkWithVariables(nlp.GetOptVariables()); auto cost = std::make_shared(cnt); nlp.AddCostSet(cost); - auto exact_jac = nlp.EvaluateCostFunctionGradient(positions[0].data()); - auto numerical_jac = trajopt_ifopt::calcNumericalCostGradient(positions[0].data(), nlp, 1e-8); + auto exact_jac = nlp.EvaluateCostFunctionGradient(var->value().data()); + auto numerical_jac = trajopt_ifopt::calcNumericalCostGradient(var->value().data(), nlp, 1e-8); EXPECT_NEAR(exact_jac(0, 0), numerical_jac(0, 0), 1e-6); // Squared cost jacobian = 2 * y(x) * dy(x) // y(x) = x^2 + 4x + 3 // dy(x) = 2x + 4 - const double x = positions[0](0); + const double x = var->value()(0); const double jac = 2 * (std::pow(x, 2) + 4 * x + 3) * (2 * x + 4); EXPECT_NEAR(exact_jac(0, 0), jac, 1e-6); EXPECT_NEAR(numerical_jac(0, 0), jac, 1e-6); @@ -133,35 +133,35 @@ TEST(CostWrapperUnit, WeightedSquaredCost) // NOLINT ifopt::Problem nlp; // 3) Add Variables - std::vector vars; - std::vector positions; + std::vector> nodes; + std::shared_ptr var; { + auto node = std::make_unique("Node"); Eigen::VectorXd pos(1); pos << -4; - positions.push_back(pos); const std::vector var_names = { "x" }; - auto var = std::make_shared(pos, var_names); - vars.push_back(var); - nlp.AddVariableSet(var); + var = node->addVar("position", { "x" }, pos, { ifopt::NoBound }); + nodes.push_back(std::move(node)); } + nlp.AddVariableSet(std::make_shared("joint-trajectory", std::move(nodes))); // 4) add cost Eigen::VectorXd weights(1); weights(0) = 2; - auto cnt = std::make_shared(vars[0]); + auto cnt = std::make_shared(var); cnt->LinkWithVariables(nlp.GetOptVariables()); auto cost = std::make_shared(cnt, weights); nlp.AddCostSet(cost); - auto exact_jac = nlp.EvaluateCostFunctionGradient(positions[0].data()); - auto numerical_jac = trajopt_ifopt::calcNumericalCostGradient(positions[0].data(), nlp, 1e-8); + auto exact_jac = nlp.EvaluateCostFunctionGradient(var->value().data()); + auto numerical_jac = trajopt_ifopt::calcNumericalCostGradient(var->value().data(), nlp, 1e-8); EXPECT_NEAR(exact_jac(0, 0), numerical_jac(0, 0), 1e-6); // Squared cost jacobian = 2 * y(x) * dy(x) // y(x) = x^2 + 4x + 3 // dy(x) = 2x + 4 - const double x = positions[0](0); + const double x = var->value()(0); const double jac = 2 * weights(0) * (std::pow(x, 2) + 4 * x + 3) * (2 * x + 4); EXPECT_NEAR(exact_jac(0, 0), jac, 1e-6); EXPECT_NEAR(numerical_jac(0, 0), jac, 1e-6); @@ -174,34 +174,34 @@ TEST(CostWrapperUnit, AbsoluteCost) // NOLINT ifopt::Problem nlp; // 3) Add Variables - std::vector vars; - std::vector positions; + std::vector> nodes; + std::shared_ptr var; { + auto node = std::make_unique("Node"); Eigen::VectorXd pos(1); pos << -4; - positions.push_back(pos); const std::vector var_names = { "x" }; - auto var = std::make_shared(pos, var_names); - vars.push_back(var); - nlp.AddVariableSet(var); + var = node->addVar("position", { "x" }, pos, { ifopt::NoBound }); + nodes.push_back(std::move(node)); } + nlp.AddVariableSet(std::make_shared("joint-trajectory", std::move(nodes))); // 4) add cost - auto cnt = std::make_shared(vars[0]); + auto cnt = std::make_shared(var); cnt->LinkWithVariables(nlp.GetOptVariables()); auto cost = std::make_shared(cnt); nlp.AddCostSet(cost); - auto exact_jac = nlp.EvaluateCostFunctionGradient(positions[0].data()); - auto numerical_jac = trajopt_ifopt::calcNumericalCostGradient(positions[0].data(), nlp, 1e-8); + auto exact_jac = nlp.EvaluateCostFunctionGradient(var->value().data()); + auto numerical_jac = trajopt_ifopt::calcNumericalCostGradient(var->value().data(), nlp, 1e-8); EXPECT_NEAR(exact_jac(0, 0), numerical_jac(0, 0), 1e-6); // Absolute cost jacobian = (y(x) / |y(x)|) * dy(x) // y(x) = x^2 + 4x + 3 // dy(x) = 2x + 4 - const double x = positions[0](0); - const double jac = ((std::pow(x, 2) + 4 * x + 3) / std::abs(std::pow(x, 2) + 4 * x + 3)) * (2 * x + 4); + const double x = var->value()(0); + const double jac = ((std::pow(x, 2) + (4 * x) + 3) / std::abs(std::pow(x, 2) + (4 * x) + 3)) * ((2 * x) + 4); EXPECT_NEAR(exact_jac(0, 0), jac, 1e-6); EXPECT_NEAR(numerical_jac(0, 0), jac, 1e-6); } @@ -213,36 +213,37 @@ TEST(CostWrapperUnit, WeightedAbsoluteCost) // NOLINT ifopt::Problem nlp; // 3) Add Variables - std::vector vars; - std::vector positions; + std::vector> nodes; + std::shared_ptr var; { + auto node = std::make_unique("Node"); Eigen::VectorXd pos(1); pos << -4; - positions.push_back(pos); const std::vector var_names = { "x" }; - auto var = std::make_shared(pos, var_names); - vars.push_back(var); - nlp.AddVariableSet(var); + var = node->addVar("position", { "x" }, pos, { ifopt::NoBound }); + nodes.push_back(std::move(node)); } + nlp.AddVariableSet(std::make_shared("joint-trajectory", std::move(nodes))); // 4) add cost Eigen::VectorXd weights(1); weights(0) = 2; - auto cnt = std::make_shared(vars[0]); + auto cnt = std::make_shared(var); cnt->LinkWithVariables(nlp.GetOptVariables()); auto cost = std::make_shared(cnt, weights); nlp.AddCostSet(cost); - auto exact_jac = nlp.EvaluateCostFunctionGradient(positions[0].data()); - auto numerical_jac = trajopt_ifopt::calcNumericalCostGradient(positions[0].data(), nlp, 1e-8); // NOLINT + auto exact_jac = nlp.EvaluateCostFunctionGradient(var->value().data()); + auto numerical_jac = trajopt_ifopt::calcNumericalCostGradient(var->value().data(), nlp, 1e-8); // NOLINT EXPECT_NEAR(exact_jac(0, 0), numerical_jac(0, 0), 1e-6); // Absolute cost jacobian = (y(x) / |y(x)|) * dy(x) // y(x) = x^2 + 4x + 3 // dy(x) = 2x + 4 - const double x = positions[0](0); - const double jac = (weights(0) * (std::pow(x, 2) + 4 * x + 3) / std::abs(std::pow(x, 2) + 4 * x + 3)) * (2 * x + 4); + const double x = var->value()(0); + const double jac = + (weights(0) * (std::pow(x, 2) + (4 * x) + 3) / std::abs(std::pow(x, 2) + (4 * x) + 3)) * ((2 * x) + 4); EXPECT_NEAR(exact_jac(0, 0), jac, 1e-6); EXPECT_NEAR(numerical_jac(0, 0), jac, 1e-6); } diff --git a/trajopt_ifopt/test/discrete_collision_gradient_unit.cpp b/trajopt_ifopt/test/discrete_collision_gradient_unit.cpp index 901877bd..cd7f5634 100644 --- a/trajopt_ifopt/test/discrete_collision_gradient_unit.cpp +++ b/trajopt_ifopt/test/discrete_collision_gradient_unit.cpp @@ -45,7 +45,9 @@ TRAJOPT_IGNORE_WARNINGS_POP #include #include #include -#include +#include +#include +#include #include using namespace trajopt_ifopt; @@ -82,6 +84,7 @@ void runDiscreteGradientTest(const Environment::Ptr& env, double coeff) tesseract_scene_graph::StateSolver::Ptr const state_solver = env->getStateSolver(); DiscreteContactManager::Ptr const manager = env->getDiscreteContactManager(); const tesseract_kinematics::JointGroup::ConstPtr manip = env->getJointGroup("manipulator"); + const std::vector bounds = trajopt_ifopt::toBounds(manip->getLimits().joint_limits); manager->setActiveCollisionObjects(manip->getActiveLinkNames()); manager->setDefaultCollisionMargin(0); @@ -92,17 +95,19 @@ void runDiscreteGradientTest(const Environment::Ptr& env, double coeff) ifopt::Problem nlp; // 3) Add Variables - std::vector vars; + std::vector> nodes; + std::vector> vars; std::vector positions; { + nodes.push_back(std::make_unique("Joint_Position_0")); Eigen::VectorXd pos(2); pos << -0.75, 0.75; positions.push_back(pos); - auto var = std::make_shared(pos, manip->getJointNames(), "Joint_Position_0"); - vars.push_back(var); - nlp.AddVariableSet(var); + vars.push_back(nodes.back()->addVar("position", manip->getJointNames(), pos, bounds)); } + nlp.AddVariableSet(std::make_shared("joint_trajectory", std::move(nodes))); + // Step 3: Setup collision const double margin_coeff = coeff; const double margin = 0.2; diff --git a/trajopt_ifopt/test/inverse_kinematics_constraint_unit.cpp b/trajopt_ifopt/test/inverse_kinematics_constraint_unit.cpp index 2f6272a3..825de417 100644 --- a/trajopt_ifopt/test/inverse_kinematics_constraint_unit.cpp +++ b/trajopt_ifopt/test/inverse_kinematics_constraint_unit.cpp @@ -37,8 +37,11 @@ TRAJOPT_IGNORE_WARNINGS_PUSH TRAJOPT_IGNORE_WARNINGS_POP #include -#include +#include +#include +#include #include +#include using namespace trajopt_ifopt; using namespace std; @@ -58,6 +61,7 @@ class InverseKinematicsConstraintUnit : public testing::TestWithParam joint_bounds; Eigen::Index n_dof{ -1 }; @@ -73,17 +77,21 @@ class InverseKinematicsConstraintUnit : public testing::TestWithParamgetKinematicGroup("right_arm"); n_dof = kin_group->numJoints(); + joint_bounds = trajopt_ifopt::toBounds(kin_group->getLimits().joint_limits); kinematic_info = std::make_shared(kin_group, "base_footprint", "r_gripper_tool_frame"); auto pos = Eigen::VectorXd::Ones(kin_group->numJoints()); - auto var0 = std::make_shared(pos, kin_group->getJointNames(), "Joint_Position_0"); - auto var1 = std::make_shared(pos, kin_group->getJointNames(), "Joint_Position_1"); - nlp.AddVariableSet(var0); - nlp.AddVariableSet(var1); + std::vector> nodes; + nodes.push_back(std::make_unique("Joint_Position_0")); + auto var0 = nodes.back()->addVar("position", kin_group->getJointNames(), pos, joint_bounds); + nodes.push_back(std::make_unique("Joint_Position_1")); + auto var1 = nodes.back()->addVar("position", kin_group->getJointNames(), pos, joint_bounds); - // 4) Add constraints + nlp.AddVariableSet(std::make_shared("joint_trajectory", std::move(nodes))); + + // Add constraints auto target_pose = Eigen::Isometry3d::Identity(); constraint = std::make_shared(target_pose, kinematic_info, var0, var1); nlp.AddConstraintSet(constraint); @@ -111,7 +119,7 @@ TEST_F(InverseKinematicsConstraintUnit, GetValue) // NOLINT { ifopt::ConstraintSet::Jacobian jac_block; jac_block.resize(n_dof, n_dof); - constraint->FillJacobianBlock("Joint_Position_0", jac_block); + constraint->FillJacobianBlock("joint_trajectory", jac_block); // Check that the size is correct EXPECT_EQ(jac_block.nonZeros(), n_dof); // Check that it is identity @@ -131,7 +139,7 @@ TEST_F(InverseKinematicsConstraintUnit, GetValue) // NOLINT { ifopt::ConstraintSet::Jacobian jac_block; jac_block.resize(n_dof, n_dof); - constraint->FillJacobianBlock("Joint_Position_1", jac_block); + constraint->FillJacobianBlock("invalid_var_set", jac_block); EXPECT_EQ(jac_block.nonZeros(), 0); } } @@ -146,8 +154,11 @@ TEST_F(InverseKinematicsConstraintUnit, GetSetBounds) // NOLINT // Check that setting bounds works { const Eigen::VectorXd pos = Eigen::VectorXd::Ones(n_dof); - auto var0 = std::make_shared(pos, kin_group->getJointNames(), "Joint_Position_0"); - auto var1 = std::make_shared(pos, kin_group->getJointNames(), "Joint_Position_1"); + std::vector> nodes; + nodes.push_back(std::make_unique("Joint_Position_0")); + auto var0 = nodes.back()->addVar("position", kin_group->getJointNames(), pos, joint_bounds); + nodes.push_back(std::make_unique("Joint_Position_1")); + auto var1 = nodes.back()->addVar("position", kin_group->getJointNames(), pos, joint_bounds); auto target_pose = Eigen::Isometry3d::Identity(); auto constraint_2 = diff --git a/trajopt_ifopt/test/joint_terms_unit.cpp b/trajopt_ifopt/test/joint_terms_unit.cpp index 816a4bf2..3c80f9cd 100644 --- a/trajopt_ifopt/test/joint_terms_unit.cpp +++ b/trajopt_ifopt/test/joint_terms_unit.cpp @@ -33,7 +33,9 @@ TRAJOPT_IGNORE_WARNINGS_POP #include #include #include -#include +#include +#include +#include #include using namespace trajopt_ifopt; @@ -44,26 +46,53 @@ TEST(JointTermsUnit, JointPosConstraintUnit) // NOLINT { CONSOLE_BRIDGE_logDebug("JointTermsUnit, JointPosConstraintUnit"); - std::vector position_vars; const std::vector joint_names(10, "name"); - Eigen::VectorXd init1(10); - init1 << 0, 1, 2, 3, 4, 5, 6, 7, 8, 9; - position_vars.push_back(std::make_shared(init1, joint_names, "test_var1")); + Eigen::VectorXd init_vals(10); + init_vals << 0, 1, 2, 3, 4, 5, 6, 7, 8, 9; + std::vector bounds(10, ifopt::NoBound); - Eigen::VectorXd init2(10); - init2 << 10, 11, 12, 13, 14, 15, 16, 17, 18, 19; - position_vars.push_back(std::make_shared(init2, joint_names, "test_var2")); + auto node = std::make_unique(); + std::shared_ptr position_var = node->addVar("state", joint_names, init_vals, bounds); + std::vector> nodes; + nodes.push_back(std::move(node)); - Eigen::VectorXd targets(10); - targets << 20, 21, 22, 23, 24, 25, 26, 27, 28, 29; + auto variables = std::make_shared("variable-sets", false); + variables->AddComponent(std::make_shared("joint_trajectory", std::move(nodes))); + + std::vector targets; + Eigen::VectorXd target(10); + target << 20, 21, 22, 23, 24, 25, 26, 27, 28, 29; const std::string name("test_cnt"); const Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(10, 1); - const JointPosConstraint position_cnt(targets, position_vars, coeffs, name); + JointPosConstraint position_cnt(target, position_var, coeffs, name); - EXPECT_EQ(position_cnt.GetRows(), targets.size() * static_cast(position_vars.size())); + // Must link with variables or GetValues and GetJacobian throw exception. + position_cnt.LinkWithVariables(variables); + + EXPECT_EQ(position_cnt.GetRows(), static_cast(target.size())); EXPECT_EQ(position_cnt.GetName(), name); - EXPECT_EQ(position_cnt.GetBounds().size(), targets.size() * static_cast(position_vars.size())); + EXPECT_EQ(position_cnt.GetBounds().size(), static_cast(target.size())); + + Eigen::VectorXd position_vals = position_cnt.GetValues(); + EXPECT_EQ(position_vals.size(), static_cast(target.size())); + + // Test forward diff + EXPECT_TRUE(position_vals.isApprox(init_vals, 1e-6)); + + ifopt::ConstraintSet::Jacobian jac = position_cnt.GetJacobian(); + ifopt::Problem::Jacobian num_jac = trajopt_ifopt::calcNumericalConstraintGradient(*variables, position_cnt); + + EXPECT_EQ(jac.rows(), target.size()); + EXPECT_EQ(jac.cols(), target.size()); + + for (Eigen::Index i = 0; i < target.size(); ++i) + { + for (Eigen::Index j = 0; j < target.size(); ++j) + { + EXPECT_NEAR(jac.coeffRef(i, j), num_jac.coeffRef(i, j), 1e-3); + } + } } /** @brief Tests the Joint Velocity Constraint */ @@ -72,24 +101,29 @@ TEST(JointTermsUnit, JointVelConstraintUnit) // NOLINT CONSOLE_BRIDGE_logDebug("JointTermsUnit, JointVelConstraintUnit"); // y = x^3 + 5*x^2 + 2*x + 1 - auto f = [](double x) { return (x * x * x + 5 * x * x + 2 * x + 1); }; + auto f = [](double x) { return ((x * x * x) + (5 * x * x) + (2 * x) + 1); }; - auto variables = std::make_shared("variable-sets", false); - std::vector position_vars; + std::vector> nodes; + std::vector> position_vars; position_vars.reserve(27); std::vector x_vals; x_vals.reserve(27); for (int i = -13; i < 14; ++i) { + auto node = std::make_unique(); + const std::string var_id = "test_var_" + std::to_string(position_vars.size()); const std::vector joint_names{ "x", "y" }; - Eigen::VectorXd val(2); - val << f(i), f(i); - auto var = std::make_shared(val, joint_names, "test_var_" + std::to_string(position_vars.size())); - position_vars.push_back(var); - variables->AddComponent(var); + std::vector bounds(2, ifopt::NoBound); + Eigen::VectorXd vals(2); + vals << f(i), f(i); + position_vars.push_back(node->addVar(var_id, joint_names, vals, bounds)); + nodes.push_back(std::move(node)); + x_vals.push_back(i); } + auto variables = std::make_shared("variable-sets", false); + variables->AddComponent(std::make_shared("joint_trajectory", std::move(nodes))); Eigen::VectorXd targets(2); targets << 0, 0; @@ -119,7 +153,6 @@ TEST(JointTermsUnit, JointVelConstraintUnit) // NOLINT ifopt::ConstraintSet::Jacobian jac = velocity_cnt.GetJacobian(); ifopt::Problem::Jacobian num_jac = trajopt_ifopt::calcNumericalConstraintGradient(*variables, velocity_cnt); - EXPECT_EQ(jac.rows(), static_cast(x_vals.size() - 1) * targets.size()); EXPECT_EQ(jac.cols(), static_cast(x_vals.size()) * targets.size()); @@ -138,25 +171,31 @@ TEST(JointTermsUnit, JointVelConstraintMinimumUnit) // NOLINT CONSOLE_BRIDGE_logDebug("JointTermsUnit, JointVelConstraintMinimumUnit"); // y = x^3 + 5*x^2 + 2*x + 1 - auto f = [](double x) { return (x * x * x + 5 * x * x + 2 * x + 1); }; + auto f = [](double x) { return ((x * x * x) + (5 * x * x) + (2 * x) + 1); }; - auto variables = std::make_shared("variable-sets", false); - std::vector position_vars; + std::vector> nodes; + std::vector> position_vars; position_vars.reserve(2); std::vector x_vals; x_vals.reserve(2); for (int i = -13; i < -11; ++i) { + auto node = std::make_unique(); + const std::string var_id = "test_var_" + std::to_string(position_vars.size()); const std::vector joint_names{ "x", "y" }; - Eigen::VectorXd val(2); - val << f(i), f(i); - auto var = std::make_shared(val, joint_names, "test_var_" + std::to_string(position_vars.size())); - position_vars.push_back(var); - variables->AddComponent(var); + std::vector bounds(2, ifopt::NoBound); + Eigen::VectorXd vals(2); + vals << f(i), f(i); + position_vars.push_back(node->addVar(var_id, joint_names, vals, bounds)); + nodes.push_back(std::move(node)); + x_vals.push_back(i); } + auto variables = std::make_shared("variable-sets", false); + variables->AddComponent(std::make_shared("joint_trajectory", std::move(nodes))); + Eigen::VectorXd targets(2); targets << 0, 0; const std::string name("test_joint_vel_cnt"); @@ -204,25 +243,31 @@ TEST(JointTermsUnit, JointAccelConstraintUnit) // NOLINT CONSOLE_BRIDGE_logDebug("JointTermsUnit, JointVelConstraintUnit"); // y = x^3 + 5*x^2 + 2*x + 1 - auto f = [](double x) { return (x * x * x + 5 * x * x + 2 * x + 1); }; + auto f = [](double x) { return ((x * x * x) + (5 * x * x) + (2 * x) + 1); }; - auto variables = std::make_shared("variable-sets", false); - std::vector position_vars; + std::vector> nodes; + std::vector> position_vars; position_vars.reserve(27); std::vector x_vals; x_vals.reserve(27); for (int i = -13; i < 14; ++i) { + auto node = std::make_unique(); + const std::string var_id = "test_var_" + std::to_string(position_vars.size()); const std::vector joint_names{ "x", "y" }; - Eigen::VectorXd val(2); - val << f(i), f(i); - auto var = std::make_shared(val, joint_names, "test_var_" + std::to_string(position_vars.size())); - position_vars.push_back(var); - variables->AddComponent(var); + std::vector bounds(2, ifopt::NoBound); + Eigen::VectorXd vals(2); + vals << f(i), f(i); + position_vars.push_back(node->addVar(var_id, joint_names, vals, bounds)); + nodes.push_back(std::move(node)); + x_vals.push_back(i); } + auto variables = std::make_shared("variable-sets", false); + variables->AddComponent(std::make_shared("joint_trajectory", std::move(nodes))); + Eigen::VectorXd targets(2); targets << 0, 0; const std::string name("test_joint_accel_cnt"); @@ -244,7 +289,7 @@ TEST(JointTermsUnit, JointAccelConstraintUnit) // NOLINT { for (Eigen::Index j = 0; j < targets.size(); ++j) { - const double expected_val = (f(x_vals[i]) - 2.0 * f(x_vals[i + 1]) + f(x_vals[i + 2])); + const double expected_val = (f(x_vals[i]) - (2.0 * f(x_vals[i + 1])) + f(x_vals[i + 2])); EXPECT_NEAR(accel_vals((static_cast(i) * targets.size()) + j), expected_val, 1e-6); } } @@ -254,7 +299,7 @@ TEST(JointTermsUnit, JointAccelConstraintUnit) // NOLINT { for (Eigen::Index j = 0; j < targets.size(); ++j) { - const double expected_val = (f(x_vals[i]) - 2.0 * f(x_vals[i - 1]) + f(x_vals[i - 2])); + const double expected_val = (f(x_vals[i]) - (2.0 * f(x_vals[i - 1])) + f(x_vals[i - 2])); EXPECT_NEAR(accel_vals((static_cast(i) * targets.size()) + j), expected_val, 1e-6); } } @@ -280,25 +325,31 @@ TEST(JointTermsUnit, JointAccelConstraintMinimumUnit) // NOLINT CONSOLE_BRIDGE_logDebug("JointTermsUnit, JointAccelConstraintMinimumUnit"); // y = x^3 + 5*x^2 + 2*x + 1 - auto f = [](double x) { return (x * x * x + 5 * x * x + 2 * x + 1); }; + auto f = [](double x) { return ((x * x * x) + (5 * x * x) + (2 * x) + 1); }; - auto variables = std::make_shared("variable-sets", false); - std::vector position_vars; + std::vector> nodes; + std::vector> position_vars; position_vars.reserve(4); std::vector x_vals; x_vals.reserve(4); for (int i = -13; i < -9; ++i) { + auto node = std::make_unique(); + const std::string var_id = "test_var_" + std::to_string(position_vars.size()); const std::vector joint_names{ "x", "y" }; - Eigen::VectorXd val(2); - val << f(i), f(i); - auto var = std::make_shared(val, joint_names, "test_var_" + std::to_string(position_vars.size())); - position_vars.push_back(var); - variables->AddComponent(var); + std::vector bounds(2, ifopt::NoBound); + Eigen::VectorXd vals(2); + vals << f(i), f(i); + position_vars.push_back(node->addVar(var_id, joint_names, vals, bounds)); + nodes.push_back(std::move(node)); + x_vals.push_back(i); } + auto variables = std::make_shared("variable-sets", false); + variables->AddComponent(std::make_shared("joint_trajectory", std::move(nodes))); + Eigen::VectorXd targets(2); targets << 0, 0; const std::string name("test_joint_accel_cnt"); @@ -320,7 +371,7 @@ TEST(JointTermsUnit, JointAccelConstraintMinimumUnit) // NOLINT { for (Eigen::Index j = 0; j < targets.size(); ++j) { - const double expected_val = (f(x_vals[i]) - 2.0 * f(x_vals[i + 1]) + f(x_vals[i + 2])); + const double expected_val = (f(x_vals[i]) - (2.0 * f(x_vals[i + 1])) + f(x_vals[i + 2])); EXPECT_NEAR(accel_vals((static_cast(i) * targets.size()) + j), expected_val, 1e-6); } } @@ -330,7 +381,7 @@ TEST(JointTermsUnit, JointAccelConstraintMinimumUnit) // NOLINT { for (Eigen::Index j = 0; j < targets.size(); ++j) { - const double expected_val = (f(x_vals[i]) - 2.0 * f(x_vals[i - 1]) + f(x_vals[i - 2])); + const double expected_val = (f(x_vals[i]) - (2.0 * f(x_vals[i - 1])) + f(x_vals[i - 2])); EXPECT_NEAR(accel_vals((static_cast(i) * targets.size()) + j), expected_val, 1e-6); } } @@ -356,25 +407,31 @@ TEST(JointTermsUnit, JointJerkConstraintUnit) // NOLINT CONSOLE_BRIDGE_logDebug("JointTermsUnit, JointJerkConstraintUnit"); // y = x^3 + 5*x^2 + 2*x + 1 - auto f = [](double x) { return (x * x * x + 5 * x * x + 2 * x + 1); }; + auto f = [](double x) { return ((x * x * x) + (5 * x * x) + (2 * x) + 1); }; - auto variables = std::make_shared("variable-sets", false); - std::vector position_vars; + std::vector> nodes; + std::vector> position_vars; position_vars.reserve(27); std::vector x_vals; x_vals.reserve(27); for (int i = -13; i < 14; ++i) { + auto node = std::make_unique(); + const std::string var_id = "test_var_" + std::to_string(position_vars.size()); const std::vector joint_names{ "x", "y" }; - Eigen::VectorXd val(2); - val << f(i), f(i); - auto var = std::make_shared(val, joint_names, "test_var_" + std::to_string(position_vars.size())); - position_vars.push_back(var); - variables->AddComponent(var); + std::vector bounds(2, ifopt::NoBound); + Eigen::VectorXd vals(2); + vals << f(i), f(i); + position_vars.push_back(node->addVar(var_id, joint_names, vals, bounds)); + nodes.push_back(std::move(node)); + x_vals.push_back(i); } + auto variables = std::make_shared("variable-sets", false); + variables->AddComponent(std::make_shared("joint_trajectory", std::move(nodes))); + Eigen::VectorXd targets(2); targets << 0, 0; const std::string name("test_joint_jerk_cnt"); @@ -396,7 +453,8 @@ TEST(JointTermsUnit, JointJerkConstraintUnit) // NOLINT { for (Eigen::Index j = 0; j < targets.size(); ++j) { - const double expected_val = (-f(x_vals[i]) + 3.0 * f(x_vals[i + 1]) - 3.0 * f(x_vals[i + 2])) + f(x_vals[i + 3]); + const double expected_val = + (-f(x_vals[i]) + (3.0 * f(x_vals[i + 1])) - (3.0 * f(x_vals[i + 2]))) + f(x_vals[i + 3]); EXPECT_NEAR(jerk_vals((static_cast(i) * targets.size()) + j), expected_val, 1e-6); } } @@ -406,7 +464,7 @@ TEST(JointTermsUnit, JointJerkConstraintUnit) // NOLINT { for (Eigen::Index j = 0; j < targets.size(); ++j) { - const double expected_val = f(x_vals[i]) - 3.0 * f(x_vals[i - 1]) + 3.0 * f(x_vals[i - 2]) - f(x_vals[i - 3]); + const double expected_val = f(x_vals[i]) - (3.0 * f(x_vals[i - 1])) + (3.0 * f(x_vals[i - 2])) - f(x_vals[i - 3]); EXPECT_NEAR(jerk_vals((static_cast(i) * targets.size()) + j), expected_val, 1e-6); } } @@ -432,25 +490,31 @@ TEST(JointTermsUnit, JointJerkConstraintMinimumUnit) // NOLINT CONSOLE_BRIDGE_logDebug("JointTermsUnit, JointJerkConstraintMinimumUnit"); // y = x^3 + 5*x^2 + 2*x + 1 - auto f = [](double x) { return (x * x * x + 5 * x * x + 2 * x + 1); }; + auto f = [](double x) { return ((x * x * x) + (5 * x * x) + (2 * x) + 1); }; - auto variables = std::make_shared("variable-sets", false); - std::vector position_vars; + std::vector> nodes; + std::vector> position_vars; position_vars.reserve(6); std::vector x_vals; x_vals.reserve(6); for (int i = -13; i < -7; ++i) { + auto node = std::make_unique(); + const std::string var_id = "test_var_" + std::to_string(position_vars.size()); const std::vector joint_names{ "x", "y" }; - Eigen::VectorXd val(2); - val << f(i), f(i); - auto var = std::make_shared(val, joint_names, "test_var_" + std::to_string(position_vars.size())); - position_vars.push_back(var); - variables->AddComponent(var); + std::vector bounds(2, ifopt::NoBound); + Eigen::VectorXd vals(2); + vals << f(i), f(i); + position_vars.push_back(node->addVar(var_id, joint_names, vals, bounds)); + nodes.push_back(std::move(node)); + x_vals.push_back(i); } + auto variables = std::make_shared("variable-sets", false); + variables->AddComponent(std::make_shared("joint_trajectory", std::move(nodes))); + Eigen::VectorXd targets(2); targets << 0, 0; const std::string name("test_joint_jerk_cnt"); @@ -482,7 +546,7 @@ TEST(JointTermsUnit, JointJerkConstraintMinimumUnit) // NOLINT { for (Eigen::Index j = 0; j < targets.size(); ++j) { - const double expected_val = f(x_vals[i]) - 3.0 * f(x_vals[i - 1]) + 3.0 * f(x_vals[i - 2]) - f(x_vals[i - 3]); + const double expected_val = f(x_vals[i]) - (3.0 * f(x_vals[i - 1])) + (3.0 * f(x_vals[i - 2])) - f(x_vals[i - 3]); EXPECT_NEAR(jerk_vals((static_cast(i) * targets.size()) + j), expected_val, 1e-6); } } diff --git a/trajopt_ifopt/test/simple_collision_unit.cpp b/trajopt_ifopt/test/simple_collision_unit.cpp index 0a0eb7ba..50dc8210 100644 --- a/trajopt_ifopt/test/simple_collision_unit.cpp +++ b/trajopt_ifopt/test/simple_collision_unit.cpp @@ -39,7 +39,10 @@ TRAJOPT_IGNORE_WARNINGS_PUSH TRAJOPT_IGNORE_WARNINGS_POP #include -#include +#include +#include +#include +#include #include #include #include @@ -82,6 +85,7 @@ TEST_F(SimpleCollisionTest, spheres) // NOLINT auto state_solver = env->getStateSolver(); DiscreteContactManager::Ptr const manager = env->getDiscreteContactManager(); const tesseract_kinematics::JointGroup::ConstPtr manip = env->getJointGroup("manipulator"); + const std::vector bounds = trajopt_ifopt::toBounds(manip->getLimits().joint_limits); manager->setActiveCollisionObjects(manip->getActiveLinkNames()); manager->setDefaultCollisionMargin(0); @@ -92,16 +96,17 @@ TEST_F(SimpleCollisionTest, spheres) // NOLINT ifopt::Problem nlp; // 3) Add Variables - std::vector vars; + std::vector> nodes; + std::vector> vars; std::vector positions; { + nodes.push_back(std::make_unique("Joint_Position_0")); Eigen::VectorXd pos(2); pos << -0.75, 0.75; positions.push_back(pos); - auto var = std::make_shared(pos, manip->getJointNames(), "Joint_Position_0"); - vars.push_back(var); - nlp.AddVariableSet(var); + vars.push_back(nodes.back()->addVar("position", manip->getJointNames(), pos, bounds)); } + nlp.AddVariableSet(std::make_shared("joint_trajectory", std::move(nodes))); // Step 3: Setup collision const double margin_coeff = 10; diff --git a/trajopt_ifopt/test/variable_sets_unit.cpp b/trajopt_ifopt/test/variable_sets_unit.cpp index 7b9fb64b..81a477c0 100644 --- a/trajopt_ifopt/test/variable_sets_unit.cpp +++ b/trajopt_ifopt/test/variable_sets_unit.cpp @@ -27,46 +27,194 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include #include TRAJOPT_IGNORE_WARNINGS_POP -#include +#include +#include +#include #include -using namespace trajopt_ifopt; -using namespace std; +// ----------------------- +// Var tests +// ----------------------- -/** - * @brief Tests the joint position variable - */ -TEST(VariableSetsUnit, joint_position_1) // NOLINT +TEST(VarUnit, ScalarVarConstructionAndAccess) +{ + // Single scalar at index 1 + trajopt_ifopt::Var v(0, "my_scalar", 1); + + EXPECT_EQ(v.getParent(), nullptr); + EXPECT_EQ(v.getIndex(), 0); + EXPECT_EQ(v.size(), 1); + EXPECT_EQ(v.value().size(), 1); + EXPECT_DOUBLE_EQ(v.value()(0), 1.0); + EXPECT_EQ(v.getIdentifier(), "my_scalar"); + EXPECT_EQ(v.name()[0], "my_scalar"); +} + +TEST(VarUnit, VectorVarConstructionAndAccess) +{ + Eigen::VectorXd x(3); + x << 10, 20.0, 30.0; + + const std::vector names{ "a", "b", "c" }; + const std::vector bounds(3, ifopt::NoBound); + + // Vector starts at index 1, length 3 -> x[1..3] + trajopt_ifopt::Var v(0, "position", names, x, bounds); + EXPECT_EQ(v.getParent(), nullptr); + EXPECT_EQ(v.getIndex(), 0); + EXPECT_EQ(v.size(), 3); + + const Eigen::VectorXd& values = v.value(); + ASSERT_EQ(values.size(), 3); + EXPECT_DOUBLE_EQ(values[0], 10.0); + EXPECT_DOUBLE_EQ(values[1], 20.0); + EXPECT_DOUBLE_EQ(values[2], 30.0); + + EXPECT_EQ(v.getIdentifier(), "position"); + EXPECT_EQ(v.name(), names); +} + +// ----------------------- +// Node tests +// ----------------------- + +TEST(NodeUnit, NameParentAndVarManagement) +{ + trajopt_ifopt::Node node("node0"); + + EXPECT_EQ(node.getName(), "node0"); + EXPECT_EQ(node.getParent(), nullptr); + EXPECT_EQ(node.size(), 0); + + // Add scalar var + auto scalar_var = node.addVar("s", 1); + EXPECT_TRUE(static_cast(scalar_var)); + EXPECT_TRUE(node.hasVar("s")); + EXPECT_EQ(node.getVar("s")->getIndex(), 0); + EXPECT_EQ(node.size(), 1); + + // Add vector var with 3 components + Eigen::VectorXd vector_var_pos(3); + vector_var_pos << -1.0, 10.0, 20.0; + + const std::vector names{ "a", "b", "c" }; + const std::vector bounds(3, ifopt::NoBound); + + auto vector_var = node.addVar("v", names, vector_var_pos, bounds); + EXPECT_TRUE(static_cast(vector_var)); + EXPECT_TRUE(node.hasVar("v")); + EXPECT_EQ(node.getVar("v")->getIndex(), 1); + EXPECT_EQ(node.size(), 1 + 3); + + // getVar should return the same objects + auto fetched_scalar = node.getVar("s"); + auto fetched_vector = node.getVar("v"); + ASSERT_TRUE(static_cast(fetched_scalar)); + ASSERT_TRUE(static_cast(fetched_vector)); + + // Check that setVariables routes segments correctly to each Var + Eigen::VectorXd x(node.size()); + x << 0.5, 1.0, 2.0, 3.0; + node.setVariables(x); + + EXPECT_DOUBLE_EQ(fetched_scalar->value()(0), 0.5); + + const Eigen::VectorXd& vec = fetched_vector->value(); + ASSERT_EQ(vec.size(), 3); + EXPECT_DOUBLE_EQ(vec[0], 1.0); + EXPECT_DOUBLE_EQ(vec[1], 2.0); + EXPECT_DOUBLE_EQ(vec[2], 3.0); +} + +// ----------------------- +// NodesVariables tests +// ----------------------- + +TEST(NodesVariablesUnit, AddNodesAndGetNodes) +{ + std::vector> nodes; + + nodes.push_back(std::make_unique("n0")); + nodes.back()->addVar("s0", 1); + + Eigen::VectorXd vector_var_pos(2); + vector_var_pos << -1.0, 10.0; + + const std::vector names{ "v0", "v1" }; + const std::vector bounds(2, ifopt::NoBound); + nodes.push_back(std::make_unique("n1")); + nodes.back()->addVar("v", names, vector_var_pos, bounds); + + // Create variable set + trajopt_ifopt::NodesVariables vars("nodes", std::move(nodes)); + + auto out_nodes = vars.GetNodes(); + ASSERT_EQ(out_nodes.size(), 2U); + + EXPECT_EQ(out_nodes[0]->getName(), "n0"); + EXPECT_EQ(out_nodes[1]->getName(), "n1"); + + // Parent pointer on each node should be set + EXPECT_EQ(out_nodes[0]->getParent(), &vars); + EXPECT_EQ(out_nodes[1]->getParent(), &vars); + + // GetNode should be consistent with GetNodes (assume 0-based indexing) + auto n0 = vars.GetNode(0); + auto n1 = vars.GetNode(1); + ASSERT_TRUE(static_cast(n0)); + ASSERT_TRUE(static_cast(n1)); + EXPECT_EQ(n0->getName(), "n0"); + EXPECT_EQ(n1->getName(), "n1"); +} + +TEST(NodesVariablesUnit, SetVariablesPropagatesToNodesAndVars) { - CONSOLE_BRIDGE_logDebug("VariableSetsUnit, joint_position_1"); - - const std::string name("test_var"); - Eigen::VectorXd init(10); - init << 0, 1, 2, 3, 4, 5, 6, 7, 8, 9; - JointPosition position_var(init, std::vector(), name); - - // Check that everything has been initialized correctly - EXPECT_EQ(position_var.GetRows(), init.size()); - EXPECT_EQ(position_var.GetName(), name); - EXPECT_EQ(position_var.GetBounds().size(), init.size()); - EXPECT_TRUE(init.isApprox(position_var.GetValues())); - - // Check that setting variables works - Eigen::VectorXd changed(10); - changed << 10, 11, 12, 13, 14, 15, 16, 17, 18, 19; - position_var.SetVariables(changed); - EXPECT_TRUE(changed.isApprox(position_var.GetValues())); - - // Check that setting bounds works - const ifopt::Bounds bounds(-0.1234, 0.5678); - std::vector bounds_vec = std::vector(static_cast(init.size()), bounds); - position_var.SetBounds(bounds_vec); - std::vector results_vec = position_var.GetBounds(); - for (std::size_t i = 0; i < bounds_vec.size(); i++) - { - EXPECT_EQ(bounds_vec[i].lower_, results_vec[i].lower_); - EXPECT_EQ(bounds_vec[i].upper_, results_vec[i].upper_); - } + std::vector> nodes; + // Node 0: one scalar + nodes.push_back(std::make_unique("n0")); + nodes.back()->addVar("s0", 1); + + // Node 1: one 2D vector + Eigen::VectorXd vector_var_pos(2); + vector_var_pos << -1.0, 10.0; + + const std::vector names{ "v0", "v1" }; + const std::vector bounds(2, ifopt::NoBound); + nodes.push_back(std::make_unique("n1")); + nodes.back()->addVar("v", names, vector_var_pos, bounds); + + // Create variable set + trajopt_ifopt::NodesVariables vars("nodes", std::move(nodes)); + + // Total variables: 1 (n0.s0) + 2 (n1.v) = 3 + Eigen::VectorXd x(3); + x << 5.0, 6.0, 7.0; + + // Set into NodesVariables, which should update internal storage and nodes + vars.SetVariables(x); + + // GetValues should roundtrip the vector + Eigen::VectorXd out = vars.GetValues(); + ASSERT_EQ(out.size(), x.size()); + EXPECT_TRUE(out.isApprox(x)); + + // Check that the corresponding node/var views see the same data + auto n0 = vars.GetNode(0); + auto n1 = vars.GetNode(1); + ASSERT_TRUE(static_cast(n0)); + ASSERT_TRUE(static_cast(n1)); + + auto n0_scalar = n0->getVar("s0"); + auto n1_vec = n1->getVar("v"); + ASSERT_TRUE(static_cast(n0_scalar)); + ASSERT_TRUE(static_cast(n1_vec)); + + EXPECT_DOUBLE_EQ(n0_scalar->value()(0), 5.0); + + const Eigen::VectorXd& v_values = n1_vec->value(); + ASSERT_EQ(v_values.size(), 2); + EXPECT_DOUBLE_EQ(v_values[0], 6.0); + EXPECT_DOUBLE_EQ(v_values[1], 7.0); } //////////////////////////////////////////////////////////////////// diff --git a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/callbacks/joint_state_plotter.h b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/callbacks/joint_state_plotter.h index 290131a2..f1109b70 100644 --- a/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/callbacks/joint_state_plotter.h +++ b/trajopt_optimizers/trajopt_sqp/include/trajopt_sqp/callbacks/joint_state_plotter.h @@ -62,18 +62,18 @@ class JointStatePlottingCallback : public SQPCallback * @brief Add a variable set to be plotted * @param joint_position JointPosition variable to be plotted. They should all be the same size */ - void addVariableSet(const std::shared_ptr& joint_position); + void addVariableSet(const std::shared_ptr& joint_position); /** * @brief Adds multiple variable sets to be plotted * @param joint_positions JointPosition variables to be plotted. They should all be the same size */ - void addVariableSet(const std::vector>& joint_positions); + void addVariableSet(const std::vector>& joint_positions); bool execute(const QPProblem& problem, const SQPResults& sqp_results) override; protected: - std::vector> joint_positions_; + std::vector> joint_positions_; std::shared_ptr plotter_; std::unique_ptr state_solver_; }; diff --git a/trajopt_optimizers/trajopt_sqp/package.xml b/trajopt_optimizers/trajopt_sqp/package.xml index f2768b0d..156c63f8 100644 --- a/trajopt_optimizers/trajopt_sqp/package.xml +++ b/trajopt_optimizers/trajopt_sqp/package.xml @@ -19,6 +19,7 @@ trajopt_ifopt trajopt_common tesseract_common + tesseract_state_solver tesseract_visualization gtest diff --git a/trajopt_optimizers/trajopt_sqp/src/callbacks/joint_state_plotter.cpp b/trajopt_optimizers/trajopt_sqp/src/callbacks/joint_state_plotter.cpp index 1af53aaa..f8e9ab75 100644 --- a/trajopt_optimizers/trajopt_sqp/src/callbacks/joint_state_plotter.cpp +++ b/trajopt_optimizers/trajopt_sqp/src/callbacks/joint_state_plotter.cpp @@ -23,7 +23,9 @@ */ #include -#include +#include +#include +#include #include #include @@ -46,14 +48,13 @@ void JointStatePlottingCallback::plot(const QPProblem& /*problem*/) plotter_->plotTrajectory(trajectory, *state_solver_); } -void JointStatePlottingCallback::addVariableSet( - const std::shared_ptr& joint_position) +void JointStatePlottingCallback::addVariableSet(const std::shared_ptr& joint_position) { joint_positions_.push_back(joint_position); } void JointStatePlottingCallback::addVariableSet( - const std::vector >& joint_positions) + const std::vector >& joint_positions) { for (const auto& cnt : joint_positions) joint_positions_.push_back(cnt); diff --git a/trajopt_optimizers/trajopt_sqp/src/trust_region_sqp_solver.cpp b/trajopt_optimizers/trajopt_sqp/src/trust_region_sqp_solver.cpp index 8621f3bc..b81d3355 100644 --- a/trajopt_optimizers/trajopt_sqp/src/trust_region_sqp_solver.cpp +++ b/trajopt_optimizers/trajopt_sqp/src/trust_region_sqp_solver.cpp @@ -105,14 +105,14 @@ void TrustRegionSQPSolver::solve(const QPProblem::Ptr& qp_problem) const double elapsed_time = std::chrono::duration(Clock::now() - start_time).count() / 1000.0; if (elapsed_time > params.max_time) { - CONSOLE_BRIDGE_logInform("Elapsed time %f has exceeded max time %f", elapsed_time, params.max_time); + CONSOLE_BRIDGE_logDebug("Elapsed time %f has exceeded max time %f", elapsed_time, params.max_time); status_ = SQPStatus::OPT_TIME_LIMIT; break; } if (results_.overall_iteration >= params.max_iterations) { - CONSOLE_BRIDGE_logInform("Iteration limit"); + CONSOLE_BRIDGE_logDebug("Iteration limit"); status_ = SQPStatus::ITERATION_LIMIT; break; } @@ -146,7 +146,7 @@ void TrustRegionSQPSolver::solve(const QPProblem::Ptr& qp_problem) if (status_ == SQPStatus::RUNNING) { status_ = SQPStatus::PENALTY_ITERATION_LIMIT; - CONSOLE_BRIDGE_logInform("Penalty iteration limit, optimization couldn't satisfy all constraints"); + CONSOLE_BRIDGE_logDebug("Penalty iteration limit, optimization couldn't satisfy all constraints"); } // Final Cleanup @@ -161,13 +161,13 @@ bool TrustRegionSQPSolver::verifySQPSolverConvergence() // Check if constraints are satisfied if (results_.best_constraint_violations.size() == 0) { - CONSOLE_BRIDGE_logInform("Optimization has converged and there are no constraints"); + CONSOLE_BRIDGE_logDebug("Optimization has converged and there are no constraints"); return true; } if (results_.best_constraint_violations.maxCoeff() < params.cnt_tolerance) { - CONSOLE_BRIDGE_logInform("woo-hoo! constraints are satisfied (to tolerance %.2e)", params.cnt_tolerance); + CONSOLE_BRIDGE_logDebug("woo-hoo! constraints are satisfied (to tolerance %.2e)", params.cnt_tolerance); return true; } @@ -183,14 +183,14 @@ void TrustRegionSQPSolver::adjustPenalty() { if (results_.best_constraint_violations[idx] > params.cnt_tolerance) { - CONSOLE_BRIDGE_logInform("Not all constraints are satisfied. Increasing constraint penalties for %d", idx); + CONSOLE_BRIDGE_logDebug("Not all constraints are satisfied. Increasing constraint penalties for %d", idx); results_.merit_error_coeffs[idx] *= params.merit_coeff_increase_ratio; } } } else { - CONSOLE_BRIDGE_logInform("Not all constraints are satisfied. Increasing constraint penalties uniformly"); + CONSOLE_BRIDGE_logDebug("Not all constraints are satisfied. Increasing constraint penalties uniformly"); results_.merit_error_coeffs *= params.merit_coeff_increase_ratio; } setBoxSize(fmax(results_.box_size[0], params.min_trust_box_size / params.trust_shrink_ratio * 1.5)); @@ -222,7 +222,7 @@ bool TrustRegionSQPSolver::stepSQPSolver() if (results_.box_size.maxCoeff() < params.min_trust_box_size) { - CONSOLE_BRIDGE_logInform("Converged because trust region is tiny"); + CONSOLE_BRIDGE_logDebug("Converged because trust region is tiny"); status_ = SQPStatus::NLP_CONVERGED; return true; } diff --git a/trajopt_optimizers/trajopt_sqp/test/benchmarks/solve_benchmarks.cpp b/trajopt_optimizers/trajopt_sqp/test/benchmarks/solve_benchmarks.cpp index d473761a..de8bad3b 100644 --- a/trajopt_optimizers/trajopt_sqp/test/benchmarks/solve_benchmarks.cpp +++ b/trajopt_optimizers/trajopt_sqp/test/benchmarks/solve_benchmarks.cpp @@ -13,6 +13,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include +#include #include #include #include @@ -20,7 +21,9 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include -#include +#include +#include +#include #include #include @@ -39,26 +42,20 @@ class SimpleCollisionConstraintIfopt : public ifopt::ConstraintSet { public: SimpleCollisionConstraintIfopt(DiscreteCollisionEvaluator::Ptr collision_evaluator, - JointPosition::ConstPtr position_var, + std::shared_ptr position_var, const std::string& name = "SimpleCollisionConstraint") : ifopt::ConstraintSet(3, name) , position_var_(std::move(position_var)) , collision_evaluator_(std::move(collision_evaluator)) { // Set n_dof_ for convenience - n_dof_ = position_var_->GetRows(); + n_dof_ = position_var_->size(); assert(n_dof_ > 0); bounds_ = std::vector(3, ifopt::BoundSmallerZero); } - Eigen::VectorXd GetValues() const final - { - // Get current joint values - Eigen::VectorXd joint_vals = this->GetVariables()->GetComponent(position_var_->GetName())->GetValues(); - - return CalcValues(joint_vals); - } + Eigen::VectorXd GetValues() const final { return CalcValues(position_var_->value()); } // Set the limits on the constraint values std::vector GetBounds() const final { return bounds_; } @@ -66,13 +63,10 @@ class SimpleCollisionConstraintIfopt : public ifopt::ConstraintSet void FillJacobianBlock(std::string var_set, Jacobian& jac_block) const final { // Only modify the jacobian if this constraint uses var_set - if (var_set == position_var_->GetName()) - { - // Get current joint values - VectorXd joint_vals = this->GetVariables()->GetComponent(position_var_->GetName())->GetValues(); + if (var_set != position_var_->getParent()->getParent()->GetName()) // NOLINT + return; - CalcJacobianBlock(joint_vals, jac_block); // NOLINT - } + CalcJacobianBlock(position_var_->value(), jac_block); // NOLINT } Eigen::VectorXd CalcValues(const Eigen::Ref& joint_vals) const @@ -137,7 +131,8 @@ class SimpleCollisionConstraintIfopt : public ifopt::ConstraintSet for (int j = 0; j < n_dof_; j++) { // Collision is 1 x n_dof - jac_block.coeffRef(static_cast(i), j) = -1.0 * grad_results[i].gradients[0].gradient[j]; + jac_block.coeffRef(static_cast(i), position_var_->getIndex() + j) = + -1.0 * grad_results[i].gradients[0].gradient[j]; } } else if (grad_results[i].gradients[1].has_gradient) @@ -146,7 +141,8 @@ class SimpleCollisionConstraintIfopt : public ifopt::ConstraintSet for (int j = 0; j < n_dof_; j++) { // Collision is 1 x n_dof - jac_block.coeffRef(static_cast(i), j) = -1.0 * grad_results[i].gradients[1].gradient[j]; + jac_block.coeffRef(static_cast(i), position_var_->getIndex() + j) = + -1.0 * grad_results[i].gradients[1].gradient[j]; } } } @@ -161,11 +157,8 @@ class SimpleCollisionConstraintIfopt : public ifopt::ConstraintSet /** @brief Bounds on the constraint value. Default: std::vector(1, ifopt::BoundSmallerZero) */ std::vector bounds_; - /** - * @brief Pointers to the vars used by this constraint. - * Do not access them directly. Instead use this->GetVariables()->GetComponent(position_var->GetName())->GetValues() - */ - JointPosition::ConstPtr position_var_; + /** @brief Pointers to the vars used by this constraint. */ + std::shared_ptr position_var_; DiscreteCollisionEvaluator::Ptr collision_evaluator_; }; @@ -177,16 +170,19 @@ static void BM_TRAJOPT_IFOPT_SIMPLE_COLLISION_SOLVE(benchmark::State& state, con { auto qp_problem = std::make_shared(); tesseract_kinematics::JointGroup::ConstPtr manip = env->getJointGroup("manipulator"); - std::vector vars; + const std::vector bounds = trajopt_ifopt::toBounds(manip->getLimits().joint_limits); + + // Add Variables + std::vector> nodes; + std::vector> vars; std::vector positions; { Eigen::VectorXd pos(2); pos << -0.75, 0.75; - positions.push_back(pos); - auto var = std::make_shared(pos, manip->getJointNames(), "Joint_Position_0"); - vars.push_back(var); - qp_problem->addVariableSet(var); + nodes.push_back(std::make_unique("Joint_Position_0")); + vars.push_back(nodes.back()->addVar("position", manip->getJointNames(), pos, bounds)); } + qp_problem->addVariableSet(std::make_shared("joint_trajectory", std::move(nodes))); // Step 3: Setup collision auto trajopt_collision_cnt_config = std::make_shared(0.2, 1); @@ -210,8 +206,11 @@ static void BM_TRAJOPT_IFOPT_SIMPLE_COLLISION_SOLVE(benchmark::State& state, con qp_problem->addCostSet(collision_cost, trajopt_sqp::CostPenaltyType::HINGE); Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(2, 1); - auto jp_cost = std::make_shared(Eigen::Vector2d(0, 0), vars, coeffs); - qp_problem->addCostSet(jp_cost, trajopt_sqp::CostPenaltyType::SQUARED); + for (const auto& var : vars) + { + auto jp_cost = std::make_shared(Eigen::Vector2d(0, 0), var, coeffs); + qp_problem->addCostSet(jp_cost, trajopt_sqp::CostPenaltyType::SQUARED); + } qp_problem->setup(); @@ -238,6 +237,7 @@ static void BM_TRAJOPT_IFOPT_PLANNING_SOLVE(benchmark::State& state, const Envir { auto qp_problem = std::make_shared(); tesseract_kinematics::JointGroup::ConstPtr manip = env->getJointGroup("right_arm"); + const std::vector bounds = trajopt_ifopt::toBounds(manip->getLimits().joint_limits); // Initial trajectory tesseract_common::TrajArray trajectory(6, 7); @@ -249,14 +249,14 @@ static void BM_TRAJOPT_IFOPT_PLANNING_SOLVE(benchmark::State& state, const Envir trajectory.row(5) << 0.062, 1.287, 0.1, -1.554, -3.011, -0.268, 2.988; // Add Variables - std::vector vars; + std::vector> nodes; + std::vector> vars; for (Eigen::Index i = 0; i < 6; ++i) { - auto var = std::make_shared( - trajectory.row(i), manip->getJointNames(), "Joint_Position_" + std::to_string(i)); - vars.push_back(var); - qp_problem->addVariableSet(var); + nodes.push_back(std::make_unique("Joint_Position_" + std::to_string(i))); + vars.push_back(nodes.back()->addVar("position", manip->getJointNames(), trajectory.row(i), bounds)); } + qp_problem->addVariableSet(std::make_shared("joint_trajectory", std::move(nodes))); double margin_coeff = 20; double margin = 0.025; @@ -273,16 +273,14 @@ static void BM_TRAJOPT_IFOPT_PLANNING_SOLVE(benchmark::State& state, const Envir // Add constraints { // Fix start position - std::vector fixed_vars = { vars[0] }; Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(manip->numJoints(), 5); - auto cnt = std::make_shared(trajectory.row(0), fixed_vars, coeffs); + auto cnt = std::make_shared(trajectory.row(0), vars[0], coeffs); qp_problem->addConstraintSet(cnt); } { // Fix end position - std::vector fixed_vars = { vars[5] }; Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(manip->numJoints(), 5); - auto cnt = std::make_shared(trajectory.row(5), fixed_vars, coeffs); + auto cnt = std::make_shared(trajectory.row(5), vars[5], coeffs); qp_problem->addConstraintSet(cnt); } @@ -293,7 +291,7 @@ static void BM_TRAJOPT_IFOPT_PLANNING_SOLVE(benchmark::State& state, const Envir auto collision_evaluator = std::make_shared( collision_cache, manip, env, *trajopt_collision_config); - std::array position_vars{ vars[i - 1], vars[i] }; + std::array, 2> position_vars{ vars[i - 1], vars[i] }; if (i == 1) position_vars_fixed = { true, false }; diff --git a/trajopt_optimizers/trajopt_sqp/test/cart_position_optimization_unit.cpp b/trajopt_optimizers/trajopt_sqp/test/cart_position_optimization_unit.cpp index 44665394..29065f70 100644 --- a/trajopt_optimizers/trajopt_sqp/test/cart_position_optimization_unit.cpp +++ b/trajopt_optimizers/trajopt_sqp/test/cart_position_optimization_unit.cpp @@ -46,9 +46,12 @@ TRAJOPT_IGNORE_WARNINGS_POP #include #include -#include +#include +#include +#include #include #include +#include const bool DEBUG = false; @@ -88,6 +91,8 @@ void runCartPositionOptimization(const trajopt_sqp::QPProblem::Ptr& qp_problem, // Extract necessary kinematic information const tesseract_kinematics::JointGroup::ConstPtr manip = env->getJointGroup("right_arm"); + const tesseract_common::KinematicLimits limits = manip->getLimits(); + const std::vector bounds = trajopt_ifopt::toBounds(limits.joint_limits); // Get target position Eigen::VectorXd start_pos(manip->numJoints()); @@ -99,15 +104,16 @@ void runCartPositionOptimization(const trajopt_sqp::QPProblem::Ptr& qp_problem, Eigen::Isometry3d target_pose = manip->calcFwdKin(joint_target).at("r_gripper_tool_frame"); // 3) Add Variables - std::vector vars; + std::vector> nodes; + std::vector> vars; for (int ind = 0; ind < 1; ind++) { + auto node = std::make_unique("Joint_Position_" + std::to_string(ind)); auto zero = Eigen::VectorXd::Zero(7); - auto var = std::make_shared( - zero, manip->getJointNames(), "Joint_Position_" + std::to_string(ind)); - vars.push_back(var); - qp_problem->addVariableSet(var); + vars.push_back(node->addVar("position", manip->getJointNames(), zero, bounds)); + nodes.push_back(std::move(node)); } + qp_problem->addVariableSet(std::make_shared("joint_trajectory", std::move(nodes))); // 4) Add constraints for (const auto& var : vars) @@ -139,7 +145,7 @@ void runCartPositionOptimization(const trajopt_sqp::QPProblem::Ptr& qp_problem, } /** @brief Applies a cartesian position constraint and solves the ifopt problem with trajopt_sqp */ -TEST_F(CartPositionOptimization, cart_position_optimization_trajopt_problem) // NOLINT +TEST_F(CartPositionOptimization, cart_position_optimization_ifopt_problem) // NOLINT { CONSOLE_BRIDGE_logDebug("CartPositionOptimization, cart_position_optimization_trajopt_problem"); auto qp_problem = std::make_shared(); @@ -147,9 +153,17 @@ TEST_F(CartPositionOptimization, cart_position_optimization_trajopt_problem) // } /** @brief Applies a cartesian position constraint and solves the ifopt problem with trajopt_sqp */ -TEST_F(CartPositionOptimization, cart_position_optimization_ifopt_problem) // NOLINT +TEST_F(CartPositionOptimization, cart_position_optimization_trajopt_problem) // NOLINT { CONSOLE_BRIDGE_logDebug("CartPositionOptimization, cart_position_optimization_ifopt_problem"); auto qp_problem = std::make_shared(); runCartPositionOptimization(qp_problem, env); } + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + + // pnh.param("plotting", plotting, false); + return RUN_ALL_TESTS(); +} diff --git a/trajopt_optimizers/trajopt_sqp/test/cast_cost_attached_unit.cpp b/trajopt_optimizers/trajopt_sqp/test/cast_cost_attached_unit.cpp index b22dae8d..abdd9bb5 100644 --- a/trajopt_optimizers/trajopt_sqp/test/cast_cost_attached_unit.cpp +++ b/trajopt_optimizers/trajopt_sqp/test/cast_cost_attached_unit.cpp @@ -47,11 +47,14 @@ TRAJOPT_IGNORE_WARNINGS_POP #include -#include +#include +#include +#include #include #include #include #include +#include #include #include @@ -141,38 +144,41 @@ void runCastAttachedLinkWithGeomTest(const trajopt_sqp::QPProblem::Ptr& qp_probl const tesseract_scene_graph::StateSolver::Ptr state_solver = env->getStateSolver(); const ContinuousContactManager::Ptr manager = env->getContinuousContactManager(); const tesseract_kinematics::JointGroup::ConstPtr manip = env->getJointGroup("manipulator"); + const std::vector bounds = trajopt_ifopt::toBounds(manip->getLimits().joint_limits); manager->setActiveCollisionObjects(manip->getActiveLinkNames()); manager->setDefaultCollisionMargin(0); // 3) Add Variables - std::vector vars; + std::vector> nodes; + std::vector> vars; std::vector positions; { + nodes.push_back(std::make_unique("Joint_Position_0")); Eigen::VectorXd pos(2); pos << -1.9, 0; positions.push_back(pos); - auto var = std::make_shared(pos, manip->getJointNames(), "Joint_Position_0"); - vars.push_back(var); - qp_problem->addVariableSet(var); + vars.push_back(nodes.back()->addVar("position", manip->getJointNames(), pos, bounds)); } + { + nodes.push_back(std::make_unique("Joint_Position_1")); Eigen::VectorXd pos(2); pos << 0, 1.9; positions.push_back(pos); - auto var = std::make_shared(pos, manip->getJointNames(), "Joint_Position_1"); - vars.push_back(var); - qp_problem->addVariableSet(var); + vars.push_back(nodes.back()->addVar("position", manip->getJointNames(), pos, bounds)); } + { + nodes.push_back(std::make_unique("Joint_Position_2")); Eigen::VectorXd pos(2); pos << 1.9, 3.8; positions.push_back(pos); - auto var = std::make_shared(pos, manip->getJointNames(), "Joint_Position_2"); - vars.push_back(var); - qp_problem->addVariableSet(var); + vars.push_back(nodes.back()->addVar("position", manip->getJointNames(), pos, bounds)); } + qp_problem->addVariableSet(std::make_shared("joint_trajectory", std::move(nodes))); + // Step 3: Setup collision const double margin_coeff = 20; const double margin = 0.3; @@ -182,16 +188,14 @@ void runCastAttachedLinkWithGeomTest(const trajopt_sqp::QPProblem::Ptr& qp_probl // 4) Add constraints { // Fix start position - const std::vector fixed_vars = { vars[0] }; const Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(manip->numJoints(), 5); - auto cnt = std::make_shared(positions[0], fixed_vars, coeffs); + auto cnt = std::make_shared(positions[0], vars[0], coeffs); qp_problem->addConstraintSet(cnt); } { // Fix end position - const std::vector fixed_vars = { vars[2] }; const Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(manip->numJoints(), 5); - auto cnt = std::make_shared(positions[2], fixed_vars, coeffs); + auto cnt = std::make_shared(positions[2], vars[2], coeffs); qp_problem->addConstraintSet(cnt); } @@ -201,7 +205,7 @@ void runCastAttachedLinkWithGeomTest(const trajopt_sqp::QPProblem::Ptr& qp_probl auto collision_evaluator = std::make_shared( collision_cache, manip, env, trajopt_collision_config); - const std::array position_vars{ vars[i - 1], vars[i] }; + const std::array, 2> position_vars{ vars[i - 1], vars[i] }; auto cnt = std::make_shared( collision_evaluator, position_vars, false, false, 3); qp_problem->addConstraintSet(cnt); @@ -260,38 +264,41 @@ void runCastAttachedLinkWithoutGeomTest(const trajopt_sqp::QPProblem::Ptr& qp_pr const tesseract_scene_graph::StateSolver::Ptr state_solver = env->getStateSolver(); const ContinuousContactManager::Ptr manager = env->getContinuousContactManager(); const tesseract_kinematics::JointGroup::ConstPtr manip = env->getJointGroup("manipulator"); + const std::vector bounds = trajopt_ifopt::toBounds(manip->getLimits().joint_limits); manager->setActiveCollisionObjects(manip->getActiveLinkNames()); manager->setDefaultCollisionMargin(0); // 3) Add Variables - std::vector vars; + std::vector> nodes; + std::vector> vars; std::vector positions; { + nodes.push_back(std::make_unique("Joint_Position_0")); Eigen::VectorXd pos(2); pos << -1.9, 0; positions.push_back(pos); - auto var = std::make_shared(pos, manip->getJointNames(), "Joint_Position_0"); - vars.push_back(var); - qp_problem->addVariableSet(var); + vars.push_back(nodes.back()->addVar("position", manip->getJointNames(), pos, bounds)); } + { + nodes.push_back(std::make_unique("Joint_Position_1")); Eigen::VectorXd pos(2); pos << 0, 1.9; positions.push_back(pos); - auto var = std::make_shared(pos, manip->getJointNames(), "Joint_Position_1"); - vars.push_back(var); - qp_problem->addVariableSet(var); + vars.push_back(nodes.back()->addVar("position", manip->getJointNames(), pos, bounds)); } + { + nodes.push_back(std::make_unique("Joint_Position_2")); Eigen::VectorXd pos(2); pos << 1.9, 3.8; positions.push_back(pos); - auto var = std::make_shared(pos, manip->getJointNames(), "Joint_Position_2"); - vars.push_back(var); - qp_problem->addVariableSet(var); + vars.push_back(nodes.back()->addVar("position", manip->getJointNames(), pos, bounds)); } + qp_problem->addVariableSet(std::make_shared("joint_trajectory", std::move(nodes))); + // Step 3: Setup collision const double margin_coeff = 10; const double margin = 0.02; @@ -302,16 +309,14 @@ void runCastAttachedLinkWithoutGeomTest(const trajopt_sqp::QPProblem::Ptr& qp_pr // 4) Add constraints { // Fix start position - const std::vector fixed_vars = { vars[0] }; const Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(manip->numJoints(), 5); - auto cnt = std::make_shared(positions[0], fixed_vars, coeffs); + auto cnt = std::make_shared(positions[0], vars[0], coeffs); qp_problem->addConstraintSet(cnt); } { // Fix end position - const std::vector fixed_vars = { vars[2] }; const Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(manip->numJoints(), 5); - auto cnt = std::make_shared(positions[2], fixed_vars, coeffs); + auto cnt = std::make_shared(positions[2], vars[2], coeffs); qp_problem->addConstraintSet(cnt); } @@ -322,7 +327,7 @@ void runCastAttachedLinkWithoutGeomTest(const trajopt_sqp::QPProblem::Ptr& qp_pr auto collision_evaluator = std::make_shared( collision_cache, manip, env, trajopt_collision_config); - const std::array position_vars{ vars[i - 1], vars[i] }; + const std::array, 2> position_vars{ vars[i - 1], vars[i] }; auto cnt = std::make_shared( collision_evaluator, position_vars, vars_fixed[0], vars_fixed[1], 2); diff --git a/trajopt_optimizers/trajopt_sqp/test/cast_cost_octomap_unit.cpp b/trajopt_optimizers/trajopt_sqp/test/cast_cost_octomap_unit.cpp index 079f70fc..3b9b6957 100644 --- a/trajopt_optimizers/trajopt_sqp/test/cast_cost_octomap_unit.cpp +++ b/trajopt_optimizers/trajopt_sqp/test/cast_cost_octomap_unit.cpp @@ -46,11 +46,14 @@ TRAJOPT_IGNORE_WARNINGS_POP #include -#include +#include +#include +#include #include #include #include #include +#include #include #include @@ -131,38 +134,41 @@ void runCastOctomapTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const Env const tesseract_scene_graph::StateSolver::Ptr state_solver = env->getStateSolver(); const ContinuousContactManager::Ptr manager = env->getContinuousContactManager(); const tesseract_kinematics::JointGroup::ConstPtr manip = env->getJointGroup("manipulator"); + const std::vector bounds = trajopt_ifopt::toBounds(manip->getLimits().joint_limits); manager->setActiveCollisionObjects(manip->getActiveLinkNames()); manager->setDefaultCollisionMargin(0); // 3) Add Variables - std::vector vars; + std::vector> nodes; + std::vector> vars; std::vector positions; { + nodes.push_back(std::make_unique("Joint_Position_0")); Eigen::VectorXd pos(2); pos << -1.9, 0; positions.push_back(pos); - auto var = std::make_shared(pos, manip->getJointNames(), "Joint_Position_0"); - vars.push_back(var); - qp_problem->addVariableSet(var); + vars.push_back(nodes.back()->addVar("position", manip->getJointNames(), pos, bounds)); } + { + nodes.push_back(std::make_unique("Joint_Position_1")); Eigen::VectorXd pos(2); pos << 0, 1.9; positions.push_back(pos); - auto var = std::make_shared(pos, manip->getJointNames(), "Joint_Position_1"); - vars.push_back(var); - qp_problem->addVariableSet(var); + vars.push_back(nodes.back()->addVar("position", manip->getJointNames(), pos, bounds)); } + { + nodes.push_back(std::make_unique("Joint_Position_2")); Eigen::VectorXd pos(2); pos << 1.9, 3.8; positions.push_back(pos); - auto var = std::make_shared(pos, manip->getJointNames(), "Joint_Position_2"); - vars.push_back(var); - qp_problem->addVariableSet(var); + vars.push_back(nodes.back()->addVar("position", manip->getJointNames(), pos, bounds)); } + qp_problem->addVariableSet(std::make_shared("joint_trajectory", std::move(nodes))); + // Step 3: Setup collision const double margin_coeff = 10; const double margin = 0.02; @@ -172,16 +178,14 @@ void runCastOctomapTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const Env // 4) Add constraints { // Fix start position - const std::vector fixed_vars = { vars[0] }; const Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(manip->numJoints(), 5); - auto cnt = std::make_shared(positions[0], fixed_vars, coeffs); + auto cnt = std::make_shared(positions[0], vars[0], coeffs); qp_problem->addConstraintSet(cnt); } { // Fix end position - const std::vector fixed_vars = { vars[2] }; const Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(manip->numJoints(), 5); - auto cnt = std::make_shared(positions[2], fixed_vars, coeffs); + auto cnt = std::make_shared(positions[2], vars[2], coeffs); qp_problem->addConstraintSet(cnt); } @@ -192,7 +196,7 @@ void runCastOctomapTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const Env auto collision_evaluator = std::make_shared( collision_cache, manip, env, trajopt_collision_config); - const std::array position_vars{ vars[i - 1], vars[i] }; + const std::array, 2> position_vars{ vars[i - 1], vars[i] }; auto cnt = std::make_shared( collision_evaluator, position_vars, vars_fixed[0], vars_fixed[1], 3); diff --git a/trajopt_optimizers/trajopt_sqp/test/cast_cost_unit.cpp b/trajopt_optimizers/trajopt_sqp/test/cast_cost_unit.cpp index e1e4b555..f758e328 100644 --- a/trajopt_optimizers/trajopt_sqp/test/cast_cost_unit.cpp +++ b/trajopt_optimizers/trajopt_sqp/test/cast_cost_unit.cpp @@ -42,8 +42,11 @@ TRAJOPT_IGNORE_WARNINGS_POP #include #include #include -#include +#include +#include +#include #include +#include #include #include @@ -83,38 +86,41 @@ void runCastTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const Environmen const tesseract_scene_graph::StateSolver::Ptr state_solver = env->getStateSolver(); const ContinuousContactManager::Ptr manager = env->getContinuousContactManager(); const tesseract_kinematics::JointGroup::ConstPtr manip = env->getJointGroup("manipulator"); + const std::vector bounds = trajopt_ifopt::toBounds(manip->getLimits().joint_limits); manager->setActiveCollisionObjects(manip->getActiveLinkNames()); manager->setDefaultCollisionMargin(0); // 3) Add Variables - std::vector vars; + std::vector> nodes; + std::vector> vars; std::vector positions; { + nodes.push_back(std::make_unique("Joint_Position_0")); Eigen::VectorXd pos(2); pos << -1.9, 0; positions.push_back(pos); - auto var = std::make_shared(pos, manip->getJointNames(), "Joint_Position_0"); - vars.push_back(var); - qp_problem->addVariableSet(var); + vars.push_back(nodes.back()->addVar("position", manip->getJointNames(), pos, bounds)); } + { + nodes.push_back(std::make_unique("Joint_Position_1")); Eigen::VectorXd pos(2); pos << 0, 1.9; positions.push_back(pos); - auto var = std::make_shared(pos, manip->getJointNames(), "Joint_Position_1"); - vars.push_back(var); - qp_problem->addVariableSet(var); + vars.push_back(nodes.back()->addVar("position", manip->getJointNames(), pos, bounds)); } + { + nodes.push_back(std::make_unique("Joint_Position_2")); Eigen::VectorXd pos(2); pos << 1.9, 3.8; positions.push_back(pos); - auto var = std::make_shared(pos, manip->getJointNames(), "Joint_Position_2"); - vars.push_back(var); - qp_problem->addVariableSet(var); + vars.push_back(nodes.back()->addVar("position", manip->getJointNames(), pos, bounds)); } + qp_problem->addVariableSet(std::make_shared("joint_trajectory", std::move(nodes))); + // Step 3: Setup collision const double margin_coeff = 10; const double margin = 0.02; @@ -124,16 +130,14 @@ void runCastTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const Environmen // 4) Add constraints { // Fix start position - const std::vector fixed_vars = { vars[0] }; const Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(manip->numJoints(), 5); - auto cnt = std::make_shared(positions[0], fixed_vars, coeffs); + auto cnt = std::make_shared(positions[0], vars[0], coeffs); qp_problem->addConstraintSet(cnt); } { // Fix end position - const std::vector fixed_vars = { vars[2] }; const Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(manip->numJoints(), 5); - auto cnt = std::make_shared(positions[2], fixed_vars, coeffs); + auto cnt = std::make_shared(positions[2], vars[2], coeffs); qp_problem->addConstraintSet(cnt); } @@ -144,7 +148,7 @@ void runCastTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const Environmen auto collision_evaluator = std::make_shared( collision_cache, manip, env, trajopt_collision_config); - const std::array position_vars{ vars[i - 1], vars[i] }; + const std::array, 2> position_vars{ vars[i - 1], vars[i] }; auto cnt = std::make_shared( collision_evaluator, position_vars, vars_fixed[0], vars_fixed[1], 3); diff --git a/trajopt_optimizers/trajopt_sqp/test/cast_cost_world_unit.cpp b/trajopt_optimizers/trajopt_sqp/test/cast_cost_world_unit.cpp index 38069b95..9b7a60d7 100644 --- a/trajopt_optimizers/trajopt_sqp/test/cast_cost_world_unit.cpp +++ b/trajopt_optimizers/trajopt_sqp/test/cast_cost_world_unit.cpp @@ -43,7 +43,10 @@ TRAJOPT_IGNORE_WARNINGS_POP #include -#include +#include +#include +#include +#include #include #include #include @@ -115,38 +118,41 @@ void runCastWorldTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const Envir const tesseract_scene_graph::StateSolver::Ptr state_solver = env->getStateSolver(); const ContinuousContactManager::Ptr manager = env->getContinuousContactManager(); const tesseract_kinematics::JointGroup::ConstPtr manip = env->getJointGroup("manipulator"); + const std::vector bounds = trajopt_ifopt::toBounds(manip->getLimits().joint_limits); manager->setActiveCollisionObjects(manip->getActiveLinkNames()); manager->setDefaultCollisionMargin(0); // 3) Add Variables - std::vector vars; + std::vector> nodes; + std::vector> vars; std::vector positions; { + nodes.push_back(std::make_unique("Joint_Position_0")); Eigen::VectorXd pos(2); pos << -1.9, 0; positions.push_back(pos); - auto var = std::make_shared(pos, manip->getJointNames(), "Joint_Position_0"); - vars.push_back(var); - qp_problem->addVariableSet(var); + vars.push_back(nodes.back()->addVar("position", manip->getJointNames(), pos, bounds)); } + { + nodes.push_back(std::make_unique("Joint_Position_1")); Eigen::VectorXd pos(2); pos << 0, 1.9; positions.push_back(pos); - auto var = std::make_shared(pos, manip->getJointNames(), "Joint_Position_1"); - vars.push_back(var); - qp_problem->addVariableSet(var); + vars.push_back(nodes.back()->addVar("position", manip->getJointNames(), pos, bounds)); } + { + nodes.push_back(std::make_unique("Joint_Position_2")); Eigen::VectorXd pos(2); pos << 1.9, 3.8; positions.push_back(pos); - auto var = std::make_shared(pos, manip->getJointNames(), "Joint_Position_2"); - vars.push_back(var); - qp_problem->addVariableSet(var); + vars.push_back(nodes.back()->addVar("position", manip->getJointNames(), pos, bounds)); } + qp_problem->addVariableSet(std::make_shared("joint_trajectory", std::move(nodes))); + // Step 3: Setup collision const double margin_coeff = 10; const double margin = 0.02; @@ -156,16 +162,14 @@ void runCastWorldTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const Envir // 4) Add constraints { // Fix start position - const std::vector fixed_vars = { vars[0] }; const Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(manip->numJoints(), 5); - auto cnt = std::make_shared(positions[0], fixed_vars, coeffs); + auto cnt = std::make_shared(positions[0], vars[0], coeffs); qp_problem->addConstraintSet(cnt); } { // Fix end position - const std::vector fixed_vars = { vars[2] }; const Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(manip->numJoints(), 5); - auto cnt = std::make_shared(positions[2], fixed_vars, coeffs); + auto cnt = std::make_shared(positions[2], vars[2], coeffs); qp_problem->addConstraintSet(cnt); } @@ -176,7 +180,7 @@ void runCastWorldTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const Envir auto collision_evaluator = std::make_shared( collision_cache, manip, env, trajopt_collision_config); - const std::array position_vars{ vars[i - 1], vars[i] }; + const std::array, 2> position_vars{ vars[i - 1], vars[i] }; auto cnt = std::make_shared( collision_evaluator, position_vars, vars_fixed[0], vars_fixed[1], 3); diff --git a/trajopt_optimizers/trajopt_sqp/test/joint_acceleration_optimization_unit.cpp b/trajopt_optimizers/trajopt_sqp/test/joint_acceleration_optimization_unit.cpp index 3db0f4d0..2b55c56f 100644 --- a/trajopt_optimizers/trajopt_sqp/test/joint_acceleration_optimization_unit.cpp +++ b/trajopt_optimizers/trajopt_sqp/test/joint_acceleration_optimization_unit.cpp @@ -39,7 +39,9 @@ TRAJOPT_IGNORE_WARNINGS_POP #include #include #include -#include +#include +#include +#include #include const bool DEBUG = false; @@ -69,40 +71,36 @@ void runAccelerationConstraintOptimizationTest(const trajopt_sqp::QPProblem::Ptr qp_solver->solver_->settings()->setAdaptiveRho(false); // 2) Add Variables - std::vector vars; + std::vector> nodes; + std::vector> vars; const std::vector joint_names(7, "name"); + const auto bounds = std::vector(7, ifopt::NoBound); { + auto node = std::make_unique("Joint_Position_0"); auto pos = Eigen::VectorXd::Zero(7); - auto var = std::make_shared(pos, joint_names, "Joint_Position_0"); - auto bounds = std::vector(7, ifopt::NoBound); - var->SetBounds(bounds); - vars.push_back(var); - qp_problem->addVariableSet(var); + vars.push_back(node->addVar("position", joint_names, pos, bounds)); + nodes.push_back(std::move(node)); } for (int ind = 1; ind < 4; ind++) { + auto node = std::make_unique("Joint_Position_" + std::to_string(ind)); auto pos = Eigen::VectorXd::Ones(7) * 10; - auto var = - std::make_shared(pos, joint_names, "Joint_Position_" + std::to_string(ind)); - auto bounds = std::vector(7, ifopt::NoBound); - var->SetBounds(bounds); - vars.push_back(var); - qp_problem->addVariableSet(var); + vars.push_back(node->addVar("position", joint_names, pos, bounds)); + nodes.push_back(std::move(node)); } + qp_problem->addVariableSet(std::make_shared("joint_trajectory", std::move(nodes))); + // 3) Add constraints const Eigen::VectorXd start_pos = Eigen::VectorXd::Zero(7); - std::vector start; - start.push_back(vars.front()); Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(7, 5); auto start_constraint = - std::make_shared(start_pos, start, coeffs, "StartPosition"); + std::make_shared(start_pos, vars.front(), coeffs, "StartPosition"); qp_problem->addConstraintSet(start_constraint); const Eigen::VectorXd end_pos = Eigen::VectorXd::Ones(7) * 10; - std::vector end; - end.push_back(vars.back()); - auto end_constraint = std::make_shared(end_pos, end, coeffs, "EndPosition"); + auto end_constraint = + std::make_shared(end_pos, vars.back(), coeffs, "EndPosition"); qp_problem->addConstraintSet(end_constraint); // 4) Add costs @@ -138,41 +136,36 @@ TEST_F(AccelerationConstraintOptimization, acceleration_constraint_optimization_ ifopt::Problem nlp_ipopt; // 2) Add Variables - std::vector vars; + std::vector> nodes; + std::vector> vars; const std::vector joint_names(7, "name"); + const auto bounds = std::vector(7, ifopt::NoBound); { + auto node = std::make_unique("Joint_Position_0"); auto pos = Eigen::VectorXd::Zero(7); - auto var = std::make_shared(pos, joint_names, "Joint_Position_0"); - auto bounds = std::vector(7, ifopt::NoBound); - var->SetBounds(bounds); - vars.push_back(var); - nlp_ipopt.AddVariableSet(var); + vars.push_back(node->addVar("position", joint_names, pos, bounds)); + nodes.push_back(std::move(node)); } for (int ind = 1; ind < 4; ind++) { + auto node = std::make_unique("Joint_Position_" + std::to_string(ind)); auto pos = Eigen::VectorXd::Ones(7) * 10; - auto var = - std::make_shared(pos, joint_names, "Joint_Position_" + std::to_string(ind)); - auto bounds = std::vector(7, ifopt::NoBound); - var->SetBounds(bounds); - vars.push_back(var); - nlp_ipopt.AddVariableSet(var); + vars.push_back(node->addVar("position", joint_names, pos, bounds)); + nodes.push_back(std::move(node)); } + nlp_ipopt.AddVariableSet(std::make_shared("joint_trajectory", std::move(nodes))); + // 3) Add constraints const Eigen::VectorXd start_pos = Eigen::VectorXd::Zero(7); - std::vector start; - start.push_back(vars.front()); - Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(7, 5); auto start_constraint = - std::make_shared(start_pos, start, coeffs, "StartPosition"); + std::make_shared(start_pos, vars.front(), coeffs, "StartPosition"); nlp_ipopt.AddConstraintSet(start_constraint); const Eigen::VectorXd end_pos = Eigen::VectorXd::Ones(7) * 10; - std::vector end; - end.push_back(vars.back()); - auto end_constraint = std::make_shared(end_pos, end, coeffs, "EndPosition"); + auto end_constraint = + std::make_shared(end_pos, vars.back(), coeffs, "EndPosition"); nlp_ipopt.AddConstraintSet(end_constraint); // 4) Add costs diff --git a/trajopt_optimizers/trajopt_sqp/test/joint_jerk_optimization_unit.cpp b/trajopt_optimizers/trajopt_sqp/test/joint_jerk_optimization_unit.cpp index 9ca20ca9..4ae88256 100644 --- a/trajopt_optimizers/trajopt_sqp/test/joint_jerk_optimization_unit.cpp +++ b/trajopt_optimizers/trajopt_sqp/test/joint_jerk_optimization_unit.cpp @@ -40,7 +40,9 @@ TRAJOPT_IGNORE_WARNINGS_POP #include #include #include -#include +#include +#include +#include #include const bool DEBUG = false; @@ -70,44 +72,36 @@ void runJerkConstraintOptimizationTest(const trajopt_sqp::QPProblem::Ptr& qp_pro qp_solver->solver_->settings()->setAdaptiveRho(false); // 2) Add Variables - std::vector vars; const std::vector joint_names(7, "name"); + const auto bounds = std::vector(7, ifopt::NoBound); + std::vector> nodes; + std::vector> vars; + for (int ind = 0; ind < 6; ind++) { - auto pos = Eigen::VectorXd::Zero(7); - auto var = std::make_shared(pos, joint_names, "Joint_Position_0"); - auto bounds = std::vector(7, ifopt::NoBound); - var->SetBounds(bounds); - vars.push_back(var); - qp_problem->addVariableSet(var); - } - for (int ind = 1; ind < 6; ind++) - { + auto node = std::make_unique("Joint_Position_" + std::to_string(ind)); Eigen::VectorXd pos; - if (ind == 5) + if (ind == 0) + pos = Eigen::VectorXd::Zero(7); + else if (ind == 5) pos = Eigen::VectorXd::Ones(7) * 10; else - pos = Eigen::VectorXd::Ones(7) * ((ind / 5.0) * 10 + 0.01); - auto var = - std::make_shared(pos, joint_names, "Joint_Position_" + std::to_string(ind)); - auto bounds = std::vector(7, ifopt::NoBound); - var->SetBounds(bounds); - vars.push_back(var); - qp_problem->addVariableSet(var); + pos = Eigen::VectorXd::Ones(7) * ((ind / 5.0) * (10 + 0.01)); + vars.push_back(node->addVar("position", joint_names, pos, bounds)); + nodes.push_back(std::move(node)); } + qp_problem->addVariableSet(std::make_shared("joint_trajectory", std::move(nodes))); + // 3) Add constraints const Eigen::VectorXd start_pos = Eigen::VectorXd::Zero(7); - std::vector start; - start.push_back(vars.front()); Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(7, 5); auto start_constraint = - std::make_shared(start_pos, start, coeffs, "StartPosition"); + std::make_shared(start_pos, vars.front(), coeffs, "StartPosition"); qp_problem->addConstraintSet(start_constraint); const Eigen::VectorXd end_pos = Eigen::VectorXd::Ones(7) * 10; - std::vector end; - end.push_back(vars.back()); - auto end_constraint = std::make_shared(end_pos, end, coeffs, "EndPosition"); + auto end_constraint = + std::make_shared(end_pos, vars.back(), coeffs, "EndPosition"); qp_problem->addConstraintSet(end_constraint); // 4) Add costs diff --git a/trajopt_optimizers/trajopt_sqp/test/joint_position_optimization_unit.cpp b/trajopt_optimizers/trajopt_sqp/test/joint_position_optimization_unit.cpp index 9315fdd0..d7f106f3 100644 --- a/trajopt_optimizers/trajopt_sqp/test/joint_position_optimization_unit.cpp +++ b/trajopt_optimizers/trajopt_sqp/test/joint_position_optimization_unit.cpp @@ -38,7 +38,9 @@ TRAJOPT_IGNORE_WARNINGS_POP #include #include #include -#include +#include +#include +#include const bool DEBUG = false; @@ -67,31 +69,30 @@ void runJointPositionOptimizationTest(const trajopt_sqp::QPProblem::Ptr& qp_prob qp_solver->solver_->settings()->setRelativeTolerance(1e-6); // 2) Add Variables - std::vector vars; + std::vector> nodes; + std::vector> vars; for (int ind = 0; ind < 2; ind++) { + auto node = std::make_unique("Joint_Position_" + std::to_string(ind)); auto pos = Eigen::VectorXd::Zero(7); const std::vector joint_names(7, "name"); - auto var = - std::make_shared(pos, joint_names, "Joint_Position_" + std::to_string(ind)); - vars.push_back(var); - qp_problem->addVariableSet(var); + const std::vector bounds(7, ifopt::NoBound); + vars.push_back(node->addVar("position", joint_names, pos, bounds)); + nodes.push_back(std::move(node)); } + qp_problem->addVariableSet(std::make_shared("joint_trajectory", std::move(nodes))); + // 3) Add constraints const Eigen::VectorXd start_pos = Eigen::VectorXd::Zero(7); - std::vector start; - start.push_back(vars.front()); - const Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(7, 1); auto start_constraint = - std::make_shared(start_pos, start, coeffs, "StartPosition"); + std::make_shared(start_pos, vars.front(), coeffs, "StartPosition"); qp_problem->addConstraintSet(start_constraint); const Eigen::VectorXd end_pos = Eigen::VectorXd::Ones(7); - std::vector end; - end.push_back(vars.back()); - auto end_constraint = std::make_shared(end_pos, end, coeffs, "EndPosition"); + auto end_constraint = + std::make_shared(end_pos, vars.back(), coeffs, "EndPosition"); qp_problem->addConstraintSet(end_constraint); qp_problem->setup(); @@ -110,40 +111,37 @@ void runJointPositionOptimizationTest(const trajopt_sqp::QPProblem::Ptr& qp_prob qp_problem->print(); } -/** - * @brief Applies a joint position constraint and solves the problem with IPOPT - */ +/** @brief Applies a joint position constraint and solves the problem with IPOPT */ TEST_F(JointPositionOptimization, joint_position_optimization_ipopt) // NOLINT { // 1) create Problem ifopt::Problem nlp_ipopt; // 2) Add Variables - std::vector vars; + std::vector> nodes; + std::vector> vars; for (int ind = 0; ind < 2; ind++) { + auto node = std::make_unique("Joint_Position_" + std::to_string(ind)); auto pos = Eigen::VectorXd::Zero(7); const std::vector joint_names(7, "name"); - auto var = - std::make_shared(pos, joint_names, "Joint_Position_" + std::to_string(ind)); - vars.push_back(var); - nlp_ipopt.AddVariableSet(var); + const std::vector bounds(7, ifopt::NoBound); + vars.push_back(node->addVar("position", joint_names, pos, bounds)); + nodes.push_back(std::move(node)); } + nlp_ipopt.AddVariableSet(std::make_shared("joint_trajectory", std::move(nodes))); + // 3) Add constraints const Eigen::VectorXd start_pos = Eigen::VectorXd::Zero(7); - std::vector start; - start.push_back(vars.front()); - const Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(7, 1); auto start_constraint = - std::make_shared(start_pos, start, coeffs, "StartPosition"); + std::make_shared(start_pos, vars.front(), coeffs, "StartPosition"); nlp_ipopt.AddConstraintSet(start_constraint); const Eigen::VectorXd end_pos = Eigen::VectorXd::Ones(7); - std::vector end; - end.push_back(vars.back()); - auto end_constraint = std::make_shared(end_pos, end, coeffs, "EndPosition"); + auto end_constraint = + std::make_shared(end_pos, vars.back(), coeffs, "EndPosition"); nlp_ipopt.AddConstraintSet(end_constraint); ifopt::IpoptSolver solver; diff --git a/trajopt_optimizers/trajopt_sqp/test/joint_velocity_optimization_unit.cpp b/trajopt_optimizers/trajopt_sqp/test/joint_velocity_optimization_unit.cpp index a060e528..97081c35 100644 --- a/trajopt_optimizers/trajopt_sqp/test/joint_velocity_optimization_unit.cpp +++ b/trajopt_optimizers/trajopt_sqp/test/joint_velocity_optimization_unit.cpp @@ -39,7 +39,9 @@ TRAJOPT_IGNORE_WARNINGS_POP #include #include #include -#include +#include +#include +#include #include const bool DEBUG = false; @@ -69,40 +71,35 @@ void runVelocityConstraintOptimizationTest(const trajopt_sqp::QPProblem::Ptr& qp qp_solver->solver_->settings()->setAdaptiveRho(false); // 2) Add Variables - std::vector vars; + std::vector> nodes; + std::vector> vars; const std::vector joint_names(7, "name"); + const auto bounds = std::vector(7, ifopt::NoBound); { + auto node = std::make_unique("Joint_Position_0"); auto pos = Eigen::VectorXd::Zero(7); - auto var = std::make_shared(pos, joint_names, "Joint_Position_0"); - auto bounds = std::vector(7, ifopt::NoBound); - var->SetBounds(bounds); - vars.push_back(var); - qp_problem->addVariableSet(var); + vars.push_back(node->addVar("position", joint_names, pos, bounds)); + nodes.push_back(std::move(node)); } for (int ind = 1; ind < 3; ind++) { + auto node = std::make_unique("Joint_Position_" + std::to_string(ind)); auto pos = Eigen::VectorXd::Ones(7) * 10; - auto var = - std::make_shared(pos, joint_names, "Joint_Position_" + std::to_string(ind)); - auto bounds = std::vector(7, ifopt::NoBound); - var->SetBounds(bounds); - vars.push_back(var); - qp_problem->addVariableSet(var); + vars.push_back(node->addVar("position", joint_names, pos, bounds)); + nodes.push_back(std::move(node)); } + qp_problem->addVariableSet(std::make_shared("joint_trajectory", std::move(nodes))); // 3) Add constraints const Eigen::VectorXd start_pos = Eigen::VectorXd::Zero(7); - std::vector start; - start.push_back(vars.front()); Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(7, 5); auto start_constraint = - std::make_shared(start_pos, start, coeffs, "StartPosition"); + std::make_shared(start_pos, vars.front(), coeffs, "StartPosition"); qp_problem->addConstraintSet(start_constraint); const Eigen::VectorXd end_pos = Eigen::VectorXd::Ones(7) * 10; - std::vector end; - end.push_back(vars.back()); - auto end_constraint = std::make_shared(end_pos, end, coeffs, "EndPosition"); + auto end_constraint = + std::make_shared(end_pos, vars.back(), coeffs, "EndPosition"); qp_problem->addConstraintSet(end_constraint); // 4) Add costs @@ -136,41 +133,37 @@ TEST_F(VelocityConstraintOptimization, velocity_constraint_optimization_ipopt) ifopt::Problem nlp_ipopt; // 2) Add Variables - std::vector vars; + const auto bounds = std::vector(7, ifopt::NoBound); + std::vector> nodes; + std::vector> vars; const std::vector joint_names(7, "name"); { + auto node = std::make_unique("Joint_Position_0"); auto pos = Eigen::VectorXd::Zero(7); - auto var = std::make_shared(pos, joint_names, "Joint_Position_0"); - auto bounds = std::vector(7, ifopt::NoBound); - var->SetBounds(bounds); - vars.push_back(var); - nlp_ipopt.AddVariableSet(var); + vars.push_back(node->addVar("position", joint_names, pos, bounds)); + nodes.push_back(std::move(node)); } + for (int ind = 1; ind < 3; ind++) { + auto node = std::make_unique("Joint_Position_" + std::to_string(ind)); auto pos = Eigen::VectorXd::Ones(7) * 10; - auto var = - std::make_shared(pos, joint_names, "Joint_Position_" + std::to_string(ind)); - auto bounds = std::vector(7, ifopt::NoBound); - var->SetBounds(bounds); - vars.push_back(var); - nlp_ipopt.AddVariableSet(var); + vars.push_back(node->addVar("position", joint_names, pos, bounds)); + nodes.push_back(std::move(node)); } + nlp_ipopt.AddVariableSet(std::make_shared("joint_trajectory", std::move(nodes))); + // 3) Add constraints const Eigen::VectorXd start_pos = Eigen::VectorXd::Zero(7); - std::vector start; - start.push_back(vars.front()); - Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(7, 5); auto start_constraint = - std::make_shared(start_pos, start, coeffs, "StartPosition"); + std::make_shared(start_pos, vars.front(), coeffs, "StartPosition"); nlp_ipopt.AddConstraintSet(start_constraint); const Eigen::VectorXd end_pos = Eigen::VectorXd::Ones(7) * 10; - std::vector end; - end.push_back(vars.back()); - auto end_constraint = std::make_shared(end_pos, end, coeffs, "EndPosition"); + auto end_constraint = + std::make_shared(end_pos, vars.back(), coeffs, "EndPosition"); nlp_ipopt.AddConstraintSet(end_constraint); // 4) Add costs diff --git a/trajopt_optimizers/trajopt_sqp/test/numerical_ik_unit.cpp b/trajopt_optimizers/trajopt_sqp/test/numerical_ik_unit.cpp index 7bedf9dc..bf439aa3 100644 --- a/trajopt_optimizers/trajopt_sqp/test/numerical_ik_unit.cpp +++ b/trajopt_optimizers/trajopt_sqp/test/numerical_ik_unit.cpp @@ -39,11 +39,14 @@ TRAJOPT_IGNORE_WARNINGS_PUSH #include TRAJOPT_IGNORE_WARNINGS_POP -#include +#include +#include +#include #include #include #include #include +#include #include #include @@ -84,16 +87,19 @@ void runNumericalIKTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const Env const tesseract_scene_graph::StateSolver::Ptr state_solver = env->getStateSolver(); const ContinuousContactManager::Ptr manager = env->getContinuousContactManager(); const tesseract_kinematics::JointGroup::ConstPtr manip = env->getJointGroup("left_arm"); + const std::vector bounds = trajopt_ifopt::toBounds(manip->getLimits().joint_limits); manager->setActiveCollisionObjects(manip->getActiveLinkNames()); manager->setDefaultCollisionMargin(0); // 3) Add Variables + std::vector> nodes; + auto node = std::make_unique("Joint_Position_0"); Eigen::VectorXd cur_position(7); // env->getCurrentJointValues(forward_kinematics->getJointNames()); cur_position << 0, 0, 0, -0.001, 0, -0.001, 0; - auto var = std::make_shared( - cur_position, manip->getJointNames(), manip->getLimits(), "Joint_Position_0"); - qp_problem->addVariableSet(var); + auto var = node->addVar("position", manip->getJointNames(), cur_position, bounds); + nodes.push_back(std::move(node)); + qp_problem->addVariableSet(std::make_shared("joint_trajectory", std::move(nodes))); // 4) Add constraints Eigen::Isometry3d target_pose = Eigen::Isometry3d::Identity(); diff --git a/trajopt_optimizers/trajopt_sqp/test/planning_unit.cpp b/trajopt_optimizers/trajopt_sqp/test/planning_unit.cpp index a825c995..9fdd40ce 100644 --- a/trajopt_optimizers/trajopt_sqp/test/planning_unit.cpp +++ b/trajopt_optimizers/trajopt_sqp/test/planning_unit.cpp @@ -44,8 +44,11 @@ TRAJOPT_IGNORE_WARNINGS_POP #include #include #include -#include +#include +#include +#include #include +#include #include #include @@ -98,6 +101,7 @@ void runPlanningTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const Enviro const tesseract_scene_graph::StateSolver::Ptr state_solver = env->getStateSolver(); const ContinuousContactManager::Ptr manager = env->getContinuousContactManager(); const tesseract_kinematics::JointGroup::ConstPtr manip = env->getJointGroup("right_arm"); + const std::vector bounds = trajopt_ifopt::toBounds(manip->getLimits().joint_limits); manager->setActiveCollisionObjects(manip->getActiveLinkNames()); manager->setDefaultCollisionMargin(0); @@ -112,14 +116,16 @@ void runPlanningTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const Enviro trajectory.row(5) << 0.062, 1.287, 0.1, -1.554, -3.011, -0.268, 2.988; // Add Variables - std::vector vars; + std::vector> nodes; + std::vector> vars; for (Eigen::Index i = 0; i < 6; ++i) { - auto var = std::make_shared( - trajectory.row(i), manip->getJointNames(), "Joint_Position_" + std::to_string(i)); + auto node = std::make_unique("Joint_Position_" + std::to_string(i)); + auto var = node->addVar("position", manip->getJointNames(), trajectory.row(i), bounds); vars.push_back(var); - qp_problem->addVariableSet(var); + nodes.push_back(std::move(node)); } + qp_problem->addVariableSet(std::make_shared("joint_trajectory", std::move(nodes))); const double margin_coeff = 20; const double margin = 0.025; @@ -136,16 +142,14 @@ void runPlanningTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const Enviro // Add constraints { // Fix start position - const std::vector fixed_vars = { vars[0] }; const Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(manip->numJoints(), 5); - auto cnt = std::make_shared(trajectory.row(0), fixed_vars, coeffs); + auto cnt = std::make_shared(trajectory.row(0), vars[0], coeffs); qp_problem->addConstraintSet(cnt); } { // Fix end position - const std::vector fixed_vars = { vars[5] }; const Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(manip->numJoints(), 5); - auto cnt = std::make_shared(trajectory.row(5), fixed_vars, coeffs); + auto cnt = std::make_shared(trajectory.row(5), vars[5], coeffs); qp_problem->addConstraintSet(cnt); } @@ -156,7 +160,7 @@ void runPlanningTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const Enviro auto collision_evaluator = std::make_shared( collision_cache, manip, env, trajopt_collision_config); - const std::array position_vars{ vars[i - 1], vars[i] }; + const std::array, 2> position_vars{ vars[i - 1], vars[i] }; if (i == 1) vars_fixed = { true, false }; diff --git a/trajopt_optimizers/trajopt_sqp/test/simple_collision_unit.cpp b/trajopt_optimizers/trajopt_sqp/test/simple_collision_unit.cpp index f635f5db..4d99b143 100644 --- a/trajopt_optimizers/trajopt_sqp/test/simple_collision_unit.cpp +++ b/trajopt_optimizers/trajopt_sqp/test/simple_collision_unit.cpp @@ -42,13 +42,16 @@ TRAJOPT_IGNORE_WARNINGS_POP #include #include +#include #include #include #include #include #include #include -#include +#include +#include +#include #include #include @@ -67,26 +70,20 @@ class SimpleCollisionConstraintIfopt : public ifopt::ConstraintSet { public: SimpleCollisionConstraintIfopt(DiscreteCollisionEvaluator::Ptr collision_evaluator, - JointPosition::ConstPtr position_var, + std::shared_ptr position_var, const std::string& name = "SimpleCollisionConstraint") : ifopt::ConstraintSet(3, name) , position_var_(std::move(position_var)) , collision_evaluator_(std::move(collision_evaluator)) { // Set n_dof_ for convenience - n_dof_ = position_var_->GetRows(); + n_dof_ = position_var_->size(); assert(n_dof_ > 0); bounds_ = std::vector(3, ifopt::BoundSmallerZero); } - Eigen::VectorXd GetValues() const final - { - // Get current joint values - const Eigen::VectorXd joint_vals = this->GetVariables()->GetComponent(position_var_->GetName())->GetValues(); - - return CalcValues(joint_vals); - } + Eigen::VectorXd GetValues() const final { return CalcValues(position_var_->value()); } // Set the limits on the constraint values std::vector GetBounds() const final { return bounds_; } @@ -94,13 +91,10 @@ class SimpleCollisionConstraintIfopt : public ifopt::ConstraintSet void FillJacobianBlock(std::string var_set, Jacobian& jac_block) const final { // Only modify the jacobian if this constraint uses var_set - if (var_set != position_var_->GetName()) // NOLINT + if (var_set != position_var_->getParent()->getParent()->GetName()) // NOLINT return; - // Get current joint values - const VectorXd joint_vals = this->GetVariables()->GetComponent(position_var_->GetName())->GetValues(); - - CalcJacobianBlock(joint_vals, jac_block); // NOLINT + CalcJacobianBlock(position_var_->value(), jac_block); // NOLINT } Eigen::VectorXd CalcValues(const Eigen::Ref& joint_vals) const @@ -138,9 +132,6 @@ class SimpleCollisionConstraintIfopt : public ifopt::ConstraintSet void CalcJacobianBlock(const Eigen::Ref& joint_vals, Jacobian& jac_block) const { - // Reserve enough room in the sparse matrix - jac_block.reserve(n_dof_ * 3); - // Calculate collisions const trajopt_common::CollisionCacheData::ConstPtr cdata = collision_evaluator_->CalcCollisions(joint_vals, bounds_.size()); @@ -165,7 +156,8 @@ class SimpleCollisionConstraintIfopt : public ifopt::ConstraintSet for (int j = 0; j < n_dof_; j++) { // Collision is 1 x n_dof - jac_block.coeffRef(static_cast(i), j) = -1.0 * grad_results[i].gradients[0].gradient[j]; + jac_block.coeffRef(static_cast(i), position_var_->getIndex() + j) = + -1.0 * grad_results[i].gradients[0].gradient[j]; } } else if (grad_results[i].gradients[1].has_gradient) @@ -174,7 +166,8 @@ class SimpleCollisionConstraintIfopt : public ifopt::ConstraintSet for (int j = 0; j < n_dof_; j++) { // Collision is 1 x n_dof - jac_block.coeffRef(static_cast(i), j) = -1.0 * grad_results[i].gradients[1].gradient[j]; + jac_block.coeffRef(static_cast(i), position_var_->getIndex() + j) = + -1.0 * grad_results[i].gradients[1].gradient[j]; } } } @@ -193,7 +186,7 @@ class SimpleCollisionConstraintIfopt : public ifopt::ConstraintSet * @brief Pointers to the vars used by this constraint. * Do not access them directly. Instead use this->GetVariables()->GetComponent(position_var->GetName())->GetValues() */ - JointPosition::ConstPtr position_var_; + std::shared_ptr position_var_; DiscreteCollisionEvaluator::Ptr collision_evaluator_; }; @@ -225,6 +218,7 @@ void runSimpleCollisionTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const const tesseract_scene_graph::StateSolver::Ptr state_solver = env->getStateSolver(); const DiscreteContactManager::Ptr manager = env->getDiscreteContactManager(); const tesseract_kinematics::JointGroup::ConstPtr manip = env->getJointGroup("manipulator"); + const std::vector bounds = trajopt_ifopt::toBounds(manip->getLimits().joint_limits); manager->setActiveCollisionObjects(manip->getActiveLinkNames()); manager->setDefaultCollisionMargin(0); @@ -232,17 +226,21 @@ void runSimpleCollisionTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const collisions.clear(); // 3) Add Variables - std::vector vars; + std::vector> nodes; + std::vector> vars; std::vector positions; { + auto node = std::make_unique("Joint_Position_0"); Eigen::VectorXd pos(2); pos << -0.75, 0.75; positions.push_back(pos); - auto var = std::make_shared(pos, manip->getJointNames(), "Joint_Position_0"); + auto var = node->addVar("position", manip->getJointNames(), pos, bounds); vars.push_back(var); - qp_problem->addVariableSet(var); + nodes.push_back(std::move(node)); } + qp_problem->addVariableSet(std::make_shared("joint_trajectory", std::move(nodes))); + // Step 3: Setup collision trajopt_common::TrajOptCollisionConfig trajopt_collision_cnt_config(0.2, 1); trajopt_collision_cnt_config.collision_margin_buffer = 0.05; @@ -265,7 +263,7 @@ void runSimpleCollisionTest(const trajopt_sqp::QPProblem::Ptr& qp_problem, const qp_problem->addCostSet(collision_cost, trajopt_sqp::CostPenaltyType::HINGE); const Eigen::VectorXd coeffs = Eigen::VectorXd::Constant(2, 1); - auto jp_cost = std::make_shared(Eigen::Vector2d(0, 0), vars, coeffs); + auto jp_cost = std::make_shared(Eigen::Vector2d(0, 0), vars[0], coeffs); qp_problem->addCostSet(jp_cost, trajopt_sqp::CostPenaltyType::SQUARED); qp_problem->setup(); diff --git a/trajopt_sco/src/optimizers.cpp b/trajopt_sco/src/optimizers.cpp index 452fdc3f..a804fb8d 100644 --- a/trajopt_sco/src/optimizers.cpp +++ b/trajopt_sco/src/optimizers.cpp @@ -924,18 +924,19 @@ OptStatus BasicTrustRegionSQP::optimize() { if (results_.cnt_viols[idx] > param_.cnt_tolerance) { - LOG_INFO("Not all constraints are satisfied. Increasing constraint penalties for %s", CSTR(cnt_names[idx])); + LOG_DEBUG("Not all constraints are satisfied. Increasing constraint penalties for %s", + CSTR(cnt_names[idx])); merit_error_coeffs[idx] *= param_.merit_coeff_increase_ratio; } } } else { - LOG_INFO("Not all constraints are satisfied. Increasing constraint penalties uniformly"); + LOG_DEBUG("Not all constraints are satisfied. Increasing constraint penalties uniformly"); for (auto& merit_error_coeff : merit_error_coeffs) merit_error_coeff *= param_.merit_coeff_increase_ratio; } - LOG_INFO("New merit_error_coeffs: %s", CSTR(merit_error_coeffs)); + LOG_DEBUG("New merit_error_coeffs: %s", CSTR(merit_error_coeffs)); param_.trust_box_size = fmax(param_.trust_box_size, param_.min_trust_box_size / param_.trust_shrink_ratio * 1.5); } } /* merit adjustment loop */