Пример #1
0
std::vector<Eigen::Matrix4f > Trajectory::createWorkspaceTrajectory( VirtualRobot::RobotNodePtr r )
{
	VR_ASSERT(rns);
	if (!r)
		r = rns->getTCP();
	VR_ASSERT(r);

	RobotPtr robot = rns->getRobot();
	VR_ASSERT(robot);

	Eigen::VectorXf jv;
	rns->getJointValues(jv);

	std::vector<Eigen::Matrix4f > result;

	for (size_t i = 0; i < path.size(); i++)
	{
		// get tcp coords:
		robot->setJointValues(rns,path[i]);
		Eigen::Matrix4f m;
		result.push_back(r->getGlobalPose());
	}
	robot->setJointValues(rns,jv);
	return result;
}
Пример #2
0
    bool CoMIK::computeSteps(float stepSize, float minumChange, int maxNStep)
    {
        std::vector<RobotNodePtr> rn = rns->getAllRobotNodes();
        RobotPtr robot = rns->getRobot();
        std::vector<float> jv(rns->getSize(), 0.0f);
        int step = 0;
        checkTolerances();
        float lastDist = FLT_MAX;

        while (step < maxNStep)
        {
            Eigen::VectorXf dTheta = this->computeStep(stepSize);

            // Check for singularities
            if (!isValid(dTheta))
            {
                VR_INFO << "Singular Jacobian" << endl;
                return false;
            }

            for (unsigned int i = 0; i < rn.size(); i++)
            {
                jv[i] = (rn[i]->getJointValue() + dTheta[i]);
            }

            //rn[i]->setJointValue(rn[i]->getJointValue() + dTheta[i]);
            robot->setJointValues(rns, jv);

            // check tolerances
            if (checkTolerances())
            {
                VR_INFO << "Tolerances ok, loop:" << step << endl;
                return true;
            }

            float d = dTheta.norm();

            if (dTheta.norm() < minumChange)
            {
                VR_INFO << "Could not improve result any more (dTheta.norm()=" << d << "), loop:" << step << endl;
                return false;
            }

            if (checkImprovement && d > lastDist)
            {
                VR_INFO << "Could not improve result any more (dTheta.norm()=" << d << ", last loop's norm:" << lastDist << "), loop:" << step << endl;
                return false;
            }

            lastDist = d;
            step++;
        }

#ifndef NO_FAILURE_OUTPUT
        //VR_INFO << "IK failed, loop:" << step << endl;
        //VR_INFO << "error:" << (target - rns->getCoM().head(target.rows())).norm() << endl;
#endif
        return false;
    }
