Add JointAccellConstraint and JointJerkConstraint (#275)

Author: Levi-Armstrong

trajopt_ifopt/CMakeLists.txt

@@ -38,6 +38,8 @@ set(TRAJOPT_IFOPT_SOURCE_FILES
     src/costs/squared_cost.cpp
     src/constraints/cartesian_position_constraint.cpp
     src/constraints/inverse_kinematics_constraint.cpp
+    src/constraints/joint_acceleration_constraint.cpp
+    src/constraints/joint_jerk_constraint.cpp
     src/constraints/joint_position_constraint.cpp
     src/constraints/joint_velocity_constraint.cpp
     src/constraints/collision/collision_types.cpp

trajopt_ifopt/include/trajopt_ifopt/constraints/joint_acceleration_constraint.h

@@ -0,0 +1,107 @@
+/**
+ * @file joint_acceleration_constraint.h
+ * @brief The joint_acceleration constraint
+ *
+ * @author Ben Greenberg
+ * @date April 22, 2021
+ * @version TODO
+ * @bug No known bugs
+ *
+ * @copyright Copyright (c) 2021, 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_JOINT_ACCELERATION_CONSTRAINT_H
+#define TRAJOPT_IFOPT_JOINT_JOINT_ACCELERATION_CONSTRAINT_H
+
+#include <trajopt_utils/macros.h>
+TRAJOPT_IGNORE_WARNINGS_PUSH
+#include <ifopt/constraint_set.h>
+#include <Eigen/Eigen>
+TRAJOPT_IGNORE_WARNINGS_POP
+
+#include <trajopt_ifopt/variable_sets/joint_position_variable.h>
+
+namespace trajopt_ifopt
+{
+/**
+ * @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 JointAccelConstraint : 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 name Name of the constraint
+   */
+  JointAccelConstraint(const Eigen::VectorXd& targets,
+                       const std::vector<trajopt_ifopt::JointPosition::ConstPtr>& position_vars,
+                       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 name Name of the constraint
+   */
+  JointAccelConstraint(const std::vector<ifopt::Bounds>& bounds,
+                       const std::vector<trajopt_ifopt::JointPosition::ConstPtr>& position_vars,
+                       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 number of joints in a single JointPosition */
+  long n_dof_;
+  /** @brief The number of JointPositions passed in */
+  long n_vars_;
+
+  /** @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<trajopt_ifopt::JointPosition::ConstPtr> position_vars_;
+  std::unordered_map<std::string, Eigen::Index> index_map_;
+};
+};  // namespace trajopt_ifopt
+#endif

trajopt_ifopt/include/trajopt_ifopt/constraints/joint_jerk_constraint.h

@@ -0,0 +1,107 @@
+/**
+ * @file joint_jerk_constraint.h
+ * @brief The join jerk constraint
+ *
+ * @author Levi Armstrong
+ * @date November 1, 2021
+ * @version TODO
+ * @bug No known bugs
+ *
+ * @copyright Copyright (c) 2021, 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_JOINT_JERK_CONSTRAINT_H
+#define TRAJOPT_IFOPT_JOINT_JOINT_JERK_CONSTRAINT_H
+
+#include <trajopt_utils/macros.h>
+TRAJOPT_IGNORE_WARNINGS_PUSH
+#include <ifopt/constraint_set.h>
+#include <Eigen/Eigen>
+TRAJOPT_IGNORE_WARNINGS_POP
+
+#include <trajopt_ifopt/variable_sets/joint_position_variable.h>
+
+namespace trajopt_ifopt
+{
+/**
+ * @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 JointJerkConstraint : 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 name Name of the constraint
+   */
+  JointJerkConstraint(const Eigen::VectorXd& targets,
+                      const std::vector<trajopt_ifopt::JointPosition::ConstPtr>& position_vars,
+                      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 name Name of the constraint
+   */
+  JointJerkConstraint(const std::vector<ifopt::Bounds>& bounds,
+                      const std::vector<trajopt_ifopt::JointPosition::ConstPtr>& position_vars,
+                      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 number of joints in a single JointPosition */
+  long n_dof_;
+  /** @brief The number of JointPositions passed in */
+  long n_vars_;
+
+  /** @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<trajopt_ifopt::JointPosition::ConstPtr> position_vars_;
+  std::unordered_map<std::string, Eigen::Index> index_map_;
+};
+};  // namespace trajopt_ifopt
+#endif

trajopt_ifopt/include/trajopt_ifopt/constraints/joint_velocity_constraint.h

@@ -48,25 +48,25 @@ class JointVelConstraint : public ifopt::ConstraintSet
   using ConstPtr = std::shared_ptr<const JointVelConstraint>;
 
   /**
-   * @brief Constructs a velocity contraint from these variables, setting the bounds to the target
+   * @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 name Name of the constraint
    */
   JointVelConstraint(const Eigen::VectorXd& targets,
-                     std::vector<JointPosition::ConstPtr> position_vars,
+                     const std::vector<JointPosition::ConstPtr>& position_vars,
                      const std::string& name = "JointVel");
 
   /**
-   * @brief Constructs a velocity contraint from these variables, setting the bounds to those passed in.
+   * @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 name Name of the constraint
    */
   JointVelConstraint(const std::vector<ifopt::Bounds>& bounds,
-                     std::vector<JointPosition::ConstPtr> position_vars,
+                     const std::vector<JointPosition::ConstPtr>& position_vars,
                      const std::string& name = "JointVel");
 
   /**
@@ -84,7 +84,7 @@ class JointVelConstraint : public ifopt::ConstraintSet
   /**
    * @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
+   * @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;
 

trajopt_ifopt/include/trajopt_ifopt/utils/ifopt_utils.h

@@ -126,9 +126,36 @@ Eigen::VectorXd calcBoundsErrors(const Eigen::Ref<const Eigen::VectorXd>& input,
 Eigen::VectorXd calcBoundsViolations(const Eigen::Ref<const Eigen::VectorXd>& input,
                                      const std::vector<ifopt::Bounds>& bounds);
 
+/**
+ * @brief Calculate the numerical cost gradient at the provided values
+ * @param x The variable values to calculate the gradient about
+ * @param nlp The nlp problem
+ * @param epsilon The epsilon to leverage for perturbing the values
+ * @return The numerical cost gradient
+ */
 ifopt::Problem::VectorXd calcNumericalCostGradient(const double* x, ifopt::Problem& nlp, double epsilon = 1e-8);
 
+/**
+ * @brief Calculate the numerical constraint gradient at the provided values
+ * @param x The variable values to calculate the gradient about
+ * @param nlp The nlp problem
+ * @param epsilon The epsilon to leverage for perturbing the values
+ * @return The numerical constraint gradient
+ */
 ifopt::Problem::Jacobian calcNumericalConstraintGradient(const double* x, ifopt::Problem& nlp, double epsilon = 1e-8);
 
+/**
+ * @brief Calculate the numerical constraint gradient at the provided values
+ * @details This is primarily used buy unit tests to avoid needing to create a problem to calculate the gradient of a
+ * single constraint.
+ * @param variables The variable values to calculate the gradient about
+ * @param constraint_set The constraint to numerically calculate the gradient for
+ * @param epsilon The epsilon to leverage for perturbing the values
+ * @return The numerical constraint gradient
+ */
+ifopt::Problem::Jacobian calcNumericalConstraintGradient(ifopt::Component& variables,
+                                                         ifopt::ConstraintSet& constraint_set,
+                                                         double epsilon = 1e-8);
+
 }  // namespace trajopt_ifopt
 #endif