void stage_plot_callback(boost::shared_ptr<PointRobotBSPPlanner> planner, OSGViewerPtr viewer, OptProb* prob, DblVec& x) { vector<GraphHandlePtr> handles; handles.clear(); OpenRAVEPlotterMixin<PointRobotBSPPlanner>::plot_opt_trajectory(planner, planner->rad, viewer, prob, x, &handles); vector<double> color_params; for (int i = 0; i <= planner->T; ++i) { color_params.push_back(((double)i)/((double)planner->T-1.0)); } StateT state; VarianceT sigma; Vector3d mean; Matrix3d cov; for (int i = 0; i <= planner->T; ++i) { BeliefT b = getVec(x, planner->helper->belief_vars.row(i)); planner->helper->belief_func->extract_state_and_sigma(b, &state, &sigma); belief_to_endeffector_noise(planner->rad, planner->link, state, sigma, &mean, &cov); handles.push_back(viewer->PlotEllipseXYContour(gaussian_as_transform(mean, cov), OR::Vector(0,color_params[i],1.0-color_params[i],1))); } BeliefT b = getVec(x, planner->helper->belief_vars.row(0)); for (int i = 0; i <= planner->T; ++i) { planner->helper->belief_func->extract_state_and_sigma(b, &state, &sigma); belief_to_endeffector_noise(planner->rad, planner->link, state, sigma, &mean, &cov); handles.push_back(viewer->PlotEllipseXYContour(gaussian_as_transform(mean, cov), OR::Vector(0,color_params[i],1.0-color_params[i],1), true)); if (i < planner->T) b = planner->helper->belief_func->call(b, getVec(x, planner->helper->control_vars.row(i))); } viewer->Idle(); }
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(); }
DriveControl::DriveControl(OpenRAVE::RobotBasePtr robot, OSGViewerPtr viewer) : m_robot(robot), m_viewer(viewer) { viewer->AddKeyCallback(osgGA::GUIEventAdapter::KEY_Left, boost::bind(&DriveControl::MoveRobot, this, 0,.05, 0)); viewer->AddKeyCallback(osgGA::GUIEventAdapter::KEY_Right, boost::bind(&DriveControl::MoveRobot,this, 0,-.05, 0)); viewer->AddKeyCallback(osgGA::GUIEventAdapter::KEY_Up, boost::bind(&DriveControl::MoveRobot, this,.05,0, 0)); viewer->AddKeyCallback(osgGA::GUIEventAdapter::KEY_Down, boost::bind(&DriveControl::MoveRobot, this,-.05,0, 0)); viewer->AddKeyCallback(osgGA::GUIEventAdapter::KEY_Leftbracket, boost::bind(&DriveControl::MoveRobot, this,0,0, .05)); viewer->AddKeyCallback(osgGA::GUIEventAdapter::KEY_Rightbracket, boost::bind(&DriveControl::MoveRobot, this,0,0, -.05)); }
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(); }
ManipulatorControl::ManipulatorControl(OpenRAVE::RobotBase::ManipulatorPtr manip, OSGViewerPtr viewer) { m_manip = manip; m_viewer = viewer; viewer->AddMouseCallback(boost::bind(&ManipulatorControl::ProcessMouseInput, this, _1)); }
void AdjustTransparency(float da) { alpha += da; alpha = fmin(alpha, 1); alpha = fmax(alpha, 0); viewer->SetAllTransparency(alpha); }