virtual void RenderWorld() { Robot* robot=world->robots[0].robot; RobotController* rc=sim.robotControllers[0]; SimViewProgram::RenderWorld(); //draw current commanded configuration -- transparent if(drawCommanded) { GLColor newColor(0,1,0,0.5); world->robots[0].view.SetColors(newColor); Config q; sim.controlSimulators[0].GetCommandedConfig(q); robot->UpdateConfig(q); world->robots[0].view.Draw(); } if(drawDesired) { Config curBest; robotInterface->GetEndConfig(curBest); robot->UpdateConfig(curBest); world->robots[0].view.SetColors(GLColor(1,1,0,0.5)); world->robots[0].view.Draw(); } if(drawUI) { uis[currentUI]->DrawGL(); } //draw desired path if(drawPath) { Real tstart = robotInterface->GetCurTime(); Real tend = robotInterface->GetEndTime(); Real dt = 0.05; //draw end effector path glDisable(GL_LIGHTING); glColor3f(1,1,0); glLineWidth(2.0); glBegin(GL_LINES); int istart=(int)Ceil(tstart/dt); int iend=(int)Ceil(tend/dt); for(int i=istart;i<iend;i++) { Real t1=i*dt; Real t2=t1+0.5*dt; robotInterface->GetConfig(t1,robot->q); robot->UpdateFrames(); glVertex3v(robot->links.back().T_World.t); robotInterface->GetConfig(t2,robot->q); robot->UpdateFrames(); glVertex3v(robot->links.back().T_World.t); } glEnd(); } //draw collision feedback if(drawContacts) { DrawContacts(); } }
virtual void RenderWorld() { Robot* robot=world->robots[0].robot; RobotController* rc=sim.robotControllers[0]; SimGUIBackend::SetForceColors(); SimGUIBackend::RenderWorld(); //draw current commanded configuration -- transparent if(drawCommanded) { GLColor newColor(0,1,0,0.5); world->robots[0].view.SetColors(newColor); Config q; sim.controlSimulators[0].GetCommandedConfig(q); robot->UpdateConfig(q); world->robots[0].view.Draw(); } if(drawDesired) { Config curBest; robotInterface->GetEndConfig(curBest); robot->UpdateConfig(curBest); world->robots[0].view.SetColors(GLColor(1,1,0,0.5)); world->robots[0].view.Draw(); /* if(curGoal) { glPointSize(5.0); glDisable(GL_LIGHTING); glColor3f(1,1,0); glBegin(GL_POINTS); glVertex3v(curGoal->ikGoal.endPosition); glEnd(); //draw end effector path glColor3f(1,0.5,0); glBegin(GL_LINE_STRIP); for(Real t=c->pathParameter;t<c->ramp.endTime;t+=0.05) { c->ramp.Evaluate(t,robot->q); robot->UpdateFrames(); glVertex3v(robot->links[curGoal->ikGoal.link].T_World*curGoal->ikGoal.localPosition); } for(size_t i=0;i<c->path.ramps.size();i++) { for(Real t=0;t<c->path.ramps[i].endTime;t+=0.05) { c->path.ramps[i].Evaluate(t,robot->q); robot->UpdateFrames(); glVertex3v(robot->links[curGoal->ikGoal.link].T_World*curGoal->ikGoal.localPosition); } } glEnd(); glEnable(GL_LIGHTING); } */ } if(drawUI) { ui->DrawGL(); } //draw desired path if(drawPath) { Real tstart = robotInterface->GetCurTime(); Real tend = robotInterface->GetEndTime(); Real dt = 0.05; //draw end effector path glDisable(GL_LIGHTING); glColor3f(1,1,0); glLineWidth(2.0); glBegin(GL_LINES); int istart=(int)Ceil(tstart/dt); int iend=(int)Ceil(tend/dt); for(int i=istart;i<iend;i++) { Real t1=i*dt; Real t2=t1+0.5*dt; robotInterface->GetConfig(t1,robot->q); robot->UpdateFrames(); glVertex3v(robot->links.back().T_World.t); robotInterface->GetConfig(t2,robot->q); robot->UpdateFrames(); glVertex3v(robot->links.back().T_World.t); } glEnd(); } //draw collision feedback if(drawContacts) { DrawContacts(); } }