Robotsystem from Robot Group Leipzig: muscledarm.cpp Source File

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

- File List
- File Members

# muscledarm.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: muscledarm.cpp,v $
    00023  *   Revision 1.1.4.11  2006/03/30 12:34:56  martius
    00024  *   documentation updated
    00025  *
    00026  *   Revision 1.1.4.10  2006/03/28 14:24:37  fhesse
    00027  *   minor changes
    00028  *
    00029  *   Revision 1.1.4.9  2006/01/31 09:58:10  fhesse
    00030  *   basically working now
    00031  *
    00032  *   Revision 1.1.4.8  2006/01/20 12:58:14  fhesse
    00033  *   bodies placed correctly
    00034  *
    00035  *   Revision 1.1.4.7  2006/01/13 12:22:07  fhesse
    00036  *   partially working
    00037  *
    00038  *   Revision 1.1.4.6  2006/01/10 16:45:53  fhesse
    00039  *   not working osg version
    00040  *
    00041  *   Revision 1.1.4.5  2006/01/10 09:37:36  fhesse
    00042  *   partially moved to osg
    00043  *
    00044  *   Revision 1.1.4.4  2005/12/16 16:36:04  fhesse
    00045  *   manual control via keyboard
    00046  *   setMotors via dJointAddSliderForce
    00047  *
    00048  *   Revision 1.1.4.3  2005/11/24 16:15:57  fhesse
    00049  *   moved from main branch, sensors improved
    00050  *
    00051  *   Revision 1.3  2005/11/17 16:29:24  fhesse
    00052  *   initial version
    00053  *
    00054  *   Revision 1.2  2005/11/15 12:35:19  fhesse
    00055  *   muscles drawn as muscles, sphere drawn on tip of lower arm
    00056  *
    00057  *   Revision 1.1  2005/11/11 15:37:06  fhesse
    00058  *   preinitial version
    00059  *                                                                 *
    00060  *                                                                         *
    00061  ***************************************************************************/
    00062
    00063 #include <iostream>
    00064 #include <assert.h>
    00065
    00066 #include "mathutils.h"
    00067 #include "muscledarm.h"
    00068
    00069 namespace lpzrobots{
    00070
    00071   MuscledArm::MuscledArm(const OdeHandle& odeHandle, const OsgHandle& osgHandle, const MuscledArmConf& conf):
    00072     OdeRobot(odeHandle, osgHandle), conf(conf){
    00073
    00074     // prepare name;
    00075     Configurable::insertCVSInfo(name, "$RCSfile: muscledarm.cpp,v $",
    00076                                 "$Revision: 1.1.4.11 $");
    00077
    00078
    00079     parentspace=odeHandle.space;
    00080     factorMotors=0.1;
    00081     factorSensors=8.0;//20.0;
    00082     damping=1;
    00083
    00084     sensorno=0;
    00085     if (conf.jointAngleSensors)
    00086       sensorno+=2;
    00087     if (conf.jointAngleRateSensors)
    00088       sensorno+=2+4;
    00089     if (conf.muscleLengthSensors)
    00090       sensorno+=6;
    00091
    00092     if (conf.jointActuator){
    00093       motorno=2;
    00094     }else{
    00095       motorno=6;
    00096     }
    00097
    00098     print=0;
    00099
    00100     this->osgHandle.color = Color(1,1,0);
    00101     max_l=0;
    00102     max_r=0;
    00103     min_l=10;
    00104     min_r=10;
    00105
    00106     for (int i=0; i<NUMParts; i++){
    00107       old_dist[i].x=0;
    00108       old_dist[i].y=0;
    00109       old_dist[i].z=0;
    00110     }
    00111
    00112     for (int i=0; i<6; i++){
    00113       force_[i]=0;
    00114     }
    00115
    00116     base_width=SIDE;
    00117     base_length=SIDE*1.7;
    00118     upperArm_width=SIDE*0.2;
    00119     upperArm_length=SIDE*3.0;
    00120     lowerArm_width=SIDE*0.2;
    00121     lowerArm_length=SIDE*4.0;
    00122     mainMuscle_width=SIDE*0.2;
    00123     mainMuscle_length=SIDE*2;
    00124     smallMuscle_width=SIDE*0.1;
    00125     smallMuscle_length=SIDE*0.5;
    00126
    00127     joint_offset=0.01;
    00128
    00129     created=false;
    00130   };
    00131
    00132   /** sets actual motorcommands
    00133       @param motors motors scaled to [-1,1]
    00134       @param motornumber length of the motor array
    00135   */
    00136   void MuscledArm::setMotors(const motor* motors, int motornumber){
    00137     if (!conf.jointActuator) {
    00138       for (int i=SJ_mM1; i<=SJ_sM4; i++){
    00139         // just adding force to slider joint
    00140         //((SliderJoint*)joint[i])->addForce(factorMotors * motors[i-SJ_mM1]);
    00141
    00142         //
    00143         ((SliderJoint*)joint[i])->
    00144           addForce(factorMotors * (motors[i-SJ_mM1]- 8 * ((SliderJoint*)joint[i])->getPosition1()));
    00145       }
    00146     }else{
    00147       for(int i=HJ_BuA; i<= HJ_uAlA; i++){ // two hinge joints
    00148         //      ((HingeJoint*)joint[i])->addTorque(motors[i-HJ_BuA]);
    00149         ((HingeJoint*)joint[i])->addTorque
    00150           (M_PI/3 //range of joint is -M_PI/3 .. M_PI/3, and so is the result of getPosition1()
    00151            * motors[i-HJ_BuA]
    00152            - ((HingeJoint*)joint[i])->getPosition1());
    00153       }
    00154     }
    00155   };
    00156
    00157
    00158   /** returns actual sensorvalues
    00159       @param sensors sensors scaled to [-1,1] (more or less)
    00160       @param sensornumber length of the sensor array
    00161       @return number of actually written sensors
    00162   */
    00163   int MuscledArm::getSensors(sensor* sensors, int sensornumber){
    00164     int written=0;
    00165     if ((conf.jointAngleSensors) && ((written+1)<sensornumber) ){
    00166       sensors[written]= 3/M_PI*((HingeJoint*)joint[HJ_BuA])->getPosition1();
    00167       written++;
    00168       sensors[written]= 3/M_PI*((HingeJoint*)joint[HJ_uAlA])->getPosition1();
    00169       written++;
    00170     }
    00171
    00172     if ( (conf.jointAngleRateSensors) && ((written+1)<sensornumber) ) {
    00173       sensors[written]= factorSensors * ((HingeJoint*)joint[HJ_BuA])->getPosition1Rate();
    00174       written++;
    00175       sensors[written]= factorSensors * ((HingeJoint*)joint[HJ_uAlA])->getPosition1Rate();
    00176       written++;
    00177       written--;
    00178       // add 4 sensors with 0 value to have number sensor equal number motors when
    00179       // using muscle actuation (needed by InvertNChannelController)
    00180       for (int j=0; j<=4; j++, written++){
    00181         sensors[written]=0.0;
    00182       }
    00183     }
    00184
    00185     if ( (conf.muscleLengthSensors) && ((written+5)<sensornumber) ) {
    00186       for (int j=SJ_mM1; j<=SJ_sM4; j++, written++){
    00187         sensors[written]=factorSensors * ((SliderJoint*)joint[j])->getPosition1();
    00188       }
    00189     }
    00190     //printf("written= %i \n",written);
    00191     return written;
    00192   };
    00193
    00194   /** sets the pose of the vehicle
    00195       @params pose desired 4x4 pose matrix
    00196   */
    00197   void MuscledArm::place(const osg::Matrix& pose){
    00198     // the position of the robot is the center of the base
    00199     // to set the arm on the ground when the z component of the position is 0
    00200     // base_width/2 is added
    00201     osg::Matrix p2;
    00202     p2 = pose * osg::Matrix::translate(osg::Vec3(0, 0, base_width/2));
    00203     create(p2);
    00204   };
    00205
    00206
    00207   /** returns a vector with the positions of all segments of the robot
    00208       @param poslist vector of positions (of all robot segments)
    00209       @return length of the list
    00210   */
    00211   int MuscledArm::getSegmentsPosition(vector<Position> &poslist){
    00212     assert(created);
    00213     for (int i=0; i<NUMParts; i++){
    00214       poslist.push_back(Position(dBodyGetPosition(object[i]->getBody())));
    00215     }
    00216     return NUMParts;
    00217   };
    00218
    00219
    00220   /**
    00221    * draws the arm
    00222    */
    00223   void MuscledArm::update(){
    00224     assert(created); // robot must exist
    00225     for (int i =0; i<NUMParts; i++){
    00226       object[i]->update();
    00227     }
    00228     for (int i=0/*HJ_BuA*/; i<NUMJoints; i++){
    00229       joint[i]->update();
    00230     }
    00231
    00232   };
    00233
    00234
    00235
    00236   void MuscledArm::doInternalStuff(const GlobalData& globalData){
    00237
    00238     double k[6];
    00239     k[0] = 0.5; // spring constant between mainMuscle11 and mainMuscle12
    00240     k[1] = 0.2; // spring constant between mainMuscle21 and mainMuscle22
    00241     k[2] = 0.5; // spring constant between smallMuscle11 and smallMuscle12
    00242     k[3] = 0.2; // spring constant between smallMuscle21 and smallMuscle22
    00243     k[4] = 0.5; // spring constant between smallMuscle31 and smallMuscle32
    00244     k[5] = 0.2; // spring constant between smallMuscle41 and smallMuscle42
    00245
    00246     //   damping=0.1;
    00247
    00248     //   double force;
    00249     //   for(int i=SJ_mM1; i<(SJ_sM4+1); i++){
    00250     //     //calculating force
    00251     //     force=((SliderJoint*)joint[i])->getPosition1()  * k[i];
    00252     //     //damping
    00253     //     force=force + damping * ((SliderJoint*)joint[i])->getPosition1Rate() ;
    00254     //     ((SliderJoint*)joint[i])->addForce(0.1*force);
    00255     //   }
    00256
    00257
    00258     // add a spring force to keep the bodies together, otherwise they will
    00259     // fly apart along the slider axis.
    00260
    00261     //         //mainMuscle11  =  3
    00262     //         //smallMuscle42 = 15
    00263     const dReal *p1;
    00264     const dReal *p2;
    00265     for (int i=mainMuscle11; i<smallMuscle42; i+=2){
    00266       p1 = dBodyGetPosition (object[i]->getBody());
    00267       p2 = dBodyGetPosition (object[i+1]->getBody());
    00268
    00269       Position dist;
    00270       //distance between slider joint elements
    00271       dist.x=p2[0]-p1[0];
    00272       dist.y=p2[1]-p1[1];
    00273       dist.z=p2[2]-p1[2];
    00274
    00275       Position force;
    00276       //calculating force
    00277       force=dist*k[(int)(i*0.5)-1];
    00278
    00279       //damping
    00280       force=force + (dist - old_dist[i] )*damping;
    00281
    00282       dBodyAddForce (object[i]->getBody(), force.x, force.y, force.z);
    00283       dBodyAddForce (object[i+1]->getBody(), -force.x, -force.y, -force.z);
    00284       old_dist[i]=dist;
    00285     }
    00286
    00287   }
    00288
    00289   void MuscledArm::mycallback(void *data, dGeomID o1, dGeomID o2){
    00290     MuscledArm* me = (MuscledArm*)data;
    00291
    00292     if (// collision between fixed body and upper arm
    00293         ((o1 == me->object[base]->getGeom()) && (o2 == me->object[upperArm]->getGeom()))
    00294         || ((o2 == me->object[base]->getGeom()) && (o1 == me->object[upperArm]->getGeom()))
    00295         // collision between upper arm and lower arm
    00296         || ((o1 == me->object[upperArm]->getGeom()) && (o2 == me->object[lowerArm]->getGeom()))
    00297         || ((o2 == me->object[upperArm]->getGeom()) && (o1 == me->object[lowerArm]->getGeom()))
    00298         // collision between fixed body and lower arm
    00299         || ((o1 == me->object[base]->getGeom()) && (o2 == me->object[lowerArm]->getGeom()))
    00300         || ((o2 == me->object[base]->getGeom()) && (o1 == me->object[lowerArm]->getGeom()))
    00301         ){
    00302
    00303       int i,n;
    00304       const int N = 10;
    00305       dContact contact[N];
    00306
    00307       n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact));
    00308       for (i=0; i<n; i++) {
    00309         contact[i].surface.mode = dContactSoftERP | dContactSoftCFM | dContactApprox1;
    00310         contact[i].surface.mu = 0.01;
    00311         contact[i].surface.soft_erp = 1;
    00312         contact[i].surface.soft_cfm = 0.00001;
    00313         dJointID c = dJointCreateContact( me->odeHandle.world, me->odeHandle.jointGroup, &contact[i]);
    00314         dJointAttach ( c , dGeomGetBody(contact[i].geom.g1) , dGeomGetBody(contact[i].geom.g2));
    00315       }
    00316     }
    00317
    00318   }
    00319
    00320
    00321   bool MuscledArm::collisionCallback(void *data, dGeomID o1, dGeomID o2){
    00322
    00323     //checks if both of the collision objects are part of the robot
    00324     if( o1 == (dGeomID)odeHandle.space || o2 == (dGeomID)odeHandle.space) {
    00325
    00326       // treat inner collisions in mycallback  => now done with joint stops
    00327       //dSpaceCollide(arm_space, this, mycallback);
    00328
    00329       int i,n;
    00330       const int N = 10;
    00331       dContact contact[N];
    00332
    00333       n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact));
    00334       for (i=0; i<n; i++) {
    00335         if( contact[i].geom.g1 == object[base]->getGeom() ||
    00336             contact[i].geom.g2 == object[base]->getGeom() ||
    00337             contact[i].geom.g1 == object[upperArm]->getGeom()  ||
    00338             contact[i].geom.g2 == object[upperArm]->getGeom()  ||
    00339             contact[i].geom.g1 == object[lowerArm]->getGeom()  ||
    00340             contact[i].geom.g2 == object[lowerArm]->getGeom()  ||
    00341             contact[i].geom.g1 == object[hand]->getGeom()  ||
    00342             contact[i].geom.g2 == object[hand]->getGeom()  ){
    00343           // only treat collisions with fixed body, upper arm ,lower arm or hand
    00344           contact[i].surface.mode = dContactSoftERP | dContactSoftCFM | dContactApprox1;
    00345           contact[i].surface.mu = 0.01;
    00346           contact[i].surface.soft_erp = 1;
    00347           contact[i].surface.soft_cfm = 0.00001;
    00348
    00349           dJointID c = dJointCreateContact( odeHandle.world, odeHandle.jointGroup, &contact[i]);
    00350           dJointAttach ( c , dGeomGetBody(contact[i].geom.g1) , dGeomGetBody(contact[i].geom.g2));
    00351         }
    00352       }
    00353       return true;
    00354     } else {
    00355       return false;
    00356     }
    00357
    00358     return true;
    00359   }
    00360
    00361
    00362   /** creates arm at desired position
    00363       @param pos struct Position with desired position
    00364   */
    00365   void MuscledArm::create(const osg::Matrix& pose){
    00366     if (created) {
    00367       destroy();
    00368     }
    00369     // create vehicle space and add it to parentspace
    00370     odeHandle.space = dSimpleSpaceCreate(parentspace);
    00371
    00372     // create base
    00373     object[base] = new Box(base_width, base_width, base_length);
    00374     object[base] -> init(odeHandle, MASS, osgHandle,Primitive::Geom  | Primitive::Draw);
    00375     //    if(conf.strained){
    00376
    00377     // place base
    00378     object[base] -> setPose(osg::Matrix::rotate(M_PI/2, 1, 0, 0)
    00379                             * pose);
    00380     // create and place upper arm
    00381     object[upperArm] = new Box(upperArm_width, upperArm_width, upperArm_length);
    00382     this->osgHandle.color = Color(1, 0, 0, 1.0f);
    00383     object[upperArm] -> init(odeHandle, MASS, osgHandle);
    00384     object[upperArm] -> setPose(osg::Matrix::rotate(M_PI/2, 0, 1, 0) * osg::Matrix::translate
    00385                                 (-(base_width/2+upperArm_length/2)-joint_offset,0,0) * pose);
    00386     // create and place lower arm
    00387     object[lowerArm] = new Box(lowerArm_width, lowerArm_width, lowerArm_length);
    00388     this->osgHandle.color = Color(0,1,0);
    00389     object[lowerArm] -> init(odeHandle, MASS, osgHandle);
    00390     object[lowerArm] -> setPose(osg::Matrix::rotate(M_PI/2, 1, 0, 0) * osg::Matrix::translate
    00391                                 (-(base_width/2+upperArm_length+lowerArm_width/2+2*joint_offset),
    00392                                  lowerArm_length/4,0) * pose);
    00393
    00394
    00395     // create and place sphere at tip of lower arm
    00396     osg::Matrix ps;
    00397     ps.makeIdentity();
    00398     Primitive* o1 = new Sphere(lowerArm_width);
    00399     // move to end of arm in local coordinates
    00400     object[hand] = new Transform(object[lowerArm], o1,
    00401                                  osg::Matrix::translate(0, 0,-lowerArm_length*0.5) * ps);
    00402     object[hand]->init(odeHandle, /*mass*/0, osgHandle, Primitive::Geom  | Primitive::Draw);
    00403
    00404     osg::Vec3 pos;
    00405     // hinge joint between upper arm and fixed body
    00406     pos=object[base]->getPosition();
    00407     joint[HJ_BuA] = new HingeJoint(object[base], object[upperArm],
    00408                                    osg::Vec3(pos[0]-base_width/2, pos[1], pos[2]),
    00409                                    osg::Vec3(0, 0, 1));
    00410     joint[HJ_BuA]->init(odeHandle, osgHandle, true);
    00411     // set stops to make sure arm stays in useful range
    00412     joint[HJ_BuA]->setParam(dParamLoStop,-M_PI/3);
    00413     joint[HJ_BuA]->setParam(dParamHiStop, M_PI/3);
    00414
    00415     // hinge joint between upperArm and lowerArm
    00416     pos=object[upperArm]->getPosition();
    00417     joint[HJ_uAlA] = new HingeJoint(object[upperArm], object[lowerArm],
    00418                                     osg::Vec3(pos[0]-upperArm_length/2-joint_offset,
    00419                                               pos[1], pos[2]),
    00420                                     osg::Vec3(0, 0, 1));
    00421     joint[HJ_uAlA]->init(odeHandle, osgHandle, true);
    00422     // set stops to make sure arm stays in useful range
    00423     joint[HJ_uAlA]->setParam(dParamLoStop,-M_PI/3);
    00424     joint[HJ_uAlA]->setParam(dParamHiStop, M_PI/3);
    00425
    00426
    00427
    00428     //       if (conf.includeMuscles) {
    00429
    00430     // create and place boxes for mainMuscles
    00431     for (int i= mainMuscle11; i<smallMuscle11; i++){
    00432       object[i] = new Box(mainMuscle_width, mainMuscle_width, mainMuscle_length);
    00433       //object[i] = new Capsule(mainMuscle_width/2,mainMuscle_length);
    00434       object[i] -> init(odeHandle, MASS, osgHandle);
    00435       if (i==mainMuscle11) this->osgHandle.color = Color(0.4,0.4,0);
    00436       if (i==mainMuscle12) this->osgHandle.color = Color(0,1,1);
    00437       if (i==mainMuscle21) this->osgHandle.color = Color(1,1,0);
    00438     }
    00439     /* left main Muscle */
    00440     /**************************************/
    00441     pos=object[upperArm]->getPosition();
    00442     object[mainMuscle11] -> setPose(osg::Matrix::rotate(M_PI/2, 0, 1, 0) * osg::Matrix::translate
    00443                                     (// moved towards base, aligned with end of upperArm
    00444                                      pos[0]+(upperArm_length-mainMuscle_length)/2,
    00445                                      // moved to left of upper arm, aligned with end of base
    00446                                      pos[1]-(base_length/2-mainMuscle_width/2),
    00447                                      0)  // height is ok
    00448                                     * pose);
    00449     object[mainMuscle12] -> setPose(osg::Matrix::rotate(M_PI/2, 0, 1, 0) * osg::Matrix::translate
    00450                                     (// moved away from base, aligned with end of upperArm
    00451                                      pos[0]-(upperArm_length-mainMuscle_length)/2,
    00452                                      // moved to left of upper arm, aligned with end of base
    00453                                      pos[1]-(base_length/2-mainMuscle_width/2),
    00454                                      0)  // height is ok
    00455                                     * pose);
    00456     /* right main Muscle */
    00457     /**************************************/
    00458     object[mainMuscle21] -> setPose(osg::Matrix::rotate(M_PI/2, 0, 1, 0) * osg::Matrix::translate
    00459                                     (// moved towards base, aligned with end of upperArm
    00460                                      pos[0]+(upperArm_length-mainMuscle_length)/2,
    00461                                      // moved to right of upper arm, aligned with end of base
    00462                                      pos[1]+(base_length/2-mainMuscle_width/2),
    00463                                      0)  // height is ok
    00464                                     * pose);
    00465     object[mainMuscle22] -> setPose(osg::Matrix::rotate(M_PI/2, 0, 1, 0) * osg::Matrix::translate
    00466                                     (// moved away from base, aligned with end of upper
    00467                                      pos[0]-(upperArm_length-mainMuscle_length)/2,
    00468                                      // moved to right of upper arm, aligned with end of base
    00469                                      pos[1]+(base_length/2-mainMuscle_width/2),
    00470                                      0)  // height is ok
    00471                                     * pose);
    00472     /* joints for left main Muscle */
    00473     /**************************************/
    00474     // hinge joint between mainMuscle11 and fixed body */
    00475     pos=object[mainMuscle11]->getPosition();
    00476     joint[HJ_BmM11] = new HingeJoint(object[base], object[mainMuscle11],
    00477                                      osg::Vec3(pos[0]+mainMuscle_length/2+joint_offset,
    00478                                                pos[1], pos[2]), osg::Vec3(0, 0, 1));
    00479     joint[HJ_BmM11]->init(odeHandle, osgHandle, true);
    00480
    00481     // hinge joint between mainMuscle12 and lowerArm */
    00482     pos=object[mainMuscle12]->getPosition();
    00483     joint[HJ_lAmM12] = new HingeJoint(object[lowerArm], object[mainMuscle12],
    00484                                       osg::Vec3(pos[0]-mainMuscle_length/2-joint_offset,
    00485                                                 pos[1], pos[2]), osg::Vec3(0, 0, 1));
    00486     joint[HJ_lAmM12]->init(odeHandle, osgHandle, true);
    00487     // slider joint between mainMuscle11 and mainMuscle12
    00488     joint[SJ_mM1] = new SliderJoint(object[mainMuscle11], object[mainMuscle12],
    00489                                     /*anchor (not used)*/osg::Vec3(0, 0, 0),
    00490                                     osg::Vec3(1, 0, 0));
    00491     joint[SJ_mM1] -> init(odeHandle, osgHandle, /*withVisual*/ false);//true);
    00492
    00493
    00494     /* joints for right main Muscle */
    00495     /**************************************/
    00496     // hinge joint between mainMuscle21 and fixed body */
    00497     pos=object[mainMuscle21]->getPosition();
    00498     joint[HJ_BmM21] = new HingeJoint(object[base], object[mainMuscle21],
    00499                                      osg::Vec3(pos[0]+mainMuscle_length/2+joint_offset,
    00500                                                pos[1], pos[2]), osg::Vec3(0, 0, 1));
    00501     joint[HJ_BmM21]->init(odeHandle, osgHandle, true);
    00502
    00503     // hinge joint between mainMuscle22 and lowerArm */
    00504     pos=object[mainMuscle22]->getPosition();
    00505     joint[HJ_lAmM22] = new HingeJoint(object[lowerArm], object[mainMuscle22],
    00506                                       osg::Vec3(pos[0]-mainMuscle_length/2-joint_offset,
    00507                                                 pos[1], pos[2]), osg::Vec3(0, 0, 1));
    00508     joint[HJ_lAmM22]->init(odeHandle, osgHandle, true);
    00509     // slider joint between mainMuscle21 and mainMuscle22
    00510     joint[SJ_mM2] = new SliderJoint(object[mainMuscle21], object[mainMuscle22],
    00511                                     /*anchor (not used)*/osg::Vec3(0, 0, 0),
    00512                                     osg::Vec3(1, 0, 0));
    00513     joint[SJ_mM2] -> init(odeHandle, osgHandle, /*withVisual*/ false);//true);
    00514
    00515
    00516
    00517     // create and place boxes for smallMuscles
    00518     /*****************************************************************/
    00519     for (int i= smallMuscle11; i<hand; i++){
    00520       object[i] = new Box(smallMuscle_width, smallMuscle_width, smallMuscle_length);
    00521       object[i] -> init(odeHandle, MASS, osgHandle);
    00522     }
    00523     /* lower left small Muscle */
    00524     /**************************************/
    00525     pos=object[mainMuscle11]->getPosition();
    00526     object[smallMuscle11] -> setPose(osg::Matrix::rotate(M_PI*0.5, -1,-1, 0)* osg::Matrix::translate
    00527                                      // move center of this box to lower end of mainMuscle11
    00528                                      (pos[0]+mainMuscle_length/2
    00529                                       // move box away from base to align lower edges of
    00530                                       // mainMuscle11 and this muscle
    00531                                       -smallMuscle_length/2,
    00532                                       -smallMuscle_length,  // moved to left
    00533                                       0) // height is ok
    00534                                      * pose);
    00535     pos=object[smallMuscle11]->getPosition();
    00536     //object[smallMuscle12] -> setPose(osg::Matrix::rotate(M_PI*0.5, 0, 1, -1)* osg::Matrix::translate
    00537     object[smallMuscle12] -> setPose(osg::Matrix::rotate(M_PI*0.5, -1, -1, 0)* osg::Matrix::translate
    00538                                      (//calculate upper shift using sideward offset from smallMuscle11
    00539                                       //(to align smallMuscle11 and this muscle)
    00540                                       pos[0] -tan(M_PI/4)*(smallMuscle_length/2),
    00541                                       -smallMuscle_length/2, // moved to left
    00542                                       0) * pose); // heigth is ok
    00543     /* lower right small Muscle */
    00544     /**************************************/
    00545     pos=object[smallMuscle11]->getPosition();
    00546     object[smallMuscle21] -> setPose(osg::Matrix::rotate(M_PI*0.5, -1, 1, 0)* osg::Matrix::translate
    00547                                      // place as smallMuscle11, accecpt that it is on the right side
    00548                                      // of the upperArm (-> -pos[1])
    00549                                      (pos[0], -pos[1], 0) * pose); //height is ok
    00550     pos=object[smallMuscle12]->getPosition();
    00551     object[smallMuscle22] -> setPose(osg::Matrix::rotate(M_PI*0.5, -1, 1, 0) * osg::Matrix::translate
    00552                                      // place as smallMuscle12, accecpt that it is on the right side
    00553                                      // of the upperArm (-> -pos[1])
    00554                                      (pos[0], -pos[1], 0)* pose);  // height is ok
    00555     /* upper left small Muscle */
    00556     /**************************************/
    00557     pos=object[mainMuscle12]->getPosition();
    00558     object[smallMuscle31] -> setPose(osg::Matrix::rotate(M_PI*0.5, -1, 1, 0) * osg::Matrix::translate
    00559                                      // move center of this box to lower end of mainMuscle12
    00560                                      (pos[0]-mainMuscle_length/2
    00561                                       // move box in direction of base to align upper edges of
    00562                                       // mainMuscle12 and this muscle
    00563                                       +smallMuscle_length/2,
    00564                                       -smallMuscle_length,  // move to left
    00565                                       0) // height is ok
    00566                                      * pose);
    00567     pos=object[smallMuscle31]->getPosition();
    00568     object[smallMuscle32] -> setPose(osg::Matrix::rotate(M_PI*0.5, -1, 1, 0)* osg::Matrix::translate
    00569                                      //calculate shift toweards base using sideward offset from
    00570                                      //smallMuscle31 (to align smallMuscle11 and this muscle)
    00571                                      (pos[0]+tan(M_PI/4)*(smallMuscle_length/2),
    00572                                       pos[1]+smallMuscle_length/2, // move to left
    00573                                       0)
    00574                                      * pose);  // height is ok
    00575
    00576     /* upper right small Muscle */
    00577     /**************************************/
    00578     pos=object[smallMuscle31]->getPosition();
    00579     object[smallMuscle41] -> setPose(osg::Matrix::rotate(M_PI*0.5, -1, -1, 0) * osg::Matrix::translate
    00580                                      // place as smallMuscle31, accecpt that it is on the right side
    00581                                      // of the upperArm (-> -pos[1])
    00582                                      (pos[0], -pos[1],
    00583                                       0)* pose); //height is ok
    00584     pos=object[smallMuscle32]->getPosition();
    00585     object[smallMuscle42] -> setPose(osg::Matrix::rotate(M_PI*0.5, -1, -1, 0)* osg::Matrix::translate
    00586                                      // place as smallMuscle32, accecpt that it is on the right side
    00587                                      // of the upperArm (-> -pos[1])
    00588                                      (pos[0], -pos[1],
    00589                                       0) * pose); //height is ok
    00590
    00591     /* joints for lower left small Muscle */
    00592     /**************************************/
    00593     // hinge joint between base and smallMuscle11 */
    00594     joint[HJ_BsM11] = new HingeJoint(object[base], object[smallMuscle11],
    00595                                      // same as HJ between base and mainMuscle11
    00596                                      joint[HJ_BmM11]->getAnchor(),
    00597                                      ((OneAxisJoint*)joint[HJ_BmM11])->getAxis1());
    00598     joint[HJ_BsM11]->init(odeHandle, osgHandle, true);
    00599
    00600     // hinge joint between upperArm and smallMuscle12 */
    00601     pos=joint[HJ_BuA]->getAnchor();
    00602     joint[HJ_uAsM12] = new HingeJoint(object[upperArm], object[smallMuscle12],
    00603                                       // above HJ between base and upperArm
    00604                                       osg::Vec3(pos[0]-upperArm_length/4-0.01, pos[1], pos[2]),
    00605                                       ((OneAxisJoint*)joint[HJ_BuA])->getAxis1());
    00606     joint[HJ_uAsM12]->init(odeHandle, osgHandle, true);
    00607     // slider joint between smallMuscle11 and smallMuscle12
    00608     joint[SJ_sM1] = new SliderJoint(object[smallMuscle11], object[smallMuscle12],
    00609                                     /*anchor (not used)*/osg::Vec3(0, 0, 0),
    00610                                     osg::Vec3(1,-1, 0));
    00611     joint[SJ_sM1] -> init(odeHandle, osgHandle, /*withVisual*/ false, 0.05);
    00612
    00613
    00614
    00615     /* joints for lower right small Muscle */
    00616     /**************************************/
    00617     // hinge joint between base and smallMuscle21 */
    00618     joint[HJ_BsM21] = new HingeJoint(object[base], object[smallMuscle21],
    00619                                      // same as HJ between base and mainMuscle2l
    00620                                      joint[HJ_BmM21]->getAnchor(),
    00621                                      ((OneAxisJoint*)joint[HJ_BmM21])->getAxis1());
    00622     joint[HJ_BsM21]->init(odeHandle, osgHandle, true);
    00623
    00624     // hinge joint between upperArm and smallMuscle22 */
    00625     joint[HJ_uAsM22] = new HingeJoint(object[upperArm], object[smallMuscle22],
    00626                                       // same as HJ between upperArm and smallMuscle12
    00627                                       joint[HJ_uAsM12]->getAnchor(),
    00628                                       ((OneAxisJoint*)joint[HJ_uAsM12])->getAxis1());
    00629     joint[HJ_uAsM22]->init(odeHandle, osgHandle, true);
    00630     // slider joint between smallMuscle21 and smallMuscle22
    00631     joint[SJ_sM2] = new SliderJoint(object[smallMuscle21], object[smallMuscle22],
    00632                                     /*anchor (not used)*/osg::Vec3(0, 0, 0),
    00633                                     osg::Vec3(1,1, 0));
    00634     joint[SJ_sM2] -> init(odeHandle, osgHandle, /*withVisual*/ false, 0.05);
    00635
    00636
    00637
    00638
    00639
    00640     /* joints for upper left small Muscle */
    00641     /**************************************/
    00642     // hinge joint between lowerArm and smallMuscle31 */
    00643     joint[HJ_lAsM31] = new HingeJoint(object[lowerArm], object[smallMuscle31],
    00644                                       // same as HJ between lowerArm and mainMusclel2
    00645                                       joint[HJ_lAmM12]->getAnchor(),
    00646                                       ((OneAxisJoint*)joint[HJ_lAmM12])->getAxis1());
    00647     joint[HJ_lAsM31]->init(odeHandle, osgHandle, true);
    00648     // hinge joint between upperArm and smallMuscle32 */
    00649     pos=joint[HJ_uAlA]->getAnchor();
    00650     joint[HJ_uAsM32] = new HingeJoint(object[upperArm], object[smallMuscle32],
    00651                                       // below HJ between upperArm and lowerArm
    00652                                       osg::Vec3(pos[0]+upperArm_length/4+0.01, pos[1], pos[2]),
    00653                                       ((OneAxisJoint*)joint[HJ_uAlA])->getAxis1());
    00654     joint[HJ_uAsM32]->init(odeHandle, osgHandle, true);
    00655     // slider joint between smallMuscle31 and smallMuscle32
    00656     joint[SJ_sM3] = new SliderJoint(object[smallMuscle31], object[smallMuscle32],
    00657                                     /*anchor (not used)*/osg::Vec3(0, 0, 0),
    00658                                     osg::Vec3(-1,-1, 0));
    00659     joint[SJ_sM3] -> init(odeHandle, osgHandle, /*withVisual*/ false, 0.05);
    00660
    00661
    00662     /* joints for upper right small Muscle */
    00663     /***************************************/
    00664     // hinge joint between base and smallMuscle21 */
    00665     joint[HJ_lAsM41] = new HingeJoint(object[lowerArm], object[smallMuscle41],
    00666                                       // same as HJ between lowerArm and mainMuscle22
    00667                                       joint[HJ_lAmM22]->getAnchor(),
    00668                                       ((OneAxisJoint*)joint[HJ_lAmM22])->getAxis1());
    00669     joint[HJ_lAsM41]->init(odeHandle, osgHandle, true);
    00670
    00671     // hinge joint between upperArm and smallMuscle42 */
    00672     joint[HJ_uAsM42] = new HingeJoint(object[upperArm], object[smallMuscle42],
    00673                                       //same as HJ between upperArm and smallMuscle32
    00674                                       joint[HJ_uAsM32]->getAnchor(),
    00675                                       ((OneAxisJoint*)joint[HJ_BuA])->getAxis1());
    00676     joint[HJ_uAsM42]->init(odeHandle, osgHandle, true);
    00677     // slider joint between smallMuscle41 and smallMuscle42
    00678     joint[SJ_sM4] = new SliderJoint(object[smallMuscle41], object[smallMuscle42],
    00679                                     /*anchor (not used)*/osg::Vec3(0, 0, 0),
    00680                                     osg::Vec3(-1,1, 0));
    00681     joint[SJ_sM4] -> init(odeHandle, osgHandle, /*withVisual*/ false, 0.05);
    00682
    00683
    00684     /************************************************************************************/
    00685     created=true;
    00686   };
    00687
    00688
    00689
    00690
    00691
    00692   /** destroys vehicle and space
    00693    */
    00694   void MuscledArm::destroy(){
    00695     if (created){
    00696       for (int i=0; i<NUMParts; i++){
    00697         if(object[i]) delete object[i];
    00698       }
    00699       for (int i=0; i<NUMJoints; i++){
    00700         if(joint[i]) delete joint[i];
    00701       }
    00702       dSpaceDestroy(odeHandle.space);
    00703     }
    00704     created=false;
    00705   };
    00706
    00707
    00708
    00709   Primitive* MuscledArm::getMainObject() const {
    00710     return object[hand];
    00711   };
    00712
    00713   Configurable::paramlist MuscledArm::getParamList() const{
    00714     paramlist list;
    00715     list.push_back(pair<paramkey, paramval> (string("factorMotors"), factorMotors));
    00716     list.push_back(pair<paramkey, paramval> (string("factorSensors"), factorSensors));
    00717     list.push_back(pair<paramkey, paramval> (string("damping"), damping));
    00718     list.push_back(pair<paramkey, paramval> (string("print"), print));
    00719     return list;
    00720   };
    00721
    00722   Configurable::paramval MuscledArm::getParam(const paramkey& key) const{
    00723     if(key == "factorMotors") return factorMotors;
    00724     else if(key == "factorSensors") return factorSensors;
    00725     else if(key == "damping") return damping;
    00726     else if(key == "print") return print;
    00727     else  return Configurable::getParam(key) ;
    00728   };
    00729
    00730   bool MuscledArm::setParam(const paramkey& key, paramval val){
    00731     if(key == "factorMotors") factorMotors=val;
    00732     else if(key == "factorSensors") factorSensors = val;
    00733     else if(key == "damping") damping = val;
    00734     else if(key == "print") print = val;
    00735     else return Configurable::setParam(key, val);
    00736     return true;
    00737   };
    00738
    00739 }