VirtualRobot::MathTools::ConvexHull2DPtr SupportPolygon::updateSupportPolygon(float maxFloorDist)
{
CollisionCheckerPtr colChecker = colModels[0]->getCollisionChecker();
std::vector< CollisionModelPtr >::iterator i = colModels.begin();
std::vector< MathTools::ContactPoint > points;
while (i != colModels.end())
{
colChecker->getContacts(floor, *i, points, maxFloorDist);
i++;
}
currentContactPoints2D.clear();
for (size_t u = 0; u < points.size(); u++)
{
Eigen::Vector2f pt2d = MathTools::projectPointToPlane2D(points[u].p, floor);
currentContactPoints2D.push_back(pt2d);
}
if (currentContactPoints2D.size() < 3)
{
suportPolygonFloor.reset();
}
else
{
suportPolygonFloor = MathTools::createConvexHull2D(currentContactPoints2D);
}
return suportPolygonFloor;
}
void PlotCollisionGeometry(const osgGA::GUIEventAdapter & ea) {
if (handles.size() == 0) {
cc->PlotCollisionGeometry(handles);
vector<Collision> collisions;
cc->AllVsAll(collisions);
PlotCollisions(collisions, *env, handles, .02);
}
else
handles.clear();
}
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());
}
void stabilityWindow::updateSupportVisu()
{
supportVisu->removeAllChildren();
if (UI.checkBoxSupportPolygon->isChecked())
{
/*SoMaterial *material = new SoMaterial;
material->diffuseColor.setValue(1.0f,0.2f,0.2f);
supportVisu->addChild(material);*/
MathTools::Plane p = MathTools::getFloorPlane();
//supportVisu->addChild(CoinVisualizationFactory::CreatePlaneVisualization(p.p,p.n,100000.0f,0.5f));
std::vector< CollisionModelPtr > colModels = robot->getCollisionModels();
CollisionCheckerPtr colChecker = CollisionChecker::getGlobalCollisionChecker();
std::vector< CollisionModelPtr >::iterator i = colModels.begin();
std::vector< MathTools::ContactPoint > points;
while (i != colModels.end())
{
colChecker->getContacts(p, *i, points, 5.0f);
i++;
}
std::vector< Eigen::Vector2f > points2D;
//MathTools::Plane plane2(Eigen::Vector3f(0,0,0),Eigen::Vector3f(0,1.0f,0));
for (size_t u = 0; u < points.size(); u++)
{
Eigen::Vector2f pt2d = MathTools::projectPointToPlane2D(points[u].p, p);
points2D.push_back(pt2d);
}
MathTools::ConvexHull2DPtr cv = MathTools::createConvexHull2D(points2D);
SoSeparator* visu = CoinVisualizationFactory::CreateConvexHull2DVisualization(cv, p, VisualizationFactory::Color::Blue(), VisualizationFactory::Color::Black(), 6.0f, Eigen::Vector3f(0, 0, 2.0f));
supportVisu->addChild(visu);
}
}
TEST(collision_checker, box_distance) {
EnvironmentBasePtr env = RaveCreateEnvironment();
ASSERT_TRUE(env->Load(data_dir() + "/box.xml"));
KinBodyPtr box0 = env->GetKinBody("box");
box0->SetName("box0");
ASSERT_TRUE(env->Load(DATA_DIR + string("/box.xml")));
KinBodyPtr box1 = env->GetKinBody("box");
box1->SetName("box1");
vector<KinBodyPtr> bodies; env->GetBodies(bodies);
RAVELOG_DEBUG("%i kinbodies in rave env\n", bodies.size());
CollisionCheckerPtr checker = CreateCollisionChecker(env);
#define EXPECT_NUM_COLLISIONS(n)\
vector<Collision> collisions;\
checker->AllVsAll(collisions);\
EXPECT_EQ(collisions.size(), n);\
collisions.clear();\
checker->BodyVsAll(*box0, collisions);\
EXPECT_EQ(collisions.size(), n);
{
checker->SetContactDistance(0);
box1->SetTransform(OpenRAVE::Transform(Vector(1,0,0,0), Vector(.9,0,0)));
box0->SetTransform(OpenRAVE::Transform(Vector(1,0,0,0), Vector(0,0,0)));
EXPECT_NUM_COLLISIONS(1);
}
{
checker->SetContactDistance(0);
box1->SetTransform(OpenRAVE::Transform(Vector(1,0,0,0), Vector(1.1,0,0)));
box0->SetTransform(OpenRAVE::Transform(Vector(1,0,0,0), Vector(0,0,0)));
EXPECT_NUM_COLLISIONS(0);
}
{
checker->SetContactDistance(.04);
box1->SetTransform(OpenRAVE::Transform(Vector(1,0,0,0), Vector(1.1,0,0)));
box0->SetTransform(OpenRAVE::Transform(Vector(1,0,0,0), Vector(0,0,0)));
EXPECT_NUM_COLLISIONS(0);
}
{
checker->SetContactDistance(.1);
box1->SetTransform(OpenRAVE::Transform(Vector(1,0,0,0), Vector(1.09,0,0)));
box0->SetTransform(OpenRAVE::Transform(Vector(1,0,0,0), Vector(0,0,0)));
EXPECT_NUM_COLLISIONS(1);
}
{
checker->SetContactDistance(.2);
box1->SetTransform(OpenRAVE::Transform(Vector(1,0,0,0), Vector(1.1,0,0)));
box0->SetTransform(OpenRAVE::Transform(Vector(1,0,0,0), Vector(0,0,0)));
EXPECT_NUM_COLLISIONS(1);
}
{
env->Remove(box1);
EXPECT_NUM_COLLISIONS(0);
}
}
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);
}
}