Robotsystem from Robot Group Leipzig: component.cpp Source File

- Main Page
- Modules
- Namespaces
- Classes
- Files
- Directories
- Examples

- File List
- File Members

# component.cpp

Go to the documentation of this file.

    00001 /***************************************************************************
    00002  *   Copyright (C) 2005 by Robot Group Leipzig                             *
    00003  *    martius@informatik.uni-leipzig.de                                    *
    00004  *    fhesse@informatik.uni-leipzig.de                                     *
    00005  *    der@informatik.uni-leipzig.de                                        *
    00006  *                                                                         *
    00007  *   This program is free software; you can redistribute it and/or modify  *
    00008  *   it under the terms of the GNU General Public License as published by  *
    00009  *   the Free Software Foundation; either version 2 of the License, or     *
    00010  *   (at your option) any later version.                                   *
    00011  *                                                                         *
    00012  *   This program is distributed in the hope that it will be useful,       *
    00013  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
    00014  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
    00015  *   GNU General Public License for more details.                          *
    00016  *                                                                         *
    00017  *   You should have received a copy of the GNU General Public License     *
    00018  *   along with this program; if not, write to the                         *
    00019  *   Free Software Foundation, Inc.,                                       *
    00020  *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
    00021  *                                                                         *
    00022  *   $Log: component.cpp,v $
    00023  *   Revision 1.7  2005/11/14 12:48:31  martius
    00024  *   *** empty log message ***
    00025  *
    00026  *   Revision 1.6  2005/11/09 13:27:32  fhesse
    00027  *   GPL added
    00028  *                                                                *
    00029  ***************************************************************************/
    00030
    00031 #include "component.h"
    00032 #include "drawgeom.h"
    00033
    00034 namespace university_of_leipzig {
    00035 namespace robots {
    00036
    00037   OdeHandle temp(NULL, NULL, NULL);
    00038   //  dJointGroupID joint_group_id = dJointGroupCreate(0);
    00039
    00040 /*****************************************************************************/
    00041 /* AbstractComponent                                                         */
    00042 /*****************************************************************************/
    00043
    00044 AbstractComponent::AbstractComponent(OdeHandle &r_ode_handle) :
    00045   ode_handle(r_ode_handle)
    00046 {
    00047 }
    00048
    00049
    00050 AbstractComponent::~AbstractComponent()
    00051 {
    00052 }
    00053
    00054
    00055 unsigned AbstractComponent::get_sub_component_count() const
    00056 {
    00057   return 0;
    00058 }
    00059
    00060
    00061 IComponent& AbstractComponent::get_sub_component(unsigned index) const
    00062 {
    00063   IndexOutOfBoundsException().raise();
    00064
    00065   OdeHandle oh(NULL, NULL, NULL);
    00066   return *(new SimplePhysicalComponent(oh, NULL, NULL));
    00067 }
    00068
    00069
    00070 unsigned AbstractComponent::
    00071 expose_wires(WireContainer &out_r_wire_container)
    00072 {
    00073   return 0;
    00074 }
    00075
    00076
    00077 const IComponent* AbstractComponent::
    00078 does_contain_geom(const dGeomID geom_id, bool b_recursive) const
    00079 {
    00080   return NULL;
    00081 }
    00082
    00083
    00084 Configurable::paramkey AbstractComponent::getName() const
    00085 {
    00086   return static_cast<paramkey>("SomeComponent");
    00087 }
    00088
    00089
    00090 Configurable::paramlist AbstractComponent::getParamList() const
    00091 {
    00092   paramlist list;
    00093
    00094   return list;
    00095 }
    00096
    00097
    00098 Configurable::paramval AbstractComponent::
    00099 getParam(const paramkey& key) const
    00100 {
    00101   return Configurable::getParam(key) ;
    00102 }
    00103
    00104
    00105 bool AbstractComponent::setParam(const paramkey& key, paramval val){
    00106   return Configurable::setParam(key, val);
    00107 }
    00108
    00109
    00110 /*****************************************************************************/
    00111 /* AbstractCompoundComponent                                                 */
    00112 /*****************************************************************************/
    00113
    00114 AbstractCompoundComponent::
    00115 AbstractCompoundComponent(OdeHandle &r_ode_handle) :
    00116   AbstractComponent(r_ode_handle)
    00117 {
    00118 }
    00119
    00120
    00121 AbstractCompoundComponent::~AbstractCompoundComponent()
    00122 {
    00123 }
    00124
    00125
    00126 unsigned AbstractCompoundComponent::get_sub_component_count() const
    00127 {
    00128   return component_container.size();
    00129 }
    00130
    00131
    00132 IComponent &AbstractCompoundComponent::get_sub_component(unsigned index) const
    00133 {
    00134   if(index >= component_container.size())
    00135     IndexOutOfBoundsException().raise();
    00136
    00137   ComponentContainer::const_iterator it = component_container.begin();
    00138   for(unsigned i = 0; i < index; ++i)
    00139     ++it;
    00140
    00141   return *(*it);;
    00142 }
    00143
    00144
    00145 unsigned AbstractCompoundComponent::
    00146 expose_wires(WireContainer &out_r_wire_container)
    00147 {
    00148   unsigned retval = 0; //out_r_wire_container.size();
    00149
    00150   ComponentContainer::const_iterator it = component_container.begin();
    00151
    00152   while(component_container.end() != it)
    00153     retval += (*(it++))->expose_wires(out_r_wire_container);
    00154
    00155   return retval;
    00156 }
    00157
    00158
    00159 const IComponent* AbstractCompoundComponent::
    00160 does_contain_geom(dGeomID geom_id, bool b_recursive) const
    00161 {
    00162   const IComponent *p;
    00163   ComponentContainer::const_iterator it = component_container.begin();
    00164
    00165   while(component_container.end() != it) {
    00166     p = (*(it++))->does_contain_geom(geom_id, b_recursive);
    00167
    00168     if(NULL != p)
    00169       return p;
    00170   }
    00171
    00172
    00173   return NULL;
    00174 }
    00175
    00176
    00177 void AbstractCompoundComponent::draw() const
    00178 {
    00179   // for all sub components: draw()
    00180   ComponentContainer::const_iterator it = component_container.begin();
    00181   while(component_container.end() != it) {
    00182     (*it)->draw();
    00183     ++it;
    00184   }
    00185 }
    00186
    00187
    00188 Configurable::paramlist AbstractCompoundComponent::getParamList() const
    00189 {
    00190   paramlist list;
    00191
    00192   for(ComponentContainer::const_iterator it = component_container.begin();
    00193       it != component_container.end();
    00194       ++it) {
    00195     list += (*it)->getParamList();
    00196   }
    00197
    00198   return list;
    00199 }
    00200
    00201
    00202 Configurable::paramval AbstractCompoundComponent::
    00203 getParam(const paramkey& key) const
    00204 {
    00205   paramval val = 0.0;
    00206   ComponentContainer::const_iterator it = component_container.begin();
    00207
    00208   while(it != component_container.end() && val == 0.0) {
    00209     val = (*it)->getParam(key);
    00210     ++it;
    00211   }
    00212
    00213   return val;
    00214 }
    00215
    00216
    00217 bool AbstractCompoundComponent::setParam(const paramkey& key, paramval val)
    00218 {
    00219   for(ComponentContainer::iterator it = component_container.begin();
    00220       it != component_container.end();
    00221       ++it)
    00222     (*it)->setParam(key, val);
    00223
    00224   return true;
    00225 }
    00226
    00227
    00228
    00229 /*****************************************************************************/
    00230 /* AbstractMotorComponent                                                    */
    00231 /*****************************************************************************/
    00232 AbstractMotorComponent::AbstractMotorComponent(dJointID _joint_id) :
    00233   AbstractComponent(temp),
    00234   joint_id(_joint_id)
    00235 {
    00236 }
    00237
    00238
    00239
    00240 /*****************************************************************************/
    00241 /* UniversalMotorComponent                                                   */
    00242 /*****************************************************************************/
    00243 UniversalMotorComponent::
    00244 UniversalMotorComponent(dJointID _joint_id, char _axis) :
    00245   AbstractMotorComponent(_joint_id),
    00246
    00247   axis(_axis),
    00248   wire(this)
    00249 {
    00250   if(dJointTypeUniversal != dJointGetType(joint_id))
    00251     InvalidArgumentException().raise();
    00252
    00253   if(axis != 0 && axis != 1)
    00254     InvalidArgumentException().raise();
    00255
    00256   param_show_axis = 1.0;
    00257 }
    00258
    00259
    00260 IComponent::paramlist UniversalMotorComponent::getParamList() const
    00261 {
    00262   paramlist list;
    00263   list.push_back(pair<paramkey, paramval> (string("show_axis"),
    00264                                            param_show_axis));
    00265
    00266   return list;
    00267 }
    00268
    00269
    00270 Configurable::paramval UniversalMotorComponent::
    00271 getParam(const paramkey& key) const
    00272 {
    00273   if(key == "show_axis")
    00274     return param_show_axis;
    00275   else
    00276     return Configurable::getParam(key) ;
    00277 }
    00278
    00279
    00280 bool UniversalMotorComponent::setParam(const paramkey& key, paramval val){
    00281   if(key == "show_axis")
    00282     param_show_axis = val;
    00283   else
    00284     return Configurable::setParam(key, val);
    00285
    00286   return true;
    00287 }
    00288
    00289
    00290
    00291 void UniversalMotorComponent::
    00292 set_angular_velocity(dReal angular_velocity)
    00293 {
    00294   //  std::cout << angular_velocity << "\n";
    00295   if(0 == axis) {
    00296     dJointSetUniversalParam (joint_id ,dParamVel,  angular_velocity * 10);
    00297     dJointSetUniversalParam (joint_id, dParamFMax, 2.5);
    00298   }
    00299   else if(1 == axis) {
    00300     dJointSetUniversalParam (joint_id ,dParamVel2, angular_velocity * 10);
    00301     dJointSetUniversalParam (joint_id, dParamFMax2, 2.5);
    00302   }
    00303   else
    00304     std::cerr << "shouldnt happen\n";
    00305 }
    00306
    00307
    00308 dReal UniversalMotorComponent::
    00309 get_angular_velocity() const
    00310 {
    00311   if(0 == axis)
    00312     return dJointGetUniversalAngle1(joint_id);
    00313   else if(1 == axis)
    00314     return dJointGetUniversalAngle2(joint_id);
    00315   else
    00316     std::cerr << "shouldnt happen\n";
    00317
    00318   return static_cast<dReal>(0);
    00319 }
    00320
    00321
    00322 unsigned UniversalMotorComponent::
    00323 expose_wires(WireContainer &out_r_wire_container)
    00324 {
    00325   //d  unsigned retval = out_r_wire_container.size();
    00326
    00327   out_r_wire_container.insert(out_r_wire_container.end(), &wire);
    00328   //out_r_wire_container.insert(&wire, out_r_wire_container.end());
    00329   return 1;
    00330 }
    00331
    00332
    00333 void UniversalMotorComponent::draw() const
    00334 {
    00335   // draws the axis...
    00336   dVector3 v3_anchor;
    00337   //dVector3 v3_anchor2;
    00338
    00339   dVector3 v3_direction;
    00340
    00341   float a_point_0[3];
    00342   float a_point_1[3];
    00343
    00344   dJointGetUniversalAnchor(joint_id, v3_anchor);
    00345
    00346   // dJointGetUniversalAnchor2(joint_id, v3_anchor2);
    00347
    00348
    00349
    00350   if(param_show_axis >= 0.5) {
    00351     if(0 == axis) {
    00352       dJointGetUniversalAxis1 (joint_id, v3_direction);
    00353
    00354       for(unsigned i = 0; i < 3; ++i) {
    00355         a_point_0[i] = v3_anchor[i] - v3_direction[i] / 2.0;
    00356         a_point_1[i] = v3_anchor[i] + v3_direction[i] / 2.0;
    00357       }
    00358
    00359       dsSetColor(1.0, 0.0, 0.0);
    00360       dsDrawLine(a_point_0, a_point_1);
    00361       /*
    00362       for(unsigned j = 0; j < 3; ++j) {
    00363         a_point_0[j] = v3_anchor2[j] - v3_direction[j] / 2.0;
    00364         a_point_1[j] = v3_anchor2[j] + v3_direction[j] / 2.0;
    00365       }
    00366
    00367       dsSetColor(1.0, 0.0, 0.0);
    00368       dsDrawLine(a_point_0, a_point_1);
    00369       */
    00370     }
    00371     else { // ( 1 == axis)
    00372
    00373       dJointGetUniversalAxis2  (joint_id, v3_direction);
    00374
    00375       for(unsigned i = 0; i < 3; ++i) {
    00376         a_point_0[i] = v3_anchor[i] - v3_direction[i] / 2.0;
    00377         a_point_1[i] = v3_anchor[i] + v3_direction[i] / 2.0;
    00378       }
    00379
    00380       dsSetColor(0.0, 0.0, 1.0);
    00381       dsDrawLine(a_point_0, a_point_1);
    00382
    00383
    00384       /*
    00385       for(unsigned j = 0; j < 3; ++j) {
    00386         a_point_0[j] = v3_anchor2[j] - v3_direction[j] / 2.0;
    00387         a_point_1[j] = v3_anchor2[j] + v3_direction[j] / 2.0;
    00388       }
    00389
    00390       dsSetColor(1.0, 0.0, 0.0);
    00391       dsDrawLine(a_point_0, a_point_1);
    00392       */
    00393
    00394     }
    00395   }
    00396   dsSetColor(1.0, 1.0, 1.0);
    00397 }
    00398
    00399 /*
    00400 const IComponent* UniversalMotorComponent::
    00401 does_contain_geom(const dGeomID geom_id, bool b_recursive) const
    00402 {
    00403   return NULL;
    00404 }
    00405 */
    00406
    00407 bool UniversalMotorComponent::
    00408 collision_callback(OdeHandle *p_ode_handle, dGeomID geom_id_0, dGeomID geom_id_1) const
    00409 {
    00410   return false;
    00411 }
    00412
    00413 /*
    00414 unsigned UniversalMotorComponent::get_sub_component_count() const
    00415 {
    00416   return 0;
    00417 }
    00418
    00419
    00420 IComponent& UniversalMotorComponent::get_sub_component(unsigned index) const
    00421 {
    00422   IndexOutOfBoundsException().raise();
    00423
    00424   return *(new SimplePhysicalComponent());
    00425 }
    00426 */
    00427
    00428 /*****************************************************************************/
    00429 /* UniversalMotorWire                                                        */
    00430 /*****************************************************************************/
    00431 MotorWire::MotorWire(AbstractMotorComponent *_p_motor) :
    00432   p_motor(_p_motor)
    00433 {
    00434   if(NULL == p_motor)
    00435     InvalidArgumentException().raise();
    00436 }
    00437
    00438
    00439 IComponent& MotorWire::get_component() const
    00440 {
    00441   return *p_motor;
    00442 }
    00443
    00444
    00445 dReal MotorWire::get() const
    00446 {
    00447   return p_motor->get_angular_velocity();
    00448 }
    00449
    00450
    00451 void MotorWire::put(dReal value)
    00452 {
    00453   p_motor->set_angular_velocity(value);
    00454 }
    00455
    00456
    00457 /*****************************************************************************/
    00458 /* SimplePhysicalComponent                                                   */
    00459 /*****************************************************************************/
    00460 SimplePhysicalComponent::
    00461 SimplePhysicalComponent(OdeHandle &r_ode_handle,
    00462                         dBodyID _body_id,
    00463                         dGeomID _geom_id) :
    00464   AbstractComponent(r_ode_handle),
    00465   body_id(_body_id),
    00466   geom_id(_geom_id)
    00467 {
    00468 }
    00469
    00470
    00471 void SimplePhysicalComponent::set_body_id(dBodyID _body_id)
    00472 {
    00473   body_id = _body_id;
    00474 }
    00475
    00476
    00477 dBodyID SimplePhysicalComponent::get_body_id() const
    00478 {
    00479   return body_id;
    00480 }
    00481
    00482
    00483 void SimplePhysicalComponent::set_geom_id(dGeomID _geom_id)
    00484 {
    00485   geom_id = _geom_id;
    00486 }
    00487
    00488
    00489 dGeomID SimplePhysicalComponent::get_geom_id() const
    00490 {
    00491   return geom_id;
    00492 }
    00493
    00494 /*
    00495 unsigned SimplePhysicalComponent::
    00496 expose_wires(WireContainer &out_r_wire_container)
    00497 {
    00498   return 0;
    00499 }
    00500
    00501
    00502 unsigned SimplePhysicalComponent::get_sub_component_count() const
    00503 {
    00504   return 0;
    00505 }
    00506
    00507
    00508 IComponent& SimplePhysicalComponent::
    00509 get_sub_component(unsigned index) const
    00510 {
    00511   IndexOutOfBoundsException().raise();
    00512
    00513   return *(new SimplePhysicalComponent());
    00514 }
    00515 */
    00516
    00517 // <TODO> this is not correct right now
    00518 // the snake component currently does all collision handling
    00519 // even though those simple components might define their own collission
    00520 // handling routine too
    00521 bool SimplePhysicalComponent::
    00522 collision_callback(OdeHandle *p_ode_handle, dGeomID geom_id_0, dGeomID geom_id_1) const
    00523 {
    00524   return false;
    00525   if(!(does_contain_geom(geom_id_0, true) ||
    00526        does_contain_geom(geom_id_1, true)))
    00527     return false;
    00528
    00529   // ok, we have a collision here - create the contact joints
    00530   const unsigned max_contact_count = 10;
    00531   unsigned n;
    00532   dContact a_contact[max_contact_count];
    00533
    00534   n = dCollide(geom_id_0,
    00535                geom_id_1,
    00536                max_contact_count,
    00537                &a_contact->geom,
    00538                sizeof(dContact));
    00539
    00540   for (unsigned i = 0; i < n; ++i) {
    00541     a_contact[i].surface.mode = dContactSlip1   | dContactSlip2   |
    00542       dContactSoftERP | dContactSoftCFM |
    00543       dContactApprox1;
    00544     a_contact[i].surface.mu       = 0.8; //conf.frictionGround;
    00545     a_contact[i].surface.slip1    = 0.005;
    00546     a_contact[i].surface.slip2    = 0.005;
    00547     a_contact[i].surface.soft_erp = 0.9;
    00548     a_contact[i].surface.soft_cfm = 0.001;
    00549
    00550     dJointID joint_id =
    00551       dJointCreateContact(ode_handle.world,
    00552                           ode_handle.jointGroup,
    00553                           &a_contact[i]);
    00554
    00555     dJointAttach(joint_id ,
    00556                  dGeomGetBody(a_contact[i].geom.g1),
    00557                  dGeomGetBody(a_contact[i].geom.g2));
    00558   }
    00559   return true;
    00560 }
    00561
    00562
    00563 const IComponent* SimplePhysicalComponent::
    00564 does_contain_geom(const dGeomID _geom_id, bool b_recursive) const
    00565 {
    00566   if(_geom_id == geom_id)
    00567     return this;
    00568   else return NULL;
    00569 }
    00570
    00571
    00572 void SimplePhysicalComponent::draw() const
    00573 {
    00574   drawGeom(geom_id, NULL, NULL);
    00575   return;
    00576
    00577   if(dCCylinderClass == dGeomGetClass(geom_id)) {
    00578     dReal radius;
    00579     dReal length;
    00580
    00581     dGeomCCylinderGetParams(geom_id, &radius, &length);
    00582
    00583     dsDrawCappedCylinder(dGeomGetPosition(geom_id),
    00584                          dGeomGetRotation(geom_id),
    00585                          length,
    00586                          radius);
    00587
    00588     /*    std::cout << length << "\n";
    00589     std::cout << radius << "\n";
    00590     std::cout << "---------------------\n";*/
    00591
    00592   }
    00593   else if(dBoxClass == dGeomGetClass(geom_id)) {
    00594     dVector3 v3_length;
    00595     dGeomBoxGetLengths(geom_id, v3_length);
    00596     /*
    00597     std::cout << "x = " << v3_length[0] << "\n";
    00598     std::cout << "y = " << v3_length[1] << "\n";
    00599     std::cout << "z = " << v3_length[2] << "\n";
    00600     */
    00601     dsDrawBox(dGeomGetPosition(geom_id),
    00602               dGeomGetRotation(geom_id),
    00603               v3_length);
    00604   }
    00605   else if(dPlaneClass == dGeomGetClass(geom_id)) {
    00606     /*    dVector4 v4_params;
    00607     dGeomPlaneGetParams(geom_id, v4_params);
    00608
    00609     dsDrawPlane(dGeomGetPosition(geom_id),
    00610                 dGeomGetRotation(geom_id),
    00611                 v4[4]);    */
    00612     // can't draw - there is no dsDrawPlane
    00613   }
    00614   else if(dSphereClass == dGeomGetClass(geom_id)) {
    00615     dsDrawSphere(dGeomGetPosition(geom_id),
    00616                  dGeomGetRotation(geom_id),
    00617                  dGeomSphereGetRadius(geom_id));
    00618   }
    00619   else {
    00620
    00621     std::cerr << "unknown geom type - cannot draw :(\n";
    00622     //exception::
    00623   }
    00624
    00625
    00626 }
    00627
    00628
    00629
    00630 /*****************************************************************************/
    00631 /* RobotArmComponent                                                         */
    00632 /*****************************************************************************/
    00633 /*
    00634 double atan_ex(double x, double y)
    00635 {
    00636   if(0.0 == y)
    00637     y = 0.00001;
    00638   //return atan(x / y);
    00639
    00640   if(0.0 <= x)  // 1, 4
    00641     return atan(x / y);
    00642   else {
    00643     if(0.0 <= y)
    00644       return PI + atan(x / y);
    00645     else
    00646       return -PI + atan(x / y);
    00647    }
    00648 }
    00649 */
    00650
    00651 /**
    00652  * atan_ex
    00653  *
    00654  * utility function, not used here - should be moved to another file
    00655  *
    00656  */
    00657 double atan_ex(double x, double y)
    00658 {
    00659
    00660   if(0.0 == y)
    00661     y = 0.00001; // replace by some epsilon constant
    00662
    00663   if(0.0 <= x) {  // 1, 4
    00664     if(0.0 <= y)
    00665       return atan(x / y);
    00666     else
    00667       return 2 * M_PI + atan(x / y);
    00668   }
    00669   else
    00670     return M_PI + atan(x / y);
    00671 }
    00672
    00673
    00674 double quat_mag(const dQuaternion q)
    00675 {
    00676   double square_mag = 0.0;
    00677   for(int i = 0; i < 4; ++i)
    00678     square_mag += q[i] * q[i];
    00679
    00680   return sqrt(square_mag);
    00681 }
    00682
    00683
    00684 void quat_norm(dQuaternion q)
    00685 {
    00686   double mag = quat_mag(q);
    00687
    00688   for(int i = 0; i < 4; ++i)
    00689     q[i] /= mag;
    00690 }
    00691
    00692
    00693
    00694 /**
    00695  * conjugate_quaternion
    00696  *
    00697  *
    00698  */
    00699 void quat_conj(dQuaternion q)
    00700 {
    00701   for(int i = 0; i < 3; ++i)
    00702     q[i] = -q[i];
    00703 }
    00704
    00705
    00706 void quat_inv(dQuaternion q)
    00707 {
    00708   dQuaternion tmp;
    00709
    00710   memcpy(tmp ,q, sizeof(dQuaternion));
    00711   quat_norm(tmp);
    00712   quat_conj(tmp);
    00713
    00714   memcpy(q ,tmp, sizeof(dQuaternion));
    00715 }
    00716
    00717
    00718 void qv_mult(dVector3 v3_result, dQuaternion q, const dVector3 v3)
    00719 {
    00720   dQuaternion inv_q;
    00721
    00722   memcpy(inv_q, q, sizeof(dQuaternion));
    00723
    00724   quat_inv(inv_q);
    00725
    00726   for(int i = 0; i < 4; ++i)
    00727     v3_result[i] = q[i] * v3[i] * inv_q[i];
    00728 }
    00729
    00730
    00731
    00732 /**
    00733  * multiply
    00734  *
    00735  * is there some ode-multiply routine???!!!
    00736  */
    00737 void xxx_multiply(dVector3 v3_result, const dMatrix3 m33, const dVector3 v3)
    00738 {
    00739   //  if(v3_result == v3)
    00740
    00741
    00742   v3_result[0] = m33[0 * 3 + 0] * v3[0] +
    00743                  m33[0 * 3 + 1] * v3[1] +
    00744                  m33[0 * 3 + 2] * v3[2];
    00745
    00746   v3_result[1] = m33[1 * 3 + 0] * v3[0] +
    00747                  m33[1 * 3 + 1] * v3[1] +
    00748                  m33[1 * 3 + 2] * v3[2];
    00749
    00750   v3_result[2] = m33[2 * 3 + 0] * v3[0] +
    00751                  m33[2 * 3 + 1] * v3[1] +
    00752                  m33[2 * 3 + 2] * v3[2];
    00753 }
    00754
    00755 void yyy_multiply(dVector3 v3_result, const dMatrix3 m33, const dVector3 v3)
    00756 {
    00757   //  if(v3_result == v3)
    00758
    00759
    00760   v3_result[0] = m33[0 * 4 + 0] * v3[0] +
    00761                  m33[1 * 4 + 0] * v3[1] +
    00762                  m33[2 * 4 + 0] * v3[2];
    00763     //m33[3 * 3 + 0];
    00764
    00765   v3_result[1] = m33[0 * 4 + 1] * v3[0] +
    00766                  m33[1 * 4 + 1] * v3[1] +
    00767                  m33[2 * 4 + 1] * v3[2];
    00768     // m33[3 * 3 + 1];
    00769
    00770   v3_result[2] = m33[0 * 4 + 2] * v3[0] +
    00771                  m33[1 * 4 + 2] * v3[1] +
    00772                  m33[2 * 4 + 2] * v3[2];
    00773     // m33[3 * 3 + 2];
    00774 }
    00775
    00776 /**
    00777  *
    00778  *
    00779  * this class class has following sub components:
    00780  * UniversalMotorComponent
    00781  * SimpleBodyComponent
    00782  */
    00783 CCURobotArmComponent::CCURobotArmComponent(const RobotArmDescription &r_desc) :
    00784   // ode_handle(*(r_desc.p_ode_handle))
    00785   AbstractCompoundComponent(*r_desc.p_ode_handle)
    00786 {
    00787   if(NULL == r_desc.p_ode_handle)
    00788     InvalidArgumentException().raise();
    00789
    00790   if(NULL == r_desc.p_vertex_list)
    00791     InvalidArgumentException().raise();
    00792
    00793   if(1 >= r_desc.p_vertex_list->size())
    00794     InvalidArgumentException().raise();
    00795
    00796   //  ode_handle = *r_desc.p_ode_handle;
    00797
    00798   joint_group_id = dJointGroupCreate(0);
    00799
    00800   dMass mass;
    00801
    00802
    00803   VertexList::const_iterator it_vertex_previous =
    00804     r_desc.p_vertex_list->begin();
    00805
    00806   VertexList::const_iterator it_vertex_current =
    00807     ++r_desc.p_vertex_list->begin();
    00808
    00809
    00810   dBodyID body_id_previous = NULL;
    00811   dBodyID body_id_current  = NULL;
    00812
    00813   dGeomID geom_id_current;
    00814   Vector3<dReal> v3_delta;
    00815
    00816   // rotation stuff
    00817   Vector3<dReal> v3_rotation_axis;
    00818
    00819   dQuaternion a_q_rotation[2];
    00820
    00821   dQuaternion *p_q_rotation_previous = &a_q_rotation[0];
    00822   dQuaternion *p_q_rotation_current  = &a_q_rotation[1];
    00823
    00824   std::cout << "size = " << r_desc.p_vertex_list->size() << "\n";
    00825   while(r_desc.p_vertex_list->end() != it_vertex_current) {
    00826
    00827     std::cout << "ANOTHER LOOP\n";
    00828     v3_delta = *it_vertex_current - *it_vertex_previous;
    00829
    00830
    00831
    00832     body_id_current = dBodyCreate(ode_handle.world);
    00833
    00834
    00835     /*
    00836     dMassSetCappedCylinderTotal(&mass,
    00837                                 r_desc.segment_mass,
    00838                                 2,
    00839                                 r_desc.segment_radius,
    00840                                 v3_delta.length());
    00841
    00842     */
    00843
    00844     dMassSetCappedCylinder(&mass,
    00845                            1,
    00846                            1,
    00847                            r_desc.segment_radius,
    00848                            v3_delta.length());
    00849
    00850     dMassAdjust(&mass, r_desc.segment_mass);
    00851
    00852     dBodySetMass(body_id_current, &mass);
    00853
    00854
    00855     geom_id_current = dCreateCCylinder(ode_handle.space,
    00856                                        r_desc.segment_radius,
    00857                                        v3_delta.length());
    00858
    00859
    00860     // rotation_axis = angle symmetry axis
    00861     v3_rotation_axis = Vector3<dReal>(0.0, 0.0, 1.0) +
    00862                        (v3_delta.get_unit_vector() -
    00863                         Vector3<dReal>(0.0, 0.0, 1.0)) / 2.0;
    00864
    00865     v3_rotation_axis.make_unit_length();
    00866
    00867
    00868     dQFromAxisAndAngle(*p_q_rotation_current,
    00869                        v3_rotation_axis.x,
    00870                        v3_rotation_axis.y,
    00871                        v3_rotation_axis.z,
    00872                        M_PI);
    00873
    00874     dGeomSetBody(geom_id_current, body_id_current);
    00875
    00876     dBodySetPosition(body_id_current,
    00877     r_desc.v3_position.x + (it_vertex_previous->x + it_vertex_current->x)/2.0,
    00878     r_desc.v3_position.y + (it_vertex_previous->y + it_vertex_current->y)/2.0,
    00879     r_desc.v3_position.z + (it_vertex_previous->z + it_vertex_current->z)/2.0);
    00880
    00881     dBodySetQuaternion(body_id_current, *p_q_rotation_current);
    00882
    00883
    00884     // the position of the body is exactly between 2 control points
    00885     //dBodySetPosition(geom_id_current,
    00886
    00887
    00888     component_container.insert(component_container.end(),
    00889                                new SimplePhysicalComponent(ode_handle,
    00890                                                            body_id_current,
    00891                                                            geom_id_current));
    00892
    00893
    00894
    00895     if(it_vertex_previous != r_desc.p_vertex_list->begin()) {
    00896       std::cout << "sdfsdfhdsovbgfdshzvfbsd\n";
    00897       dJointID joint_id_current;
    00898       // set up the joints
    00899       // since i does not equal zero, new and old rotation matrices are valid
    00900
    00901
    00902       joint_id_current = dJointCreateUniversal(ode_handle.world,
    00903                                                0);
    00904
    00905       dJointAttach(joint_id_current, body_id_previous, body_id_current);
    00906
    00907       // set the anchor
    00908       dJointSetUniversalAnchor(joint_id_current,
    00909                                r_desc.v3_position.x + it_vertex_previous->x,
    00910                                r_desc.v3_position.x + it_vertex_previous->y,
    00911                                r_desc.v3_position.x + it_vertex_previous->z);
    00912
    00913       // so... lets try to rotate the hinge axis...
    00914
    00915       // set axis 1 (belongs to the previous segment)
    00916       {
    00917         dVector3 a_point_0 = {-0.5, 0.0, v3_delta.length() / 2.0, 0.0};
    00918         dVector3 a_point_1 = { 0.5, 0.0, v3_delta.length() / 2.0, 0.0};
    00919         dVector3 a_rotated_point_0;
    00920         dVector3 a_rotated_point_1;
    00921         dVector3 a_rotated_axis;
    00922         dMatrix3 a_m;
    00923
    00924
    00925         dQtoR(*p_q_rotation_previous, a_m);
    00926
    00927         yyy_multiply(a_rotated_point_0, a_m, a_point_0);
    00928         yyy_multiply(a_rotated_point_1, a_m, a_point_1);
    00929
    00930
    00931
    00932         for(int i = 0; i < 3; ++i)
    00933           a_rotated_axis[i] = a_rotated_point_1[i] - a_rotated_point_0[i];
    00934
    00935
    00936         dJointSetUniversalAxis1(joint_id_current,
    00937                                 a_rotated_axis[0],
    00938                                 a_rotated_axis[1],
    00939                                 a_rotated_axis[2]);
    00940
    00941         component_container.
    00942           insert(component_container.end(),
    00943                  new UniversalMotorComponent(joint_id_current,
    00944                                              0));
    00945       }
    00946
    00947
    00948       // set axis 2 (belongs to the current segment)
    00949       {
    00950         dVector3 a_point_0 = {0.0, -0.5, v3_delta.length() / 2.0, 0.0};
    00951         dVector3 a_point_1 = {0.0,  0.5, v3_delta.length() / 2.0, 0.0};
    00952         dVector3 a_rotated_point_0;
    00953         dVector3 a_rotated_point_1;
    00954         dVector3 a_rotated_axis;
    00955         dMatrix3 a_m;
    00956
    00957
    00958         dQtoR(*p_q_rotation_current, a_m);
    00959
    00960         yyy_multiply(a_rotated_point_0, a_m, a_point_0);
    00961         yyy_multiply(a_rotated_point_1, a_m, a_point_1);
    00962
    00963         for(int i = 0; i < 3; ++i)
    00964           a_rotated_axis[i] = a_rotated_point_1[i] - a_rotated_point_0[i];
    00965
    00966         dJointSetUniversalAxis2(joint_id_current,
    00967                                 a_rotated_axis[0],
    00968                                 a_rotated_axis[1],
    00969                                 a_rotated_axis[2]);
    00970
    00971         component_container.
    00972           insert(component_container.end(),
    00973                  new UniversalMotorComponent(joint_id_current,
    00974                                              1));
    00975       }
    00976     }
    00977
    00978     std::swap(p_q_rotation_previous, p_q_rotation_current);
    00979     std::swap(body_id_previous     , body_id_current);
    00980
    00981     ++it_vertex_previous;
    00982     ++it_vertex_current;
    00983   }
    00984
    00985 }
    00986
    00987
    00988
    00989 CCURobotArmComponent::~CCURobotArmComponent()
    00990 {
    00991   // delete all components
    00992   ComponentContainer::iterator it = component_container.begin();
    00993   while(component_container.end() != it) {
    00994     delete *it;
    00995     ++it;
    00996   }
    00997
    00998   dJointGroupDestroy(joint_group_id);
    00999 }
    01000
    01001
    01002 void CCURobotArmComponent::draw() const
    01003 {
    01004   // for all sub components: draw()
    01005   ComponentContainer::const_iterator it = component_container.begin();
    01006   while(component_container.end() != it) {
    01007     (*it)->draw();
    01008     ++it;
    01009   }
    01010 }
    01011
    01012 /*
    01013 unsigned CCURobotArmComponent::
    01014 expose_wires(WireContainer &out_r_wire_container)
    01015 {
    01016   unsigned retval = out_r_wire_container.size();
    01017
    01018   ComponentContainer::const_iterator it = component_container.begin();
    01019
    01020   while(component_container.end() != it)
    01021     (*(it++))->expose_wires(out_r_wire_container);
    01022
    01023   return retval;
    01024 }
    01025 */
    01026
    01027 const IComponent* CCURobotArmComponent::
    01028 does_contain_geom(dGeomID geom_id, bool b_recursive) const
    01029 {
    01030   const IComponent *p;
    01031   ComponentContainer::const_iterator it = component_container.begin();
    01032
    01033   while(component_container.end() != it) {
    01034     p = (*(it++))->does_contain_geom(geom_id, b_recursive);
    01035
    01036     if(NULL != p)
    01037       return p;
    01038   }
    01039
    01040
    01041   return NULL;
    01042 }
    01043
    01044
    01045 /**
    01046  * collision_callback
    01047  *
    01048  * this function only does collision handling for geoms part of this object
    01049  *
    01050  * hmm.. there should be a mechanism for transferring collision handling
    01051  * to sub components
    01052  */
    01053 bool CCURobotArmComponent::collision_callback(OdeHandle *p_ode_handle,
    01054                                              dGeomID geom_id_0,
    01055                                              dGeomID geom_id_1) const
    01056 {
    01057   // check if at least one of the two geoms is part of this component
    01058   const IComponent *p_component_0 = does_contain_geom(geom_id_0, true);
    01059   const IComponent *p_component_1 = does_contain_geom(geom_id_1, true);
    01060
    01061   // if && is used instead: both geoms must be part of this component
    01062   if(NULL == p_component_0 || NULL == p_component_1)
    01063     return false;
    01064
    01065   // at least one geom is part of one of this component's (sub) components
    01066
    01067   // check if it is a collision between two adjacing segments
    01068   // these collisions will be ignored
    01069
    01070
    01071   dBodyID body_id_0 = dGeomGetBody(geom_id_0);
    01072   dBodyID body_id_1 = dGeomGetBody(geom_id_1);
    01073
    01074
    01075   if(NULL != body_id_0 && NULL != body_id_1) {
    01076     // if those bodies are connected: no collision
    01077     if(dAreConnected(body_id_0, body_id_1))
    01078       return true;
    01079   }
    01080   return false;
    01081
    01082   /*
    01083   JointIdList::iterator it = lst_joint_id.begin();
    01084   while(lst_joint_id.end() != it) {
    01085     if((dJointGetBody(*it, 0) == body_id_0 &&
    01086       dJointGetBody(*it, 1) == body_id_1) ||
    01087       (dJointGetBody(*it, 1) == body_id_0 &&
    01088       dJointGetBody(*it, 0) == body_id_1))
    01089     return false;
    01090     }
    01091   */
    01092
    01093   // ok, we have a collision here - create the contact joints
    01094   const unsigned max_contact_count = 10;
    01095   unsigned n;
    01096   dContact a_contact[max_contact_count];
    01097
    01098   n = dCollide(geom_id_0,
    01099                geom_id_1,
    01100                max_contact_count,
    01101                &a_contact->geom,
    01102                sizeof(dContact));
    01103
    01104   for (unsigned i = 0; i < n; ++i) {
    01105     a_contact[i].surface.mode = dContactSlip1   | dContactSlip2   |
    01106                                 dContactSoftERP | dContactSoftCFM |
    01107                                 dContactApprox1;
    01108     a_contact[i].surface.mu       = 0.8; //conf.frictionGround;
    01109     a_contact[i].surface.slip1    = 0.005;
    01110     a_contact[i].surface.slip2    = 0.005;
    01111     a_contact[i].surface.soft_erp = 0.9;
    01112     a_contact[i].surface.soft_cfm = 0.00001;
    01113
    01114     dJointID joint_id =
    01115       dJointCreateContact(ode_handle.world,
    01116                           ode_handle.jointGroup,
    01117                           &a_contact[i]);
    01118
    01119     dJointAttach(joint_id ,
    01120                  dGeomGetBody(a_contact[i].geom.g1),
    01121                  dGeomGetBody(a_contact[i].geom.g2));
    01122   }
    01123
    01124
    01125   return true;
    01126 }
    01127
    01128
    01129
    01130 /*****************************************************************************/
    01131 /* PlaygroundComponent                                                       */
    01132 /*****************************************************************************/
    01133 /*PlaygroundComponent::PlaygroundComponent(const PlaygroundDescription &r_desc)
    01134 {
    01135
    01136 }*/
    01137
    01138
    01139 /*****************************************************************************/
    01140 /* PlaneComponent                                                            */
    01141 /*****************************************************************************/
    01142 /*PlaneComponentDescription::PlaneComponentDescription()
    01143 {
    01144   v3_normal = Vector3<dReal>(0.0, 0.0, 1.0);
    01145   d         = 0.0;
    01146 }
    01147
    01148
    01149 PlaneComponent::PlaneComponent(const PlaneComponentDescription &r_desc) :
    01150   AbstractComponent(r_desc.p_ode_handle)
    01151 {
    01152   if(NULL == r_desc.p_ode_handle)
    01153     InvalidArgumentException().raise();
    01154
    01155   geom_id = dCreatePlane(r_desc.p_ode_handle->space,
    01156                          r_desc.v3_normal.x,
    01157                          r_desc.v3_normal.y,
    01158                          r_desc.v3_normal.z,
    01159                          r_desc.d);
    01160 }
    01161
    01162
    01163 PlaneComponent::~PlaneComponent()
    01164 {
    01165   dGeomDestroy(geom_id);
    01166 }
    01167
    01168 */
    01169
    01170 SpiderDescription::SpiderDescription()
    01171 {
    01172
    01173 }
    01174
    01175
    01176 /**
    01177  * SpiderComponent
    01178  *
    01179  * this class class has following sub components:
    01180  * UniversalMotorComponent
    01181  * CCURobotArmComponent
    01182  * SimpleBodyComponent
    01183  */
    01184 SpiderComponent::SpiderComponent(const SpiderDescription &r_desc) :
    01185   AbstractCompoundComponent(*r_desc.p_ode_handle)
    01186   //  ode_handle(*(r_desc.p_ode_handle))
    01187 {
    01188   if(NULL == r_desc.p_ode_handle)
    01189     InvalidArgumentException().raise();
    01190
    01191   if(NULL == r_desc.p_angle_list)
    01192     InvalidArgumentException().raise();
    01193
    01194   if(0 >= r_desc.p_angle_list->size())
    01195     InvalidArgumentException().raise();
    01196
    01197   //  ode_handle = *r_desc.p_ode_handle;
    01198
    01199   //joint_group_id = dJointGroupCreate(0);
    01200
    01201
    01202   dMass   spider_body_mass;
    01203   dBodyID spider_body_id;
    01204   dGeomID spider_geom_id;
    01205
    01206
    01207
    01208   // create the body (for the sphere)
    01209   spider_body_id = dBodyCreate(ode_handle.world);
    01210
    01211
    01212   // set up the mass
    01213   dMassSetSphereTotal(&spider_body_mass,
    01214                       r_desc.sphere_mass,
    01215                       r_desc.sphere_radius);
    01216
    01217   dBodySetMass(spider_body_id, &spider_body_mass);
    01218
    01219
    01220   // set up the sphere geom
    01221   spider_geom_id = dCreateSphere(ode_handle.space,
    01222                                  r_desc.sphere_radius);
    01223
    01224   /*
    01225   dMassSetBoxTotal(&spider_body_mass,
    01226                    1.0,
    01227                    1.0, 1.0, 0.25);
    01228
    01229   dBodySetMass(spider_body_id, &spider_body_mass);
    01230
    01231   spider_geom_id = dCreateBox(ode_handle.space,
    01232                               1.0, 1.0, 0.25);
    01233   */
    01234   /*
    01235   std::cout << "-----------------" << "\n";
    01236   std::cout << r_desc.position.x << "\n";
    01237   std::cout << r_desc.position.y << "\n";
    01238   std::cout << r_desc.position.z << "\n";
    01239   */
    01240   dGeomSetBody(spider_geom_id, spider_body_id);
    01241
    01242
    01243   dGeomSetPosition(spider_geom_id,
    01244                    r_desc.position.x,
    01245                    r_desc.position.y,
    01246                    r_desc.position.z);
    01247
    01248   /*
    01249   dGeomSdetPosition(spider_geom_id,
    01250                    r_desc.position.x,
    01251                    r_desc.position.y,
    01252                    r_desc.position.z);
    01253   */
    01254   // insert a simple physical component into the component container
    01255   component_container.insert(component_container.end(),
    01256                              new SimplePhysicalComponent(ode_handle,
    01257                                                          spider_body_id,
    01258                                                          spider_geom_id));
    01259
    01260
    01261   VertexList vl;
    01262
    01263   // create the joints (which are needed for connecting the legs)
    01264   // and the legs
    01265   for(AngleList::iterator it_angle = r_desc.p_angle_list->begin();
    01266       it_angle != r_desc.p_angle_list->end();
    01267       ++it_angle) {
    01268
    01269
    01270     // create the robot arm component
    01271     RobotArmDescription rad;
    01272
    01273
    01274     vl.clear();
    01275     // position of the segments:
    01276     // x = cos(angle) * (sphere_radius + segment_length * i)
    01277
    01278     for(unsigned i = 0; i <= r_desc.segment_count; ++i) {
    01279       dReal r = r_desc.sphere_radius +
    01280                 r_desc.segment_radius + 0.1 + r_desc.segment_length * i;
    01281       vl.insert(vl.end(), Vertex(cos(it_angle->x) * r + r_desc.position.x,
    01282                                  sin(it_angle->x) * r + r_desc.position.y,
    01283                                  0.0                  + r_desc.position.z));
    01284     }
    01285
    01286     rad.p_ode_handle   = r_desc.p_ode_handle;
    01287     rad.segment_mass   = r_desc.segment_mass;
    01288     rad.segment_radius = r_desc.segment_radius;
    01289     rad.p_vertex_list  = &vl;
    01290
    01291     std::cout << "CREATING\n";
    01292     CCURobotArmComponent *p_robot_arm = new CCURobotArmComponent(rad);
    01293
    01294     // insert the robot arm into the sub component container
    01295     component_container.
    01296       insert(component_container.end(),
    01297              p_robot_arm);
    01298
    01299
    01300     // cast that subcomponent to a simple component
    01301     SimplePhysicalComponent *p_segment =
    01302     dynamic_cast<SimplePhysicalComponent*>(&p_robot_arm->get_sub_component(0));
    01303
    01304
    01305     const dReal *vv;
    01306     vv = dGeomGetPosition(p_segment->get_geom_id());
    01307
    01308     std::cout << "...---..." << p_robot_arm->get_sub_component_count() << "\n";
    01309     std::cout << vv[0] << "\n";
    01310     std::cout << vv[1] << "\n";
    01311     std::cout << vv[2] << "\n";
    01312
    01313     // create a new joint
    01314     dJointID joint_id = dJointCreateUniversal(ode_handle.world,
    01315                                               0);
    01316
    01317     // attach the first segment of the arm to the body
    01318     dJointAttach(joint_id, spider_body_id, p_segment->get_body_id());
    01319     // dJointAttach(joint_id, spider_body_id,  p_segment->get_body_id());
    01320
    01321
    01322     // set the anchor of the joint
    01323     VertexList::iterator it_vertex = vl.begin();
    01324     dJointSetUniversalAnchor(joint_id,
    01325                              it_vertex->x,
    01326                              it_vertex->y,
    01327                              it_vertex->z);
    01328
    01329     /*
    01330     std::cout << "----------------\n";
    01331     std::cout << it_vertex->x << "\n";
    01332     std::cout << it_vertex->y << "\n";
    01333     std::cout << it_vertex->z << "\n";
    01334     */
    01335     // set up first axis
    01336     dJointSetUniversalAxis1(joint_id, 0.0, 0.0, 1.0);
    01337
    01338
    01339     // set up second axis
    01340     dJointSetUniversalAxis2(joint_id,
    01341                             sin(it_angle->x),
    01342                             -cos(it_angle->x),
    01343                             0.0);
    01344
    01345     dVector3 v;
    01346     dJointGetUniversalAnchor(joint_id, v);
    01347     std::cout << "x1 = " << v[0] << "\n";
    01348     std::cout << "y1 = " << v[1] << "\n";
    01349     std::cout << "z1 = " << v[2] << "\n";
    01350
    01351     dJointGetUniversalAnchor2(joint_id, v);
    01352     std::cout << "x2 = " << v[0] << "\n";
    01353     std::cout << "y2 = " << v[1] << "\n";
    01354     std::cout << "z2 = " << v[2] << "\n";
    01355
    01356
    01357     // set up first motor
    01358     component_container.
    01359       insert(component_container.end(),
    01360              new UniversalMotorComponent(joint_id, 0));
    01361
    01362     // set up second motor
    01363     component_container.
    01364       insert(component_container.end(),
    01365              new UniversalMotorComponent(joint_id, 1));
    01366
    01367   }
    01368 }
    01369
    01370
    01371
    01372 /**
    01373  * collision_callback
    01374  *
    01375  * this function only does collision handling for geoms part of this object
    01376  *
    01377  * hmm.. there should be a mechanism for transferring collision handling
    01378  * to sub components
    01379  */
    01380 bool SpiderComponent::collision_callback(OdeHandle *p_ode_handle,
    01381                                          dGeomID geom_id_0,
    01382                                          dGeomID geom_id_1) const
    01383 {
    01384   // check if the sphere collided with something (the first entry
    01385   // in the component container)
    01386   ComponentContainer::const_iterator it = component_container.begin();
    01387
    01388   const SimplePhysicalComponent *p_spider_body =
    01389     dynamic_cast<const SimplePhysicalComponent*>(*it);
    01390
    01391   if(geom_id_0 == p_spider_body->get_geom_id() ||
    01392      geom_id_1 == p_spider_body->get_geom_id()) {
    01393     return false;
    01394     /*
    01395     const unsigned max_contact_count = 10;
    01396     unsigned n;
    01397     dContact a_contact[max_contact_count];
    01398
    01399     n = dCollide(geom_id_0,
    01400                  geom_id_1,
    01401                  max_contact_count,
    01402                  &a_contact->geom,
    01403                  sizeof(dContact));
    01404
    01405     for (unsigned i = 0; i < n; ++i) {
    01406       a_contact[i].surface.mode = dContactSlip1   | dContactSlip2   |
    01407         dContactSoftERP | dContactSoftCFM |
    01408         dContactApprox1;
    01409       a_contact[i].surface.mu       = 0.8; //conf.frictionGround;
    01410       a_contact[i].surface.slip1    = 0.005;
    01411       a_contact[i].surface.slip2    = 0.005;
    01412       a_contact[i].surface.soft_erp = 0.9;
    01413       a_contact[i].surface.soft_cfm = 0.001;
    01414
    01415       dJointID joint_id =
    01416         dJointCreateContact(ode_handle.world,
    01417                             ode_handle.jointGroup,
    01418                             &a_contact[i]);
    01419
    01420       dJointAttach(joint_id ,
    01421                    dGeomGetBody(a_contact[i].geom.g1),
    01422                    dGeomGetBody(a_contact[i].geom.g2));
    01423     }
    01424     */
    01425   }
    01426
    01427
    01428   for(++it;
    01429       it != component_container.end();
    01430       ++it) {
    01431
    01432     if(true == (*it)->collision_callback(p_ode_handle, geom_id_0, geom_id_1))
    01433       return true;
    01434   }
    01435   return false;
    01436 }
    01437
    01438
    01439
    01440 }
    01441 }
    01442
    01443