void GLscene::updateScene()
{
if (m_log->index()<0) return;
#ifdef USE_COLLISION_STATE
LogManager<OpenHRP::CollisionDetectorService::CollisionState> *lm
= (LogManager<OpenHRP::CollisionDetectorService::CollisionState> *)m_log;
GLbody *glbody = dynamic_cast<GLbody *>(body(0).get());
OpenHRP::CollisionDetectorService::CollisionState &co = lm->state();
if (co.angle.length() == glbody->numJoints()){
for (int i=0; i<glbody->numJoints(); i++){
GLlink *j = (GLlink *)glbody->joint(i);
if (j){
j->setQ(co.angle[i]);
}
}
}
#else
LogManager<TimedPosture> *lm
= (LogManager<TimedPosture> *)m_log;
GLbody *glbody = dynamic_cast<GLbody *>(body(0).get());
TimedPosture &ts = lm->state();
if (ts.posture.size() == glbody->numJoints()){
for (int i=0; i<glbody->numJoints(); i++){
GLlink *j = (GLlink *)glbody->joint(i);
if (j){
j->setQ(ts.posture[i]);
}
}
}
#endif
}
void GLscene::showStatus()
{
if (m_log->index()<0) return;
LogManager<OpenHRP::SceneState> *lm
= (LogManager<OpenHRP::SceneState> *)m_log;
OpenHRP::SceneState &sstate = lm->state();
if (m_showingStatus){
GLbody *glbody = NULL;
OpenHRP::RobotState *rstate = NULL;
for (unsigned int i=0; i<numBodies(); i++){
if (body(i)->numJoints()){
glbody = dynamic_cast<GLbody *>(body(i).get());
rstate = &sstate.states[i];
break;
}
}
#define HEIGHT_STEP 12
int width = m_width - 410;
int height = m_height-HEIGHT_STEP;
char buf[256];
for (unsigned int i=0; i<glbody->numJoints(); i++){
hrp::Link *l = glbody->joint(i);
if (l){
int x = width;
// joint ID
sprintf(buf, "%2d",i);
glRasterPos2f(x, height);
drawString2(buf);
white();
x += 8*3;
// joint name, current angle
sprintf(buf, "%13s %8.3f",
l->name.c_str(),
rstate->q[i]*180/M_PI);
glRasterPos2f(x, height);
drawString2(buf);
x += 8*(14+9);
height -= HEIGHT_STEP;
}
}
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
glEnable(GL_BLEND);
glColor4f(0.0,0.0,0.0, 0.5);
if (m_showSlider){
glRectf(width,SLIDER_AREA_HEIGHT,m_width,m_height);
}else{
glRectf(width,0,m_width,m_height);
}
glDisable(GL_BLEND);
}
}
void GLscene::updateScene()
{
if (m_log->index()<0) return;
LogManager<TimedRobotState> *lm
= (LogManager<TimedRobotState> *)m_log;
GLbody *glbody = dynamic_cast<GLbody *>(body(0).get());
OpenHRP::StateHolderService::Command &com = lm->state().command;
if (com.baseTransform.length() == 12){
double *tform = com.baseTransform.get_buffer();
glbody->setPosition(tform);
glbody->setRotation(tform+3);
hrp::Link *root = glbody->rootLink();
root->p << tform[0], tform[1], tform[2];
root->R << tform[3], tform[4], tform[5],
tform[6], tform[7], tform[8],
tform[9], tform[10], tform[11];
}
if (com.jointRefs.length() == glbody->numJoints()){
for (int i=0; i<glbody->numJoints(); i++){
GLlink *j = (GLlink *)glbody->joint(i);
if (j){
j->q = com.jointRefs[i];
j->setQ(com.jointRefs[i]);
}
}
}
glbody->calcForwardKinematics();
glbody->updateLinkColdetModelPositions();
for (int i=0; i<glbody->numLinks(); i++){
((GLlink *)glbody->link(i))->highlight(false);
}
for (size_t i=0; i<m_pairs.size(); i++){
if (m_pairs[i]->checkCollision()){
((GLlink *)m_pairs[i]->link(0))->highlight(true);
((GLlink *)m_pairs[i]->link(1))->highlight(true);
std::cout << m_pairs[i]->link(0)->name << "<->" << m_pairs[i]->link(1)->name << std::endl;
}
}
}
void GLscene::showStatus()
{
if (m_log->index()<0) return;
LogManager<TimedRobotState> *lm
= (LogManager<TimedRobotState> *)m_log;
OpenHRP::RobotHardwareService::RobotState &rstate = lm->state().state;
if (m_showingStatus){
GLbody *glbody = dynamic_cast<GLbody *>(body(0).get());
#define HEIGHT_STEP 12
int width = m_width - 410;
int height = m_height-HEIGHT_STEP;
char buf[256];
for (int i=0; i<glbody->numJoints(); i++){
hrp::Link *l = glbody->joint(i);
if (l){
int ss = rstate.servoState[i][0];
int x = width;
// joint ID
sprintf(buf, "%2d",i);
if (!isCalibrated(ss)){
yellow();
}else if(isServoOn(ss)){
red();
}
glRasterPos2f(x, height);
drawString2(buf);
white();
x += 8*3;
// power status
if (isPowerOn(ss)) blue();
glRasterPos2f(x, height);
drawString2("o");
if (isPowerOn(ss)) white();
x += 8*2;
// joint name, current angle, command angle and torque
sprintf(buf, "%13s %8.3f %8.3f %6.1f",
l->name.c_str(),
rstate.angle[i]*180/M_PI,
rstate.command[i]*180/M_PI,
rstate.torque[i]*180/M_PI);
glRasterPos2f(x, height);
drawString2(buf);
x += 8*(14+9+9+7);
// servo alarms
sprintf(buf, "%03x", servoAlarm(ss));
glRasterPos2f(x, height);
drawString2(buf);
x += 8*4;
// driver temperature
int temp = temperature(ss);
if (!temp){
sprintf(buf, "--", temp);
}else{
sprintf(buf, "%2d", temp);
}
if (temp >= 60) red();
glRasterPos2f(x, height);
drawString2(buf);
if (temp >= 60) white();
x += 8*3;
height -= HEIGHT_STEP;
}
}
if (rstate.accel.length()){
glRasterPos2f(width, height);
height -= HEIGHT_STEP;
drawString2("acc:");
for (unsigned int i=0; i<rstate.accel.length(); i++){
sprintf(buf, " %8.4f %8.4f %8.4f",
rstate.accel[i][0], rstate.accel[i][1], rstate.accel[i][2]);
glRasterPos2f(width, height);
height -= HEIGHT_STEP;
drawString2(buf);
}
}
if (rstate.rateGyro.length()){
glRasterPos2f(width, height);
height -= HEIGHT_STEP;
drawString2("rate:");
for (unsigned int i=0; i<rstate.rateGyro.length(); i++){
sprintf(buf, " %8.4f %8.4f %8.4f",
rstate.rateGyro[i][0], rstate.rateGyro[i][1], rstate.rateGyro[i][2]);
glRasterPos2f(width, height);
height -= HEIGHT_STEP;
drawString2(buf);
}
}
if (rstate.force.length()){
glRasterPos2f(width, height);
height -= HEIGHT_STEP;
drawString2("force/torque:");
for (unsigned int i=0; i<rstate.force.length(); i++){
sprintf(buf, " %6.1f %6.1f %6.1f %6.2f %6.2f %6.2f",
rstate.force[i][0],
rstate.force[i][1],
rstate.force[i][2],
rstate.force[i][3],
rstate.force[i][4],
rstate.force[i][5]);
glRasterPos2f(width, height);
height -= HEIGHT_STEP;
drawString2(buf);
}
}
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
glEnable(GL_BLEND);
glColor4f(0.0,0.0,0.0, 0.5);
if (m_showSlider){
glRectf(width,SLIDER_AREA_HEIGHT,m_width,m_height);
}else{
glRectf(width,0,m_width,m_height);
}
glDisable(GL_BLEND);
}else{
// !m_showingRobotState
bool servo=false, power=false;
for (unsigned int i=0; i<rstate.servoState.length(); i++){
if (isServoOn(rstate.servoState[i][0])) servo = true;
if (isPowerOn(rstate.servoState[i][0])) power = true;
}
struct timeval tv;
gettimeofday(&tv, NULL);
double dt = tv.tv_sec + tv.tv_usec/1e6 - lm->time(m_log->length()-1);
if (dt < 1.0) green(); else black();
glRectf(m_width-115,m_height-45,m_width-85,m_height-15);
if (power) blue(); else black();
glRectf(m_width- 80,m_height-45,m_width-50,m_height-15);
if (servo) red(); else black();
glRectf(m_width- 45,m_height-45,m_width-15,m_height-15);
}
}
void GLscene::showStatus()
{
if (m_log->index()<0) return;
LogManager<SceneState> *lm
= (LogManager<SceneState> *)m_log;
SceneState &state = lm->state();
if (m_showingStatus){
GLbody *glbody = NULL;
BodyState *bstate = NULL;
for (unsigned int i=0; i<numBodies(); i++){
if (body(i)->numJoints()){
glbody = dynamic_cast<GLbody *>(body(i).get());
bstate = &state.bodyStates[i];
break;
}
}
if (!glbody) return;
#define HEIGHT_STEP 12
int width = m_width - 350;
int height = m_height-HEIGHT_STEP;
char buf[256];
double q[glbody->numJoints()];
for (int i=0; i<glbody->numLinks(); i++){
Link* l = glbody->link(i);
if (l->jointId >= 0) q[l->jointId] = bstate->q[i];
}
for (int i=0; i<glbody->numJoints(); i++){
GLlink *l = (GLlink *)glbody->joint(i);
if (l){
sprintf(buf, "%2d %15s %8.3f", i, l->name.c_str(),
q[i]*180/M_PI);
glRasterPos2f(width, height);
height -= HEIGHT_STEP;
drawString2(buf);
}
}
if (bstate->acc.size()){
glRasterPos2f(width, height);
height -= HEIGHT_STEP;
drawString2("acc:");
for (unsigned int i=0; i<bstate->acc.size(); i++){
sprintf(buf, " %8.4f %8.4f %8.4f",
bstate->acc[i][0], bstate->acc[i][1], bstate->acc[i][2]);
glRasterPos2f(width, height);
height -= HEIGHT_STEP;
drawString2(buf);
}
}
if (bstate->rate.size()){
glRasterPos2f(width, height);
height -= HEIGHT_STEP;
drawString2("rate:");
for (unsigned int i=0; i<bstate->rate.size(); i++){
sprintf(buf, " %8.4f %8.4f %8.4f",
bstate->rate[i][0], bstate->rate[i][1], bstate->rate[i][2]);
glRasterPos2f(width, height);
height -= HEIGHT_STEP;
drawString2(buf);
}
}
if (bstate->force.size()){
glRasterPos2f(width, height);
height -= HEIGHT_STEP;
drawString2("force/torque:");
for (unsigned int i=0; i<bstate->force.size(); i++){
sprintf(buf, " %6.1f %6.1f %6.1f %6.2f %6.2f %6.2f",
bstate->force[i][0],
bstate->force[i][1],
bstate->force[i][2],
bstate->force[i][3],
bstate->force[i][4],
bstate->force[i][5]);
glRasterPos2f(width, height);
height -= HEIGHT_STEP;
drawString2(buf);
}
}
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
glEnable(GL_BLEND);
glColor4f(0.0,0.0,0.0, 0.5);
if (m_showSlider){
glRectf(width,SLIDER_AREA_HEIGHT,m_width,m_height);
}else{
glRectf(width,0,m_width,m_height);
}
glDisable(GL_BLEND);
}
}
