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