apollo机器人3d源码

/* Copyright (C) 2007, 2008 by Team Apollo Nanjing University of Posts and Telecommunications, China This program is free software; you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation; version 2 of the License. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program; if not, write to the Free Software Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA. */ #include "nao.h" #include <zeitgeist/logserver/logserver.h> using namespace std; using namespace salt; using namespace boost; using namespace zeitgeist; Nao::Nao() { SetName("nao"); /** these two values must be in constructor */ JID_MIN = JID_ROOT; JID_MAX = JID_RLEG_6; PART_MIN = JID_ROOT; PART_MAX = JID_RLEG_6; FootLength = 0.08; FootWidth = 0.16; FootHeight = 0.03; FootMass = 0.2; ShankLength = 0.08; ShankWidth = 0.07; ShankHeight = 0.11; ShankMass = 0.225; ThighLength = 0.07; ThighWidth = 0.07; ThighHeight = 0.14; ThighMass = 0.275; TorsoLength = 0.1; TorsoWidth = 0.1; TorsoHeight = 0.18; TorsoMass = 1.2171; TorsoInitX = 0.0f; TorsoInitY = 0.0f; TorsoInitZ = 0.385; Hip1RelTorso_X = 0.055; Hip1RelTorso_Y = -0.01; Hip1RelTorso_Z = -0.115; Hip2RelHip1_X = 0; Hip2RelHip1_Y = 0; Hip2RelHip1_Z = 0; ThighRelHip2_X = 0; ThighRelHip2_Y = 0.01; //set 0.005 to check the tri-angle algorithm ThighRelHip2_Z = -0.04; ShankRelThigh_X = 0; ShankRelThigh_Y = 0.005; ShankRelThigh_Z = -0.125; AnkleRelShank_X = 0; AnkleRelShank_Y = -0.01; AnkleRelShank_Z = -0.055; FootRelAnkle_X = 0; FootRelAnkle_Y = 0.03; FootRelAnkle_Z = -0.035; Hip1Axis_X = -0.7071; Hip1Axis_Y = 0; Hip1Axis_Z = 0.7071; } Nao::~Nao() { } void Nao::SetupLinks() { uLINK[JID_ROOT].name = "j0"; uLINK[JID_ROOT].eff_name = "e0"; uLINK[JID_ROOT].sister = 0; uLINK[JID_ROOT].child = JID_LLEG_1; uLINK[JID_ROOT].mother = 0; uLINK[JID_ROOT].a = Vector3f(0, 0, 0); uLINK[JID_ROOT].c = Vector3f(0, 0, 0); uLINK[JID_ROOT].b = Vector3f(0, 0, 0); uLINK[JID_ROOT].q = 0; uLINK[JID_ROOT].p = Vector3f(TorsoInitX, TorsoInitY, TorsoInitZ); uLINK[JID_ROOT].R.Identity(); /** ----------------------------- legs begin ---------------------- */ uLINK[JID_LLEG_1].name = "llj1"; uLINK[JID_LLEG_1].eff_name = "lle1"; uLINK[JID_LLEG_1].sister = JID_RLEG_1; uLINK[JID_LLEG_1].child = JID_LLEG_2; uLINK[JID_LLEG_1].mother = JID_ROOT; uLINK[JID_LLEG_1].a = Vector3f(Hip1Axis_X, Hip1Axis_Y, -Hip1Axis_Z); uLINK[JID_LLEG_1].c = Vector3f(0, 0, 0); uLINK[JID_LLEG_1].b = Vector3f(-Hip1RelTorso_X, Hip1RelTorso_Y, Hip1RelTorso_Z) - uLINK[JID_LLEG_1].c + uLINK[JID_ROOT].c; uLINK[JID_LLEG_1].q = 0; uLINK[JID_LLEG_2].name = "llj2"; uLINK[JID_LLEG_2].eff_name = "lle2"; uLINK[JID_LLEG_2].sister = 0; uLINK[JID_LLEG_2].child = JID_LLEG_3; uLINK[JID_LLEG_2].mother = JID_LLEG_1; uLINK[JID_LLEG_2].a = Vector3f(0, 1, 0); // Vector3f(1, 0, 0) uLINK[JID_LLEG_2].c = Vector3f(0, 0, 0); uLINK[JID_LLEG_2].b = Vector3f(Hip2RelHip1_X, Hip2RelHip1_Y, Hip2RelHip1_Z) - uLINK[JID_LLEG_2].c + uLINK[JID_LLEG_1].c; uLINK[JID_LLEG_2].q = 0; uLINK[JID_LLEG_3].name = "llj3"; uLINK[JID_LLEG_3].eff_name = "lle3"; uLINK[JID_LLEG_3].sister = 0; uLINK[JID_LLEG_3].child = JID_LLEG_4; uLINK[JID_LLEG_3].mother = JID_LLEG_2; uLINK[JID_LLEG_3].a = Vector3f(1, 0, 0); // Vector3f(0, 1, 0) uLINK[JID_LLEG_3].c = Vector3f(ThighRelHip2_X, ThighRelHip2_Y, ThighRelHip2_Z); uLINK[JID_LLEG_3].b = Vector3f(ThighRelHip2_X, ThighRelHip2_Y, ThighRelHip2_Z) - uLINK[JID_LLEG_3].c + uLINK[JID_LLEG_2].c; uLINK[JID_LLEG_3].q = 0; uLINK[JID_LLEG_4].name = "llj4"; uLINK[JID_LLEG_4].eff_name = "lle4"; uLINK[JID_LLEG_4].sister = 0; uLINK[JID_LLEG_4].child = JID_LLEG_5; uLINK[JID_LLEG_4].mother = JID_LLEG_3; uLINK[JID_LLEG_4].a = Vector3f(1, 0, 0); uLINK[JID_LLEG_4].c = Vector3f(0, 0.01, -0.045); uLINK[JID_LLEG_4].b = Vector3f(ShankRelThigh_X, ShankRelThigh_Y, ShankRelThigh_Z) - uLINK[JID_LLEG_4].c + uLINK[JID_LLEG_3].c; uLINK[JID_LLEG_4].q = 0; uLINK[JID_LLEG_5].name = "llj5"; uLINK[JID_LLEG_5].eff_name = "lle5"; uLINK[JID_LLEG_5].sister = 0; uLINK[JID_LLEG_5].child = JID_LLEG_6; uLINK[JID_LLEG_5].mother = JID_LLEG_4; uLINK[JID_LLEG_5].a = Vector3f(1, 0, 0); uLINK[JID_LLEG_5].c = Vector3f(0, 0, 0); uLINK[JID_LLEG_5].b = Vector3f(AnkleRelShank_X, AnkleRelShank_Y, AnkleRelShank_Z) - uLINK[JID_LLEG_5].c + uLINK[JID_LLEG_4].c; uLINK[JID_LLEG_5].q = 0; uLINK[JID_LLEG_6].name = "llj6"; uLINK[JID_LLEG_6].eff_name = "lle6"; uLINK[JID_LLEG_6].sister = 0; uLINK[JID_LLEG_6].child = 0; uLINK[JID_LLEG_6].mother = JID_LLEG_5; uLINK[JID_LLEG_6].a = Vector3f(0, 1, 0); uLINK[JID_LLEG_6].c = Vector3f(FootRelAnkle_X, FootRelAnkle_Y, FootRelAnkle_Z); uLINK[JID_LLEG_6].b = Vector3f(FootRelAnkle_X, FootRelAnkle_Y, FootRelAnkle_Z) - uLINK[JID_LLEG_6].c + uLINK[JID_LLEG_5].c; uLINK[JID_LLEG_6].q = 0; uLINK[JID_RLEG_1].name = "rlj1"; uLINK[JID_RLEG_1].eff_name = "rle1"; uLINK[JID_RLEG_1].sister = 0; uLINK[JID_RLEG_1].child = JID_RLEG_2; uLINK[JID_RLEG_1].mother = JID_ROOT; uLINK[JID_RLEG_1].a = Vector3f(Hip1Axis_X, Hip1Axis_Y, Hip1Axis_Z); uLINK[JID_RLEG_1].c = Vector3f(0, 0, 0); uLINK[JID_RLEG_1].b = Vector3f(Hip1RelTorso_X, Hip1RelTorso_Y, Hip1RelTorso_Z) - uLINK[JID_RLEG_1].c + uLINK[JID_ROOT].c; uLINK[JID_RLEG_1].q = 0; uLINK[JID_RLEG_2].name = "rlj2"; uLINK[JID_RLEG_2].eff_name = "rle2"; uLINK[JID_RLEG_2].sister = 0; uLINK[JID_RLEG_2].child = JID_RLEG_3; uLINK[JID_RLEG_2].mother = JID_RLEG_1; uLINK[JID_RLEG_2].a = Vector3f(0, 1, 0); // Vector3f(1, 0, 0) uLINK[JID_RLEG_2].c = Vector3f(0, 0, 0); uLINK[JID_RLEG_2].b = Vector3f(Hip2RelHip1_X, Hip2RelHip1_Y, Hip2RelHip1_Z) - uLINK[JID_RLEG_2].c + uLINK[JID_RLEG_1].c; uLINK[JID_RLEG_2].q = 0; uLINK[JID_RLEG_3].name = "rlj3"; uLINK[JID_RLEG_3].eff_name = "rle3"; uLINK[JID_RLEG_3].sister = 0; uLINK[JID_RLEG_3].child = JID_RLEG_4; uLINK[JID_RLEG_3].mother = JID_RLEG_2; uLINK[JID_RLEG_3].a = Vector3f(1, 0, 0); // Vector3f(0, 1, 0) uLINK[JID_RLEG_3].c = Vector3f(ThighRelHip2_X, ThighRelHip2_Y, ThighRelHip2_Z); uLINK[J