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