Working joint velocity constraint using new variable set

Author: Levi-Armstrong

trajopt_ifopt/CMakeLists.txt

@@ -94,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_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

trajopt_ifopt/include/trajopt_ifopt/variable_sets/node.h

@@ -3,6 +3,7 @@
 
 #include <trajopt_ifopt/variable_sets/var.h>
 #include <unordered_map>
+#include <memory>
 
 namespace trajopt_ifopt
 {
@@ -18,7 +19,7 @@ class Node
 
   const std::string& getName() const;
 
-  NodesVariables* getParent() const;
+  const NodesVariables* getParent() const;
 
   std::shared_ptr<const Var> addVar(const std::string& name);
 

trajopt_ifopt/include/trajopt_ifopt/variable_sets/nodes_variables.h

@@ -48,6 +48,12 @@ class NodesVariables : public ifopt::VariableSet
 public:
   using Ptr = std::shared_ptr<NodesVariables>;
 
+  /**
+   * @param variable_name  The name of the variables in the optimization problem.
+   */
+  NodesVariables(const std::string& variable_name);
+  virtual ~NodesVariables() = default;
+
   /**
    * @brief Add node to the variable set
    * @param node The node to append
@@ -105,13 +111,6 @@ class NodesVariables : public ifopt::VariableSet
   Eigen::Index GetDim() const;
 
 protected:
-  /**
-   * @param n_dim  The number of dimensions (x,y,..) each node has.
-   * @param variable_name  The name of the variables in the optimization problem.
-   */
-  NodesVariables(const std::string& variable_name);
-  virtual ~NodesVariables() = default;
-
   Eigen::VectorXd values_;
   VecBound bounds_;  ///< the bounds on the node values.
   std::vector<std::shared_ptr<Node>> nodes_;

trajopt_ifopt/include/trajopt_ifopt/variable_sets/var.h

@@ -3,10 +3,10 @@
 
 #include <Eigen/Core>
 #include <vector>
-#include <memory>
 
 namespace trajopt_ifopt
 {
+class Node;
 /**
  * @brief This is the class which the constraints should be storing
  * @details This class contains all information necessary for filling the jacobian
@@ -17,37 +17,53 @@ class Var
 public:
   Var() = default;
   ~Var() = default;
-  Var(Eigen::Index index, std::string name);
-  Var(Eigen::Index index, Eigen::Index length, std::string identifier, std::vector<std::string> names);
+  Var(Eigen::Index index, std::string name, Node* parent = nullptr);
+  Var(Eigen::Index index,
+      Eigen::Index length,
+      std::string identifier,
+      std::vector<std::string> names,
+      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 */
+  const std::string& getIdentifier() const;
+
+  /** @brief Get the parent node */
+  const Node* getParent() const;
+
   /**
    * @brief Set the variables. This is the full vector of variables and it will extract its variables from the full
    * list.
    * @param x The full vector of variables
    */
   void setVariables(const Eigen::Ref<const Eigen::VectorXd>& x);
 
+  /**
+   * @brief Get the index in the full set of variables that these are stored
+   * @return The start index
+   */
+  Eigen::Index getIndex() const;
+
   /**
    * @brief Get the variable size
    * @return The size
    */
   Eigen::Index size() const;
 
-  template <typename T>
-  const T& value() const
-  {
-    throw std::runtime_error("This should never be used");
-  }
+  /**
+   * @brief Get variable values
+   * @return The values
+   */
+  const Eigen::VectorXd& value() const;
 
-  template <typename T>
-  const T& name() const
-  {
-    throw std::runtime_error("This should never be used");
-  }
+  /**
+   * @brief Get teh variable names
+   * @return The names
+   */
+  const std::vector<std::string>& name() const;
 
 private:
   friend class Node;
@@ -56,12 +72,7 @@ class Var
   std::string identifier_;
   std::vector<std::string> names_;
   Eigen::VectorXd values_;
-
-  /**
-   * @brief Get the index in the full set of variables that these are stored
-   * @return The start index
-   */
-  Eigen::Index getIndex() const;
+  Node* parent_{ nullptr };
 
   /**
    * @brief Increment the start index by the provided value
@@ -70,37 +81,6 @@ class Var
   void incrementIndex(Eigen::Index value);
 };
 
-template <>
-inline const double& Var::value() const
-{
-  assert(values_.size() == 1);
-  assert(index_ > -1);
-  assert(length_ == 1);
-  return values_[0];
-}
-
-template <>
-inline const Eigen::VectorXd& Var::value() const
-{
-  assert(index_ > -1);
-  assert(length_ > 1);
-  assert(names_.size() > 1);
-  return values_;
-}
-
-template <>
-inline const std::string& Var::name() const
-{
-  assert(names_.size() == 1);
-  return names_[0];
-}
-
-template <>
-inline const std::vector<std::string>& Var::name() const
-{
-  assert(names_.size() > 1);
-  return names_;
-}
 }  // namespace trajopt_ifopt
 
 #endif  // TRAJOPT_IFOPT_VAR_H