Add nodes variable set

trajopt_ifopt/CMakeLists.txt

@@ -43,7 +43,11 @@ 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/joint_position_variable.cpp
+    src/variable_sets/node.cpp
+    src/variable_sets/nodes_observer.cpp
+    src/variable_sets/nodes_variables.cpp
+    src/variable_sets/var.cpp)
 
 add_library(${PROJECT_NAME} ${TRAJOPT_IFOPT_SOURCE_FILES})
 target_link_libraries(
@@ -90,11 +94,11 @@ install(
 # Testing ##
 # ######################################################################################################################
 
-if(TRAJOPT_ENABLE_TESTING)
+# if(TRAJOPT_ENABLE_TESTING)
   enable_testing()
   add_custom_target(run_tests)
   add_subdirectory(test)
-endif()
+# endif()
 
 if(TRAJOPT_PACKAGE)
   cpack(

trajopt_ifopt/include/trajopt_ifopt/constraints/joint_acceleration_constraint.h

@@ -33,7 +33,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
  *
@@ -108,5 +108,83 @@ class JointAccelConstraint : public ifopt::ConstraintSet
   std::vector<std::shared_ptr<const JointPosition>> position_vars_;
   std::unordered_map<std::string, Eigen::Index> index_map_;
 };
+
+/**
+ * @brief This creates a joint acceleration constraint and allows bounds to be set on a joint position
+ *
+ * Joint acceleration is calculated as a = th_2 - 2th_1 + th_0
+ */
+class JointAccelConstraint2 : public ifopt::ConstraintSet
+{
+public:
+  using Ptr = std::shared_ptr<JointAccelConstraint>;
+  using ConstPtr = std::shared_ptr<const JointAccelConstraint>;
+
+  /**
+   * @brief Constructs a acceleration contraint from these variables, setting the bounds to the target
+   * @param targets Joint Acceleration targets (length should be n_dof). Upper and lower bounds are set to this value
+   * @param position_vars Joint positions used to calculate acceleration. These vars are assumed to be continuous and in
+   * order.
+   * @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
+   */
+  JointAccelConstraint2(const Eigen::VectorXd& targets,
+                        const std::vector<std::shared_ptr<const Var>>& position_vars,
+                        const Eigen::VectorXd& coeffs,
+                        const std::string& name = "JointAccel");
+
+  /**
+   * @brief Constructs a acceleration contraint from these variables, setting the bounds to those passed in.
+   * @param bounds Bounds on target joint acceleration (length should be n_dof)
+   * @param position_vars Joint positions used to calculate acceleration. These vars are assumed to be continuous and in
+   * order.
+   * @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
+   */
+  JointAccelConstraint2(const std::vector<ifopt::Bounds>& bounds,
+                        const std::vector<std::shared_ptr<const JointPosition>>& position_vars,
+                        const Eigen::VectorXd& coeffs,
+                        const std::string& name = "JointAccel");
+
+  /**
+   * @brief Returns the values associated with the constraint. In this case that is the approximate joint acceleration.
+   * @return Returns jointAcceleration. Length is n_dof_ * n_vars
+   */
+  Eigen::VectorXd GetValues() const override;
+
+  /**
+   * @brief  Returns the "bounds" of this constraint. How these are enforced is up to the solver
+   * @return Returns the "bounds" of this constraint
+   */
+  std::vector<ifopt::Bounds> GetBounds() const override;
+
+  /**
+   * @brief Fills the jacobian block associated with the given var_set.
+   * @param var_set Name of the var_set to which the jac_block is associated
+   * @param jac_block Block of the overall jacobian associated with these constraints and the var_set variable
+   */
+  void FillJacobianBlock(std::string var_set, Jacobian& jac_block) const override;
+
+private:
+  /** @brief The var_set associated with this constraint */
+  std::string var_set_;
+
+  /** @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_;
+
+  /** @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 Var>> position_vars_;
+};
 }  // namespace trajopt_ifopt
 #endif

trajopt_ifopt/include/trajopt_ifopt/constraints/joint_jerk_constraint.h

@@ -33,6 +33,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
@@ -108,5 +109,83 @@ class JointJerkConstraint : public ifopt::ConstraintSet
   std::vector<std::shared_ptr<const JointPosition>> position_vars_;
   std::unordered_map<std::string, Eigen::Index> index_map_;
 };
+
+/**
+ * @brief This creates a joint acceleration constraint and allows bounds to be set on a joint position
+ *
+ * Joint acceleration is calculated as a = th_2 - 2th_1 + th_0
+ */
+class JointJerkConstraint2 : public ifopt::ConstraintSet
+{
+public:
+  using Ptr = std::shared_ptr<JointJerkConstraint>;
+  using ConstPtr = std::shared_ptr<const JointJerkConstraint>;
+
+  /**
+   * @brief Constructs a jerk constraint from these variables, setting the bounds to the target
+   * @param targets Joint Jerk targets (length should be n_dof). Upper and lower bounds are set to this value
+   * @param position_vars Joint positions used to calculate acceleration. These vars are assumed to be continuous and in
+   * order.
+   * @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
+   */
+  JointJerkConstraint2(const Eigen::VectorXd& targets,
+                       const std::vector<std::shared_ptr<const Var>>& position_vars,
+                       const Eigen::VectorXd& coeffs,
+                       const std::string& name = "JointJerk");
+
+  /**
+   * @brief Constructs a jerk constraint from these variables, setting the bounds to those passed in.
+   * @param bounds Bounds on target joint acceleration (length should be n_dof)
+   * @param position_vars Joint positions used to calculate acceleration. These vars are assumed to be continuous and in
+   * order.
+   * @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
+   */
+  JointJerkConstraint2(const std::vector<ifopt::Bounds>& bounds,
+                       const std::vector<std::shared_ptr<const JointPosition>>& position_vars,
+                       const Eigen::VectorXd& coeffs,
+                       const std::string& name = "JointJerk");
+
+  /**
+   * @brief Returns the values associated with the constraint. In this case that is the approximate joint jerk.
+   * @return Returns joint jerk. Length is n_dof_ * n_vars
+   */
+  Eigen::VectorXd GetValues() const override;
+
+  /**
+   * @brief  Returns the "bounds" of this constraint. How these are enforced is up to the solver
+   * @return Returns the "bounds" of this constraint
+   */
+  std::vector<ifopt::Bounds> GetBounds() const override;
+
+  /**
+   * @brief Fills the jacobian block associated with the given var_set.
+   * @param var_set Name of the var_set to which the jac_block is associated
+   * @param jac_block Block of the overall jacobian associated with these constraints and the var_set variable
+   */
+  void FillJacobianBlock(std::string var_set, Jacobian& jac_block) const override;
+
+private:
+  /** @brief The var_set associated with this constraint */
+  std::string var_set_;
+
+  /** @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_;
+
+  /** @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 Var>> position_vars_;
+};
 }  // namespace trajopt_ifopt
 #endif

trajopt_ifopt/include/trajopt_ifopt/constraints/joint_position_constraint.h

@@ -33,7 +33,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
  */
@@ -105,5 +105,78 @@ class JointPosConstraint : public ifopt::ConstraintSet
    * Do not access them directly. Instead use this->GetVariables()->GetComponent(position_var->GetName())->GetValues()*/
   std::vector<std::shared_ptr<const JointPosition>> position_vars_;
 };
+
+/**
+ * @brief This creates a joint position constraint. Allows bounds to be set on a joint position
+ */
+class JointPosConstraint2 : public ifopt::ConstraintSet
+{
+public:
+  using Ptr = std::shared_ptr<JointPosConstraint>;
+  using ConstPtr = std::shared_ptr<const JointPosConstraint>;
+
+  /**
+   * @brief JointPosConstraint
+   * @param targets Target joint position (length should be n_dof). Upper and lower bounds are set to this value
+   * @param position_vars Variables to which this constraint is applied. Note that all variables should have the same
+   * number of components (joint DOF)
+   * @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
+   */
+  JointPosConstraint2(const Eigen::VectorXd& target,
+                      const std::shared_ptr<const Var>& position_var,
+                      const Eigen::VectorXd& coeffs,
+                      const std::string& name = "JointPos");
+
+  /**
+   * @brief JointPosConstraint
+   * @param bounds Bounds on target joint position (length should be n_dof)
+   * @param position_vars Variables to which this constraint is applied
+   * @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
+   */
+  JointPosConstraint2(const std::vector<ifopt::Bounds>& bounds,
+                      const std::shared_ptr<const Var>& position_vars,
+                      const Eigen::VectorXd& coeffs,
+                      const std::string& name = "JointPos");
+
+  /**
+   * @brief Returns the values associated with the constraint. In this case that is the concatenated joint values
+   * associated with each of the joint positions should be n_dof_ * n_vars_ long
+   * @return
+   */
+  Eigen::VectorXd GetValues() const override;
+
+  /**
+   * @brief  Returns the "bounds" of this constraint. How these are enforced is up to the solver
+   * @return Returns the "bounds" of this constraint
+   */
+  std::vector<ifopt::Bounds> GetBounds() const override;
+
+  /**
+   * @brief Fills the jacobian block associated with the given var_set.
+   * @param var_set Name of the var_set to which the jac_block is associated
+   * @param jac_block Block of the overal jacobian associated with these constraints and the var_set variable
+   */
+  void FillJacobianBlock(std::string var_set, Jacobian& jac_block) const override;
+
+private:
+  /** @brief The var_set associated with this constraint */
+  std::string var_set_;
+
+  /** @brief The number of joints in a single JointPosition */
+  long n_dof_;
+
+  /** @brief The coeff to apply to error and gradient */
+  Eigen::VectorXd coeffs_;
+
+  /** @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 Var> position_var_;
+};
 }  // namespace trajopt_ifopt
 #endif

trajopt_ifopt/include/trajopt_ifopt/constraints/joint_velocity_constraint.h

@@ -33,7 +33,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
  *
@@ -108,5 +108,84 @@ class JointVelConstraint : public ifopt::ConstraintSet
   std::vector<std::shared_ptr<const JointPosition>> position_vars_;
   std::unordered_map<std::string, Eigen::Index> index_map_;
 };
+
+/**
+ * @brief This creates a joint velocity constraint and allows bounds to be set on a joint position
+ *
+ * Joint velocity is calculated as v = th_1 - th_0
+ */
+class JointVelConstraint2 : public ifopt::ConstraintSet
+{
+public:
+  using Ptr = std::shared_ptr<JointVelConstraint>;
+  using ConstPtr = std::shared_ptr<const JointVelConstraint>;
+
+  /**
+   * @brief Constructs a velocity constraint from these variables, setting the bounds to the target
+   * @param targets Joint Velocity targets (length should be n_dof). Upper and lower bounds are set to this value
+   * @param position_vars Joint positions used to calculate velocity. These vars are assumed to be continuous and in
+   * order.
+   * @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
+   */
+  JointVelConstraint2(const Eigen::VectorXd& targets,
+                      const std::vector<std::shared_ptr<const Var>>& position_vars,
+                      const Eigen::VectorXd& coeffs,
+                      const std::string& name = "JointVel");
+
+  /**
+   * @brief Constructs a velocity constraint from these variables, setting the bounds to those passed in.
+   * @param bounds Bounds on target joint velocity (length should be n_dof)
+   * @param position_vars Joint positions used to calculate velocity. These vars are assumed to be continuous and in
+   * order.
+   * @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
+   */
+  JointVelConstraint2(const std::vector<ifopt::Bounds>& bounds,
+                      const std::vector<std::shared_ptr<const Var>>& position_vars,
+                      const Eigen::VectorXd& coeffs,
+                      const std::string& name = "JointVel");
+
+  /**
+   * @brief Returns the values associated with the constraint. In this case that is the approximate joint velocity.
+   * @return Returns jointVelocity. Length is n_dof_ * (n_vars - 1)
+   */
+  Eigen::VectorXd GetValues() const override;
+
+  /**
+   * @brief  Returns the "bounds" of this constraint. How these are enforced is up to the solver
+   * @return Returns the "bounds" of this constraint
+   */
+  std::vector<ifopt::Bounds> GetBounds() const override;
+
+  /**
+   * @brief Fills the jacobian block associated with the given var_set.
+   * @param var_set Name of the var_set to which the jac_block is associated
+   * @param jac_block Block of the overall jacobian associated with these constraints and the var_set variable
+   */
+  void FillJacobianBlock(std::string var_set, Jacobian& jac_block) const override;
+
+private:
+  /** @brief The var_set associated with this constraint */
+  std::string var_set_;
+
+  /** @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_;
+
+  /** @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 Var>> position_vars_;
+};
 }  // namespace trajopt_ifopt
 #endif