Exemplos de CollisionCheckerPtr::ContinuousCheckTrajectory em C++ (Cpp)

Exemplo n.º 1

Arquivo: cast-cost-unit.cpp Projeto: animesh-garg/trajopt

TEST(cast, boxes) {
  EnvironmentBasePtr env = RaveCreateEnvironment();
  ASSERT_TRUE(env->Load(data_dir() + "/box.xml"));
  ASSERT_TRUE(env->Load(data_dir() + "/boxbot.xml"));
  KinBodyPtr box = env->GetKinBody("box");
  RobotBasePtr boxbot = env->GetRobot("boxbot");
  RobotAndDOFPtr rad(new RobotAndDOF(boxbot, IntVec(), DOF_X | DOF_Y, Vector()));
  rad->GetRobot()->SetActiveDOFs(rad->GetJointIndices(), DOF_X | DOF_Y, Vector());
  Json::Value root = readJsonFile(string(DATA_DIR) + "/box_cast_test.json");
  DblVec start_dofs; start_dofs += -1.9, 0;
  rad->SetDOFValues(start_dofs);
  TrajOptProbPtr prob = ConstructProblem(root, env);
  TrajArray traj = prob->GetInitTraj();


  //shouldn't be necessary:
#if 0
  ASSERT_TRUE(!!prob);
  double dist_pen = .02, coeff = 10;
  prob->addCost(CostPtr(new CollisionCost(dist_pen, coeff, rad, prob->GetVarRow(0), prob->GetVarRow(1))));
  prob->addCost(CostPtr(new CollisionCost(dist_pen, coeff, rad, prob->GetVarRow(1), prob->GetVarRow(2))));
  CollisionCheckerPtr checker = CollisionChecker::GetOrCreate(*prob->GetEnv());
  {
    vector<Collision> collisions;
    checker->ContinuousCheckTrajectory(traj, *rad, collisions);
  }
  vector<Collision> collisions;
  cout << "traj: " << endl << traj << endl;
  checker->CastVsAll(*rad, rad->GetRobot()->GetLinks(), toDblVec(traj.row(0)), toDblVec(traj.row(1)), collisions);
  cout << collisions.size() << endl;
#endif

  BasicTrustRegionSQP opt(prob);
  if (plotting) opt.addCallback(PlotCallback(*prob));
  opt.initialize(trajToDblVec(prob->GetInitTraj()));
  opt.optimize();
  if (plotting) PlotCallback(*prob)(NULL, opt.x());




}

Exemplo n.º 2

TEST(continuous_collisions, boxes) {
	EnvironmentBasePtr env = RaveCreateEnvironment();
	ASSERT_TRUE(env->Load(data_dir() + "/box.xml"));
	ASSERT_TRUE(env->Load(data_dir() + "/boxbot.xml"));
	KinBodyPtr box = env->GetKinBody("box");
	RobotBasePtr boxbot = env->GetRobot("boxbot");

	CollisionCheckerPtr checker = CreateCollisionChecker(env);
	{
		RobotAndDOFPtr rad(new RobotAndDOF(boxbot, IntVec(), DOF_X | DOF_Y, Vector()));
		TrajArray traj(2,2);
		traj << -1.9,0,  0,1.9;
		vector<Collision> collisions;
		checker->ContinuousCheckTrajectory(traj, rad, collisions);
		ASSERT_EQ(collisions.size(), 1);
		Collision col = collisions[0];
		Vector robot_normal = (float)(col.linkA == boxbot->GetLinks()[0].get() ? 1. : -1.) * col.normalB2A;
		EXPECT_VECTOR_NEAR(robot_normal, Vector(-1, 0, 0), 1e-4);
	}

#define TRAJ_TEST_BOILERPLATE\
		RobotAndDOFPtr rad(new RobotAndDOF(boxbot, IntVec(), DOF_X | DOF_Y, Vector()));\
		vector<Collision> collisions;\
		checker->CastVsAll(*rad, rad->GetRobot()->GetLinks(), toDblVec(traj.row(0)), toDblVec(traj.row(1)), collisions);\
		ASSERT_EQ(collisions.size(), 1);\
		Collision col = collisions[0];\
		Vector robot_normal = (float)(col.linkA == boxbot->GetLinks()[0].get() ? 1. : -1.) * col.normalB2A;

	{
		TrajArray traj(2,2);
		traj << -1.9,0,  0,1.9;
		TRAJ_TEST_BOILERPLATE
		EXPECT_VECTOR_NEAR(robot_normal, Vector(-1/sqrtf(2), 1/sqrtf(2), 0), 1e-4);
		EXPECT_NEAR(col.time, .5, 1e-1);
	}
	{
		TrajArray traj(2,2);
		traj << 0, .9,  0,2;
		TRAJ_TEST_BOILERPLATE
		EXPECT_VECTOR_NEAR(robot_normal, Vector(0,1,0), 1e-4);
		EXPECT_NEAR(col.time, 0, 1e-6);
	}
	{
		TrajArray traj(2,2);
		traj << 0,2,  0,.9;
		TRAJ_TEST_BOILERPLATE
		EXPECT_VECTOR_NEAR(robot_normal, Vector(0,1,0), 1e-4);
		EXPECT_NEAR(col.time, 1, 1e-6);
	}
	{
		RobotAndDOFPtr rad(new RobotAndDOF(boxbot, IntVec(), DOF_X | DOF_Y | DOF_Z, Vector()));
		vector<Collision> collisions;
		vector<DblVec> multi_joints;
		multi_joints.push_back(toDblVec(Eigen::Vector3d(0,1.9,5)));
		multi_joints.push_back(toDblVec(Eigen::Vector3d(-1.9,0,5)));
		multi_joints.push_back(toDblVec(Eigen::Vector3d(0,-2,5)));
		multi_joints.push_back(toDblVec(Eigen::Vector3d(2,0,5)));
		multi_joints.push_back(toDblVec(Eigen::Vector3d(0,1.8,-5)));
		multi_joints.push_back(toDblVec(Eigen::Vector3d(-1.9,0,-5)));
		multi_joints.push_back(toDblVec(Eigen::Vector3d(0,-2,-5)));
		multi_joints.push_back(toDblVec(Eigen::Vector3d(2,0,-5)));
		checker->MultiCastVsAll(*rad, rad->GetRobot()->GetLinks(), multi_joints, collisions);
		ASSERT_EQ(collisions.size(), 1);
		Collision col = collisions[0];
		Vector robot_normal = (float)(col.linkA == boxbot->GetLinks()[0].get() ? 1. : -1.) * col.normalB2A;
		EXPECT_VECTOR_NEAR(robot_normal, Vector(1/sqrtf(2), -1/sqrtf(2), 0), 1e-4);
	}

}