Segfault when constructing problem with collision

Hello,

I am able to compute trajectories without collision checking with my robot model. However, when I add collision cost to json file it is throwing seg fault. I'm on Ubuntu 16.04 with Openrave 0.9 (compiled from master).

This is the json file:

{
  "basic_info" : {
    "n_steps" : 9,
    "manip" : "whole_body",
    "start_fixed" : true
  },
  "costs" : [
  {
    "name" : "disc_coll",
    "type" : "collision",
    "params" :
    {
      "coeffs" : [ 50 ],
      "continuous" : false,
      "dist_pen" : [ 0.040 ],
      "first_step" : 0,
      "last_step" : 7
    }
    }
  ],
  "constraints" : [
  {
    "type" : "pose",
    "params" : {
      "xyz" : [2.0, 0.5, 0.8],
      "wxyz": [0.966, 0.0, 0.0, 0.259],
      "link": "hand_palm_link",
      "timestep" : 7
                }

  }
  ],

  "init_info" : {
      "type" : "stationary"
  }
}

And this is my function

void FRASIEROpenRAVE::computeTrajectory(){

  OpenRAVE::EnvironmentMutex::scoped_lock lockenv(env_->GetMutex());
  std::string json_path;
  json_path = config_path_ + "whole_body_traj_collision.json";

  Json::Value whole_body_traj_json;
  Json::Reader reader;
  std::ifstream i(json_path);

  bool json_parsing_success = reader.parse(i, whole_body_traj_json, true);

  if (!json_parsing_success) {
    std::cout << "can't read json!"  << std::endl;
    std::cout << reader.getFormatedErrorMessages() << std::endl;
  }
  std::vector<OpenRAVE::KinBodyPtr> bodies;

  env_->GetBodies(bodies);
   for(auto i:bodies){
     std::cout<<i->GetName()<<std::endl;
   }

   std::vector<double> q, q0;
   q0 = {0, 0, 0, 0, 0, 0, 0, 0};
   std::vector<int> q_idx;
   //manip_->GetArmIndices(q_idx);

   manip_ = hsr_->GetManipulator("whole_body");
   hsr_->SetActiveDOFs(manip_->GetArmIndices());

   hsr_->SetDOFValues(q0,1,manip_->GetArmIndices());
   hsr_->GetActiveDOFValues(q);
   for (auto k:q) {
     std::cout << k << '\n';
   }

   ros::Duration(1).sleep();
   // CollisionCheckerPtr colll = CreateCollisionChecker(env_);
   trajopt::ProblemConstructionInfo pci(env_);
   pci.fromJson(whole_body_traj_json);
   trajopt::TrajOptProbPtr traj_prob = trajopt::ConstructProblem(pci);

  trajopt::TrajOptResultPtr traj_result = trajopt::OptimizeProblem(traj_prob, true);
  std::cout << "traj : " << std::endl << traj_result->traj << std::endl;

}

And finally this is gdb output:

Thread 1 "frasier_openrav" received signal SIGSEGV, Segmentation fault.
0x00007fffed1e70e4 in convexHullSupport(btVector3 const&, btVector3 const*, int, btVector3 const&) [clone .constprop.751] () from /home/tarik/develop/robotics/trajopt/build/lib/libtrajopt.so

It is working without any problems if I don't add collision cost.

joschu / trajopt

Segfault when constructing problem with collision #23