void GLscene::updateScene()
{
if (m_log->index()<0) return;
LogManager<OpenHRP::WorldState> *lm
= (LogManager<OpenHRP::WorldState> *)m_log;
OpenHRP::WorldState &state = lm->state();
for (unsigned int i=0; i<state.characterPositions.length(); i++){
const CharacterPosition& cpos = state.characterPositions[i];
std::string cname(cpos.characterName);
GLbody *glbody = dynamic_cast<GLbody *>(body(cname).get());
if (!glbody) {
//std::cerr << "can't find a body named " << cname << std::endl;
continue;
}
for (unsigned int j=0; j<cpos.linkPositions.length(); j++){
const LinkPosition &lp = cpos.linkPositions[j];
double T[] = {lp.R[0], lp.R[3], lp.R[6],0,
lp.R[1], lp.R[4], lp.R[7],0,
lp.R[2], lp.R[5], lp.R[8],0,
lp.p[0], lp.p[1], lp.p[2],1};
#if 0
for (int i=0; i<4; i++){
for (int j=0; j<4; j++){
printf("%6.3f ", T[i*4+j]);
}
printf("\n");
}
printf("\n");
#endif
((GLlink *)glbody->link(j))->setAbsTransform(T);
}
}
}
void GLscene::showStatus()
{
char buf[256];
GLbody *glbody = dynamic_cast<GLbody *>(body(0).get());
int width = m_width - 220;
#define HEIGHT_STEP 12
int height = m_height-HEIGHT_STEP;
int x = width;
for (int i=0; i<glbody->numLinks(); i++){
hrp::Link *l = glbody->link(i);
if (l){
sprintf(buf, "%13s %4d tris",
l->name.c_str(),
l->coldetModel->getNumTriangles());
glRasterPos2f(x, height);
drawString(buf);
height -= HEIGHT_STEP;
}
}
if (m_log->index()<0) return;
LogManager<OpenHRP::CollisionDetectorService::CollisionState> *lm
= (LogManager<OpenHRP::CollisionDetectorService::CollisionState> *)m_log;
OpenHRP::CollisionDetectorService::CollisionState &co = lm->state();
height -= HEIGHT_STEP;
x = width - 34;
sprintf(buf, "Number of pair %8d", co.lines.length());
glRasterPos2f(x, height);
drawString(buf);
height -= HEIGHT_STEP;
sprintf(buf, "Calc Time [msec] %8.3f", co.computation_time);
glRasterPos2f(x, height);
drawString(buf);
height -= HEIGHT_STEP;
sprintf(buf, "Recover Time[msec] %8.3f", co.recover_time);
glRasterPos2f(x, height);
drawString(buf);
height -= HEIGHT_STEP;
sprintf(buf, "Safe Posture %8s", co.safe_posture?"true":"false");
glRasterPos2f(x, height);
drawString(buf);
height -= HEIGHT_STEP;
sprintf(buf, "Loop for check %8d", co.loop_for_check);
glRasterPos2f(x, height);
drawString(buf);
height -= HEIGHT_STEP;
}
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;
#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::updateScene()
{
if (m_log->index()<0) return;
LogManager<OpenHRP::SceneState> *lm
= (LogManager<OpenHRP::SceneState> *)m_log;
OpenHRP::SceneState &ss = lm->state();
for (unsigned int i=0; i<ss.states.length(); i++){
OpenHRP::RobotState &rs = ss.states[i];
GLbody *glbody = dynamic_cast<GLbody *>(body(i).get());
glbody->setPosition(rs.basePose.position.x,
rs.basePose.position.y,
rs.basePose.position.z);
glbody->setRotation(rs.basePose.orientation.r,
rs.basePose.orientation.p,
rs.basePose.orientation.y);
glbody->setPosture(rs.q.get_buffer());
}
}
void GLscene::updateScene()
{
if (m_log->index()<0) return;
LogManager<SceneState> *lm
= (LogManager<SceneState> *)m_log;
SceneState &state = lm->state();
for (unsigned int i=0; i<state.bodyStates.size(); i++){
const BodyState& bstate = state.bodyStates[i];
GLbody *glbody = dynamic_cast<GLbody *>(body(i).get());
glbody->setPosture(bstate.q, bstate.p, bstate.R);
if (m_showSensors){
glbody->setSensorDrawCallback(
boost::bind(&GLscene::drawSensorOutput, this, _1, _2));
}else{
glbody->setSensorDrawCallback(NULL);
}
}
}
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);
}
}
size_t GLlink::draw(){
size_t ntri=0;
glPushMatrix();
if (m_useAbsTransformToDraw){
glMultMatrixd(m_absTrans);
}else{
glMultMatrixd(m_trans);
glMultMatrixd(m_T_j);
}
if (m_drawMode != DM_COLLISION){
for (size_t i=0; i<m_shapes.size(); i++){
ntri += m_shapes[i]->draw(m_drawMode);
}
for (size_t i=0; i<m_cameras.size(); i++){
ntri += m_cameras[i]->draw(m_drawMode);
}
}else{
if (coldetModel && coldetModel->getNumTriangles()){
ntri = coldetModel->getNumTriangles();
Eigen::Vector3f n, v[3];
int vindex[3];
if (m_highlight){
float red[] = {1,0,0,1};
glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, red);
}else{
float gray[] = {0.8,0.8,0.8,1};
glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, gray);
}
glBegin(GL_TRIANGLES);
for (int i=0; i<coldetModel->getNumTriangles(); i++){
coldetModel->getTriangle(i, vindex[0], vindex[1], vindex[2]);
for (int j=0; j<3; j++){
coldetModel->getVertex(vindex[j], v[j][0], v[j][1], v[j][2]);
}
n = (v[1]-v[0]).cross(v[2]-v[0]);
n.normalize();
glNormal3fv(n.data());
for (int j=0; j<3; j++){
glVertex3fv(v[j].data());
}
}
glEnd();
}
}
for (size_t i=0; i<sensors.size(); i++){
Sensor *s = sensors[i];
double T[16];
for (int i=0; i<3; i++){
for (int j=0; j<3; j++){
T[i*4+j] = s->localR(j,i);
}
}
T[12] = s->localPos[0]; T[13] = s->localPos[1]; T[14] = s->localPos[2];
T[3] = T[7] = T[11] = 0.0; T[15] = 1.0;
glPushMatrix();
glMultMatrixd(T);
GLbody *glb = dynamic_cast<GLbody *>(body);
glb->drawSensor(s);
glPopMatrix();
}
for (size_t i=0; i<lights.size(); i++){
Light *l = lights[i];
int lid = GL_LIGHT0+l->id;
if (!l->on){
glDisable(lid);
continue;
}
double T[16];
for (int j=0; j<3; j++){
for (int k=0; k<3; k++){
T[j*4+k] = l->localR(k,j);
}
}
T[12] = l->localPos[0]; T[13] = l->localPos[1]; T[14] = l->localPos[2];
T[3] = T[7] = T[11] = 0.0; T[15] = 1.0;
glPushMatrix();
glMultMatrixd(T);
glEnable(GL_LIGHTING);
glEnable(lid);
GLfloat pos[] = {0,0,0,1};
glLightfv(lid, GL_POSITION, pos);
GLfloat color[] = {l->color[0], l->color[1], l->color[2], 1};
glLightfv(lid, GL_DIFFUSE, color);
if (l->type == POINT){
glLightf(lid, GL_CONSTANT_ATTENUATION, l->attenuation[0]);
glLightf(lid, GL_LINEAR_ATTENUATION, l->attenuation[1]);
glLightf(lid, GL_QUADRATIC_ATTENUATION, l->attenuation[2]);
}else if(l->type == SPOT){
glLightf(lid, GL_CONSTANT_ATTENUATION, l->attenuation[0]);
glLightf(lid, GL_LINEAR_ATTENUATION, l->attenuation[1]);
glLightf(lid, GL_QUADRATIC_ATTENUATION, l->attenuation[2]);
glLightf(lid, GL_SPOT_EXPONENT, 20);
glLightf(lid, GL_SPOT_CUTOFF, l->cutOffAngle*180/M_PI);
GLfloat dir[] = {l->direction[0], l->direction[1], l->direction[2]};
glLightfv(lid, GL_SPOT_DIRECTION, dir);
}else if(l->type == DIRECTIONAL){
GLfloat dir[] = {l->direction[0], l->direction[1], l->direction[2]};
glLightfv(lid, GL_SPOT_DIRECTION, dir);
}
glPopMatrix();
}
if (m_showAxes){
glDisable(GL_LIGHTING);
glBegin(GL_LINES);
glColor3f(1,0,0);
glVertex3f(0,0,0); glVertex3f(0.5, 0, 0);
glColor3f(0,1,0);
glVertex3f(0,0,0); glVertex3f(0, 0.5, 0);
glColor3f(0,0,1);
glVertex3f(0,0,0); glVertex3f(0, 0, 0.5);
glEnd();
glEnable(GL_LIGHTING);
}
if (!m_useAbsTransformToDraw){
hrp::Link *l = child;
while (l){
ntri += ((GLlink *)l)->draw();
l = l->sibling;
}
}
glPopMatrix();
return ntri;
}
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);
}
}
