C++ (Cpp) RobotBasePtr::GetLinks示例

 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)));
     }

示例#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);
	}

}