Add nodes variable set

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)

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(

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<const JointPosition> position_var,
+                     std::shared_ptr<const Var> 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<ifopt::Bounds> 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<const JointPosition> position_var_;
+  /** @brief Pointers to the vars used by this constraint. */
+  std::shared_ptr<const Var> position_var_;
 
   /** @brief The cartesian line information used when calculating error */
   CartLineInfo info_;

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<const JointPosition> position_var,
+                    std::shared_ptr<const Var> position_var,
                     const std::string& name = "CartPos");
 
   CartPosConstraint(CartPosInfo info,
-                    std::shared_ptr<const JointPosition> position_var,
+                    std::shared_ptr<const Var> 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<ifopt::Bounds> 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<const JointPosition> position_var_;
+  /** @brief Pointers to the vars used by this constraint. */
+  std::shared_ptr<const Var> position_var_;
 
   /** @brief The kinematic information used when calculating error */
   CartPosInfo info_;

trajopt_ifopt/include/trajopt_ifopt/constraints/collision/continuous_collision_constraint.h

@@ -29,11 +29,12 @@
 TRAJOPT_IGNORE_WARNINGS_PUSH
 #include <Eigen/Core>
 #include <ifopt/constraint_set.h>
+#include <mutex>
 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<ContinuousCollisionEvaluator> collision_evaluator,
-                                std::array<std::shared_ptr<const JointPosition>, 2> position_vars,
+                                std::array<std::shared_ptr<const Var>, 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<Bounds>(1, ifopt::BoundSmallerZero) */
   std::vector<ifopt::Bounds> 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<std::shared_ptr<const JointPosition>, 2> position_vars_;
+  /** @brief Pointers to the vars used by this constraint. */
+  std::array<std::shared_ptr<const Var>, 2> position_vars_;
   bool vars0_fixed_{ false };
   bool vars1_fixed_{ false };
 
+  std::shared_ptr<ContinuousCollisionEvaluator> collision_evaluator_;
+
   /** @brief Used to initialize jacobian because snopt sparsity cannot change */
-  std::vector<Eigen::Triplet<double>> triplet_list_;
+  bool fixed_sparsity_{ false };
+  mutable std::once_flag init_flag_;
+  mutable std::vector<Eigen::Triplet<double>> triplet_list_;
 
-  std::shared_ptr<ContinuousCollisionEvaluator> collision_evaluator_;
+  void initSparsity() const;
 };
 }  // namespace trajopt_ifopt
 

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<ContinuousCollisionEvaluator> collision_evaluator,
-                                         std::array<std::shared_ptr<const JointPosition>, 2> position_vars,
+                                         std::array<std::shared_ptr<const Var>, 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<Bounds>(1, ifopt::BoundSmallerZero) */
   std::vector<ifopt::Bounds> 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<std::shared_ptr<const JointPosition>, 2> position_vars_;
+  /** @brief Pointers to the vars used by this constraint. */
+  std::array<std::shared_ptr<const Var>, 2> position_vars_;
 
   bool vars0_fixed_{ false };
   bool vars1_fixed_{ false };

trajopt_ifopt/include/trajopt_ifopt/constraints/collision/discrete_collision_constraint.h

@@ -29,11 +29,12 @@
 TRAJOPT_IGNORE_WARNINGS_PUSH
 #include <Eigen/Core>
 #include <ifopt/constraint_set.h>
+#include <mutex>
 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<const DiscreteCollisionConstraint>;
 
   DiscreteCollisionConstraint(std::shared_ptr<DiscreteCollisionEvaluator> collision_evaluator,
-                              std::shared_ptr<const JointPosition> position_var,
+                              std::shared_ptr<const Var> 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<Bounds>(1, ifopt::BoundSmallerZero) */
   std::vector<ifopt::Bounds> 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<const JointPosition> position_var_;
+  /** @brief Pointers to the vars used by this constraint. */
+  std::shared_ptr<const Var> position_var_;
+
+  std::shared_ptr<DiscreteCollisionEvaluator> collision_evaluator_;
 
   /** @brief Used to initialize jacobian because snopt sparsity cannot change */
-  std::vector<Eigen::Triplet<double>> triplet_list_;
+  bool fixed_sparsity_{ false };
+  mutable std::once_flag init_flag_;
+  mutable std::vector<Eigen::Triplet<double>> triplet_list_;
 
-  std::shared_ptr<DiscreteCollisionEvaluator> collision_evaluator_;
+  void initSparsity() const;
 };
+
 }  // namespace trajopt_ifopt
 #endif

trajopt_ifopt/include/trajopt_ifopt/constraints/collision/discrete_collision_numerical_constraint.h

@@ -28,11 +28,12 @@
 TRAJOPT_IGNORE_WARNINGS_PUSH
 #include <Eigen/Eigen>
 #include <ifopt/constraint_set.h>
+#include <mutex>
 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<const DiscreteCollisionNumericalConstraint>;
 
   DiscreteCollisionNumericalConstraint(std::shared_ptr<DiscreteCollisionEvaluator> collision_evaluator,
-                                       std::shared_ptr<const JointPosition> position_var,
+                                       std::shared_ptr<const Var> 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<Bounds>(1, ifopt::BoundSmallerZero) */
   std::vector<ifopt::Bounds> 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<const JointPosition> position_var_;
+  /** @brief Pointers to the vars used by this constraint. */
+  std::shared_ptr<const Var> position_var_;
+
+  std::shared_ptr<DiscreteCollisionEvaluator> collision_evaluator_;
 
   /** @brief Used to initialize jacobian because snopt sparsity cannot change */
-  std::vector<Eigen::Triplet<double>> triplet_list_;
+  bool fixed_sparsity_{ false };
+  mutable std::once_flag init_flag_;
+  mutable std::vector<Eigen::Triplet<double>> triplet_list_;
 
-  std::shared_ptr<DiscreteCollisionEvaluator> collision_evaluator_;
+  void initSparsity() const;
 };
 }  // namespace trajopt_ifopt
 #endif  // TRAJOPT_IFOPT_DISCRETE_COLLISION_NUMERICAL_CONSTRAINT_H

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<const JointPosition> constraint_var,
-                              std::shared_ptr<const JointPosition> seed_var,
+                              std::shared_ptr<const Var> constraint_var,
+                              std::shared_ptr<const Var> 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<ifopt::Bounds> 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<const JointPosition> constraint_var_;
+  /** @brief Pointers to the vars used by this constraint. */
+  std::shared_ptr<const Var> 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<const JointPosition> seed_var_;
+  std::shared_ptr<const Var> 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 */

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<std::shared_ptr<const JointPosition>>& position_vars,
+                       const std::vector<std::shared_ptr<const Var>>& 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<ifopt::Bounds>& bounds,
-                       const std::vector<std::shared_ptr<const JointPosition>>& position_vars,
+                       const std::vector<std::shared_ptr<const Var>>& 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<ifopt::Bounds> 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<std::shared_ptr<const JointPosition>> position_vars_;
-  std::unordered_map<std::string, Eigen::Index> index_map_;
+  /** @brief Pointers to the vars used by this constraint. */
+  std::vector<std::shared_ptr<const Var>> position_vars_;
 };
 }  // namespace trajopt_ifopt
 #endif