Addition of two angular constraints

trajopt/CMakeLists.txt

@@ -83,4 +83,8 @@ if (CATKIN_ENABLE_TESTING)
 
   add_rostest_gtest(${PROJECT_NAME}_cast_cost_octomap_unit test/cast_cost_octomap_unit.launch test/cast_cost_octomap_unit.cpp)
   target_link_libraries(${PROJECT_NAME}_cast_cost_octomap_unit ${PROJECT_NAME} ${Boost_SYSTEM_LIBRARY} ${Boost_PROGRAM_OPTIONS_LIBRARY} ${PCL_LIBRARIES} ${catkin_LIBRARIES})
+
+  add_rostest_gtest(${PROJECT_NAME}_angular_constraint_unit test/angular_constraint_unit.launch test/angular_constraint_unit.cpp)
+  target_link_libraries(${PROJECT_NAME}_angular_constraint_unit ${PROJECT_NAME} ${Boost_SYSTEM_LIBRARY} ${Boost_PROGRAM_OPTIONS_LIBRARY} ${catkin_LIBRARIES})
+
 endif()

trajopt/include/trajopt/kinematic_terms.hpp

@@ -1,5 +1,4 @@
 #pragma once
-
 #include <Eigen/Core>
 #include <tesseract_core/basic_env.h>
 #include <tesseract_core/basic_kin.h>
@@ -104,4 +103,105 @@ struct CartVelCalculator : VectorOfVector
 
   VectorXd operator()(const VectorXd& dof_vals) const;
 };
+
+/**
+ * @brief The AlignedAxisErrCalculator is a struct whose operator() calculates error of a given
+ * pose with respect to the rotation along the defined axis.
+ */
+struct AlignedAxisErrCalculator : public VectorOfVector
+{
+  Eigen::Matrix3d orientation_inv_;   /**< @brief param The inverse of the desired orientation */
+  tesseract::BasicKinConstPtr manip_; /**< @brief Kinematics object */
+  tesseract::BasicEnvConstPtr env_;   /**< @brief Environment object */
+  std::string link_;                  /**< @brief Link of the robot referred to */
+  Eigen::Matrix3d tcp_orientation_;   /**< @brief Tool center point orientation */
+
+  Vector3d axis_; /**< @brief Axis of rotation to align with */
+  double tol_;    /**< @brief Tolerance angle in radians */
+
+  /**
+   * @brief AlignedAxisErrCalculator Constructor
+   * @param pose Desired pose
+   * @param manip
+   * @param env
+   * @param link
+   * @param axis Axis of rotation
+   * @param tol_angle Tolerance in radians
+   * @param tcp Tool center point
+   */
+  AlignedAxisErrCalculator(const Eigen::Matrix3d& orientation,
+                           tesseract::BasicKinConstPtr manip,
+                           tesseract::BasicEnvConstPtr env,
+                           std::string link,
+                           Vector3d axis,
+                           double tol,
+                           Eigen::Matrix3d tcp_orientation = Eigen::Matrix3d::Identity())
+    : orientation_inv_(orientation.inverse())
+    , manip_(manip)
+    , env_(env)
+    , link_(link)
+    , tcp_orientation_(tcp_orientation)
+    , axis_(axis)
+    , tol_(tol)
+  {
+  }
+
+  /**
+   * @brief operator () Calculates error
+   * @param dof_vals Values of the joints for forward kinematics
+   * @return 1D vector of error beyond the allowed rotation
+   */
+  VectorXd operator()(const VectorXd& dof_vals) const;
+};
+
+/**
+ * @brief The ConicalAxisErrCalculator is a struct whose operator() returns the error of the
+ * given pose with respect to the conical constraint defined
+ *
+ */
+struct ConicalAxisErrCalculator : public VectorOfVector
+{
+  Eigen::Matrix3d orientation_inv_;   /**< @brief Inverse of the desired orientation */
+  tesseract::BasicKinConstPtr manip_; /**< @brief Kinematics object */
+  tesseract::BasicEnvConstPtr env_;   /**< @brief Environment object */
+  std::string link_;                  /**< @brief The link of the robot referred to */
+  Eigen::Matrix3d tcp_orientation_;   /**< @brief Tool center point orientation */
+
+  Vector3d axis_; /**< @brief Axis the conical tolerance is applied to */
+  double tol_;    /**< @brief Tolerance angle in radians */
+
+  /**
+   * @brief ConicalAxisErrCalculator
+   * @param pose Desired pose
+   * @param manip Kinematics Object
+   * @param env Environment object
+   * @param link Link of the robot the term applies to
+   * @param axis Axis to define the conical tolerance around
+   * @param tol_angle Tolerance angle in degrees
+   * @param tcp Tool center point
+   */
+  ConicalAxisErrCalculator(const Eigen::Matrix3d& orientation,
+                           tesseract::BasicKinConstPtr manip,
+                           tesseract::BasicEnvConstPtr env,
+                           std::string link,
+                           Vector3d axis,
+                           double tol,
+                           Eigen::Matrix3d tcp_orientation = Eigen::Matrix3d::Identity())
+    : orientation_inv_(orientation.inverse())
+    , manip_(manip)
+    , env_(env)
+    , link_(link)
+    , tcp_orientation_(tcp_orientation)
+    , axis_(axis)
+    , tol_(tol)
+  {
+  }
+
+  /**
+   * @brief operator ()
+   * @param dof_vals
+   * @return
+   */
+  VectorXd operator()(const VectorXd& dof_vals) const;
+};
 }

