void BallJoint::act(bool initial) { if(!initial) { for(unsigned int i=0; i < this->motors.size(); i++) { int numAxes = this->motors[i]->getNumberOfParsedAxes(); if(numAxes > 0) { if(this->motors[i]->getMotorType() == VELOCITY) { double desiredVelocity = this->motors[i]->getDesiredVelocity(); dJointSetAMotorParam(*(this->motors[i]->getPhysicalMotor()), dParamVel, dReal(desiredVelocity)); } if(numAxes > 1) { if(this->motors[i]->getMotorType() == VELOCITY) { double desiredVelocity = this->motors[i]->getDesiredVelocity(); dJointSetAMotorParam(*(this->motors[i]->getPhysicalMotor()), dParamVel2, dReal(desiredVelocity)); } if(numAxes > 2) { if(this->motors[i]->getMotorType() == VELOCITY) { double desiredVelocity = this->motors[i]->getDesiredVelocity(); dJointSetAMotorParam(*(this->motors[i]->getPhysicalMotor()), dParamVel3, dReal(desiredVelocity)); } } } } } } }
void BallJoint::createMotorPhysics() { for(unsigned int i=0; i<this->motors.size(); i++) { dJointID physMotor = dJointCreateAMotor(*(this->simulation->getPhysicalWorld()), 0); this->motors[i]->setPhysicalMotor(physMotor); dJointAttach(physMotor, dJointGetBody(this->physicalJoint, 0), dJointGetBody(this->physicalJoint, 1)); if(this->motors[i]->getKind() == "eulermotor") dJointSetAMotorMode(physMotor, dAMotorEuler); else dJointSetAMotorMode(physMotor, dAMotorUser); //user defined motor if(this->motors[i]->getKind() == "userdefinedmotor") { unsigned short numOfAxes = this->motors[i]->getNumberOfParsedAxes(); dJointSetAMotorNumAxes(physMotor, numOfAxes); dBodyID b1= dJointGetBody(this->physicalJoint, 0); if(b1 != NULL) { for(int j=0; j < numOfAxes; j++) { MotorAxisDescription* ax = this->motors[i]->getAxis(j); dJointSetAMotorAxis(physMotor, j, 1, dReal(ax->direction.v[0]), dReal(ax->direction.v[1]), dReal(ax->direction.v[2])); } } else { for(int j=0; j < numOfAxes; j++) { MotorAxisDescription* ax = this->motors[i]->getAxis(j); dJointSetAMotorAxis(physMotor, j, 0, dReal(ax->direction.v[0]), dReal(ax->direction.v[1]), dReal(ax->direction.v[2])); } } //maxForce and maxVelocity are stored in each axis of the motor dJointSetAMotorParam(physMotor, dParamFMax, dReal(this->motors[i]->getAxis(0)->maxForce)); if(numOfAxes > 1) { dJointSetAMotorParam(physMotor, dParamFMax2, dReal(this->motors[i]->getAxis(1)->maxForce)); if(numOfAxes > 2) { dJointSetAMotorParam(physMotor, dParamFMax3, dReal(this->motors[i]->getAxis(2)->maxForce)); } } //stops are useless for balljoint } } }
void osaODEServoMotor::SetVelocity( double qd ){ if( dJointGetType( MotorID() ) == dJointTypeAMotor ){ dJointSetAMotorParam( MotorID(), dParamVel, qd ); dJointSetAMotorParam( MotorID(), dParamFMax, ftmax ); } if( dJointGetType( MotorID() ) == dJointTypeLMotor ){ dJointSetLMotorParam( MotorID(), dParamVel, qd/1000.0 ); dJointSetLMotorParam( MotorID(), dParamFMax, ftmax ); } }
// ang2 = position angle // ang = rotation angle Robot::Wheel::Wheel(Robot* robot,int _id,float ang,float ang2,int wheeltexid) { id = _id; rob = robot; float rad = rob->cfg->robotSettings.RobotRadius - rob->cfg->robotSettings.WheelThickness / 2.0; ang *= M_PI/180.0f; ang2 *= M_PI/180.0f; float x = rob->m_x; float y = rob->m_y; float z = rob->m_z; float centerx = x+rad*cos(ang2); float centery = y+rad*sin(ang2); float centerz = z-rob->cfg->robotSettings.RobotHeight*0.5+rob->cfg->robotSettings.WheelRadius-rob->cfg->robotSettings.BottomHeight; cyl = new PCylinder(centerx,centery,centerz,rob->cfg->robotSettings.WheelRadius,rob->cfg->robotSettings.WheelThickness,rob->cfg->robotSettings.WheelMass,0.9,0.9,0.9,wheeltexid); cyl->setRotation(-sin(ang),cos(ang),0,M_PI*0.5); cyl->setBodyRotation(-sin(ang),cos(ang),0,M_PI*0.5,true); //set local rotation matrix cyl->setBodyPosition(centerx-x,centery-y,centerz-z,true); //set local position vector cyl->space = rob->space; rob->w->addObject(cyl); joint = dJointCreateHinge (rob->w->world,0); dJointAttach (joint,rob->chassis->body,cyl->body); const dReal *a = dBodyGetPosition (cyl->body); dJointSetHingeAxis (joint,cos(ang),sin(ang),0); dJointSetHingeAnchor (joint,a[0],a[1],a[2]); motor = dJointCreateAMotor(rob->w->world,0); dJointAttach(motor,rob->chassis->body,cyl->body); dJointSetAMotorNumAxes(motor,1); dJointSetAMotorAxis(motor,0,1,cos(ang),sin(ang),0); dJointSetAMotorParam(motor,dParamFMax,rob->cfg->robotSettings.Wheel_Motor_FMax); speed = 0; }
void ODE_Dynamixel::valueChanged(Value *value) { Dynamixel::valueChanged(value); if((value == mDesiredMotorTorqueValue || value == mFMaxFactor) && mJoint != 0) { dJointSetAMotorParam(mJoint, dParamFMax, mFMaxFactor->get() * Math::max(0.0, mDesiredMotorTorqueValue->get())); } }
void ODE_Dynamixel::updateActuators() { if(mJoint != 0) { mCurrentPosition = dJointGetAMotorAngle(mJoint, 0); calculateMotorFrictionAndVelocity(); dJointSetHingeParam(mHingeJoint, dParamFMax, mCalculatedFriction); dJointSetAMotorParam(mJoint, dParamVel, mCalculatedVelocity); } }
void ODE_PID_PassiveActuatorModel::updateInputValues() { if(mJoint != 0) { double desiredAngle = 0.0; PassiveActuatorAdapter *owner = dynamic_cast<PassiveActuatorAdapter*>(mOwner); if(owner != 0) { desiredAngle = (owner->getReferenceAngle() + owner->getReferenceOffsetAngle()) * owner->getGearRatio() + owner->getOffsetAngle(); } double currentAngle = dJointGetAMotorAngle(mJoint, 0); mPID_Controller.update(currentAngle, desiredAngle * Math::PI / 180.0); dJointSetAMotorParam(mJoint, dParamFMax, mMaxForce->get()); dJointSetHingeParam(mHingeJoint, dParamFMax, mFriction->get()); dJointSetAMotorParam(mJoint, dParamVel, mPID_Controller.getVelocity()); } }
void ComponentMovement::createMotorJoints() { ASSERT(physicsEngine, "Null pointer: physicsEngine"); dBodyID body = getBodyID(); ASSERT(body, "Cannot create motor joints: No physics body available"); amotor = dJointCreateAMotor(physicsEngine->getWorld(), 0); dJointAttach(amotor, body, 0); dJointSetAMotorNumAxes(amotor, 3); dJointSetAMotorAxis (amotor, 0, 1, 1, 0, 0); dJointSetAMotorAxis (amotor, 1, 1, 0, 1, 0); dJointSetAMotorAxis (amotor, 2, 1, 0, 0, 1); dJointSetAMotorParam(amotor, dParamFMax, maxForce); dJointSetAMotorParam(amotor, dParamFMax2, maxForce); dJointSetAMotorParam(amotor, dParamFMax3, maxForce); dJointSetAMotorParam(amotor, dParamVel, 0); dJointSetAMotorParam(amotor, dParamVel2, 0); dJointSetAMotorParam(amotor, dParamVel3, 0); lmotor = dJointCreateLMotor(physicsEngine->getWorld(), 0); dJointAttach(lmotor, body, 0); dJointSetLMotorNumAxes(lmotor, 2); dJointSetLMotorAxis (lmotor, 0, 1, 1, 0, 0); dJointSetLMotorAxis (lmotor, 1, 1, 0, 1, 0); dJointSetLMotorParam(lmotor, dParamFMax, maxForce); dJointSetLMotorParam(lmotor, dParamFMax2, maxForce); dJointSetLMotorParam(lmotor, dParamVel, 0); dJointSetLMotorParam(lmotor, dParamVel2, 0); }
void capsule::create_physical_body( double x, double y, double z, double radius, double length, double mass, manager& mgr) { this->radius = radius; this->length = length; world_id = mgr.ode_world(); space_id = mgr.ode_space(); //set the body orientation // dMatrix3 R; //dRFromAxisAndAngle(R,1,0,0,M_PI/2); //dBodySetRotation(body_id,R); //create the geom geom_id=dCreateCapsule(mgr.ode_space(),radius,length); object::set_geom_data(geom_id); //must make sure to set the geom data for the collision callback! dGeomSetPosition (geom_id, x, y, z); if(mass > 0) { object::create_rigid_body(x, y, z, mgr); set_mass(mass); dGeomSetBody(geom_id,body_id); //create an amotor to keep the body vertical amotor_id=dJointCreateAMotor(mgr.ode_world(),0); dJointAttach(amotor_id,body_id,0); dJointSetAMotorMode(amotor_id,dAMotorEuler); dJointSetAMotorNumAxes(amotor_id,3); dJointSetAMotorAxis(amotor_id,0,0,1,0,0); dJointSetAMotorAxis(amotor_id,1,0,0,1,0); dJointSetAMotorAxis(amotor_id,2,0,0,0,1); dJointSetAMotorAngle(amotor_id,0,0); dJointSetAMotorAngle(amotor_id,1,0); dJointSetAMotorAngle(amotor_id,2,0); dJointSetAMotorParam(amotor_id,dParamLoStop,-0); dJointSetAMotorParam(amotor_id,dParamLoStop3,-0); dJointSetAMotorParam(amotor_id,dParamLoStop2,-0); dJointSetAMotorParam(amotor_id,dParamHiStop,0); dJointSetAMotorParam(amotor_id,dParamHiStop3,0); dJointSetAMotorParam(amotor_id,dParamHiStop2,0); } }
void CPHCapture::PullingUpdate() { if(!m_taget_element->isActive()||inl_ph_world().Device().dwTimeGlobal-m_time_start>m_capture_time) { Release(); return; } Fvector dir; Fvector capture_bone_position; //CObject* object=smart_cast<CObject*>(m_character->PhysicsRefObject()); capture_bone_position.set(m_capture_bone->mTransform.c); m_character->PhysicsRefObject()->ObjectXFORM().transform_tiny(capture_bone_position); m_taget_element->GetGlobalPositionDynamic(&dir); dir.sub(capture_bone_position,dir); float dist=dir.magnitude(); if(dist>m_pull_distance) { Release(); return; } dir.mul(1.f/dist); if(dist<m_capture_distance) { m_back_force=0.f; m_joint=dJointCreateBall(0,0); m_island.AddJoint(m_joint); m_ajoint=dJointCreateAMotor(0,0); m_island.AddJoint(m_ajoint); dJointSetAMotorMode (m_ajoint, dAMotorEuler); dJointSetAMotorNumAxes (m_ajoint, 3); CreateBody(); dBodySetPosition(m_body,capture_bone_position.x,capture_bone_position.y,capture_bone_position.z); VERIFY( smart_cast<CPHElement*>(m_taget_element) ); CPHElement * e = static_cast<CPHElement*>(m_taget_element); dJointAttach(m_joint,m_body,e->get_body()); dJointAttach(m_ajoint,m_body,e->get_body()); dJointSetFeedback (m_joint, &m_joint_feedback); dJointSetFeedback (m_ajoint, &m_joint_feedback); dJointSetBallAnchor(m_joint,capture_bone_position.x,capture_bone_position.y,capture_bone_position.z); dJointSetAMotorAxis (m_ajoint, 0, 1, dir.x, dir.y, dir.z); if(dir.x>EPS) { if(dir.y>EPS) { float mag=dir.x*dir.x+dir.y*dir.y; dJointSetAMotorAxis (m_ajoint, 2, 2, -dir.y/mag, dir.x/mag, 0.f); } else if(dir.z>EPS) { float mag=dir.x*dir.x+dir.z*dir.z; dJointSetAMotorAxis (m_ajoint, 2, 2, -dir.z/mag,0.f,dir.x/mag); } else { dJointSetAMotorAxis (m_ajoint, 2, 2, 1.f,0.f,0.f); } } else { if(dir.y>EPS) { if(dir.z>EPS) { float mag=dir.y*dir.y+dir.z*dir.z; dJointSetAMotorAxis (m_ajoint, 2, 2,0.f,-dir.z/mag,dir.y/mag); } else { dJointSetAMotorAxis (m_ajoint, 2, 2, 0.f,1.f,0.f); } } else { dJointSetAMotorAxis (m_ajoint, 2, 2, 0.f,0.f,1.f); } } //float hi=-M_PI/2.f,lo=-hi; //dJointSetAMotorParam(m_ajoint,dParamLoStop ,lo); //dJointSetAMotorParam(m_ajoint,dParamHiStop ,hi); //dJointSetAMotorParam(m_ajoint,dParamLoStop2 ,lo); //dJointSetAMotorParam(m_ajoint,dParamHiStop2 ,hi); //dJointSetAMotorParam(m_ajoint,dParamLoStop3 ,lo); //dJointSetAMotorParam(m_ajoint,dParamHiStop3 ,hi); dJointSetAMotorParam(m_ajoint,dParamFMax ,m_capture_force*0.2f); dJointSetAMotorParam(m_ajoint,dParamVel ,0.f); dJointSetAMotorParam(m_ajoint,dParamFMax2 ,m_capture_force*0.2f); dJointSetAMotorParam(m_ajoint,dParamVel2 ,0.f); dJointSetAMotorParam(m_ajoint,dParamFMax3 ,m_capture_force*0.2f); dJointSetAMotorParam(m_ajoint,dParamVel3 ,0.f); /////////////////////////////////// float sf=0.1f,df=10.f; float erp=ERP(world_spring*sf,world_damping*df); float cfm=CFM(world_spring*sf,world_damping*df); dJointSetAMotorParam(m_ajoint,dParamStopERP ,erp); dJointSetAMotorParam(m_ajoint,dParamStopCFM ,cfm); dJointSetAMotorParam(m_ajoint,dParamStopERP2 ,erp); dJointSetAMotorParam(m_ajoint,dParamStopCFM2 ,cfm); dJointSetAMotorParam(m_ajoint,dParamStopERP3 ,erp); dJointSetAMotorParam(m_ajoint,dParamStopCFM3 ,cfm); ///////////////////////////////////////////////////////////////////// ///dJointSetAMotorParam(m_joint1,dParamFudgeFactor ,0.1f); //dJointSetAMotorParam(m_joint1,dParamFudgeFactor2 ,0.1f); //dJointSetAMotorParam(m_joint1,dParamFudgeFactor3 ,0.1f); ///////////////////////////////////////////////////////////////////////////// sf=0.1f,df=10.f; erp=ERP(world_spring*sf,world_damping*df); cfm=CFM(world_spring*sf,world_damping*df); dJointSetAMotorParam(m_ajoint,dParamCFM ,cfm); dJointSetAMotorParam(m_ajoint,dParamCFM2 ,cfm); dJointSetAMotorParam(m_ajoint,dParamCFM3 ,cfm); /////////////////////////// //dJointSetAMotorParam(m_ajoint,dParamLoStop ,0.f); //dJointSetAMotorParam(m_ajoint,dParamHiStop ,0.f); m_taget_element->set_LinearVel ( Fvector().set( 0 ,0, 0 ) ); m_taget_element->set_AngularVel( Fvector().set( 0 ,0, 0 ) ); m_taget_element->set_DynamicLimits(); //m_taget_object->PPhysicsShell()->set_JointResistance() e_state=cstCaptured; return; } m_taget_element->applyForce(dir,m_pull_force); }
dJointID ODE_PID_PassiveActuatorModel::createJoint(dBodyID body1, dBodyID body2) { Vector3DValue *jointAxisPoint1 = dynamic_cast<Vector3DValue*>(mOwner->getParameter("AxisPoint1")); Vector3DValue *jointAxisPoint2 = dynamic_cast<Vector3DValue*>(mOwner->getParameter("AxisPoint2")); DoubleValue *minAngleValue = dynamic_cast<DoubleValue*>(mOwner->getParameter("MinAngle")); DoubleValue *maxAngleValue = dynamic_cast<DoubleValue*>(mOwner->getParameter("MaxAngle")); if(jointAxisPoint1 == 0 || jointAxisPoint2 == 0 || minAngleValue == 0 || maxAngleValue == 0) { Core::log("ODE_PID_PassiveActuatorModel: Could not find all required parameter values."); return 0; } if(jointAxisPoint1->get().equals(jointAxisPoint2->get(), -1)) { Core::log("ODE_PID_PassiveActuatorModel: Invalid axis points " + jointAxisPoint1->getValueAsString() + " and " + jointAxisPoint2->getValueAsString() + "."); return 0; } if(jointAxisPoint1->get().equals(jointAxisPoint2->get(), -1)) { Core::log("Invalid axes for ODE_PID_PassiveActuatorModel."); return 0; } Vector3D anchor = jointAxisPoint1->get(); Vector3D axis = jointAxisPoint2->get() - jointAxisPoint1->get(); dJointID joint = dJointCreateAMotor(mWorldID, mGeneralJointGroup); dJointAttach(joint, body1, body2); dJointSetAMotorMode(joint, dAMotorEuler); dJointSetAMotorParam(joint, dParamVel, 0.0); dJointSetAMotorParam(joint, dParamFMax, 1.0); dJointSetAMotorParam(joint, dParamCFM, mCFMValue->get()); dJointSetAMotorParam(joint, dParamStopERP, mStopERPValue->get()); dJointSetAMotorParam(joint, dParamStopCFM, mStopCFMValue->get()); dJointSetAMotorParam(joint, dParamBounce, mBounceValue->get()); dJointSetAMotorParam(joint, dParamFudgeFactor, mFudgeFactorValue->get()); axis.normalize(); Vector3D perpedicular; if(axis.getY() != 0.0 || axis.getX() != 0.0) { perpedicular.set(-axis.getY(), axis.getX(), 0); } else { perpedicular.set(0, -axis.getZ(), axis.getY()); } perpedicular.normalize(); // If one of the bodies is static, the motor axis need to be defined in a different way. For different constellations of static and dynamic bodies, the turn direction of the motor changed, so this had to be added. if(body1 == 0) { dJointSetAMotorAxis(joint, 0, 0, -axis.getX(), -axis.getY(), -axis.getZ()); } else { dJointSetAMotorAxis(joint, 0, 1, axis.getX(), axis.getY(), axis.getZ()); } if(body2 == 0) { dJointSetAMotorAxis(joint, 2, 0, -perpedicular.getX(), -perpedicular.getY(), -perpedicular.getZ()); } else { dJointSetAMotorAxis(joint, 2, 2, perpedicular.getX(), perpedicular.getY(), perpedicular.getZ()); } mHingeJoint = dJointCreateHinge(mWorldID, mGeneralJointGroup); dJointAttach(mHingeJoint, body1, body2); dJointSetHingeAnchor(mHingeJoint, anchor.getX(), anchor.getY(), anchor.getZ()); dJointSetHingeAxis(mHingeJoint, axis.getX(), axis.getY(), axis.getZ()); double minAngle = (minAngleValue->get() * Math::PI) / 180.0; double maxAngle = (maxAngleValue->get() * Math::PI) / 180.0; if(body1 == 0) { double tmp = minAngle; minAngle = -1.0 * maxAngle; maxAngle = -1.0 * tmp; } dJointSetHingeParam(mHingeJoint, dParamLoStop, minAngle); dJointSetHingeParam(mHingeJoint, dParamHiStop, maxAngle); dJointSetHingeParam(mHingeJoint, dParamVel, 0.0); return joint; }
void PhysicsAMotorJoint::changed(ConstFieldMaskArg whichField, UInt32 origin, BitVector details) { //Do not respond to changes that have a Sync origin if(origin & ChangedOrigin::Sync) { return; } if(whichField & WorldFieldMask) { if(_JointID) { dJointDestroy(_JointID); _JointID = dJointCreateAMotor(getWorld()->getWorldID(), 0); } else { _JointID = dJointCreateAMotor(getWorld()->getWorldID(), 0); if(!(whichField & VelFieldMask)) { setVel(dJointGetAMotorParam(_JointID,dParamVel)); } if(!(whichField & FMaxFieldMask)) { setFMax(dJointGetAMotorParam(_JointID,dParamFMax)); } if(!(whichField & FudgeFactorFieldMask)) { setFudgeFactor(dJointGetAMotorParam(_JointID,dParamFudgeFactor)); } if(!(whichField & Vel2FieldMask)) { setVel2(dJointGetAMotorParam(_JointID,dParamVel2)); } if(!(whichField & FMax2FieldMask)) { setFMax2(dJointGetAMotorParam(_JointID,dParamFMax2)); } if(!(whichField & FudgeFactor2FieldMask)) { setFudgeFactor2(dJointGetAMotorParam(_JointID,dParamFudgeFactor2)); } if(!(whichField & Vel3FieldMask)) { setVel3(dJointGetAMotorParam(_JointID,dParamVel3)); } if(!(whichField & FMax3FieldMask)) { setFMax3(dJointGetAMotorParam(_JointID,dParamFMax3)); } if(!(whichField & FudgeFactor3FieldMask)) { setFudgeFactor3(dJointGetAMotorParam(_JointID,dParamFudgeFactor3)); } if(!(whichField & HiStopFieldMask)) { setHiStop(dJointGetAMotorParam(_JointID,dParamHiStop)); } if(!(whichField & LoStopFieldMask)) { setLoStop(dJointGetAMotorParam(_JointID,dParamLoStop)); } if(!(whichField & BounceFieldMask)) { setBounce(dJointGetAMotorParam(_JointID,dParamBounce)); } if(!(whichField & CFMFieldMask)) { setCFM(dJointGetAMotorParam(_JointID,dParamCFM)); } if(!(whichField & StopCFMFieldMask)) { setStopCFM(dJointGetAMotorParam(_JointID,dParamStopCFM)); } if(!(whichField & StopERPFieldMask)) { setStopERP(dJointGetAMotorParam(_JointID,dParamStopERP)); } if(!(whichField & HiStop2FieldMask)) { setHiStop2(dJointGetAMotorParam(_JointID,dParamHiStop2)); } if(!(whichField & LoStop2FieldMask)) { setLoStop2(dJointGetAMotorParam(_JointID,dParamLoStop2)); } if(!(whichField & Bounce2FieldMask)) { setBounce2(dJointGetAMotorParam(_JointID,dParamBounce2)); } if(!(whichField & CFM2FieldMask)) { setCFM2(dJointGetAMotorParam(_JointID,dParamCFM2)); } if(!(whichField & StopCFM2FieldMask)) { setStopCFM2(dJointGetAMotorParam(_JointID,dParamStopCFM2)); } if(!(whichField & StopERP2FieldMask)) { setStopERP2(dJointGetAMotorParam(_JointID,dParamStopERP2)); } if(!(whichField & HiStop3FieldMask)) { setHiStop3(dJointGetAMotorParam(_JointID,dParamHiStop3)); } if(!(whichField & LoStop3FieldMask)) { setLoStop3(dJointGetAMotorParam(_JointID,dParamLoStop3)); } if(!(whichField & Bounce3FieldMask)) { setBounce3(dJointGetAMotorParam(_JointID,dParamBounce3)); } if(!(whichField & CFM3FieldMask)) { setCFM3(dJointGetAMotorParam(_JointID,dParamCFM3)); } if(!(whichField & StopCFM3FieldMask)) { setStopCFM3(dJointGetAMotorParam(_JointID,dParamStopCFM3)); } if(!(whichField & StopERP3FieldMask)) { setStopERP3(dJointGetAMotorParam(_JointID,dParamStopERP3)); } } } Inherited::changed(whichField, origin, details); if((whichField & NumAxesFieldMask) || (whichField & WorldFieldMask)) { dJointSetAMotorNumAxes(_JointID,getNumAxes()); } if((whichField & Axis1FieldMask) || (whichField & Axis1ReferenceFrameFieldMask) || (whichField & WorldFieldMask)) { dJointSetAMotorAxis(_JointID,0, getAxis1ReferenceFrame(), getAxis1().x(), getAxis1().y(), getAxis1().z()); } if((whichField & Axis2FieldMask) || (whichField & Axis2ReferenceFrameFieldMask) || (whichField & WorldFieldMask)) { dJointSetAMotorAxis(_JointID,1, getAxis2ReferenceFrame(), getAxis2().x(), getAxis2().y(), getAxis2().z()); } if((whichField & Axis3FieldMask) || (whichField & Axis3ReferenceFrameFieldMask) || (whichField & WorldFieldMask)) { dJointSetAMotorAxis(_JointID,2, getAxis3ReferenceFrame(), getAxis3().x(), getAxis3().y(), getAxis3().z()); } if((whichField & VelFieldMask) || (whichField & WorldFieldMask)) { dJointSetAMotorParam(_JointID, dParamVel, getVel()); } if((whichField & FMaxFieldMask) || (whichField & WorldFieldMask)) { dJointSetAMotorParam(_JointID, dParamFMax, getFMax()); } if((whichField & FudgeFactorFieldMask) || (whichField & WorldFieldMask)) { dJointSetAMotorParam(_JointID, dParamFudgeFactor, getFudgeFactor()); } if((whichField & Vel2FieldMask) || (whichField & WorldFieldMask)) { dJointSetAMotorParam(_JointID, dParamVel2, getVel2()); } if((whichField & FMax2FieldMask) || (whichField & WorldFieldMask)) { dJointSetAMotorParam(_JointID, dParamFMax2, getFMax2()); } if((whichField & FudgeFactor2FieldMask) || (whichField & WorldFieldMask)) { dJointSetAMotorParam(_JointID, dParamFudgeFactor2, getFudgeFactor2()); } if((whichField & Vel3FieldMask) || (whichField & WorldFieldMask)) { dJointSetAMotorParam(_JointID, dParamVel3, getVel3()); } if((whichField & FMax3FieldMask) || (whichField & WorldFieldMask)) { dJointSetAMotorParam(_JointID, dParamFMax3, getFMax3()); } if((whichField & FudgeFactor3FieldMask) || (whichField & WorldFieldMask)) { dJointSetAMotorParam(_JointID, dParamFudgeFactor3, getFudgeFactor3()); } if((whichField & HiStopFieldMask) || (whichField & WorldFieldMask)) { dJointSetAMotorParam(_JointID, dParamHiStop, getHiStop()); } if((whichField & LoStopFieldMask) || (whichField & WorldFieldMask)) { dJointSetAMotorParam(_JointID, dParamLoStop, getLoStop()); } if((whichField & BounceFieldMask) || (whichField & WorldFieldMask)) { dJointSetAMotorParam(_JointID, dParamBounce, getBounce()); } if((whichField & CFMFieldMask) || (whichField & WorldFieldMask)) { dJointSetAMotorParam(_JointID, dParamCFM, getCFM()); } if((whichField & StopERPFieldMask) || (whichField & WorldFieldMask)) { dJointSetAMotorParam(_JointID, dParamStopERP, getStopERP()); } if((whichField & StopCFMFieldMask) || (whichField & WorldFieldMask)) { dJointSetAMotorParam(_JointID, dParamStopCFM, getStopCFM()); } if((whichField & HiStop2FieldMask) || (whichField & WorldFieldMask)) { dJointSetAMotorParam(_JointID, dParamHiStop2, getHiStop2()); } if((whichField & LoStop2FieldMask) || (whichField & WorldFieldMask)) { dJointSetAMotorParam(_JointID, dParamLoStop2, getLoStop2()); } if((whichField & Bounce2FieldMask) || (whichField & WorldFieldMask)) { dJointSetAMotorParam(_JointID, dParamBounce2, getBounce2()); } if((whichField & CFM2FieldMask) || (whichField & WorldFieldMask)) { dJointSetAMotorParam(_JointID, dParamCFM2, getCFM2()); } if((whichField & StopERP2FieldMask) || (whichField & WorldFieldMask)) { dJointSetAMotorParam(_JointID, dParamStopERP2, getStopERP2()); } if((whichField & StopCFM2FieldMask) || (whichField & WorldFieldMask)) { dJointSetAMotorParam(_JointID, dParamStopCFM2, getStopCFM2()); } if((whichField & HiStop3FieldMask) || (whichField & WorldFieldMask)) { dJointSetAMotorParam(_JointID, dParamHiStop3, getHiStop3()); } if((whichField & LoStop3FieldMask) || (whichField & WorldFieldMask)) { dJointSetAMotorParam(_JointID, dParamLoStop3, getLoStop3()); } if((whichField & Bounce3FieldMask) || (whichField & WorldFieldMask)) { dJointSetAMotorParam(_JointID, dParamBounce3, getBounce3()); } if((whichField & CFM3FieldMask) || (whichField & WorldFieldMask)) { dJointSetAMotorParam(_JointID, dParamCFM3, getCFM3()); } if((whichField & StopERP3FieldMask) || (whichField & WorldFieldMask)) { dJointSetAMotorParam(_JointID, dParamStopERP3, getStopERP3()); } if((whichField & StopCFM3FieldMask) || (whichField & WorldFieldMask)) { dJointSetAMotorParam(_JointID, dParamStopCFM3, getStopCFM3()); } }
State* init() { State* state = new State(); dInitODE(); SDL_Init(SDL_INIT_EVERYTHING); state->screen = SDL_SetVideoMode(WIDTH, HEIGHT, 32, SDL_OPENGL); SDL_WM_SetCaption("Physics", NULL); SDL_Flip(state->screen); SDL_ShowCursor(SDL_DISABLE); glMatrixMode(GL_PROJECTION); glLoadIdentity(); gluPerspective(100, (float)WIDTH/HEIGHT, 0.5, 1000); glMatrixMode(GL_MODELVIEW); glLoadIdentity(); glEnable(GL_LIGHTING); glEnable(GL_LIGHT0); GLfloat light_ambient[] = { 1, 1, 1, 1 }; glLightfv(GL_LIGHT0, GL_AMBIENT, light_ambient); GLfloat lightpos[] = {0, 4, 0, 1}; glLightfv(GL_LIGHT0, GL_POSITION, lightpos); glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_FASTEST); glShadeModel(GL_SMOOTH); glEnable(GL_BLEND); glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); glEnable(GL_DEPTH_TEST); glEnable(GL_CULL_FACE); glClearColor(0, 0, 0, 1); state->posx = 0;//21; state->posy = 4;//8; state->posz = 5;//21; state->rotx = 0; state->roty = 0;//-40; state->rotz = 0; state->wkey = false; state->akey = false; state->skey = false; state->dkey = false; state->gkey = false; state->simSpeed = 60; state->carcam = false; state->carbodydrawable = new Drawable("objs/carbody.obj"); state->carwheeldrawable = new Drawable("objs/carwheel.obj"); state->map = new Drawable("objs/jump2.obj"); state->cube = new Drawable("objs/cube.obj"); // state->monkey = new Drawable("objs/monkey.obj"); state->world = dWorldCreate(); dWorldSetGravity(state->world, 0, -9.8, 0); state->worldSpace = dHashSpaceCreate(0); const double carHeight = 0.65; const double carZ = 90; const double carX = 0; const float speed = -1000; const float force = 200; state->carbodybody = dBodyCreate(state->world); dBodySetPosition(state->carbodybody, carX, carHeight, carZ); dMass bodymass; dMassSetBoxTotal(&bodymass, 100, 2, 4, 1); dBodySetMass(state->carbodybody, &bodymass); dGeomID carbodygeom = dCreateBox(state->worldSpace, 2, 1, 4); dGeomSetBody(carbodygeom, state->carbodybody); state->flcarwheelbody = dBodyCreate(state->world); dBodySetPosition(state->flcarwheelbody, carX-1.5, carHeight - 0.5, carZ+2); const dMatrix3 m = { 0, 0, 1, 0 , 0, 1, 0, 0 , 0, 0, 1, 0 }; dBodySetRotation(state->flcarwheelbody, m); dMass wheelmass; dMassSetCylinder(&wheelmass, 0.1, 2, 0.5, 0.2); dBodySetMass(state->flcarwheelbody, &wheelmass); dJointID joint = dJointCreateHinge(state->world, 0); dJointAttach(joint, state->carbodybody, state->flcarwheelbody); dJointSetHingeAnchor(joint, carX-1.5, carHeight - 0.5, carZ+2); dJointSetHingeAxis(joint, 1, 0, 0); dGeomID flcarwheelgeom = dCreateCylinder(state->worldSpace, 0.5, 0.2); dGeomSetBody(flcarwheelgeom, state->flcarwheelbody); dJointID motor = dJointCreateAMotor(state->world, 0); dJointAttach(motor, state->flcarwheelbody, state->carbodybody); dJointSetAMotorNumAxes(motor, 1); dJointSetAMotorAxis(motor, 0, 1, 1, 0, 0); dJointSetAMotorParam(motor, dParamVel, speed); dJointSetAMotorParam(motor, dParamFMax, force); state->frcarwheelbody = dBodyCreate(state->world); dBodySetPosition(state->frcarwheelbody, carX+1.5, carHeight - 0.5, carZ+2); dBodySetRotation(state->frcarwheelbody, m); dBodySetMass(state->frcarwheelbody, &wheelmass); joint = dJointCreateHinge(state->world, 0); dJointAttach(joint, state->carbodybody, state->frcarwheelbody); dJointSetHingeAnchor(joint, carX+1.5, carHeight - 0.5, carZ+2); dJointSetHingeAxis(joint, 1, 0, 0); dGeomID frcarwheelgeom = dCreateCylinder(state->worldSpace, 0.5, 0.2); dGeomSetBody(frcarwheelgeom, state->frcarwheelbody); motor = dJointCreateAMotor(state->world, 0); dJointAttach(motor, state->frcarwheelbody, state->carbodybody); dJointSetAMotorNumAxes(motor, 1); dJointSetAMotorAxis(motor, 0, 1, 1, 0, 0); dJointSetAMotorParam(motor, dParamVel, speed); dJointSetAMotorParam(motor, dParamFMax, force); state->blcarwheelbody = dBodyCreate(state->world); dBodySetPosition(state->blcarwheelbody, carX-1.5, carHeight - 0.5, carZ-2); dBodySetRotation(state->blcarwheelbody, m); dBodySetMass(state->blcarwheelbody, &wheelmass); joint = dJointCreateHinge(state->world, 0); dJointAttach(joint, state->carbodybody, state->blcarwheelbody); dJointSetHingeAnchor(joint, carX-1.5, carHeight - 0.5, carZ-2); dJointSetHingeAxis(joint, 1, 0, 0); dGeomID blcarwheelgeom = dCreateCylinder(state->worldSpace, 0.5, 0.2); dGeomSetBody(blcarwheelgeom, state->blcarwheelbody); motor = dJointCreateAMotor(state->world, 0); dJointAttach(motor, state->blcarwheelbody, state->carbodybody); dJointSetAMotorNumAxes(motor, 1); dJointSetAMotorAxis(motor, 0, 1, 1, 0, 0); dJointSetAMotorParam(motor, dParamVel, speed); dJointSetAMotorParam(motor, dParamFMax, force); state->brcarwheelbody = dBodyCreate(state->world); dBodySetPosition(state->brcarwheelbody, carX+1.5, carHeight - 0.5, carZ-2); dBodySetRotation(state->brcarwheelbody, m); dBodySetMass(state->brcarwheelbody, &wheelmass); joint = dJointCreateHinge(state->world, 0); dJointAttach(joint, state->carbodybody, state->brcarwheelbody); dJointSetHingeAnchor(joint, carX+1.5, carHeight - 0.5, carZ-2); dJointSetHingeAxis(joint, 1, 0, 0); dGeomID brcarwheelgeom = dCreateCylinder(state->worldSpace, 0.5, 0.2); dGeomSetBody(brcarwheelgeom, state->brcarwheelbody); motor = dJointCreateAMotor(state->world, 0); dJointAttach(motor, state->brcarwheelbody, state->carbodybody); dJointSetAMotorNumAxes(motor, 1); dJointSetAMotorAxis(motor, 0, 1, 1, 0, 0); dJointSetAMotorParam(motor, dParamVel, speed); dJointSetAMotorParam(motor, dParamFMax, force); state->var = new double[3]; state->var = dBodyGetPosition(state->carbodybody); // cout << state->var[0] << " " << state->var[1] << " " << state->var[2] << endl; //TODO check if translation matrix from dBody can be used. dSpaceID cubespace = dHashSpaceCreate(state->worldSpace); for(int i = 0; i < NUM_CUBES/10; i++) { for(int k = 0; k < 10; k++) { state->cubebody[i*10+k] = dBodyCreate(state->world); dBodySetAutoDisableFlag(state->cubebody[i*10+k], 1); dBodySetPosition(state->cubebody[i*10+k], (i*2.01)-4, 0.9 + 2.01*k, -70); dGeomID cubegeom = dCreateBox(cubespace, 2, 2, 2); dGeomSetBody(cubegeom, state->cubebody[i*10+k]); } } { int indexSize = state->map->vertices.size()/3; unsigned int* index = new unsigned int[indexSize]; for(int i = 0; i < indexSize; i++) index[i] = i; dTriMeshDataID triMeshData = dGeomTriMeshDataCreate(); dGeomTriMeshDataBuildSingle(triMeshData, state->map->vertices.data(), 12, state->map->vertices.size()/3, index, indexSize, 12); dGeomID mapGeom = dCreateTriMesh(state->worldSpace, triMeshData, NULL, NULL, NULL); dGeomSetPosition(mapGeom, 0, 0, 0); } // state->monkeyBody = dBodyCreate(state->world); // { // int indexSize = state->monkey->vertices.size()/3; // unsigned int* index = new unsigned int[indexSize]; // for(int i = 0; i < indexSize; i++) // index[i] = i; // dTriMeshDataID triMeshData = dGeomTriMeshDataCreate(); // dGeomTriMeshDataBuildSingle(triMeshData, state->monkey->vertices.data(), 12, state->monkey->vertices.size()/3, index, indexSize, 12); // dGeomID monkeyGeom = dCreateTriMesh(state->worldSpace, triMeshData, NULL, NULL, NULL); // dGeomSetPosition(monkeyGeom, 0, 2, 0); // dGeomSetBody(monkeyGeom, state->monkeyBody); // } state->physicsContactgroup = dJointGroupCreate(0); return state; }
dJointID ODE_Dynamixel::createJoint(dBodyID body1, dBodyID body2) { if(mJointAxisPoint1->get().equals(mJointAxisPoint2->get(), -1)) { Core::log("ODE_Dynamixel: Invalid axes for ODE_Dynamixel.", true); return 0; } Vector3D anchor = mJointAxisPoint1->get(); Vector3D axis = mJointAxisPoint2->get() - mJointAxisPoint1->get(); dJointID joint = dJointCreateAMotor(mWorldID, mGeneralJointGroup); dJointAttach(joint, body1, body2); dJointSetAMotorMode(joint, dAMotorEuler); dJointSetAMotorParam(joint, dParamVel, 0.0); dJointSetAMotorParam(joint, dParamFMax, mDesiredMotorTorqueValue->get()); dJointSetAMotorParam(joint, dParamCFM, mCFMValue->get()); dJointSetAMotorParam(joint, dParamStopERP, mStopERPValue->get()); dJointSetAMotorParam(joint, dParamStopCFM, mStopCFMValue->get()); dJointSetAMotorParam(joint, dParamBounce, mBounceValue->get()); dJointSetAMotorParam(joint, dParamFudgeFactor, mFudgeFactorValue->get()); axis.normalize(); Vector3D perpedicular; if(axis.getY() != 0.0 || axis.getX() != 0.0) { perpedicular.set(-axis.getY(), axis.getX(), 0); } else { perpedicular.set(0, -axis.getZ(), axis.getY()); } perpedicular.normalize(); // If one of the bodies is static, the motor axis need to be defined in a different way. For different constellations of static and dynamic bodies, the turn direction of the motor changed, so this had to be added. if(body1 == 0) { dJointSetAMotorAxis(joint, 0, 0, -axis.getX(), -axis.getY(), -axis.getZ()); } else { dJointSetAMotorAxis(joint, 0, 1, axis.getX(), axis.getY(), axis.getZ()); } if(body2 == 0) { dJointSetAMotorAxis(joint, 2, 0, -perpedicular.getX(), -perpedicular.getY(), -perpedicular.getZ()); } else { dJointSetAMotorAxis(joint, 2, 2, perpedicular.getX(), perpedicular.getY(), perpedicular.getZ()); } mHingeJoint = dJointCreateHinge(mWorldID, mGeneralJointGroup); dJointAttach(mHingeJoint, body1, body2); dJointSetHingeAnchor(mHingeJoint, anchor.getX(), anchor.getY(), anchor.getZ()); dJointSetHingeAxis(mHingeJoint, axis.getX(), axis.getY(), axis.getZ()); if(body1 == 0) { double tmp = mMinAngle; mMinAngle = -1.0 * mMaxAngle; mMaxAngle = -1.0 * tmp; } dJointSetHingeParam(mHingeJoint, dParamLoStop, mMinAngle); dJointSetHingeParam(mHingeJoint, dParamHiStop, mMaxAngle); dJointSetHingeParam(mHingeJoint, dParamVel, 0.0); return joint; }
int main (int argc, char **argv) { // setup pointers to drawstuff callback functions dsFunctions fn; fn.version = DS_VERSION; fn.start = &start; fn.step = &simLoop; fn.command = &command; fn.stop = 0; fn.path_to_textures = DRAWSTUFF_TEXTURE_PATH; // create world dInitODE2(0); contactgroup = dJointGroupCreate(0); world = dWorldCreate(); space = dSimpleSpaceCreate(0); dMass m; dMassSetBox (&m,1,SIDE,SIDE,SIDE); dMassAdjust (&m,MASS); body[0] = dBodyCreate (world); dBodySetMass (body[0],&m); dBodySetPosition (body[0],0,0,1); geom[0] = dCreateBox(space,SIDE,SIDE,SIDE); body[1] = dBodyCreate (world); dBodySetMass (body[1],&m); dBodySetPosition (body[1],0,0,2); geom[1] = dCreateBox(space,SIDE,SIDE,SIDE); dGeomSetBody(geom[0],body[0]); dGeomSetBody(geom[1],body[1]); lmotor[0] = dJointCreateLMotor (world,0); dJointAttach (lmotor[0],body[0],body[1]); lmotor[1] = dJointCreateLMotor (world,0); dJointAttach (lmotor[1],body[0],0); amotor[0] = dJointCreateAMotor(world,0); dJointAttach(amotor[0], body[0],body[1]); amotor[1] = dJointCreateAMotor(world,0); dJointAttach(amotor[1], body[0], 0); for (int i=0; i<2; i++) { dJointSetAMotorNumAxes(amotor[i], 3); dJointSetAMotorAxis(amotor[i],0,1,1,0,0); dJointSetAMotorAxis(amotor[i],1,1,0,1,0); dJointSetAMotorAxis(amotor[i],2,1,0,0,1); dJointSetAMotorParam(amotor[i],dParamFMax,0.00001); dJointSetAMotorParam(amotor[i],dParamFMax2,0.00001); dJointSetAMotorParam(amotor[i],dParamFMax3,0.00001); dJointSetAMotorParam(amotor[i],dParamVel,0); dJointSetAMotorParam(amotor[i],dParamVel2,0); dJointSetAMotorParam(amotor[i],dParamVel3,0); dJointSetLMotorNumAxes(lmotor[i],3); dJointSetLMotorAxis(lmotor[i],0,1,1,0,0); dJointSetLMotorAxis(lmotor[i],1,1,0,1,0); dJointSetLMotorAxis(lmotor[i],2,1,0,0,1); dJointSetLMotorParam(lmotor[i],dParamFMax,0.0001); dJointSetLMotorParam(lmotor[i],dParamFMax2,0.0001); dJointSetLMotorParam(lmotor[i],dParamFMax3,0.0001); } // run simulation dsSimulationLoop (argc,argv,352,288,&fn); dJointGroupDestroy(contactgroup); dSpaceDestroy (space); dWorldDestroy (world); dCloseODE(); return 0; }
void Robot::Wheel::step() { dJointSetAMotorParam(motor,dParamVel,speed); dJointSetAMotorParam(motor,dParamFMax,rob->cfg->robotSettings.Wheel_Motor_FMax); }
dJointID ODE_U_FrictionTorqueMotorModel::createJoint(dBodyID body1, dBodyID body2) { if(mJointAxis1Point1 == 0 || mJointAxis1Point2 == 0 || mJointAxis2Point1 == 0 || mJointAxis2Point2 == 0) { Core::log("ODE_U_FrictionTorqueMotorModel: Could not find all required parmaeter Values."); return 0; } if(mJointAxis1Point1->get().equals(mJointAxis1Point2->get(), -1) || mJointAxis2Point1->get().equals(mJointAxis2Point2->get(), -1)) { Core::log("Invalid axes for ODE_U_FrictionTorqueMotorModel: " + mJointAxis1Point1->getValueAsString() + " " + mJointAxis1Point2->getValueAsString() + " " + mJointAxis2Point1->getValueAsString() + " " + mJointAxis2Point2->getValueAsString()); return 0; } Vector3D anchor = mAnchor->get(); Vector3D axis1 = mJointAxis1Point2->get() - mJointAxis1Point1->get(); Vector3D axis2 = mJointAxis2Point2->get() - mJointAxis2Point1->get(); dJointID joint = dJointCreateAMotor(mWorldID, mGeneralJointGroup); dJointAttach(joint, body1, body2); dJointSetAMotorMode(joint, dAMotorEuler); dJointSetAMotorNumAxes(joint, 2); dJointSetAMotorParam(joint, dParamVel, 0.0); dJointSetAMotorParam(joint, dParamFMax, mDesiredMotorTorqueValue->get()); dJointSetAMotorParam(joint, dParamCFM, mCFMValue->get()); dJointSetAMotorParam(joint, dParamStopERP, mStopERPValue->get()); dJointSetAMotorParam(joint, dParamStopCFM, mStopCFMValue->get()); dJointSetAMotorParam(joint, dParamBounce, mBounceValue->get()); dJointSetAMotorParam(joint, dParamFudgeFactor, mFudgeFactorValue->get()); axis1.normalize(); Vector3D perpedicular1; if(axis1.getY() != 0.0 || axis1.getX() != 0.0) { perpedicular1.set(-axis1.getY(), axis1.getX(), 0); } else { perpedicular1.set(0, -axis1.getZ(), axis1.getY()); } perpedicular1.normalize(); // If one of the bodies is static, the motor axis need to be defined in a different way. // For different constellations of static and dynamic bodies, the turn direction // of the motor changed, so this had to be added. if(body1 == 0) { dJointSetAMotorAxis(joint, 0, 0, -axis1.getX(), -axis1.getY(), -axis1.getZ()); } else { dJointSetAMotorAxis(joint, 0, 1, axis1.getX(), axis1.getY(), axis1.getZ()); } if(body2 == 0) { dJointSetAMotorAxis(joint, 2, 0, -perpedicular1.getX(), -perpedicular1.getY(), -perpedicular1.getZ()); } else { dJointSetAMotorAxis(joint, 2, 2, perpedicular1.getX(), perpedicular1.getY(), perpedicular1.getZ()); } mUniversalJoint = dJointCreateUniversal(mWorldID, mGeneralJointGroup); dJointAttach(mUniversalJoint, body1, body2); dJointSetUniversalAnchor(mUniversalJoint, anchor.getX(), anchor.getY(), anchor.getZ()); dJointSetUniversalAxis1(mUniversalJoint, axis1.getX(), axis1.getY(), axis1.getZ()); dJointSetUniversalAxis2(mUniversalJoint, axis2.getX(), axis2.getY(), axis2.getZ()); double minAngle1 = (mAxis1MinAngle->get() * Math::PI) / 180.0; double minAngle2 = (mAxis2MinAngle->get() * Math::PI) / 180.0; double maxAngle1 = (mAxis1MaxAngle->get() * Math::PI) / 180.0; double maxAngle2 = (mAxis2MaxAngle->get() * Math::PI) / 180.0; // if(body1 == 0) { // double tmp = // mMinAngle = -1.0 * mMaxAngle; // mMaxAngle = -1.0 * tmp; // } dJointSetUniversalParam(mUniversalJoint, dParamLoStop, minAngle1); dJointSetUniversalParam(mUniversalJoint, dParamHiStop, maxAngle1); dJointSetUniversalParam(mUniversalJoint, dParamLoStop2, minAngle2); dJointSetUniversalParam(mUniversalJoint, dParamHiStop2, maxAngle2); dJointSetUniversalParam(mUniversalJoint, dParamVel, 0.0); dJointSetUniversalParam(mUniversalJoint, dParamVel2, 0.0); return joint; }