woo-hoo! Now CollisionTermInfo of trajopt is implemented.

This version successfully uses Bullet collision detector from MoveIt to do the collsion checking and find singed distances. There are a lot of changes in different
files but the most changes are done in collision_terms.cpp

Things left to do:
1. This implementation is hard-coded. But it is easy to change this to a version that the user can enter a CollisionTermInfo.
Currently this is done in trajopt_interface.cpp but it should be defined by user, its parametes should be added to setup.yaml as well.

2. It needs cleanup in the code base.

3. The PlanningScene that motion planners have acces to is a ConsPtr but we need to make changes in RobotState to be able to check collision. To this end,
a child of the original PlanningScene is used through diff() function. For some reason, I can not use "checkCollision" function on this planning scene child.
It turned out if I remove all the objects from the child and add them again, "checkCollision" operates correctly

Author: ommmid

moveit_planners/trajopt/trajopt/include/trajopt/collision_terms.h

@@ -9,6 +9,7 @@
 #include <moveit/robot_model/robot_model.h>
 #include <moveit/planning_scene/planning_scene.h> 
 #include <moveit/collision_detection/collision_common.h>
+#include <moveit/collision_detection/world.h>
 
 
 namespace trajopt
@@ -26,10 +27,17 @@ struct CollisionEvaluator
   {
     // we need a child of planning scene so I can do the changing robot states and do collsiiong detenction    
     planning_scene_ = planning_scene->diff();
-    // planning_scene_->setActiveCollisionDetector(collision_detection::CollisionDetectorAllocatorFCL::create(), true);
-
-
-    // OR
+    // remove/add magic objects in planning scene
+    std::vector<std::string> objectIds = planning_scene_->getWorld()->getObjectIds();
+    for (auto objectId : objectIds)
+    {
+      collision_detection::World::ObjectConstPtr objPtr = planning_scene_->getWorld()->getObject(objectId);
+      planning_scene_->getWorldNonConst()->removeObject(objectId);  
+      planning_scene_->getWorldNonConst()->addToObject(objPtr->id_, objPtr->shapes_, objPtr->shape_poses_);
+    }
+    
+
+    // OR ?
     //planning_scene_ = planning_scene::PlanningScene::clone(planning_scene);
   }
   virtual ~CollisionEvaluator() = default;

moveit_planners/trajopt/trajopt/src/collision_terms.cpp

@@ -16,7 +16,7 @@ TRAJOPT_IGNORE_WARNINGS_POP
 
 #include <moveit/collision_detection/collision_common.h>
 #include <moveit/collision_detection_bullet/collision_detector_allocator_bullet.h>
