Fix JointPosIneqConstraint

tesseract-robotics · Apr 20, 2021 · eeb5392 · eeb5392
1 parent 6301416
commit eeb5392
Show file tree

Hide file tree

Showing 2 changed files with 19 additions and 18 deletions.
diff --git a/trajopt/src/trajectory_costs.cpp b/trajopt/src/trajectory_costs.cpp
@@ -110,7 +110,7 @@ double JointPosIneqCost::value(const DblVec& xvec)
 {
   // Convert vector from optimization to trajectory
   Eigen::MatrixXd traj = getTraj(xvec, vars_);
-  // Takes diff b/n the subsequent rows to get velocity
+  // Get the correct rows
   Eigen::MatrixXd pos = traj.block(first_step_, 0, last_step_ - first_step_ + 1, traj.cols());
   // Subtract targets to center about 0 and then subtract from tolerance
   Eigen::MatrixXd diff0 = (pos.rowwise() - targets_.transpose());
@@ -199,22 +199,23 @@ JointPosIneqConstraint::JointPosIneqConstraint(VarArray vars,
   {
     for (int j = 0; j < vars_.cols(); ++j)
     {
-      sco::AffExpr expr;
-      sco::AffExpr expr_neg;
       // pos = x1 - targ
       sco::AffExpr pos;
       sco::exprInc(pos, sco::exprMult(vars_(i, j), 1));
-      sco::exprDec(pos, targets_[j]);
-      sco::exprInc(expr, upper_tols_[j]);  // expr_ = upper_tol
+      sco::exprDec(pos, targets_[j]);  // centers about 0.
 
-      // Form upper limit expr = - (upper_tol-(vel-targ))
-      sco::exprDec(expr, pos);            // expr = upper_tol_- (vel - targets_)
-      sco::exprScale(expr, -coeffs_[j]);  // expr = - (upper_tol_- (vel - targets_)) * coeffs_
+      // Form upper limit: expr = (pos - upper_tol) * coeff
+      sco::AffExpr expr;
+      sco::exprInc(expr, pos);
+      sco::exprDec(expr, upper_tols_[j]);
+      sco::exprScale(expr, coeffs_[j]);
       expr_vec_.push_back(expr);
 
-      // Form lower limit expr = (upper_tol-(vel-targ))
-      sco::exprDec(expr_neg, pos);           // expr = lower_tol_- (vel - targets_)
-      sco::exprScale(expr_neg, coeffs_[j]);  // expr = (lower_tol_- (vel - targets_)) * coeffs_
+      // Form lower limit: expr = (lower_tol - pos) * coeff
+      sco::AffExpr expr_neg;
+      sco::exprInc(expr_neg, lower_tols_[j]);
+      sco::exprDec(expr_neg, pos);
+      sco::exprScale(expr_neg, coeffs_[j]);
       expr_vec_.push_back(expr_neg);
     }
   }
@@ -224,8 +225,8 @@ DblVec JointPosIneqConstraint::value(const DblVec& xvec)
 {
   // Convert vector from optimization to trajectory
   Eigen::MatrixXd traj = getTraj(xvec, vars_);
-  // Takes diff b/n the subsequent rows to get velocity
-  Eigen::MatrixXd pos = diffAxis0(traj.block(first_step_, 0, last_step_ - first_step_ + 1, traj.cols()));
+  // Get the correct rows
+  Eigen::MatrixXd pos = traj.block(first_step_, 0, last_step_ - first_step_ + 1, traj.cols());
   // Subtract targets to center about 0 and then subtract from tolerance
   Eigen::MatrixXd diff0 = (pos.rowwise() - targets_.transpose());
   Eigen::MatrixXd diff1 = (diff0.rowwise() - upper_tols_.transpose()) * coeffs_.asDiagonal();

diff --git a/trajopt/test/joint_costs_unit.cpp b/trajopt/test/joint_costs_unit.cpp
@@ -152,7 +152,7 @@ TEST_F(CostsTest, inequality_jointPos)  // NOLINT
   CONSOLE_BRIDGE_logDebug("CostsTest, inequality_cnt_jointPos");
 
   const double lower_tol = -0.1;
-  const double upper_tol = 0.1;
+  const double upper_tol = 0.2;
   const double cost_targ1 = 0.5;
   const double cost_targ2 = -0.5;
 
@@ -174,7 +174,7 @@ TEST_F(CostsTest, inequality_jointPos)  // NOLINT
   pci.init_info.type = InitInfo::STATIONARY;
   pci.init_info.data = start_pos.transpose().replicate(pci.basic_info.n_steps, 1);
 
-  // Constraint that limits velocity
+  // Constraint that limits position
   auto jv = std::make_shared<JointPosTermInfo>();
   jv->coeffs = std::vector<double>(7, 1.0);
   jv->targets = std::vector<double>(7.0, 0);
@@ -354,7 +354,7 @@ TEST_F(CostsTest, inequality_jointVel)  // NOLINT
   CONSOLE_BRIDGE_logDebug("CostsTest, inequality_cnt_jointVel");
 
   const double lower_tol = -0.1;
-  const double upper_tol = 0.1;
+  const double upper_tol = 0.2;
   const double cost_targ1 = 0.5;
   const double cost_targ2 = -0.5;
 
@@ -562,7 +562,7 @@ TEST_F(CostsTest, inequality_jointVel_time)  // NOLINT
   CONSOLE_BRIDGE_logDebug("CostsTest, inequality_cnt_jointVel_time");
 
   const double lower_tol = -0.1;
-  const double upper_tol = 0.1;
+  const double upper_tol = 0.2;
   const double cost_targ1 = 0.5;
   const double cost_targ2 = -0.5;
   const double dt_lower = 0.01234;
@@ -768,7 +768,7 @@ TEST_F(CostsTest, inequality_jointAcc)  // NOLINT
   CONSOLE_BRIDGE_logDebug("CostsTest, inequality_cnt_jointVel");
 
   const double lower_tol = -0.1;
-  const double upper_tol = 0.1;
+  const double upper_tol = 0.2;
   const double cost_targ1 = 0.5;
   const double cost_targ2 = -0.5;