trajopt/include/trajopt/problem_description.hpp

@@ -4,6 +4,7 @@
 #include <trajopt/common.hpp>
 #include <trajopt/json_marshal.hpp>
 #include <trajopt_sco/optimizers.hpp>
+#include <trajopt/kinematic_terms.hpp>
 
 namespace sco
 {
@@ -322,4 +323,67 @@ struct JointConstraintInfo : public TermInfo, public MakesConstraint
   void hatch(TrajOptProb& prob);
   DEFINE_CREATE(JointConstraintInfo)
 };
+
+/**
+ * @brief The AlignedAxisTermInfo struct contains information to create constraints or costs
+ * using a tolerance for the rotation about an axis
+ */
+struct AlignedAxisTermInfo : public TermInfo, public MakesConstraint, public MakesCost
+{
+  int timestep; /**< @brief The timestep of this term in the trajectory */
+  Vector4d
+      wxyz; /**< @brief 4D vector containing w, x, y, and z quaternion components for the pose orientation (in that
+               order) */
+  Vector4d tcp_wxyz;  /**< @brief Tool center point */
+  double axis_coeff;  /**< @brief Coefficient multiplied by the errors of the rotation axis from the specified axis */
+  double angle_coeff; /**< @brief Coefficient multipleid by the angle of rotation past the tolerance */
+  string link;        /**< @brief Link of the robot the term refers to */
+  Vector3d axis;      /**< @brief Axis allowed to rotate with respect to the frame of #link */
+  double tolerance;   /**< @brief Rotation tolerance about the given axis in radians */
+
+  AlignedAxisTermInfo();
+
+  /**
+   * @brief fromJson Constructs the term from a Json file
+   */
+  void fromJson(ProblemConstructionInfo& pci, const Value& v);
+
+  /**
+   * @brief hatch Add the proper cost/constraint functions to the problem
+   * @param prob The optimization problems to add the term to
+   */
+  void hatch(TrajOptProb& prob);
+  DEFINE_CREATE(AlignedAxisTermInfo)
+};
+
+/**
+ * @brief The ConicalAxisTermInfo struct contains information to create constraints or costs
+ * using a conical tolerance about an axis
+ */
+struct ConicalAxisTermInfo : public TermInfo, public MakesConstraint, public MakesCost
+{
+  int timestep; /**< @brief The timestep of this term in the trajectory */
+  Vector4d
+      wxyz; /**< @brief 4D vector containing w, x, y, and z quaternion components for the pose orientation (in that
+               order) */
+  Vector4d tcp_wxyz; /**< @brief Tool center point */
+  double weight;     /**< @brief Coefficient multiplied by the errors of the rotation axis from the specified axis */
+  string link;       /**< @brief Link of the robot the term refers to */
+  Vector3d axis;     /**< @brief Axis around which the conical tolerance will be applied */
+  double tolerance;  /**< @brief Tolerance angle of the cone in radians*/
+
+  ConicalAxisTermInfo();
+
+  /**
+   * @brief fromJson Constructs the term from a Json file
+   */
+  void fromJson(ProblemConstructionInfo& pci, const Value& v);
+
+  /**
+   * @brief hatch Add the proper cost/constraint functions to the problem
+   * @param prob The optimization problems to add the term to
+   */
+  void hatch(TrajOptProb& prob);
+  DEFINE_CREATE(ConicalAxisTermInfo)
+};
 }

trajopt/src/kinematic_terms.cpp

