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;
}
int ORCEnvironmentGetBodies(void* env, void** bodies)
{
EnvironmentBasePtr penv = GetEnvironment(env);
std::vector<KinBodyPtr> vbodies;
penv->GetBodies(vbodies);
if( !bodies ) {
return static_cast<int>(vbodies.size());
}
for(size_t i = 0; i < vbodies.size(); ++i) {
bodies[i] = new InterfaceBasePtr(vbodies[i]);
}
return vbodies.size();
}
int ORCEnvironmentGetRobots(void* env, void** robots)
{
EnvironmentBasePtr penv = GetEnvironment(env);
std::vector<RobotBasePtr> vrobots;
penv->GetRobots(vrobots);
if( !robots ) {
return static_cast<int>(vrobots.size());
}
for(size_t i = 0; i < vrobots.size(); ++i) {
robots[i] = new InterfaceBasePtr(vrobots[i]);
}
return vrobots.size();
}
static void SetUpTestCase() {
RAVELOG_DEBUG("SetupTestCase\n");
RaveInitialize(false, verbose ? Level_Debug : Level_Info);
env = RaveCreateEnvironment();
env->StopSimulation();
env->Load("robots/pr2-beta-static.zae") ;
env->Load(string(DATA_DIR) + "/table.xml");
if (plotting) {
viewer.reset(new OSGViewer(env));
viewer->UpdateSceneData();
env->AddViewer(viewer);
}
}
static void TearDownTestCase() {
RAVELOG_DEBUG("TearDownTestCase\n");
viewer.reset();
env.reset();
RaveDestroy();
}
int main(int argc, char ** argv)
{
//int num = 1;
string scenefilename = "data/lab1.env.xml";
string viewername = RaveGetDefaultViewerType(); //qtcoin
// parse the command line options
int i = 1;
while(i < argc) {
if((strcmp(argv[i], "-h") == 0)||(strcmp(argv[i], "-?") == 0)||(strcmp(argv[i], "/?") == 0)||(strcmp(argv[i], "--help") == 0)||(strcmp(argv[i], "-help") == 0)) {
RAVELOG_INFO("orloadviewer [--num n] [--scene filename] viewername\n");
return 0;
}
else if( strcmp(argv[i], "--scene") == 0 ) {
scenefilename = argv[i+1];
i += 2;
}
else
break;
}
if( i < argc ) {
viewername = argv[i++];
}
RaveInitialize(true); // start openrave core
EnvironmentBasePtr penv = RaveCreateEnvironment(); // create the main environment
RaveSetDebugLevel(Level_Debug);
boost::thread thviewer(boost::bind(SetViewer,penv,viewername));
penv->Load(scenefilename); // load the scene
UserDataPtr pregistration;
while(!pregistration) {
if( !pregistration && !!penv->GetViewer() ) {
pregistration = penv->GetViewer()->RegisterViewerThreadCallback(boost::bind(ViewerCallback,penv->GetViewer()));
}
boost::this_thread::sleep(boost::posix_time::milliseconds(1));
}
thviewer.join(); // wait for the viewer thread to exit
pregistration.reset();
penv->Destroy(); // destroy
return 0;
}
bool TaskSpaceRegion::Initialize(EnvironmentBasePtr penv_in)
{
_volume = 0;
_sumbounds = 0;
_dimensionality = 0;
for(int i = 0; i < 6; i++)
{
//compute volume in whatever dimensionality this defines
//when Bw values are backwards, it signifies an axis flip (not an error)
if(Bw[i][1] != Bw[i][0])
{
_volume = _volume * fabs(Bw[i][1] - Bw[i][0]);
_sumbounds = _sumbounds + fabs(Bw[i][1] - Bw[i][0]);
_dimensionality++;
}
}
if( stricmp(relativebodyname.c_str(), "NULL") == 0 )
{
prelativetolink.reset();
}
else
{
KinBodyPtr pobject;
pobject = penv_in->GetKinBody(relativebodyname.c_str());
if(pobject.get() == NULL)
{
RAVELOG_INFO("Error: could not find the specified object to attach frame\n");
return false;
}
//find the link
vector<KinBody::LinkPtr> vlinks = pobject->GetLinks();
bool bGotLink = false;
for(int j =0; j < vlinks.size(); j++)
{
if(strcmp(relativelinkname.c_str(), vlinks[j]->GetName().c_str()) == 0 )
{
RAVELOG_INFO("frame link: %s:%s\n",vlinks[j]->GetParent()->GetName().c_str(),vlinks[j]->GetName().c_str() );
prelativetolink = vlinks[j];
bGotLink = true;
break;
}
}
if(!bGotLink)
{
RAVELOG_INFO("Error: could not find the specified link of the object to attach frame\n");
return false;
}
}
//Print();
return true;
}
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());
}
void SetViewer(EnvironmentBasePtr penv, const string& viewername)
{
ViewerBasePtr viewer = RaveCreateViewer(penv,viewername);
BOOST_ASSERT(!!viewer);
// attach it to the environment:
penv->AddViewer(viewer);
// finally call the viewer's infinite loop (this is why a separate thread is needed)
bool showgui = true;
viewer->main(showgui);
}
int main() {
RaveInitialize(false, OpenRAVE::Level_Debug);
env = RaveCreateEnvironment();
env->StopSimulation();
// bool success = env->Load("data/pr2test2.env.xml");
{
bool success = env->Load("/home/joschu/Proj/drc/gfe.xml");
FAIL_IF_FALSE(success);
}
{
bool success = env->Load("/home/joschu/Proj/trajopt/data/test2.env.xml");
FAIL_IF_FALSE(success);
}
vector<RobotBasePtr> robots;
env->GetRobots(robots);
RobotBasePtr robot = robots[0];
vector<RobotBase::ManipulatorPtr> manips = robot->GetManipulators();
cc = CollisionChecker::GetOrCreate(*env);
viewer.reset(new OSGViewer(env));
env->AddViewer(viewer);
ManipulatorControl mc(manips[manips.size()-1], viewer);
DriveControl dc(robot, viewer);
StatePrinter sp(robot);
viewer->AddKeyCallback('a', boost::bind(&StatePrinter::PrintAll, &sp));
viewer->AddKeyCallback('q', &PlotCollisionGeometry);
viewer->AddKeyCallback('=', boost::bind(&AdjustTransparency, .05));
viewer->AddKeyCallback('-', boost::bind(&AdjustTransparency, -.05));
viewer->Idle();
env.reset();
viewer.reset();
RaveDestroy();
}
int main(int argc, char ** argv)
{
//int interval = 1000000000; // 1hz (1.0 sec)
//int interval = 500000000; // 2hz (0.5 sec)
//int interval = 10000000; // 100 hz (0.01 sec)
int interval = 40000000; // 25 hz (0.04 sec)
// At this point an explanation is necessary for the following arrays
//
// jointNames is the order makeTraj python code saves the joint values in plain text format
string jointNames[] = {"RHY", "RHR", "RHP", "RKP", "RAP", "RAR", "LHY", "LHR", "LHP", "LKP", "LAP", "LAR", "RSP", "RSR", "RSY", "REP", "RWY", "RWR", "RWP", "LSP", "LSR", "LSY", "LEP", "LWY", "LWR", "LWP", "NKY", "NK1", "NK2", "HPY", "rightThumbKnuckle1", "rightIndexKnuckle1", "rightMiddleKnuckle1", "rightRingKnuckle1", "rightPinkyKnuckle1", "leftThumbKnuckle1", "leftIndexKnuckle1", "leftMiddleKnuckle1", "leftRingKnuckle1", "leftPinkyKnuckle1"};
// openRAVE Joint Indices keeps the mapping between the order given above and hubo's joint index in openRAVE. (E.g. RHY's index in openRAVE is 1, RHR's is 3 and so on). A -1 means that the joint name does not exist in openRAVE but it exists on the real robot. (E.g. RWR)
int openRAVEJointIndices[] = {1, 3, 5, 7, 9, 11, 2, 4, 6, 8, 10, 12, 13, 15, 17, 19, 21, -1, 23, 14, 16, 18, 20, 22, -1, 24, -1, -1, -1, 0, 41, 29, 32, 35, 38, 56, 44, 47, 50, 53};
// hubo_ref Joint Indices keeps the mapping between openRAVE joint Indices and the order hubo-read-trajectory writes in ach channel. (E.g. RHY is the 26th element of H.ref[] array).
int hubo_refJointIndices[] = {26, 27, 28, 29, 30, 31, 19, 20, 21, 22, 23, 24, 11, 12, 13, 14, 15, 16, 17, 4, 5, 6, 7, 8, 9, 10, 1, 2, 3, 0, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41};
/* Joint Numbers/Index values */
string viewername = "qtcoin";
string scenefilename = "../../../openHubo/huboplus/";
RaveInitialize(true); // start openrave core
EnvironmentBasePtr penv = RaveCreateEnvironment(); // create the main environment
int i = 1;
bool vflag = false; // verbose mode flag
bool Vflag = false; // extra verbose mode flag
bool cflag = false; // self colision flag
bool fflag = false; // ref vs. ref-filter channel flag
/* For Viewer */
if(argc <= i ){
printf("Loading Headless\n");
}
while(argc > i) {
if(strcmp(argv[i], "-g") == 0) {
//RaveSetDebugLevel(Level_Debug);
boost::thread thviewer(boost::bind(SetViewer,penv,viewername));
}
else {
printf("Loading Headless\n");
}
if(strcmp(argv[i], "-v") == 0) {
vflag = true;
printf("Entering Verbose Mode");
}
else {
vflag = false;
}
if(strcmp(argv[i], "-V") == 0) {
Vflag = true;
printf("Entering Extra Verbose Mode");
}
if(strcmp(argv[i], "-f") == 0) {
fflag = true;
printf("listening to filtered channel.");
}
if(strcmp(argv[i], "-m") == 0) {
i++;
string hubomodel;
hubomodel = argv[i];
scenefilename.append(hubomodel);
scenefilename.append(".robot.xml");
cout<<"Loading model "<<scenefilename<<endl;
}
i++;
}
vector<dReal> vsetvalues;
// parse the command line options
// load the scene
if( !penv->Load(scenefilename) ) {
return 2;
}
// Wait until the robot model appears
usleep(1000000);
/* timing */
struct timespec t;
/* hubo ACH Channel */
struct hubo_ref H;
memset( &H, 0, sizeof(H));
int r;
if(fflag){
r = ach_open(&chan_num, HUBO_CHAN_REF_FILTER_NAME, NULL);
}
else{
r = ach_open(&chan_num, HUBO_CHAN_REF_NAME, NULL);
}
size_t fs;
/* read first set of data */
r = ach_get( &chan_num, &H, sizeof(H), &fs, NULL, ACH_O_LAST );
assert( sizeof(H) == fs );
int contactpoints = 0;
bool runflag = true;
while(runflag) {
{
// lock the environment to prevent data from changing
EnvironmentMutex::scoped_lock lock(penv->GetMutex());
//vector<KinBodyPtr> vbodies;
vector<RobotBasePtr> vbodies;
//penv->GetBodies(vbodies);
penv->GetRobots(vbodies);
// get the first body
if( vbodies.size() == 0 ) {
RAVELOG_ERROR("no bodies loaded\n");
return -3;
}
//KinBodyPtr pbody = vbodies.at(0);
RobotBasePtr pbody = vbodies.at(0);
vector<dReal> values;
pbody->GetDOFValues(values);
// set new values
for(int i = 0; i < (int)vsetvalues.size() && i < (int)values.size(); ++i) {
values[i] = vsetvalues[i];
}
pbody->Enable(true);
//pbody->SetVisible(true);
//CollisionReportPtr report(new CollisionReport());
//bool runflag = true;
//while(runflag) {
/* Wait until next shot */
clock_nanosleep(0,TIMER_ABSTIME,&t, NULL);
/* Get updated joint info here */
r = ach_get( &chan_num, &H, sizeof(H), &fs, NULL, ACH_O_LAST );
assert( sizeof(H) == fs );
/* set all joints from ach */
int len = (int)(sizeof(openRAVEJointIndices)/sizeof(openRAVEJointIndices[0]));
/* set all joints from ach */
for( int ii = 0; ii < (int)values.size() ; ii++ ) {
values[ii] = 0.0;
}
for( int ii = 0; ii < len; ii++){
if(openRAVEJointIndices[ii] != -1){
values[openRAVEJointIndices[ii]] = H.ref[hubo_refJointIndices[ii]];
}
}
// values[RSY] = -1.0;
// values[REB] = 1.0;
pbody->SetDOFValues(values,true);
// penv->GetCollisionChecker()->SetCollisionOptions(CO_Contacts);
// if( pbody->CheckSelfCollision(report) ) {
// cflag = true;
// if(vflag | Vflag){
// RAVELOG_INFO("body not in collision\n");
// }
// if(Vflag) {
// contactpoints = (int)report->contacts.size();
// stringstream ss;
// ss << "body in self-collision "
// << (!!report->plink1 ? report->plink1->GetName() : "") << ":"
// << (!!report->plink2 ? report->plink2->GetName() : "") << " at "
// << contactpoints << "contacts" << endl;
// for(int i = 0; i < contactpoints; ++i) {
// CollisionReport::CONTACT& c = report->contacts[i];
// ss << "contact" << i << ": pos=("
// << c.pos.x << ", " << c.pos.y << ", " << c.pos.z << "), norm=("
// << c.norm.x << ", " << c.norm.y << ", " << c.norm.z << ")" << endl;
// }
// RAVELOG_INFOA(ss.str());
// }
// }
// else {
// cflag = false;
// if(vflag | Vflag) {
// RAVELOG_INFO("body not in collision\n");
// }
// }
// get the transformations of all the links
vector<Transform> vlinktransforms;
pbody->GetLinkTransformations(vlinktransforms);
//boost::this_thread::sleep(boost::posix_time::milliseconds(2000));
// boost::this_thread::sleep(boost::posix_time::milliseconds(1));
t.tv_nsec+=interval;
tsnorm(&t);
// runflag = false;
//pbody->Enable(true);
//pbody->SetVisible(true);
usleep(10000);
pbody->SimulationStep(0.01);
penv->StepSimulation(0.01);
}
}
pause();
RaveDestroy(); // destroy
return contactpoints;
}
InterfaceBase::InterfaceBase(InterfaceType type, EnvironmentBasePtr penv) : __type(type), __penv(penv)
{
RaveInitializeFromState(penv->GlobalState()); // make sure global state is set
RegisterCommand("help",boost::bind(&InterfaceBase::_GetCommandHelp,this,_1,_2),
"display help commands.");
}
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);
}
}
TEST(collision_checker, box_distance) {
EnvironmentBasePtr env = RaveCreateEnvironment();
ASSERT_TRUE(env->Load(data_dir() + "/box.xml"));
KinBodyPtr box0 = env->GetKinBody("box");
box0->SetName("box0");
ASSERT_TRUE(env->Load(DATA_DIR + string("/box.xml")));
KinBodyPtr box1 = env->GetKinBody("box");
box1->SetName("box1");
vector<KinBodyPtr> bodies; env->GetBodies(bodies);
RAVELOG_DEBUG("%i kinbodies in rave env\n", bodies.size());
CollisionCheckerPtr checker = CreateCollisionChecker(env);
#define EXPECT_NUM_COLLISIONS(n)\
vector<Collision> collisions;\
checker->AllVsAll(collisions);\
EXPECT_EQ(collisions.size(), n);\
collisions.clear();\
checker->BodyVsAll(*box0, collisions);\
EXPECT_EQ(collisions.size(), n);
{
checker->SetContactDistance(0);
box1->SetTransform(OpenRAVE::Transform(Vector(1,0,0,0), Vector(.9,0,0)));
box0->SetTransform(OpenRAVE::Transform(Vector(1,0,0,0), Vector(0,0,0)));
EXPECT_NUM_COLLISIONS(1);
}
{
checker->SetContactDistance(0);
box1->SetTransform(OpenRAVE::Transform(Vector(1,0,0,0), Vector(1.1,0,0)));
box0->SetTransform(OpenRAVE::Transform(Vector(1,0,0,0), Vector(0,0,0)));
EXPECT_NUM_COLLISIONS(0);
}
{
checker->SetContactDistance(.04);
box1->SetTransform(OpenRAVE::Transform(Vector(1,0,0,0), Vector(1.1,0,0)));
box0->SetTransform(OpenRAVE::Transform(Vector(1,0,0,0), Vector(0,0,0)));
EXPECT_NUM_COLLISIONS(0);
}
{
checker->SetContactDistance(.1);
box1->SetTransform(OpenRAVE::Transform(Vector(1,0,0,0), Vector(1.09,0,0)));
box0->SetTransform(OpenRAVE::Transform(Vector(1,0,0,0), Vector(0,0,0)));
EXPECT_NUM_COLLISIONS(1);
}
{
checker->SetContactDistance(.2);
box1->SetTransform(OpenRAVE::Transform(Vector(1,0,0,0), Vector(1.1,0,0)));
box0->SetTransform(OpenRAVE::Transform(Vector(1,0,0,0), Vector(0,0,0)));
EXPECT_NUM_COLLISIONS(1);
}
{
env->Remove(box1);
EXPECT_NUM_COLLISIONS(0);
}
}
int main(int argc, char ** argv)
{
string scenefilename = "data/diffdrive_arm.env.xml";
string viewername = "qtcoin";
RaveInitialize(true);
EnvironmentBasePtr penv = RaveCreateEnvironment();
penv->SetDebugLevel(Level_Debug);
boost::thread thviewer(boost::bind(SetViewer,penv,viewername)); // create the viewer
usleep(400000); // wait for the viewer to init
penv->Load(scenefilename);
// attach a physics engine
penv->SetPhysicsEngine(RaveCreatePhysicsEngine(penv,"ode"));
penv->GetPhysicsEngine()->SetGravity(Vector(0,0,-9.8));
vector<RobotBasePtr> vrobots;
penv->GetRobots(vrobots);
RobotBasePtr probot = vrobots.at(0);
std::vector<dReal> q;
vector<int> wheelindices, restindices;
ControllerBasePtr wheelcontroller, armcontroller;
// create the controllers, make sure to lock environment!
{
EnvironmentMutex::scoped_lock lock(penv->GetMutex()); // lock environment
MultiControllerPtr multi(new MultiController(penv));
vector<int> dofindices(probot->GetDOF());
for(int i = 0; i < probot->GetDOF(); ++i) {
dofindices[i] = i;
}
probot->SetController(multi,dofindices,1); // control everything
// set the velocity controller on all joints that have 'wheel' in their description
for(std::vector<KinBody::JointPtr>::const_iterator itjoint = probot->GetJoints().begin(); itjoint != probot->GetJoints().end(); ++itjoint) {
if( (*itjoint)->GetName().find("wheel") != string::npos ) {
for(int i = 0; i < (*itjoint)->GetDOF(); ++i) {
wheelindices.push_back((*itjoint)->GetDOFIndex()+i);
}
}
else {
for(int i = 0; i < (*itjoint)->GetDOF(); ++i) {
restindices.push_back((*itjoint)->GetDOFIndex()+i);
}
}
}
if(wheelindices.size() > 0 ) {
wheelcontroller = RaveCreateController(penv,"odevelocity");
multi->AttachController(wheelcontroller,wheelindices,0);
}
if( restindices.size() > 0 ) {
armcontroller = RaveCreateController(penv,"idealcontroller");
multi->AttachController(armcontroller,restindices,0);
}
else {
RAVELOG_WARN("robot needs to have wheels and arm for demo to work\n");
}
}
while(1) {
{
EnvironmentMutex::scoped_lock lock(penv->GetMutex()); // lock environment
if( !!armcontroller ) {
// set a trajectory on the arm and velocity on the wheels
TrajectoryBasePtr traj = RaveCreateTrajectory(penv,"");
probot->SetActiveDOFs(restindices);
ConfigurationSpecification spec = probot->GetActiveConfigurationSpecification();
int timeoffset = spec.AddDeltaTime();
traj->Init(spec);
probot->GetActiveDOFValues(q); // get current values
vector<dReal> vdata(spec.GetDOF(),0);
std::copy(q.begin(),q.end(),vdata.begin());
traj->Insert(0,vdata);
for(int i = 0; i < 4; ++i) {
q.at(RaveRandomInt()%restindices.size()) += RaveRandomFloat()-0.5; // move a random axis
}
// check for collisions
{
RobotBase::RobotStateSaver saver(probot); // add a state saver so robot is not moved permenantly
probot->SetActiveDOFValues(q);
if( probot->CheckSelfCollision() ) { // don't check env collisions since we have physics enabled
continue; // robot in collision at final point, so reject
}
}
std::copy(q.begin(),q.end(),vdata.begin());
vdata.at(timeoffset) = 2; // trajectory takes 2s
traj->Insert(1,vdata);
planningutils::RetimeActiveDOFTrajectory(traj,probot,true);
armcontroller->SetPath(traj);
}
if( !!wheelcontroller ) {
stringstream sout,ss; ss << "setvelocity ";
for(size_t i = 0; i < wheelindices.size(); ++i) {
ss << 2*(RaveRandomFloat()-0.5) << " ";
}
if( !wheelcontroller->SendCommand(sout,ss) ) {
RAVELOG_WARN("failed to send velocity command\n");
}
}
}
// unlock the environment and wait for the arm controller to finish (wheel controller will never finish)
if( !armcontroller ) {
usleep(2000000);
}
else {
while(!armcontroller->IsDone()) {
usleep(1000);
}
}
}
thviewer.join(); // wait for the viewer thread to exit
penv->Destroy(); // destroy
return 0;
}
int main(int argc, char ** argv)
{
if( argc < 3 ) {
RAVELOG_INFO("ikloader robot iktype\n");
return 1;
}
string robotname = argv[1];
string iktype = argv[2];
RaveInitialize(true); // start openrave core
EnvironmentBasePtr penv = RaveCreateEnvironment(); // create the main environment
{
// lock the environment to prevent changes
EnvironmentMutex::scoped_lock lock(penv->GetMutex());
// load the scene
RobotBasePtr probot = penv->ReadRobotXMLFile(robotname);
if( !probot ) {
penv->Destroy();
return 2;
}
penv->Add(probot);
ModuleBasePtr pikfast = RaveCreateModule(penv,"ikfast");
penv->Add(pikfast,true,"");
stringstream ssin,ssout;
ssin << "LoadIKFastSolver " << probot->GetName() << " " << iktype;
// if necessary, add free inc for degrees of freedom
//ssin << " " << 0.04f;
// get the active manipulator
RobotBase::ManipulatorPtr pmanip = probot->GetActiveManipulator();
if( !pikfast->SendCommand(ssout,ssin) ) {
RAVELOG_ERROR("failed to load iksolver\n");
penv->Destroy();
return 1;
}
RAVELOG_INFO("testing random ik\n");
while(1) {
Transform trans;
trans.rot = quatFromAxisAngle(Vector(RaveRandomFloat()-0.5,RaveRandomFloat()-0.5,RaveRandomFloat()-0.5));
trans.trans = Vector(RaveRandomFloat()-0.5,RaveRandomFloat()-0.5,RaveRandomFloat()-0.5)*2;
vector<dReal> vsolution;
if( pmanip->FindIKSolution(IkParameterization(trans),vsolution,IKFO_CheckEnvCollisions) ) {
stringstream ss; ss << "solution is: ";
for(size_t i = 0; i < vsolution.size(); ++i) {
ss << vsolution[i] << " ";
}
ss << endl;
RAVELOG_INFO(ss.str());
}
else {
// could fail due to collisions, etc
}
}
}
RaveDestroy(); // destroy
return 0;
}
KinBodyPtr GetCppKinBody(py::object py_kb, EnvironmentBasePtr env) {
int id = py::extract<int>(py_kb.attr("GetEnvironmentId")());
return env->GetBodyFromEnvironmentId(id);
}
int main(int argc, char *argv[]) {
int T = 4;
bool sim_plotting = false;
bool stage_plotting = false;
bool first_step_only = false;
{
Config config;
config.add(new Parameter<bool>("sim_plotting", &sim_plotting, "sim_plotting"));
config.add(new Parameter<bool>("stage_plotting", &stage_plotting, "stage_plotting"));
config.add(new Parameter<bool>("first_step_only", &first_step_only, "first_step_only"));
CommandParser parser(config);
parser.read(argc, argv, true);
}
string manip_name("base_point");
string link_name("Base");
RaveInitialize();
EnvironmentBasePtr env = RaveCreateEnvironment();
env->StopSimulation();
env->Load(string(DATA_DIR) + "/point.env.xml");
OSGViewerPtr viewer;
RobotBasePtr robot = GetRobot(*env);
Vector2d start = Vector2d::Zero();
Matrix2d start_sigma = Matrix2d::Identity() * 0.2 * 0.2;
deque<Vector2d> initial_controls;
for (int i = 0; i < T; ++i) {
//initial_controls.push_back((Vector2d(0, 5) - start) / T);//Vector2d::Zero());
initial_controls.push_back(Vector2d::Zero());
}
Matrix4d goal_trans;
goal_trans << 1, 0, 0, 0,
0, 1, 0, 5,
0, 0, 1, 0,
0, 0, 0, 1;
initialize_robot(robot, start);
PointRobotBSPPlannerPtr planner(new PointRobotBSPPlanner());
planner->start = start;
planner->start_sigma = start_sigma;
planner->goal_trans = goal_trans;
planner->T = T;
planner->controls = initial_controls;
planner->robot = robot;
planner->rad = RADFromName(manip_name, robot);
planner->link = planner->rad->GetRobot()->GetLink(link_name);
planner->method = BSP::DiscontinuousBeliefSpace;
planner->initialize();
cout << "dof: " << planner->rad->GetDOF() << endl;
boost::function<void(OptProb*, DblVec&)> opt_callback;
if (stage_plotting || sim_plotting) {
viewer = OSGViewer::GetOrCreate(env);
initialize_viewer(viewer);
}
if (stage_plotting) {
opt_callback = boost::bind(&stage_plot_callback,
planner, viewer, _1, _2);
}
while (!planner->finished()) {
planner->solve(opt_callback, 1, 1);
planner->simulate_execution();
if (first_step_only) break;
if (sim_plotting) {
OpenRAVEPlotterMixin<PointRobotBSPPlanner>::sim_plot_callback(planner, planner->rad, viewer);
}
}
RaveDestroy();
return 0;
}
void do_task(Transform Ta, Transform Tb, EnvironmentBasePtr _penv, unsigned int thread_count_left)
{
std::vector< std::vector< dReal > > vsolutionsA, vsolutionsB;
dReal time_seconds;
TrajectoryBasePtr ptraj;
EnvironmentBasePtr pEnv = _penv->CloneSelf(Clone_Bodies);
if (!!(GetIKSolutions(_penv, Tb, vsolutionsB) && GetIKSolutions(_penv, Ta,vsolutionsA))){
//RAVELOG_INFO("Solution found for both source and goal\n");
}
else {
////RAVELOG_INFO("No solution found either for source or goal");
}
k_threads = 1;
unsigned int _combinations = vsolutionsA.size() * vsolutionsB.size() , combinations = 0;
std::cout << "Found " << _combinations << " combinations" << std::endl;
if (!! _combinations){
vector<boost::shared_ptr<boost::thread> > vthreads(_combinations);
for(size_t i = 0; i < vsolutionsA.size(); ++i) {
//RAVELOG_INFO("Planning for every Pose A \n");
for(size_t j = 0; j < vsolutionsB.size(); ++j) {
//RAVELOG_INFO("to every Pose B \n");
std::cout << "i " << i << "j " <<j << std::endl;
combinations +=1;
//check if all threads are already running
if (k_threads == thread_count_left || combinations == _combinations){
for(size_t k = 0; k < k_threads-1; ++k) {
std::cout << "Retrieved thread to be complete " << completed_thread << std::endl;
vthreads[k]->join();
//k_threads -= 1;
RAVELOG_INFO("joined ........................\n");
}
/*for(size_t k = 0; k < k_threads-1; ++k) {
std::cout << "Retrieved thread to be complete " << completed_thread << std::endl;
if (vthreads[k]->get_id() == completed_thread){
vthreads[k]->join();
k_threads -= 1;
RAVELOG_INFO("joined \n");
}
}*/
}
else if(k_threads <= thread_count_left){
//std::cout << k_threads-1 << std::endl;
vthreads[k_threads-1].reset(new boost::thread(boost::bind(&GetTaskTime, _penv, vsolutionsA[i], vsolutionsB[j],ptraj)));
//vthreads[k_threads].get_id();
k_threads += 1;
continue;
}
//RAVELOG_INFO("wait for threads to finish\n");
//std::cout << vthreads.size() << std::endl;
//boost::this_thread::sleep(boost::posix_time::milliseconds(10));
////RAVELOG_INFO("threads finished\n");
//vthreads.resize(0);
}
}
}
}
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;
}
int main(int argc, char **argv)
{
std::string environment_uri;
std::string robot_name;
std::string plugin_name;
size_t num_trials;
bool self = false;
bool profile = false;
// Parse arguments.
po::options_description desc("Profile OpenRAVE's memory usage.");
desc.add_options()
("num-samples", po::value<size_t>(&num_trials)->default_value(10000),
"number of samples to run")
("self", po::value<bool>(&self)->zero_tokens(),
"run self-collision checks")
("profile", po::value<bool>(&profile)->zero_tokens(),
"remove objects from environment")
("environment_uri",
po::value<std::string>(&environment_uri)->required(),
"number of samples to run")
("robot", po::value<std::string>(&robot_name)->required(),
"robot_name")
("collision_checker",
po::value<std::string>(&plugin_name)->required(),
"collision checker name")
("help",
"print usage information")
;
po::positional_options_description pd;
pd.add("environment_uri", 1);
pd.add("robot", 1);
pd.add("collision_checker", 1);
po::variables_map vm;
po::store(
po::command_line_parser(argc, argv)
.options(desc)
.positional(pd).run(),
vm
);
po::notify(vm);
if (vm.count("help")) {
std::cout << desc << std::endl;
return 1;
}
// Create the OpenRAVE environment.
RaveInitialize(true);
EnvironmentBasePtr const env = RaveCreateEnvironment();
CollisionCheckerBasePtr const collision_checker
= RaveCreateCollisionChecker(env, plugin_name);
env->SetCollisionChecker(collision_checker);
env->StopSimulation();
// "/usr/share/openrave-0.9/data/wamtest1.env.xml"
env->Load(environment_uri);
KinBodyPtr const body = env->GetKinBody(robot_name);
// Generate random configuations.
std::vector<OpenRAVE::dReal> lower;
std::vector<OpenRAVE::dReal> upper;
body->GetDOFLimits(lower, upper);
std::vector<std::vector<double> > data;
data = benchmarks::DataUtils::GenerateRandomConfigurations(
num_trials, lower, upper);
//
RAVELOG_INFO("Running %d collision checks.\n", num_trials);
boost::timer const timer;
if (profile) {
std::string const prof_name = str(
format("CheckCollision.%s.prof") % plugin_name);
RAVELOG_INFO("Writing gperftools information to '%s'.\n",
prof_name.c_str()
);
ProfilerStart(prof_name.c_str());
}
size_t num_collision = 0;
for (size_t i = 0; i < num_trials; ++i) {
body->SetDOFValues(data[i]);
bool is_collision;
if (self) {
is_collision = body->CheckSelfCollision();
} else {
is_collision = env->CheckCollision(body);
}
num_collision += !!is_collision;
}
if (profile) {
ProfilerStop();
}
double const duration = timer.elapsed();
RAVELOG_INFO(
"Ran %d collision checks (%d in collision) in %f seconds (%f checks per second).\n",
num_trials, num_collision, duration, num_trials / duration
);
return 0;
}
void SetViewer(EnvironmentBasePtr penv, const string& viewername)
{
ViewerBasePtr viewer = RaveCreateViewer(penv,viewername);
penv->AddViewer(viewer);
viewer->main(true);
}
