bool GetIKSolutions(EnvironmentBasePtr _penv, Transform Pose, std::vector< std::vector< dReal > > &vsolution){
ModuleBasePtr _pikfast = RaveCreateModule(_penv,"ikfast");
RobotBasePtr _probot = _penv->GetRobot(robotname);
_probot->SetActiveManipulator("1");
RobotBase::ManipulatorPtr _pmanip = _probot->GetActiveManipulator();
_penv->Add(_pikfast,true,"");
std::vector< std::vector< dReal > > solns;
solns.resize(6);
stringstream ssin,ssout;
string iktype = "Transform6D";
ssin << "LoadIKFastSolver " << _probot->GetName() << " " << iktype;
if( !_pikfast->SendCommand(ssout,ssin) ) {
RAVELOG_ERROR("failed to load iksolver\n");
_penv->Destroy();
return false;
}
if(_pmanip->FindIKSolutions(IkParameterization(Pose),vsolution,IKFO_CheckEnvCollisions) ) {
stringstream ss; ss << "solution is: ";
for(size_t i = 0; i < vsolution.size(); ++i) {
for(size_t j = 0; j < vsolution[i].size(); ++j){
ss << vsolution[i][j] << " " ;
} ss << endl;
}
ss << endl;
////RAVELOG_INFO(ss.str());
}
else {
// could fail due to collisions, etc
return false;
}
return true ;
}
void GetTaskTime(EnvironmentBasePtr _penv, std::vector<dReal> &vsolutionA, std::vector<dReal> &vsolutionB, TrajectoryBasePtr ptraj){
//thread::id get_id();
std::cout << "this thread ::" << boost::this_thread::get_id() << std::endl;
PlannerBasePtr planner = RaveCreatePlanner(_penv,"birrt");
//std::vector<dReal> vinitialconfig,vgoalconfig;
ptraj = RaveCreateTrajectory(_penv,"");
PlannerBase::PlannerParametersPtr params(new PlannerBase::PlannerParameters());
RobotBasePtr probot = _penv->GetRobot(robotname);
params->_nMaxIterations = 4000; // max iterations before failure
GraphHandlePtr pgraph;
{
EnvironmentMutex::scoped_lock lock(_penv->GetMutex()); // lock environment
params->SetRobotActiveJoints(probot); // set planning configuration space to current active dofs
//initial config
probot->SetActiveDOFValues(vsolutionA);
params->vinitialconfig.resize(probot->GetActiveDOF());
probot->GetActiveDOFValues(params->vinitialconfig);
//goal config
probot->SetActiveDOFValues(vsolutionB);
params->vgoalconfig.resize(probot->GetActiveDOF());
probot->GetActiveDOFValues(params->vgoalconfig);
//setting limits
vector<dReal> vlower,vupper;
probot->GetActiveDOFLimits(vlower,vupper);
//RAVELOG_INFO("starting to plan\n");
if( !planner->InitPlan(probot,params) ) {
//return ptraj;
}
// create a new output trajectory
if( !planner->PlanPath(ptraj) ) {
RAVELOG_WARN("plan failed \n");
//return NULL;
}
}
////RAVELOG_INFO(ss.str());
if (!! ptraj){
//std::cout << ptraj->GetDuration() << std::endl;
writeTrajectory(ptraj);
}
std::cout << "this thread ::" << boost::this_thread::get_id() << " has completed its work" << std::endl;
mtx__.lock();
completed_thread = boost::this_thread::get_id();
mtx__.unlock();
//return ptraj;
}
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());
}
int main()
{
traj.clear();
unsigned int mainthreadsleft = numThreads;
// boost::shared_ptr<boost::thread> ( new boost::thread(boost::bind( &track_threads )));
Ta.trans = transA;
Ta.rot = quatA;
Tb.trans = transB;
Tb.rot = quatB;
std::string scenefilename = "scenes/test6dof.mujin.zae";
std::string viewername = "qtcoin";
RaveInitialize(true); // start openrave core
EnvironmentBasePtr penv = RaveCreateEnvironment(); // create the main environment
Transform robot_t;
RaveVector< dReal > transR(c, d, 0);
robot_t.trans = transR;
//boost::thread thviewer(boost::bind(SetViewer,penv,viewername));
penv->Load(scenefilename);
RobotBasePtr probot = penv->GetRobot("RV-4F");
//removing floor for collision checking
EnvironmentBasePtr pclondedenv = penv->CloneSelf(Clone_Bodies);
pclondedenv->Remove( pclondedenv->GetKinBody("floor"));
RobotBasePtr probot_clone = pclondedenv->GetRobot("RV-4F");
unsigned int tot = ((( abs(a) + abs(c) )/discretization_x )+1) * (((( abs(b) + abs(d) )/discretization_y )+1) * (( abs( z )/discretization_z )+1));
unsigned int tot_o = tot;
for (unsigned int i = 0 ; i <= (( abs(a) + abs(c) )/discretization_x );i++) {
for (unsigned int j = 0 ; j <= (( abs(b) + abs(d) )/discretization_y ); j++) {
for (unsigned int k = 0 ; k <= ( abs( z )/discretization_z ) ; k++) {
////std::cout << transR[0] << ";" << transR[1] << ";" << transR[2] << std::endl;
//boost::this_thread::sleep(boost::posix_time::milliseconds(1000));
robot_t.trans = transR;
probot->SetTransform(robot_t);
probot_clone->SetTransform(robot_t);
if( pclondedenv->CheckCollision(RobotBaseConstPtr(probot_clone)) ){
//std::cout << "Robot in collision with the environment" << std::endl;
}
else {
do_task(Ta, Tb, penv,3);
}
tot -= 1;
std::cout << tot << "/" << tot_o << std::endl;
transR[2] = transR[2]+ discretization_z;
}
transR[2] = 0;
transR[1] = transR[1] + discretization_y;
robot_t.trans = transR;
}
transR[2] = 0;
transR[1] = c;
transR[0] = transR[0] + discretization_x;
robot_t.trans = transR;
}
//thviewer.join(); // wait for the viewer thread to exit
RaveDestroy(); // make sure to destroy the OpenRAVE runtime
penv->Destroy(); // destroy
return 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);
}
}
