bool SimulationModel::addUniversalJoint(const unsigned int rbIndex1, const unsigned int rbIndex2, const Eigen::Vector3f &pos, const Eigen::Vector3f &axis1, const Eigen::Vector3f &axis2) { UniversalJoint *uj = new UniversalJoint(); const bool res = uj->initConstraint(*this, rbIndex1, rbIndex2, pos, axis1, axis2); if (res) { m_constraints.push_back(uj); m_groupsInitialized = false; } return res; }
SimObject* UniversalJoint::clone() const { UniversalJoint* newUniversalJoint = new UniversalJoint(); //copy SimObject members newUniversalJoint->copyStandardMembers(this); bool createsNewMovID = newUniversalJoint->adjustMovableID(); //copy joint members //axes std::vector<AxisDescription>::const_iterator axesIter; for(axesIter = axes.begin(); axesIter != axes.end(); ++axesIter) { newUniversalJoint->axes.push_back(*axesIter); if(axesIter->motor != NULL) { newUniversalJoint->motors.push_back(axesIter->motor->clone()); newUniversalJoint->axes.back().motor = newUniversalJoint->motors.back(); } } //currentMotor if(!newUniversalJoint->motors.empty()) { newUniversalJoint->currentMotor = newUniversalJoint->motors.back(); } //copy universal joint members newUniversalJoint->anchorPoint = anchorPoint; newUniversalJoint->axis1 = &(newUniversalJoint->axes[0]); newUniversalJoint->axis2 = &(newUniversalJoint->axes[1]); //copy child nodes std::list<SimObject*>::const_iterator pos; for(pos = childNodes.begin(); pos != childNodes.end(); ++pos) { SimObject* childNode = (*pos)->clone(); newUniversalJoint->addChildNode(childNode, false); } if(createsNewMovID) { simulation->removeCurrentMovableID(); } SimObject* newObject = newUniversalJoint; return newObject; }
/** creates vehicle at desired position @param pos struct Position with desired position */ void SchlangeVelocity::create(const osg::Matrix& pose){ Schlange::create(pose); //*****************joint definition*********** for ( int n = 0; n < conf.segmNumber-1; n++ ) { Pos p1(objects[n]->getPosition()); Pos p2(objects[n]->getPosition()); UniversalJoint* j = new UniversalJoint(objects[n], objects[n+1], (objects[n]->getPosition() + objects[n+1]->getPosition())/2, Axis(0,0,1)* pose, Axis(0,1,0)* pose); j->init(odeHandle, osgHandle, true, conf.segmDia * 1.02); // setting stops at universal joints j->setParam(dParamLoStop, -conf.jointLimit*1.5); j->setParam(dParamHiStop, conf.jointLimit*1.5); j->setParam(dParamLoStop2, -conf.jointLimit*1.5); j->setParam(dParamHiStop2, conf.jointLimit*1.5); // making stops bouncy j->setParam (dParamBounce, 0.9 ); j->setParam (dParamBounce2, 0.9 ); // universal joints.push_back(j); } }
/** creates vehicle at desired position @param pos struct Position with desired position */ void Hexapod::create( const osg::Matrix& pose ){ if (created) { destroy(); } odeHandle.createNewSimpleSpace(parentspace,true); // color of joint axis and whiskers OsgHandle osgHandleJ = osgHandle.changeColor(Color(72./255.,16./255.,16./255.)); TwoAxisServoVel* servo; OneAxisServo* spring; OneAxisServo* spring2; FixedJoint* fixedJoint; // create body double twidth = conf.size * conf.width ;// 1/1.5; double theight = conf.size * conf.height; // 1/4; trunk = new Box(conf.size, twidth, theight); trunk->setTexture("Images/toy_fur3.jpg"); trunk->init(odeHandle, conf.mass*conf.percentageBodyMass, osgHandle); osg::Matrix trunkPos = TRANSM(0,0,conf.legLength)*pose; trunk->setPose(trunkPos); objects.push_back(trunk); if(conf.useBigBox){ Primitive* pole; double poleheight=conf.size*2; pole = new Box(poleheight, twidth*3.2, theight*1); bigboxtransform= new Transform(trunk,pole, osg::Matrix::translate(0,0,2*theight)); bigboxtransform->init(odeHandle, 0, osgHandle,Primitive::Geom /*| Primitive::Draw */); }else{ bigboxtransform=0; } osg::Matrix m0 = pose; if(conf.irSensors == true){ for(int i = -1; i < 2; i+=2){ irbox = new Box(0.1,0.1,0.1); irbox->setTexture("Images/toy_fur3.jpg"); irbox->init(odeHandle, 0.00001, osgHandle); irbox->setPose(ROTM(M_PI/4,0,0,1) * TRANSM(i*conf.size/2,0,theight/2)*trunkPos); objects.push_back(irbox); fixedJoint = new FixedJoint(trunk,irbox); fixedJoint->init(odeHandle, osgHandleJ, true, 0.4); joints.push_back(fixedJoint); } for(int i = -1; i < 2; i+=2){ irbox = new Box(0.1,0.1,0.15); irbox->setTexture("Images/toy_fur3.jpg"); irbox->init(odeHandle, 0.00001, osgHandle); irbox->setPose(TRANSM(0,i*twidth/2,theight/2 + 0.05)*trunkPos); objects.push_back(irbox); fixedJoint = new FixedJoint(trunk,irbox); fixedJoint->init(odeHandle, osgHandleJ, true, 0.4); joints.push_back(fixedJoint); } irSensorBank.init(odeHandle, osgHandle); if (conf.irFront){ // add front left and front right infrared sensor to sensorbank if required IRSensor* sensor = new IRSensor(); irSensorBank.registerSensor(sensor, objects[2], Matrix::rotate(-1*-M_PI/2, Vec3(0,1,0)) * Matrix::translate(1*0.05,0,0), conf.irRangeFront, RaySensor::drawAll); IRSensor* sensor2 = new IRSensor(); irSensorBank.registerSensor(sensor2, objects[2], Matrix::rotate(-1*-M_PI/2, Vec3(0,1,0)) * Matrix::rotate(1*-M_PI/2, Vec3(0,0,1)) * Matrix::translate(0,-0.05,0), conf.irRangeFront, RaySensor::drawAll); } if (conf.irBack){ // add front left and front right infrared sensor to sensorbank if required IRSensor* sensor = new IRSensor(); irSensorBank.registerSensor(sensor, objects[1], Matrix::rotate(1*-M_PI/2, Vec3(0,1,0)) * Matrix::translate(-1*0.05,0,0), conf.irRangeBack, RaySensor::drawAll); IRSensor* sensor2 = new IRSensor(); irSensorBank.registerSensor(sensor2, objects[1], Matrix::rotate(1*-M_PI/2, Vec3(0,1,0)) * Matrix::rotate(1*-M_PI/2, Vec3(0,0,1)) * Matrix::translate(0,0.05,0), conf.irRangeBack, RaySensor::drawAll); } if(conf.irLeft){ IRSensor* sensor = new IRSensor(); irSensorBank.registerSensor(sensor, objects[3], Matrix::rotate(-1*-M_PI/2, Vec3(0,1,0)) * Matrix::rotate(-M_PI/2, Vec3(0,0,1)) * Matrix::translate(0,-0.05,0.05), conf.irRangeLeft, RaySensor::drawAll); /* IRSensor* sensor2 = new IRSensor(); irSensorBank.registerSensor(sensor2, objects[3], Matrix::rotate(-1*-M_PI/2, Vec3(0,1,0)) * Matrix::rotate(1*-M_PI/2, Vec3(0,0,1)) * Matrix::translate(0,-0.05,0), conf.irRangeLeft, RaySensor::drawAll); */ } if(conf.irRight){ IRSensor* sensor = new IRSensor(); irSensorBank.registerSensor(sensor, objects[4], Matrix::rotate(-1*-M_PI/2, Vec3(0,1,0)) * Matrix::rotate(M_PI/2, Vec3(0,0,1)) * Matrix::translate(0,0.05,0.05), conf.irRangeLeft, RaySensor::drawAll); /* IRSensor* sensor2 = new IRSensor(); irSensorBank.registerSensor(sensor2, objects[4], Matrix::rotate(1*-M_PI/2, Vec3(0,1,0)) * Matrix::rotate(1*-M_PI/2, Vec3(0,0,1)) * Matrix::translate(0,0.05,0), conf.irRangeRight, RaySensor::drawAll); */ } } std::vector<Primitive*> tarsusParts; // legs (counted from back to front) double legdist = conf.size*0.9 / (conf.legNumber/2-1); for ( int n = 0; n < conf.legNumber; n++ ) { int v = n; double len1 = conf.legLength*0.5; double rad1 = conf.legLength/15; double len2 = conf.legLength*0.5; double rad2 = conf.legLength/15; // upper limp Primitive* coxaThorax; Pos pos = Pos(-conf.size/(2+0.2) + ((int)n/2) * legdist, n%2==0 ? - twidth/2 : twidth/2, conf.legLength - theight/3); osg::Matrix m = ROTM(M_PI/2,v%2==0 ? -1 : 1,0,0) * TRANSM(pos) * pose; coxaThorax = new Capsule(rad1, len1); coxaThorax->setTexture("Images/toy_fur3.jpg"); coxaThorax->init(odeHandle, legmass, osgHandle); osg::Matrix m1 = TRANSM(0,0,-len1/2) * ROTM(M_PI,0,0,v%2==0 ? -1 : 1) * ROTM(2*M_PI,0,v%2==0 ? -1 : 1,0) * m; coxaThorax->setPose(m1); thoraxPos.push_back(coxaThorax->getPosition()); thorax.push_back(coxaThorax); objects.push_back(coxaThorax); legs.push_back(coxaThorax); // powered hip joint Pos nullpos(0,0,0); UniversalJoint* j = new UniversalJoint(trunk, coxaThorax, nullpos * m, ROTM(M_PI,0,0,v%2==0 ? -1 : 1) * Axis(v%2==0 ? -1 : 1,0,0) * m, ROTM(M_PI,0,0,v%2==0 ? -1 : 1) * Axis(0,1,0) * m); j->init(odeHandle, osgHandleJ, true, rad1 * 2.1); joints.push_back(j); legContactArray[n].joint = j->getJoint(); // values will be set in setParam later servo = new TwoAxisServoVel(odeHandle, j,-1,1, 1, -1,1,1,0 ); // servo = new UniversalServo(j,-1,1, 1, -1,1,1,0); hipservos.push_back(servo); // lower leg Primitive* tibia; tibia = new Capsule(rad2, len2); tibia->setTexture("Images/toy_fur3.jpg"); tibia->init(odeHandle, legmass, osgHandle); osg::Matrix m2 = TRANSM(0,0,-len2/2) * ROTM(1.5,v%2==0 ? -1 : 1,0,0) * TRANSM(0,0,-len1/2) * m1; tibia->setPose(m2); objects.push_back(tibia); legs.push_back(tibia); legContactArray[n].legID = n; legContactArray[n].geomid = tibia->getGeom(); legContactArray[n].bodyID = tibia->getBody(); if(conf.useTebiaJoints){ // springy knee joint HingeJoint* k = new HingeJoint(coxaThorax, tibia, Pos(0,0,-len1/2) * m1, Axis(v%2==0 ? -1 : 1,0,0) * m1); k->init(odeHandle, osgHandleJ, true, rad1 * 2.1); // servo used as a spring spring = new HingeServo(k, -1, 1, 1, 0.01,0); // parameters are set later tebiasprings.push_back(spring); joints.push_back(k); }else{ // fixed knee joint FixedJoint* k = new FixedJoint(coxaThorax, tibia); k->init(odeHandle, osgHandleJ, false, rad1 * 2.1); joints.push_back(k); } // lower limp should not collide with body! odeHandle.addIgnoredPair(trunk,tibia); if(conf.tarsus){ // New: tarsus Primitive *tarsus; double angle = M_PI/12; double radius = rad2/2; double length = len2/2; double mass = legmass/10; tarsus = new Capsule(radius,length); tarsus->setTexture("Images/toy_fur3.jpg"); tarsus->init(odeHandle, mass, osgHandle); osg::Matrix m4; osg::Matrix m3 = ROTM(-angle,v%2==0 ? -1 : 1,0,0) * TRANSM(0,0,-len2/2) * m2; if(v < 2){ m4 = ROTM(v%2==0 ? angle : -angle,0,v%2==0 ? -1 : 1,0) * m3; }else if( v > 3){ m4 = ROTM(v%2==0 ? -angle : angle,0,v%2==0 ? -1 : 1,0) * m3; }else{ m4 = m3; } m4 = TRANSM(0,0,-length/2) * m4; tarsus->setPose(m4); tarsusParts.push_back(tarsus); objects.push_back(tarsus); if(conf.useTarsusJoints){ // springy joint HingeJoint* k = new HingeJoint(tibia, tarsus, Pos(0,0,-len2/2) * m2, Axis(v%2==0 ? -1 : 1,0,0) * m2); k->init(odeHandle, osgHandleJ, true, rad1 * 2.1); // servo used as a spring // spring2 = new HingeServo(k, -1, 1, 1, 0.01,0); // parameters are set later spring2 = new OneAxisServoVelocityControlled(odeHandle,k, -1, 1, 1, 0.01); // parameters are set later tarsussprings.push_back(spring2); joints.push_back(k); }else{ FixedJoint* k = new FixedJoint(tibia, tarsus); k->init(odeHandle, osgHandleJ, true, rad1 * 2.1); joints.push_back(k); } Primitive *section = tarsus; for(int i = 1; i < conf.numTarsusSections; i++){ double lengthS = length/1.5; double radiusS = radius/1.5; section = new Capsule(radiusS,lengthS); section->setTexture("Images/toy_fur3.jpg"); section->init(odeHandle, mass/2, osgHandle); osg::Matrix m5; if(v < 2){ m5 = TRANSM(0,0,-lengthS/2) * ROTM(v%2==0 ? angle : -angle,0,v%2==0 ? -1 : 1,0) * ROTM(v%2==0 ? angle : -angle,1,0,0) * TRANSM(0,0,-length/2) * m4; }else if(v > 3){ m5 = TRANSM(0,0,-lengthS/2) * ROTM(v%2==0 ? -angle : angle,0,v%2==0 ? -1 : 1,0) * ROTM(v%2==0 ? angle : -angle,1,0,0) * TRANSM(0,0,-length/2) * m4; }else{ m5 = TRANSM(0,0,-lengthS/2) * ROTM(v%2==0 ? angle : -angle,1,0,0) * TRANSM(0,0,-length/2) * m4; } section->setPose(m5); objects.push_back(section); tarsusParts.push_back(section); FixedJoint* fj = new FixedJoint(tarsusParts[i-1], tarsusParts[i]); fj->init(odeHandle, osgHandleJ, true, rad1 * 2.1); joints.push_back(fj); m4 = m5; } std::cout<< "legContactArray[" << n << "].legID = " << n << std::endl; legContactArray[n].legID = n; legContactArray[n].geomid = section->getGeom(); legContactArray[n].bodyID = section->getBody(); tarsusParts.clear(); } } // New: wiskers for ( int n = -1; n < 2; n+=2 ) { double l1 = conf.legLength*0.5; double t1 = conf.legLength/30; Primitive* whisker; Pos pos = Pos(conf.size/(2)+t1, n*twidth/4, conf.legLength + theight/5); osg::Matrix m = ROTM(M_PI/10, n,0,0) * ROTM(M_PI/2+M_PI/10, 0,-1,0) * TRANSM(pos) * pose; whisker = new Capsule(t1, l1); whisker->init(odeHandle, legmass/10, osgHandleJ); osg::Matrix m1 = TRANSM(0,0,-l1/2) * m; whisker->setPose(m1); objects.push_back(whisker); //FixedJoint* k = new FixedJoint(trunk, whisker); //k->init(odeHandle, osgHandle, false, 0); HingeJoint* k = new HingeJoint(trunk, whisker, Pos(0,0,0) * m, Axis(1,0,0) * m); k->init(odeHandle, osgHandleJ, true, t1 * 2.1); // servo used as a spring spring = new HingeServo(k, -M_PI/6, M_PI/6, .1, 0.01,0); whiskersprings.push_back(spring); joints.push_back(k); Primitive* whisker2; whisker2 = new Capsule(t1/2, l1); whisker2->init(odeHandle, legmass/10, osgHandleJ); osg::Matrix m2 = TRANSM(0,0,-l1/2) * ROTM(M_PI/10, n,0,0) * ROTM(M_PI/10, 0,1,0) * TRANSM(0,0,-l1/2) * m1; whisker2->setPose(m2); objects.push_back(whisker2); // k = new FixedJoint(whisker, whisker2); // k->init(odeHandle, osgHandleJ, false, 0); k = new HingeJoint(whisker, whisker2, Pos(0,0,-l1/2) * m1, Axis(0,1,0) * m1); k->init(odeHandle, osgHandleJ, true, t1 * 2.1); // servo used as a spring spring = new HingeServo(k, -M_PI/6, M_PI/6, .05, 0.01,0); whiskersprings.push_back(spring); joints.push_back(k); } notifyOnChange("dummy"); // apply all parameters. created=true; };
Joint* JointInteraction::xmlParseJoint(IrrXMLReader* xml, std::string& jointType) { bool finished = false; Constraint* constraint = NULL; Joint* joint = JointFactory::create(xml->getAttributeValue("type")); jointType = xml->getAttributeValue("type"); joint->setWorld(world); joint->setId(atoi(xml->getAttributeValue("id"))); joint->setJointInteractionClass(this); if (xml->getAttributeValue("active")) joint->setActive(atoi(xml->getAttributeValue("active"))); while (!finished && xml && xml->read()) { switch (xml->getNodeType()) { case EXN_ELEMENT: if (!strcmp("entities", xml->getNodeName())) { xmlParseEntities(xml, joint); } // else if else if (!strcmp("anchor", xml->getNodeName())) { if (!jointType.compare("ball") || !jointType.compare("Ball") || !jointType.compare("BALL")) { BallJoint* ball = dynamic_cast<BallJoint*>(joint); ball->setAnchor(atof(xml->getAttributeValue("xPos")), atof(xml->getAttributeValue("yPos")), atof(xml->getAttributeValue("zPos"))); } // if else if (!jointType.compare("hinge") || !jointType.compare("Hinge") || !jointType.compare("HINGE")) { HingeJoint* hinge = dynamic_cast<HingeJoint*>(joint); hinge->setAnchor(atof(xml->getAttributeValue("xPos")), atof(xml->getAttributeValue("yPos")), atof(xml->getAttributeValue("zPos"))); } // else if else if (!jointType.compare("universal") || !jointType.compare("Universal") || !jointType.compare("UNIVERSAL")) { UniversalJoint* universal = dynamic_cast<UniversalJoint*>(joint); universal->setAnchor(atof(xml->getAttributeValue("xPos")), atof(xml->getAttributeValue("yPos")), atof(xml->getAttributeValue("zPos"))); } // else if else if (!jointType.compare("hinge2") || !jointType.compare("Hinge2") || !jointType.compare("HINGE2")) { Hinge2Joint* hinge2 = dynamic_cast<Hinge2Joint*>(joint); hinge2->setAnchor(atof(xml->getAttributeValue("xPos")), atof(xml->getAttributeValue("yPos")), atof(xml->getAttributeValue("zPos"))); } // else if else printd(WARNING, "JointInteraction::loadJoints(): WARNING: JOINT-TYPE %s NEEDS NO anchor ELEMENT!!!\n", jointType.c_str()); } // else if else if (!strcmp("axis", xml->getNodeName())) { if (!jointType.compare("hinge") || !jointType.compare("Hinge") || !jointType.compare("HINGE")) { HingeJoint* hinge = dynamic_cast<HingeJoint*>(joint); hinge->setAxis(atof(xml->getAttributeValue("xDir")), atof(xml->getAttributeValue("yDir")), atof(xml->getAttributeValue("zDir"))); } // if else if (!jointType.compare("slider") || !jointType.compare("Slider") || !jointType.compare("SLIDER")) { SliderJoint* slider = dynamic_cast<SliderJoint*>(joint); slider->setAxis(atof(xml->getAttributeValue("xDir")), atof(xml->getAttributeValue("yDir")), atof(xml->getAttributeValue("zDir"))); } // else if else if (!jointType.compare("universal") || !jointType.compare("Universal") || !jointType.compare("UNIVERSAL")) { UniversalJoint* universal = dynamic_cast<UniversalJoint*>(joint); if (atoi(xml->getAttributeValue("index")) == 1) universal->setAxis1(atof(xml->getAttributeValue("xDir")), atof(xml->getAttributeValue("yDir")), atof(xml->getAttributeValue("zDir"))); else if (atoi(xml->getAttributeValue("index")) == 2) universal->setAxis2(atof(xml->getAttributeValue("xDir")), atof(xml->getAttributeValue("yDir")), atof(xml->getAttributeValue("zDir"))); else printd(WARNING, "JointInteraction::loadJoints(): WARNING: WRONG ATTRIBUTE FOUND IN axis ELEMENT of UniversalJoint!!!\n"); } // else if else if (!jointType.compare("hinge2") || !jointType.compare("Hinge2") || !jointType.compare("HINGE2")) { Hinge2Joint* hinge2 = dynamic_cast<Hinge2Joint*>(joint); if (atoi(xml->getAttributeValue("index")) == 1) hinge2->setAxis1(atof(xml->getAttributeValue("xDir")), atof(xml->getAttributeValue("yDir")), atof(xml->getAttributeValue("zDir"))); else if (atoi(xml->getAttributeValue("index")) == 2) hinge2->setAxis2(atof(xml->getAttributeValue("xDir")), atof(xml->getAttributeValue("yDir")), atof(xml->getAttributeValue("zDir"))); else printd(WARNING, "JointInteraction::loadJoints(): WARNING: WRONG ATTRIBUTE FOUND IN axis ELEMENT of Hinge2Joint!!!\n"); } // else if else printd(WARNING, "JointInteraction::loadJoints(): WARNING: JOINT-TYPE %s NEEDS NO axis ELEMENT!!!\n", jointType.c_str()); } // else if else if (!strcmp("angles", xml->getNodeName())) { if (!jointType.compare("hinge") || !jointType.compare("Hinge") || !jointType.compare("HINGE")) { HingeJoint* hinge = dynamic_cast<HingeJoint*>(joint); hinge->setAngles(atof(xml->getAttributeValue("min")), atof(xml->getAttributeValue("max"))); } // if else printd(WARNING, "JointInteraction::loadJoints(): WARNING: JOINT-TYPE %s HAS NO angles ELEMENT!!!\n", jointType.c_str()); } // else if else if (!strcmp("positions", xml->getNodeName())) { SliderJoint* slider = dynamic_cast<SliderJoint*>(joint); slider->setPositions(atof(xml->getAttributeValue("min")), atof(xml->getAttributeValue("max"))); } // else if else if (!strcmp("activateIF", xml->getNodeName())) { constraint = new Constraint(this); constraint->setActionType(Constraint::ACTIVATEIF); constraint->setOwner(joint); xmlParseConstraintAttributes(xml, constraint); joint->addConstraint(constraint); constraint = NULL; } // else if else if (!strcmp("deactivateIF", xml->getNodeName())) { constraint = new Constraint(this); constraint->setActionType(Constraint::DEACTIVATEIF); constraint->setOwner(joint); xmlParseConstraintAttributes(xml, constraint); joint->addConstraint(constraint); constraint = NULL; } // else if else if (!strcmp("activeIF", xml->getNodeName())) { constraint = new Constraint(this); constraint->setActionType(Constraint::ACTIVEIF); constraint->setOwner(joint); xmlParseConstraintAttributes(xml, constraint); joint->addConstraint(constraint); constraint = NULL; } // else if else if (!strcmp("inactiveIF", xml->getNodeName())) { constraint = new Constraint(this); constraint->setActionType(Constraint::INACTIVEIF); constraint->setOwner(joint); xmlParseConstraintAttributes(xml, constraint); joint->addConstraint(constraint); constraint = NULL; } // else if break; case EXN_ELEMENT_END: if (!strcmp("joint", xml->getNodeName())) finished = true; break; default: break; } // switch } // while assert(xml); return joint; } // xmlParseJoint