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,102 @@ struct CartVelCalculator : VectorOfVector
 
   VectorXd operator()(const VectorXd& dof_vals) const;
 };
+
+enum Axis {
+  X_AXIS,
+  Y_AXIS,
+  Z_AXIS
+};
+
+/**
+ * @brief The ConfinedAxisErrCalculator is a truct whose operator() calculates error of a given
+ * pose with respect ot the confined rotation along the defined axis.
+ */
+struct ConfinedAxisErrCalculator : public VectorOfVector {
+
+  Eigen::Affine3d pose_inv_; /**< param The inverse of the desired pose */
+  tesseract::BasicKinConstPtr manip_; /**< Kinematics object */
+  tesseract::BasicEnvConstPtr env_; /**< Environment object */
+  std::string link_; /**< Link of the robot referred to */
+  Eigen::Affine3d tcp_; /**< Tool center point */
+
+  Axis axis_; /**< Axis of rotation being provided with a tolerance */
+  double tol_; /**< Tolerance angle in radians */
+
+  /**
+   * @brief ConfinedAxisErrCalculator 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
+   */
+  ConfinedAxisErrCalculator(const Eigen::Affine3d& pose, tesseract::BasicKinConstPtr manip, tesseract::BasicEnvConstPtr env,
+                            std::string link, Axis axis, double tol_angle, Eigen::Affine3d tcp = Eigen::Affine3d::Identity()) :
+    pose_inv_(pose.inverse()),
+    manip_(manip),
+    env_(env),
+    link_(link),
+    tcp_(tcp),
+    axis_(axis),
+    tol_(tol_angle * M_PI / 180.0)
+  {
+  }
+
+  /**
+   * @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::Affine3d pose_inv_; /**< Inverse of the desired pose */
+  tesseract::BasicKinConstPtr manip_; /**< Kinematics object */
+  tesseract::BasicEnvConstPtr env_; /**< Environment object */
+  std::string link_; /**< The link of the robot reggered to */
+  Eigen::Affine3d tcp_; /**< Tool center point */
+
+  Axis axis_; /**< Axis the conical tolreance is applied to */
+  double tol_; /**< 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::Affine3d& pose, tesseract::BasicKinConstPtr manip, tesseract::BasicEnvConstPtr env,
+                           std::string link, Axis axis, double tol_angle, Eigen::Affine3d tcp = Eigen::Affine3d::Identity()) :
+    pose_inv_(pose.inverse()),
+    manip_(manip),
+    env_(env),
+    link_(link),
+    tcp_(tcp),
+    axis_(axis),
+    tol_(tol_angle * M_PI / 180.0)
+  {
+  }
+
+  /**
+   * @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
 {
@@ -12,6 +13,7 @@ struct OptResults;
 
 namespace trajopt
 {
+
 using namespace json_marshal;
 using namespace Json;
 
@@ -322,4 +324,71 @@ struct JointConstraintInfo : public TermInfo, public MakesConstraint
   void hatch(TrajOptProb& prob);
   DEFINE_CREATE(JointConstraintInfo)
 };
+
+/**
+ * @brief The ConfinedAxisTermInfo struct contains information to create constraints or costs
+ * using a tolerance for the rotation about an axis
+ */
+struct ConfinedAxisTermInfo : public TermInfo, public MakesConstraint, public MakesCost {
+  int timestep; /**< The timestep of this term in the trajectory */
+  Vector3d xyz; /**< Cartesian position coordinate vector for the pose */
+  Vector4d wxyz; /**< 4D vector containing w, x, y, and z quaternion components for the pose (in that order) */
+  Vector3d pos_coeffs; /**< Coefficients multiplied by xyz errors during evaluation of cost/constraint values */
+  double confined_coeff; /**< Coefficient multiplied by error of the rotation beyond tolerance */
+  double axis_coeff; /**< Coefficient multiplied by the errors of the rotation axis from the specified axis */
+
+  string link; /**< Link of the robot the term refers to */
+  Eigen::Affine3d tcp; /**< Tool center point */
+
+  Axis axis; /**< Axis allowed to rotate */
+  double tol; /**< Rotation acceptable in degrees */
+
+  ConfinedAxisTermInfo();
+
+  /**
+   * @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(ConfinedAxisTermInfo)
+};
+
+/**
+ * @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; /**< The timestep of this term in the trajectory */
+  Vector3d xyz; /**< Cartesian position coordinate vector for the pose */
+  Vector4d wxyz; /**< 4D vector containing w, x, y, and z quaternion components for the pose (in that order) */
+  Vector3d pos_coeffs; /**< Coefficients multiplied by xyz errors during evaluation of cost/constraint values */
+  double conical_coeff; /**< Coefficient multiplied by error of the axis orientation beyond tolerance */
+  double axis_coeff; /**< Coefficient multiplied by the rotation error about the axis in the conical tolerance */
+
+  string link; /**< Link of the robot the term refers to */
+  Eigen::Affine3d tcp; /**< Tool center point */
+
+  Axis axis; /**< Axis given a conical tolerance */
+  double tol; /**< Cone angle in degrees */
+
+  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,54 @@ 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 ConfinedAxisErrCalculator::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 error of the current pose
+  Affine3d pose_err = pose_inv_ * (new_pose * tcp_);
+  Quaterniond q(pose_err.rotation());
+  VectorXd err(1);
+
+  // determine the error of the rotation
+   switch(axis_) {
+      case X_AXIS: err(0) = 2.0*fabs(asin(q.x())) - tol_;
+      break;
+      case Y_AXIS: err(0) = 2.0*fabs(asin(q.y())) - tol_;
+      break;
+      case Z_AXIS: err(0) = 2.0*fabs(asin(q.z())) - tol_;
+      break;
+    }
+
+  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 error of the current pose
+  Affine3d pose_err = pose_inv_ * (new_pose * tcp_);
+
+  // get the orientation matrix of the error
+  Matrix3d orientation_err(pose_err.rotation());
+  VectorXd err(1);
+
+  // determine the error of the conical axis
+  switch(axis_) {
+     case X_AXIS: err(0) = acos(orientation_err(0, 0)) - tol_;
+     break;
+     case Y_AXIS: err(0) = acos(orientation_err(1, 1)) - tol_;
+     break;
+     case Z_AXIS: err(0) = acos(orientation_err(2, 2)) - tol_;
+     break;
+   }
+
+  return err;
+}
 
-#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
 }