@@ -88,8 +88,13 @@ VectorXd StaticCartPoseErrCalculator::operator()(const VectorXd& dof_vals) const
       env_->getState(manip_->getJointNames(), dof_vals)->transforms.at(manip_->getBaseLinkName())));
   manip_->calcFwdKin(new_pose, change_base, dof_vals, link_, *state);
 
+  // calculate the err of the current pose
   Affine3d pose_err = pose_inv_ * (new_pose * tcp_);
+
+  // get the quaternion representation of the rotation error
   Quaterniond q(pose_err.rotation());
+
+  // construct a 6D vector containing orientation and positional errors
   VectorXd err = concat(Vector3d(q.x(), q.y(), q.z()), pose_err.translation());
   return err;
 }
@@ -171,37 +176,46 @@ VectorXd CartVelCalculator::operator()(const VectorXd& dof_vals) const
   return out;
 }
 
-#if 0
-CartVelConstraint::CartVelConstraint(const VarVector& step0vars, const VarVector& step1vars, RobotAndDOFPtr manip, KinBody::LinkPtr link, double distlimit) :
-        ConstraintFromFunc(VectorOfVectorPtr(new CartVelCalculator(manip, link, distlimit)),
-             MatrixOfVectorPtr(new CartVelJacCalculator(manip, link, distlimit)), concat(step0vars, step1vars), VectorXd::Ones(0), INEQ, "CartVel") 
-{} // TODO coeffs
-#endif
+VectorXd AlignedAxisErrCalculator::operator()(const VectorXd& dof_vals) const
+{
+  // calculate the current pose given the DOF values for the robot
+  Affine3d new_pose, change_base;
+  change_base = env_->getLinkTransform(manip_->getBaseLinkName());
+  manip_->calcFwdKin(new_pose, change_base, dof_vals, link_, *env_->getState());
 
-#if 0
-struct UpErrorCalculator {
-  Vector3d dir_local_;
-  Vector3d goal_dir_world_;
-  RobotAndDOFPtr manip_;
-  OR::KinBody::LinkPtr link_;
-  MatrixXd perp_basis_; // 2x3 matrix perpendicular to goal_dir_world
-  UpErrorCalculator(const Vector3d& dir_local, const Vector3d& goal_dir_world, RobotAndDOFPtr manip, KinBody::LinkPtr link) :
-    dir_local_(dir_local),
-    goal_dir_world_(goal_dir_world),
-    manip_(manip),
-    link_(link)
-  {
-    Vector3d perp0 = goal_dir_world_.cross(Vector3d::Random()).normalized();
-    Vector3d perp1 = goal_dir_world_.cross(perp0);
-    perp_basis_.resize(2,3);
-    perp_basis_.row(0) = perp0.transpose();
-    perp_basis_.row(1) = perp1.transpose();
-  }
-  VectorXd operator()(const VectorXd& dof_vals) {
-    manip_->SetDOFValues(toDblVec(dof_vals));
-    OR::Transform newpose = link_->GetTransform();
-    return perp_basis_*(toRot(newpose.rot) * dir_local_ - goal_dir_world_);
-  }
-};
-#endif
+  // get the error of the current pose
+  Matrix3d orientation_err = orientation_inv_ * (new_pose.rotation() * tcp_orientation_);
+  AngleAxisd aa_err(orientation_err);
+
+  // gets terms for error determination
+  double angle = fabs(aa_err.angle());
+
+  // the angle error is typical: actual value - tolerance
+  double angle_err = angle - tol_;
+
+  // axis error
+  // the element wise squared difference between the desired axis and
+  // the desired axis after being rotated by the current pose "error"
+  Vector3d axis_err = (axis_ - (orientation_err * axis_)).array().square();
+
+  Vector4d err(axis_err.x(), axis_err.y(), axis_err.z(), angle_err);
+  return err;
+}
+
+VectorXd ConicalAxisErrCalculator::operator()(const VectorXd& dof_vals) const
+{
+  // calculate the current pose given the DOF values for the robot
+  Affine3d new_pose, change_base;
+  change_base = env_->getLinkTransform(manip_->getBaseLinkName());
+  manip_->calcFwdKin(new_pose, change_base, dof_vals, link_, *env_->getState());
+
+  // get the orientation matrix of the error
+  Matrix3d orientation_err = orientation_inv_ * (new_pose.rotation() * tcp_orientation_);
+  VectorXd err(1);
+
+  // determine the error of the conical axis
+  err(0) = acos((orientation_err * axis_).dot(axis_)) - tol_;
+
+  return err;
+}
 }