Пример #3
0
void GenericIKSolver::setJointsRandom()
{
	std::vector<float> jv;
	float rn = 1.0f / (float)RAND_MAX;
	for (unsigned int i=0; i<rns->getSize(); i++)
	{
		RobotNodePtr ro =  rns->getNode(i);
		float r = (float)rand() * rn;
		float v = ro->getJointLimitLo() + (ro->getJointLimitHi() - ro->getJointLimitLo()) * r;
		jv.push_back(v);
	}
	RobotPtr rob = rns->getRobot();
	rob->setJointValues(rns,jv); 
	if (translationalJoint)
	{
		translationalJoint->setJointValue(initialTranslationalJointValue);
	}
}
Пример #4
0
void startRRTVisualization()
{

    // create robot
    std::string filename("robots/examples/RrtDemo/Joint3.xml");
    VirtualRobot::RuntimeEnvironment::getDataFileAbsolute(filename);
    cout << "Loading 3DOF robot from " << filename << endl;
    RobotPtr robot = RobotIO::loadRobot(filename);

    if (!robot)
    {
        return;
    }


    float sampling_extend_stepsize = 0.04f;

    // create environment
    float posX = 0.0f, posY = -400.0f, posZ = 400.0f, sizeX = 100.0f, sizeY = 300.0f, sizeZ = 1000.0f;
    ObstaclePtr o = Obstacle::createBox(sizeX, sizeY, sizeZ);

    Eigen::Affine3f tmpT(Eigen::Translation3f(posX, posY, posZ));
    o->setGlobalPose(tmpT.matrix());


    // setup collision detection
    std::string colModelName("CollisionModel");
    SceneObjectSetPtr cms = robot->getRobotNodeSet(colModelName);
    CDManagerPtr cdm(new CDManager());
    cdm->addCollisionModel(cms);
    cdm->addCollisionModel(o);

    float planningTime = 0;
    int failed = 0;
    int loops = 1;
    bool ok;
    CSpaceSampledPtr cspace;
    std::string planningJoints("AllJoints");
    RobotNodeSetPtr planningNodes = robot->getRobotNodeSet(planningJoints);
#ifdef USE_BIRRT
    BiRrtPtr rrt;
#else
    RrtPtr rrt;
#endif
    Eigen::VectorXf start(3);
    start(0) = (float)M_PI / 4.0f;
    start(1) = (float)M_PI / 4.0f * 1.5f;
    start(2) = (float) - M_PI / 4.0f;

    Eigen::VectorXf goal(3);
    goal(0) = (float) - M_PI / 4.0f;
    goal(1) = (float)M_PI / 4.0f * 1.5f;
    goal(2) = (float) - M_PI / 4.0f;

    for (int i = 0; i < loops; i++)
    {
        // setup C-Space

        cspace.reset(new CSpaceSampled(robot, cdm, planningNodes));
        cspace->setSamplingSize(sampling_extend_stepsize);
        cspace->setSamplingSizeDCD(sampling_extend_stepsize);
        // setup planner


#ifdef USE_BIRRT
        rrt.reset(new BiRrt(cspace));
#else
        rrt.reset(new Rrt(cspace));
#endif
        rrt->setStart(start);
        rrt->setGoal(goal);

        clock_t startT = clock();
        ok = rrt->plan(true);
        clock_t endT = clock();

        float diffClock = (float)(((float)(endT - startT) / (float)CLOCKS_PER_SEC) * 1000.0f);
        planningTime += diffClock;

        if (!ok)
        {
            failed++;
        }
    }

    planningTime /= (float)loops;
    cout << "Avg planning time: " << planningTime << endl;
    cout << "failed:" << failed << endl;

    if (!ok)
    {
        cout << "planning failed..." << endl;
        return;
    }

    CSpacePathPtr solution = rrt->getSolution();
    CSpaceTreePtr tree = rrt->getTree();



    robot->setJointValues(planningNodes, start);

    // display robot
    SoSeparator* sep = new SoSeparator();
    SceneObject::VisualizationType colModel = SceneObject::Full;

    boost::shared_ptr<CoinVisualization> visualization = robot->getVisualization<CoinVisualization>(colModel);
    SoNode* visualisationNode = NULL;

    if (visualization)
    {
        visualisationNode = visualization->getCoinVisualization();
    }

    sep->addChild(visualisationNode);

    // display obstacle
    VisualizationNodePtr visuObstacle = o->getVisualization();
    std::vector<VisualizationNodePtr> visus;
    visus.push_back(visuObstacle);
    boost::shared_ptr<CoinVisualization> visualizationO(new CoinVisualization(visus));
    SoNode* obstacleSoNode = visualizationO->getCoinVisualization();
    sep->addChild(obstacleSoNode);

    // show rrt visu
    boost::shared_ptr<CoinRrtWorkspaceVisualization> w(new CoinRrtWorkspaceVisualization(robot, cspace, "EndPoint"));
    w->addTree(tree);
#ifdef USE_BIRRT
    CSpaceTreePtr tree2 = rrt->getTree2();
    w->addTree(tree2);
#endif
    w->addCSpacePath(solution);
    w->addConfiguration(start, CoinRrtWorkspaceVisualization::eGreen, 3.0f);
    w->addConfiguration(goal, CoinRrtWorkspaceVisualization::eGreen, 3.0f);
    SoSeparator* sol = w->getCoinVisualization();
    sep->addChild(sol);

    show(sep);
}
TEST_F(FeatureConstraintsTest, constrainConfiguration)
{
  // setting up environment and robot
  OpenRAVE::RaveInitialize();
  OpenRAVE::EnvironmentBasePtr env = OpenRAVE::RaveCreateEnvironment();
  OpenRAVE::RobotBasePtr herb = env->ReadRobotXMLFile("robots/herb2_padded_nosensors.robot.xml");
  env->Add(herb);
 
  OpenRAVE::RobotBase::ManipulatorPtr right_arm;
  std::vector<OpenRAVE::RobotBase::ManipulatorPtr> herb_manipulators = herb->GetManipulators();
  for (unsigned int i=0; i<herb_manipulators.size(); i++)
  {
    if(herb_manipulators[i] && (herb_manipulators[i]->GetName().compare("right_wam") == 0))
    {
      right_arm = herb_manipulators[i];
    }
  }
 
  herb->SetActiveManipulator(right_arm);
  herb->SetActiveDOFs(right_arm->GetArmIndices());

  // putting the bottle in the robot's right hand
  OpenRAVE::KinBodyPtr bottle_body = OpenRAVE::RaveCreateKinBody(env, "");
  ASSERT_TRUE(env->ReadKinBodyURI(bottle_body, "objects/household/fuze_bottle.kinbody.xml"));
  bottle_body->SetName("bottle");
  env->Add(bottle_body);

  OpenRAVE::geometry::RaveTransformMatrix<OpenRAVE::dReal> m;
  m.rotfrommat(-0.86354026,  0.50427591,  0.00200643, 
               -0.25814712, -0.44547139,  0.85727201,
                0.43319543,  0.73977094,  0.51485985);
  m.trans.x = 1.10322189;
  m.trans.y = -0.24693695;
  m.trans.z = 0.87205762;
  bottle_body->SetTransform(OpenRAVE::Transform(m));

  OpenRAVE::Transform bottle_transform1 = bottle_body->GetTransform();

  ASSERT_TRUE(herb->Grab(bottle_body));

  // adding a flying pan
  OpenRAVE::KinBodyPtr pan_body =  OpenRAVE::RaveCreateKinBody(env, "");
  ASSERT_TRUE(env->ReadKinBodyURI(pan_body, "objects/household/pan.kinbody.xml"));
  pan_body->SetName("pan");
  env->Add(pan_body);

  m.rotfrommat(2.89632833e-03,   9.99995806e-01,   1.19208782e-07,
               -9.99995806e-01,   2.89632833e-03,  -1.18864027e-07,
               -1.19208797e-07,  -1.18864013e-07,   1.00000000e+00);
  m.trans.x = 0.58;
  m.trans.y = -0.02;
  m.trans.z = 0.33;
  pan_body->SetTransform(OpenRAVE::Transform(m));

  // giving constraints access to environment and robot
  RobotPtr robot = RobotPtr(new Robot(herb));
  EnvironmentPtr environment = EnvironmentPtr(new Environment(env));
  constraints.setRobot(robot);
  constraints.setEnvironment(environment);

  // first test
  constraints.calculateConstraintValues();
  EXPECT_FALSE(constraints.areConstraintsFulfilled()); 

  // moving robot and bottle to new start configuration
  std::vector<double> new_config;
  new_config.push_back(2.0);
  new_config.push_back(1.0);
  new_config.push_back(-0.8);
  new_config.push_back(1.0);
  new_config.push_back(-1.0);
  new_config.push_back(0.0);
  new_config.push_back(0.0);

  robot->setJointValues(new_config);
  constraints.setJointValues(robot->getJointValues());
  constraints.calculateConstraintValues();

  // test we've really moved
  OpenRAVE::Transform bottle_transform2 = bottle_body->GetTransform();
  ASSERT_GT((bottle_transform1.trans - bottle_transform2.trans).lengthsqr3(), 0.1);
  ASSERT_GT((bottle_transform1.rot - bottle_transform2.rot).lengthsqr4(), 0.1);

  // actual projection test
  ASSERT_EQ(robot->getDOF(), constraints.getJointValues().size());

  std::cout << "\nprior to constraining:\n";
  std::vector<double> tmp = constraints.getTaskValues();
  for(unsigned int i=0; i<tmp.size(); i++)
  {
    std::cout << tmp[i] << " ";
  } 

  constraints.constrainCurrentConfiguration();
  std::cout << "\npost to constraining:\n";
  tmp = constraints.getTaskValues();
  for(unsigned int i=0; i<tmp.size(); i++)
  {
    std::cout << tmp[i] << " ";
  } 
  std::cout << "\n";

  EXPECT_TRUE(constraints.areConstraintsFulfilled());

  OpenRAVE::RaveDestroy();
}
Пример #6
0
bool SceneIO::processSceneRobot(rapidxml::xml_node<char>* sceneXMLNode, ScenePtr scene, const std::string& basePath )
{
	THROW_VR_EXCEPTION_IF(!sceneXMLNode, "NULL data in processSceneRobot");

	// get name
	std::string robotName = processNameAttribute(sceneXMLNode,true);
	if (robotName.empty())
	{
		THROW_VR_EXCEPTION("Please specify the name of the robot...");
		return false;
	}
	std::string initStr("initconfig");
	std::string initConfigName = processStringAttribute(initStr,sceneXMLNode,true);

	std::vector< RobotConfigPtr > configs;
	std::vector< std::vector< RobotConfig::Configuration > > configDefinitions;
	std::vector< std::string > configNames;
	Eigen::Matrix4f globalPose = Eigen::Matrix4f::Identity();
	std::string fileName;
	rapidxml::xml_node<>* node = sceneXMLNode->first_node();

	std::vector< rapidxml::xml_node<>* > rnsNodes;
	while (node)
	{
		std::string nodeName = getLowerCase(node->name());
		if (nodeName == "file")
		{
			THROW_VR_EXCEPTION_IF(!fileName.empty(), "Multiple files defined in scene's robot tag '" << robotName << "'." << endl);
			fileName = processFileNode(node,basePath);
		} else if (nodeName == "configuration")
		{
			bool c*K = processConfigurationNodeList(node, configDefinitions, configNames);
			THROW_VR_EXCEPTION_IF(!c*K, "Invalid configuration defined in scene's robot tag '" << robotName << "'." << endl);
		} else if (nodeName == "globalpose")
		{
			processTransformNode(node, robotName, globalPose);
		} else if (nodeName == "robotnodeset")
		{
			rnsNodes.push_back(node);
		} else
		{
			THROW_VR_EXCEPTION("XML definition <" << nodeName << "> not supported in scene's Robot definition <" << robotName << ">." << endl);
		}

		node = node->next_sibling();
	}

	// create & register robot
	THROW_VR_EXCEPTION_IF(fileName.empty(), "Missing file definition in scene's robot tag '" << robotName << "'." << endl);
	RobotPtr robot = RobotIO::loadRobot(fileName);
	THROW_VR_EXCEPTION_IF(!robot, "Invalid robot file in scene's robot tag '" << robotName << "'." << endl);
	robot->setGlobalPose(globalPose);
	scene->registerRobot(robot);

	// create & register node sets
	int rnsNr = 0;
	for (size_t i=0;i<rnsNodes.size();i++)
	{
		// registers rns to robot
		RobotNodeSetPtr r = processRobotNodeSet(rnsNodes[i], robot, robot->getRootNode()->getName(), rnsNr);
		THROW_VR_EXCEPTION_IF(!r, "Invalid RobotNodeSet definition " << endl);
	}

	// create & register configs
	THROW_VR_EXCEPTION_IF(configDefinitions.size()!=configNames.size(), "Invalid RobotConfig definitions " << endl);
	for (size_t i=0;i<configDefinitions.size();i++)
	{
		RobotConfigPtr rc(new RobotConfig(robot,configNames[i],configDefinitions[i]));
		scene->registerRobotConfig(robot,rc);
	}

	// process init config
	if (!initConfigName.empty())
	{
		THROW_VR_EXCEPTION_IF(!scene->hasRobotConfig(robot,initConfigName), "Scene's robot tag '" << robotName << "' does not have the initConfig '" << initConfigName << "'." << endl);
		robot->setJointValues(scene->getRobotConfig(robot,initConfigName));
	}
	return true;
}