void Read(RobotBasePtr& probot, const std::vector<char>& data,const AttributesList& atts) { _ProcessAtts(atts); if( !probot ) { probot = RaveCreateRobot(_penv,_bodytype); } probot->SetName(_bodyname); Assimp::XFileParserOpenRAVE parser(data); _Read(probot,parser.GetImportedData()); if( probot->GetName().size() == 0 ) { probot->SetName("robot"); } // add manipulators FOREACH(itmanip,_listendeffectors) { RobotBase::ManipulatorInfo manipinfo; manipinfo._name = itmanip->first->_info._name; manipinfo._sEffectorLinkName = itmanip->first->GetName(); manipinfo._sBaseLinkName = probot->GetLinks().at(0)->GetName(); manipinfo._tLocalTool = itmanip->second; manipinfo._vdirection=Vector(1,0,0); probot->_vecManipulators.push_back(RobotBase::ManipulatorPtr(new RobotBase::Manipulator(probot,manipinfo))); }
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); } }