-
+#include <moveit/collision_detection/world.h>
 
 namespace trajopt
 {
@@ -245,7 +245,6 @@ SingleTimestepCollisionEvaluator::SingleTimestepCollisionEvaluator(
 }
 
 // So, each pair has a vector of contacts, and this functions puts all of these vectors of all pairs to one big vector called dist_results
-//void SingleTimestepCollisionEvaluator::CalcCollisions(const DblVec& x, tesseract::ContactResultVector& dist_results)
 void SingleTimestepCollisionEvaluator::CalcCollisions(const DblVec& x, trajopt::ContactResultVector& dist_results)
 {
   // tesseract::ContactResultMap contacts;
@@ -288,7 +287,10 @@ void SingleTimestepCollisionEvaluator::CalcCollisions(const DblVec& x, trajopt::
   // for(std::size_t z = 0; z < collision_objs.size(); ++z)
   //   std::cout << "===>>> name of the collision object: " << collision_objs[z].id << " ";
   // std::cout << std::endl;
-  
+
+  // std::vector<std::string> obj_ids = planning_scene_->getWorld()->getObjectIds();
+  // trajopt::printVector("world object ids: ", obj_ids);
+
   // std::cout << "===>>> robot model: " << std::endl;
   // std::cout << "name: " << planning_scene_->getRobotModel()->getName() << std::endl;
   // std::cout << "root joint: " <<  planning_scene_->getRobotModel()->getRootJointName() << std::endl;
@@ -320,21 +322,16 @@ void SingleTimestepCollisionEvaluator::CalcCollisions(const DblVec& x, trajopt::
   collision_request.group_name = planning_group_;
   // collision_request.distance = true; // If true, compute proximity distance 
   collision_request.contacts = true; // If true, compute contacts. Otherwise only a binary collision yes/no is reported
-  // collision_request.max_contacts = 100;
-  // collision_request.max_contacts_per_pair = 5;
-  // collision_request.verbose = false;
+  collision_request.max_contacts = 100;
+  collision_request.max_contacts_per_pair = 5;
+  collision_request.verbose = false;
 
   // planning_scene_->printKnownObjects();
-  // collision_objs[0].operation == moveit_msgs::CollisionObject::REMOVE;
-  // planning_scene_->processCollisionObjectMsg(collision_objs[0]);
-  // collision_objs[0].operation == moveit_msgs::CollisionObject::ADD;
-  // planning_scene_->processCollisionObjectMsg(collision_objs[0]);
 
-  planning_scene_->checkCollision(collision_request, collision_result);
-  // planning_scene->checkCollision(collision_request, collision_result, rob_state);
+  planning_scene_->checkCollision(collision_request, collision_result, rob_state);
   // ====> checkCollision will check for both self-collisions and for collisions with the environment
-  // std::cout << "+++++++++++++++ single time step in collision -------- " << collision_result.collision << std::endl;
-  // std::cout << "+++++++++++++++ number of contacts ------------------- " << collision_result.contact_count << std::endl;
+  LOG_DEBUG("single time step in collision? %s", collision_result.collision ? "true" : "false");
+  LOG_DEBUG("contact count: %i", collision_result.contact_count);
 
   // tesseract:
   // ContactResultMap => AlignedMap<std::pair<std::string, std::string>, ContactResultVector>
@@ -382,7 +379,6 @@ void SingleTimestepCollisionEvaluator::CalcCollisions(const DblVec& x, trajopt::
         dist_results.push_back(contact_result);
       }
   }
-
 }
 
 // so, given a set of joint values called x, this function converts the vector of contacts to the vector of distances
@@ -421,7 +417,6 @@ CastCollisionEvaluator::CastCollisionEvaluator(planning_scene::PlanningSceneCons
 // swept volume?
 void CastCollisionEvaluator::CalcCollisions(const DblVec& x, trajopt::ContactResultVector& dist_results)
 {
-    std::cout << "CastCollisionEvaluator::CalcCollisions() is calleeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeed" << std::endl;
   // tesseract::ContactResultMap contacts;
   // tesseract::EnvStatePtr state0 = env_->getState(manip_->getJointNames(), sco::getVec(x, m_vars0));
   // tesseract::EnvStatePtr state1 = env_->getState(manip_->getJointNames(), sco::getVec(x, m_vars1));
@@ -444,48 +439,43 @@ void CastCollisionEvaluator::CalcCollisions(const DblVec& x, trajopt::ContactRes
   Eigen::VectorXd joint_values_state_0 = sco::getVec(x, m_vars0);
   std::vector<double> joint_values_DblVec_0 = std::vector<double>(joint_values_state_0.data(), joint_values_state_0.data() + joint_values_state_0.size());
 
-  // planning_scene::PlanningScenePtr planning_scene = planning_scene_->diff();
-
-  robot_state::RobotStatePtr rob_state(new robot_state::RobotState(planning_scene_->getCurrentStateNonConst())); 
-  const moveit::core::JointModelGroup* joint_model_group = rob_state->getJointModelGroup(planning_group_);
+  moveit::core::RobotState& rob_state = planning_scene_->getCurrentStateNonConst();
+  const moveit::core::JointModelGroup* joint_model_group = rob_state.getJointModelGroup(planning_group_);
 
   collision_detection::CollisionRequest collision_request;
   collision_detection::CollisionResult collision_result;
   collision_request.group_name = planning_group_;
-  collision_request.distance = true;
+  // collision_request.distance = true;
   collision_request.contacts = true;
   collision_request.max_contacts = 100;
   collision_request.max_contacts_per_pair = 5;
   collision_request.verbose = false;
 
-  rob_state->setToDefaultValues();
-  rob_state->setJointGroupPositions(joint_model_group, joint_values_DblVec_0);
-  rob_state->update();
-  robot_state::RobotState rob_state_previous(*rob_state);
+  rob_state.setToDefaultValues();
+  rob_state.setJointGroupPositions(joint_model_group, joint_values_DblVec_0);
+  rob_state.update();
+  robot_state::RobotState rob_state_previous(rob_state);
 
-  Eigen::VectorXd first_values;
-  rob_state->copyJointGroupPositions(joint_model_group, first_values);
-  std::cout << "===>>> first state joint values: " << first_values.transpose() << std::endl;
+  // Eigen::VectorXd first_values;
+  // rob_state.copyJointGroupPositions(joint_model_group, first_values);
+  // std::cout << "===>>> first state joint values: " << first_values.transpose() << std::endl;
 
   Eigen::VectorXd joint_values_state_1 = sco::getVec(x, m_vars1);
   std::vector<double> joint_values_DblVec_1 = std::vector<double>(joint_values_state_1.data(), joint_values_state_1.data() + joint_values_state_1.size());  
-  rob_state->setJointGroupPositions(joint_model_group, joint_values_DblVec_1);
-  rob_state->update();
+  rob_state.setJointGroupPositions(joint_model_group, joint_values_DblVec_1);
+  rob_state.update();
 
-  Eigen::VectorXd second_values;
-  rob_state->copyJointGroupPositions(joint_model_group, second_values);
-  std::cout << "===>>> second state joint values: " << second_values.transpose() << std::endl;
+  // Eigen::VectorXd second_values;
+  // rob_state.copyJointGroupPositions(joint_model_group, second_values);
+  // std::cout << "===>>> second state joint values: " << second_values.transpose() << std::endl;
 
   // check collision between two states
   planning_scene_->getCollisionEnv()->checkRobotCollision(collision_request, collision_result,
-                                                         *rob_state, rob_state_previous);
-
-  std::cout << "---------- in continous collision -------- " << collision_result.collision << std::endl;
+                                                         rob_state, rob_state_previous);
 
-  // std::cout << "+++++++++++++++ start state is in collision? " << collision_res.collision << std::endl;
-  // std::cout << "+++++++++++++++ number of contacts " << collision_res.contact_count << std::endl;
-
-    
+  LOG_DEBUG("single time step in collision? %s", collision_result.collision ? "true" : "false");
+  LOG_DEBUG("contact count: %i", collision_result.contact_count);
+   
   // ????? I have to calculate: cc_time, cc_type and cc_nearest_points here. test
 
   // when we use bullet in MoveIt, CollisionResult returns all the contacts in the scene by ContactMap data type.
@@ -514,12 +504,7 @@ void CastCollisionEvaluator::CalcCollisions(const DblVec& x, trajopt::ContactRes
                                      moveit_cc_time,
                                      moviet_cc_type);
         dist_results.push_back(contact_result);
-
-        std::cout << " ******* " << contact_vector[k].nearest_points[0] << std::endl;
-        std::cout << " ******* " << contact_vector[k].nearest_points[1] << std::endl;
       }
-      // dist_results.insert(dist_results.end(), it->second.begin(), it->second.end())
-      std::cout << "----------------- *************** ---------------" << std::endl;
   }
 }
 

moveit_planners/trajopt/trajopt/src/problem_description.cpp

@@ -656,6 +656,7 @@ void generateInitialTrajectory(const ProblemInfo& pci, const std::vector<double>
   // Currently all trajectories are generated without time then appended here
   if (pci.basic_info.use_time)
   {
+    std::cout << "heeeeeeeeeeeeeeeeeeeeeeeer " << std::endl;
     // add on time (default to 1 sec)
     init_traj.conservativeResize(Eigen::NoChange_t(), init_traj.cols() + 1);
 

moveit_planners/trajopt/trajopt_interface/src/trajopt_interface.cpp

@@ -185,7 +185,6 @@ bool TrajOptInterface::solve(const planning_scene::PlanningSceneConstPtr& planni
   if (!req.goal_constraints[0].position_constraints.empty() && !req.goal_constraints[0].orientation_constraints.empty())
   {
     trajopt::CartPoseTermInfoPtr cart_goal_pos(new trajopt::CartPoseTermInfo);
-
     // TODO: Feed cart_goal_pos with request information and the needed param to the setup.yaml file
     // TODO: Multiple Cartesian constraints
 
@@ -231,24 +230,42 @@ bool TrajOptInterface::solve(const planning_scene::PlanningSceneConstPtr& planni
   setJointPoseTermInfoParams(joint_start_pos, "start_pos");
   problem_info.cnt_infos.push_back(joint_start_pos);
 
-  ROS_INFO(" ======================================= Velocity Constraints, hard-coded");
+  ROS_INFO(" ======================================= Velocity Cost, hard-coded");
   // TODO: should be defined by user, its parametes should be added to setup.yaml
   trajopt::JointVelTermInfoPtr joint_vel(new trajopt::JointVelTermInfo);
 
   joint_vel->coeffs = std::vector<double>(dof, 5.0);
   joint_vel->targets = std::vector<double>(dof, 0.0);
   joint_vel->first_step = 0;
   joint_vel->last_step = problem_info.basic_info.n_steps - 1;
-  joint_vel->name = "joint_vel";
+  joint_vel->name = "joint_velocity";
   joint_vel->term_type = trajopt::TT_COST;
   problem_info.cost_infos.push_back(joint_vel);
 
+  ROS_INFO(" ======================================= Velocity Constraint, hard-coded");
+  // TODO: should be defined by user, its parametes should be added to setup.yaml
+  trajopt::JointVelTermInfoPtr joint_vel_cnt(new trajopt::JointVelTermInfo);
+
+  // Add a velocity cnt with time to insure that robot dynamics are obeyed
+  std::vector<double> vel_lower_lim{ 0, 0, 0, 0, 0, 0, 0 };
+  std::vector<double> vel_upper_lim{ 150 *3.14/180, 150 *3.14/180, 150 *3.14/180, 150 *3.14/180, 180 *3.14/180, 180 *3.14/180, 180 *3.14/180};
+
+  joint_vel_cnt->targets = std::vector<double>(dof, 0.0);
+  joint_vel_cnt->coeffs = std::vector<double>(dof, 50.0);
+  joint_vel_cnt->lower_tols = vel_lower_lim;
+  joint_vel_cnt->upper_tols = vel_upper_lim;
+  joint_vel_cnt->term_type = (trajopt::TT_CNT | trajopt::TT_USE_TIME);
+  joint_vel_cnt->first_step = 0;
+  joint_vel_cnt->last_step = problem_info.basic_info.n_steps - 1;
+  joint_vel_cnt->name = "joint_velocity_cnt";
+  // problem_info.cnt_infos.push_back(joint_vel_cnt);
+
   ROS_INFO(" ======================================= Collision Cost, hard-coded");
   // TODO: should be defined by user, its parametes should be added to trajopt_planning.yaml
   trajopt::CollisionTermInfoPtr collision(new trajopt::CollisionTermInfo);
   collision->name = "collision";
   collision->term_type = trajopt::TT_COST;
-  collision->continuous = false;
+  collision->continuous = true;
   collision->first_step = 0;
   collision->last_step = problem_info.basic_info.n_steps - 1;
   collision->gap = 1;
@@ -367,12 +384,12 @@ bool TrajOptInterface::solve(const planning_scene::PlanningSceneConstPtr& planni
              constraint.tolerance_above);
     constraints_are_ok = constraints_are_ok and joint_cnt.configure(constraint);
     constraints_are_ok = constraints_are_ok and joint_cnt.decide(last_state).satisfied;
-    if (not constraints_are_ok)
-    {
-      ROS_ERROR_STREAM_NAMED("trajopt_planner", "Goal constraints are violated: " << constraint.joint_name);
-      res.error_code.val = moveit_msgs::MoveItErrorCodes::GOAL_CONSTRAINTS_VIOLATED;
-      return false;
-    }
+    // if (not constraints_are_ok)
+    // {
+    //   ROS_ERROR_STREAM_NAMED("trajopt_planner", "Goal constraints are violated: " << constraint.joint_name);
+    //   res.error_code.val = moveit_msgs::MoveItErrorCodes::GOAL_CONSTRAINTS_VIOLATED;
+    //   return false;
+    // }
     joint_cnt_index = joint_cnt_index + 1;
   }
 

moveit_planners/trajopt/trajopt_sco/src/optimizers.cpp

@@ -270,9 +270,15 @@ OptStatus BasicTrustRegionSQP::optimize()
 
   OptStatus retval = INVALID;
 
-  for (int merit_increases = 0; merit_increases < param_.max_merit_coeff_increases; ++merit_increases)
+
+  //Strategy: with a start merit_increase value, start iteration (iter = 1). if the trust region is expanded, it goes to the next iteration (iter = 2)
+  // if the improv is not good enough, it stays in the while loop (it does not go to the next iter) and keep shrinking the trust region. if by this
+  // decreasing the improv is not achieved, then it says it is converged becasue the trust region is tiny. Then it increase the merit_increase and repeat 
+  // the process
+  for (int merit_increases = 0; merit_increases < param_.max_merit_coeff_increases; ++merit_increases) // I think: PenaltyIteration
   { /* merit adjustment loop */
-    for (int iter = 1;; ++iter)
+    LOG_INFO("merit increases: %i", merit_increases);
+    for (int iter = 1;; ++iter) // I think: ConvexifyIteration
     { /* sqp loop */
       callCallbacks();
 
@@ -328,7 +334,7 @@ OptStatus BasicTrustRegionSQP::optimize()
       //      printer(model_cost_vals), printer(cost_vals));
       //    }
 
-      while (param_.trust_box_size >= param_.min_trust_box_size)
+      while (param_.trust_box_size >= param_.min_trust_box_size) // I think: TrustRegionIteration
       {
         setTrustBoxConstraints(results_.x);
         CvxOptStatus status = model_->optimize();