Пример #1
0
void Hinge::createPhysics()
{
  ASSERT(axis);

  //
  axis->create();

  //
  ::PhysicalObject::createPhysics();

  // find bodies to connect
  Body* parentBody = dynamic_cast<Body*>(parent);
  ASSERT(!parentBody || parentBody->body);
  ASSERT(!children.empty());
  Body* childBody = dynamic_cast<Body*>(*children.begin());
  ASSERT(childBody);
  ASSERT(childBody->body);

  // create joint
  joint = dJointCreateHinge(Simulation::simulation->physicalWorld, 0);
  dJointAttach(joint, childBody->body, parentBody ? parentBody->body : 0);
  //set hinge joint parameter
  dJointSetHingeAnchor(joint, pose.translation.x, pose.translation.y, pose.translation.z);
  Vector3<> globalAxis = pose.rotation * Vector3<>(axis->x, axis->y, axis->z);
  dJointSetHingeAxis(joint, globalAxis.x, globalAxis.y, globalAxis.z);
  if(axis->cfm != -1.f)
    dJointSetHingeParam(joint, dParamCFM, dReal(axis->cfm));

  if(axis->deflection)
  {
    const Axis::Deflection& deflection = *axis->deflection;
    float minHingeLimit = deflection.min;
    float maxHingeLimit = deflection.max;
    if(minHingeLimit > maxHingeLimit)
      minHingeLimit = maxHingeLimit;
    //Set physical limits to higher values (+10%) to avoid strange hinge effects.
    //Otherwise, sometimes the motor exceeds the limits.
    float internalTolerance = (maxHingeLimit - minHingeLimit) * 0.1f;
    if(dynamic_cast<ServoMotor*>(axis->motor))
    {
      minHingeLimit -= internalTolerance;
      maxHingeLimit += internalTolerance;
    }
    dJointSetHingeParam(joint, dParamLoStop, dReal(minHingeLimit));
    dJointSetHingeParam(joint, dParamHiStop, dReal(maxHingeLimit));
    // this has to be done due to the way ODE sets joint stops
    dJointSetHingeParam(joint, dParamLoStop, dReal(minHingeLimit));
    if(deflection.stopCFM != -1.f)
      dJointSetHingeParam(joint, dParamStopCFM, dReal(deflection.stopCFM));
    if(deflection.stopERP != -1.f)
      dJointSetHingeParam(joint, dParamStopERP, dReal(deflection.stopERP));
  }

  // create motor
  if(axis->motor)
    axis->motor->create(this);

  OpenGLTools::convertTransformation(rotation, translation, transformation);
}
Пример #2
0
/*** 車輪の生成 ***/
void makeWheel()
{
    dMass mass;
    dReal r = 0.1;
    dReal w = 0.024;
    dReal m = 0.15;
    dReal d = 0.01;
    dReal tmp_z = Z;

    dReal wx[WHEEL_NUM] = {SIDE[0]/2+w/2+d, - (SIDE[0]/2+w/2+d), 0, 0};
    dReal wy[WHEEL_NUM] = {0, 0, SIDE[1]/2+w/2+d, - (SIDE[1]/2+w/2+d)};
    dReal wz[WHEEL_NUM] = {tmp_z, tmp_z, tmp_z, tmp_z};
    dReal jx[WHEEL_NUM] = {SIDE[0]/2, - SIDE[0]/2, 0, 0};
    dReal jy[WHEEL_NUM] = {0, 0, SIDE[1]/2, - SIDE[1]/2};
    dReal jz[WHEEL_NUM] = {tmp_z, tmp_z, tmp_z, tmp_z};

    for (int i = 0; i < WHEEL_NUM; i++)
    {
        wheel[i].v    = 0.0;

// make body, geom and set geom to body.
// The position and orientation of geom is abondoned.
        wheel[i].body = dBodyCreate(world);
        wheel[i].geom = dCreateCylinder(space,r, w);
        dGeomSetBody(wheel[i].geom,wheel[i].body);

// set configure of body
        dMassSetZero(&mass);
        if (i < 2) {
            dMassSetCylinderTotal(&mass,m, 1, r, w);
        } else {
            dMassSetCylinderTotal(&mass,m, 2, r, w);
        }
        dBodySetMass(wheel[i].body,&mass);

// set position and orientation of body.
        dBodySetPosition(wheel[i].body, wx[i], wy[i], wz[i]);
        dMatrix3 R;
        if (i >= 2) {
            dRFromAxisAndAngle(R,1,0,0,M_PI/2.0);
            dBodySetRotation(wheel[i].body,R);
        } else {
            dRFromAxisAndAngle(R,0,1,0,M_PI/2.0);
            dBodySetRotation(wheel[i].body,R);
        }

// make joint.
        wheel[i].joint = dJointCreateHinge(world,0);
        dJointAttach(wheel[i].joint,base.body,wheel[i].body);
        if (i < 2) {
            dJointSetHingeAxis(wheel[i].joint, 1, 0, 0);
        } else {
            dJointSetHingeAxis(wheel[i].joint, 0, -1, 0);
        }
        dJointSetHingeAnchor(wheel[i].joint, jx[i], jy[i], jz[i]);
    }
}
Пример #3
0
/**
 * This method is called if the joint should be attached.
 * It creates the ODE-joint, calculates the current anchor-position
 * and axis-orientation and attaches the Joint.
 * @param obj1 first ODE-object to attach with
 * @param obj2 second ODE-object to attach with
 **/
void HingeJoint::attachJoint(dBodyID obj1, dBodyID obj2)
{
	gmtl::Vec3f newAnchor, newAxis, scaleVec;
	gmtl::Quatf entityRot;
	gmtl::AxisAnglef axAng;
	TransformationData entityTrans;

	joint = dJointCreateHinge(world, 0);
	dJointAttach(joint, obj1, obj2);

	newAnchor = anchor;
	newAxis = axis;
	if (mainEntity != NULL)
	{
		entityTrans = mainEntity->getEnvironmentTransformation();
// get the scale values of the mainEntity
		scaleVec = entityTrans.scale;

// scale Anchor-offset by mainEntity-scale value
		newAnchor[0] *= scaleVec[0];
		newAnchor[1] *= scaleVec[1];
		newAnchor[2] *= scaleVec[2];

// scale Axis by mainEntity-scale value because of possible distortion
		newAxis[0] *= scaleVec[0];
		newAxis[1] *= scaleVec[1];
		newAxis[2] *= scaleVec[2];
		gmtl::normalize(newAxis);

// get the Rotation of the mainEntity
		entityRot = entityTrans.orientation;

// rotate Axis by mainEntity-rotation
		newAxis *= entityRot;

// rotate Anchor-offset by mainEntity-rotation
		newAnchor *= entityRot;

// transform new Anchor to world coordinates
		newAnchor += entityTrans.position;

	} // if
	dJointSetHingeAnchor(joint, newAnchor[0], newAnchor[1], newAnchor[2]);
	dJointSetHingeAxis(joint, newAxis[0], newAxis[1], newAxis[2]);

// ODE parameters for minimizing failure in Hinge Joint
// It should not be correct this way but it works better than without it ;-)
	dJointSetHingeParam(joint, dParamCFM, 0);
	dJointSetHingeParam(joint, dParamStopERP, 0);
	dJointSetHingeParam(joint, dParamStopCFM, 0);

	oldAngle = 0;

// set the maximum and minimum Joint-angles (if set).
	if (anglesSet)
		setODEAngles();
} // attachJoint
Пример #4
0
static void soy_joints_hinge_real_create (soyjointsJoint* base) {
	soyjointsHinge * self;
	struct dxWorld* _tmp0_;
	struct dxJoint* _tmp1_;
	self = (soyjointsHinge*) base;
	_tmp0_ = soy_scenes__world;
	_tmp1_ = dJointCreateHinge (_tmp0_, NULL);
	_dJointDestroy0 (((soyjointsJoint*) self)->joint);
	((soyjointsJoint*) self)->joint = _tmp1_;
}
Пример #5
0
bool ActiveHingeModel::initModel() {

	// Create the 4 components of the hinge
	hingeRoot_ = this->createBody(B_SLOT_A_ID);
	dBodyID frame = this->createBody(B_FRAME_ID);
	dBodyID servo = this->createBody(B_SERVO_ID);
	hingeTail_ = this->createBody(B_SLOT_B_ID);

	// Set the masses for the various boxes

	float separation = 0;//inMm(0.1);

	this->createBoxGeom(hingeRoot_, MASS_SLOT, osg::Vec3(0, 0, 0),
			SLOT_THICKNESS, SLOT_WIDTH, SLOT_WIDTH);

	dReal xFrame = separation + FRAME_LENGTH / 2 + SLOT_THICKNESS / 2;
	this->createBoxGeom(frame, MASS_FRAME, osg::Vec3(xFrame,
			SERVO_POSITION_OFFSET, 0),
			FRAME_LENGTH, FRAME_WIDTH, FRAME_HEIGHT);

	dReal xServo = xFrame + (FRAME_ROTATION_OFFSET - (FRAME_LENGTH / 2))
			+ SERVO_ROTATION_OFFSET - SERVO_LENGTH/2;
	this->createBoxGeom(servo, MASS_SERVO, osg::Vec3(xServo,
			SERVO_POSITION_OFFSET, 0),
			SERVO_LENGTH, SERVO_WIDTH, SERVO_HEIGHT);

	dReal xTail = xServo + SERVO_LENGTH / 2 + separation + SLOT_THICKNESS / 2;
	this->createBoxGeom(hingeTail_, MASS_SLOT, osg::Vec3(xTail, 0, 0),
			SLOT_THICKNESS, SLOT_WIDTH, SLOT_WIDTH);

	// Create joints to hold pieces in position

	// root <-> connectionPartA
	this->fixBodies(hingeRoot_, frame, osg::Vec3(1, 0, 0));

	// connectionPartA <(hinge)> connectionPArtB
	dJointID joint = dJointCreateHinge(this->getPhysicsWorld(), 0);
	dJointAttach(joint, frame, servo);
	dJointSetHingeAxis(joint, 0, 1, 0);
	dJointSetHingeAnchor(joint,
			SLOT_THICKNESS / 2 + separation + FRAME_ROTATION_OFFSET, 0, 0);

	// connectionPartB <-> tail
	this->fixBodies(servo, hingeTail_, osg::Vec3(1, 0, 0));

	// Create servo
	this->motor_.reset(
			new ServoMotor(joint, ServoMotor::DEFAULT_MAX_FORCE_SERVO,
					ServoMotor::DEFAULT_GAIN,
					ioPair(this->getId(),0)));

	return true;

}
Пример #6
0
static dJointID create_phys_joint(int t)
{
    switch (t)
    {
    case dJointTypeBall:      return dJointCreateBall     (world, 0);
    case dJointTypeHinge:     return dJointCreateHinge    (world, 0);
    case dJointTypeSlider:    return dJointCreateSlider   (world, 0);
    case dJointTypeUniversal: return dJointCreateUniversal(world, 0);
    case dJointTypeHinge2:    return dJointCreateHinge2   (world, 0);
    default:                  return 0;
    }
}
Пример #7
0
void JOINT::Create_In_Simulator(dWorldID world, OBJECT *firstObject, OBJECT *secondObject) {

	joint = dJointCreateHinge(world,0);

	dJointAttach( joint , firstObject->Get_Body() , secondObject->Get_Body() );

	dJointSetHingeAnchor(joint,x,y,z);

	dJointSetHingeAxis(joint,normalX,normalY,normalZ);

	dJointSetHingeParam(joint,dParamLoStop,lowStop);

        dJointSetHingeParam(joint,dParamHiStop,highStop);
}
Пример #8
0
IoODEHinge *IoODEHinge_rawClone(IoODEHinge *proto)
{
	IoObject *self = IoODEJoint_rawClone(proto);

	if(DATA(proto)->jointGroup)
	{
		IoODEJointGroup *jointGroup = DATA(proto)->jointGroup;

		JOINTGROUP = jointGroup;
		IoODEJointGroup_addJoint(jointGroup, self);
		JOINTID = dJointCreateHinge(WORLDID, JOINTGROUPID);
	}
	return self;
}
Пример #9
0
AppHalf::AppHalf(dWorldID world, dSpaceID space, AppDesignID app_design,
		 dBodyID main_body, dBodyID blade)
:  ebMain(main_body), ebBlade(blade)
{
	// NOTE: app_design must have a side called before this, so that A..E and K..Q are one of left or right

	// create bodies with geometries

	// create links and connect them
	jA = dJointCreateHinge(world, 0);
	dJointSetHingeAxis(jA, 0, 1, 0);	// is -1 or 1 a matter? only for angle and anglerate sign, i think.
	dReal *A = app_design->getA();
	dJointSetHingeAnchor(jA, A[0], A[1], A[2]);
	dJointConnect(jA, ebMain, bBeam);
}
Пример #10
0
static void soy_joints_hinge_real_create (soyjointsJoint* base) {
	soyjointsHinge * self;
	struct dxWorld* _tmp0_ = NULL;
	struct dxJoint* _tmp1_ = NULL;
#line 29 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
	self = (soyjointsHinge*) base;
#line 30 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
	_tmp0_ = soy_scenes__world;
#line 30 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
	_tmp1_ = dJointCreateHinge (_tmp0_, NULL);
#line 30 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
	_dJointDestroy0 (((soyjointsJoint*) self)->joint);
#line 30 "/home/jeff/Documents/libraries/libsoy/src/joints/Hinge.gs"
	((soyjointsJoint*) self)->joint = _tmp1_;
#line 247 "Hinge.c"
}
Пример #11
0
Файл: 6axis4.cpp Проект: Ry0/ODE
/*** ロボットアームの生成 ***/
void  makeArm()
{
  dMass mass;                                    // 質量パラメータ
  dReal x[NUM]      = {0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00};  // 重心 x
  dReal y[NUM]      = {0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00};  // 重心 y
  dReal z[NUM]      = {0.05, 0.55, 1.55, 2.30, 2.80, 3.35, 3.85, 4.0};  // 重心 z
  dReal length[NUM-1] = {0.10, 1.00, 1.00, 0.50, 0.50, 0.50, 0.50};  // 長さ
  dReal weight[NUM] = {9.00, 2.00, 2.00, 1.00, 1.00, 0.50, 0.50, 0.50};  // 質量
  dReal r[NUM-1]      = {0.3, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1};  // 半径
  dReal c_x[NUM]    = {0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00};  // 関節中心点 x
  dReal c_y[NUM]    = {0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00};  // 関節中心点 y
  dReal c_z[NUM]    = {0.00, 0.10, 1.10, 2.10, 2.60, 3.10, 3.60, 3.9};  // 関節中心点 z
  dReal axis_x[NUM] = {0, 0, 0, 0, 0, 0, 0, 0};              // 関節回転軸 x
  dReal axis_y[NUM] = {0, 0, 1, 1, 0, 1, 0, 0};              // 関節回転軸 y
  dReal axis_z[NUM] = {1, 1, 0, 0, 1, 0, 1, 1};              // 関節回転軸 z

  // リンクの生成
  for (int i = 0; i < NUM-1; i++) {
    rlink[i].body = dBodyCreate(world);
    dBodySetPosition(rlink[i].body, x[i], y[i], z[i]);
    dMassSetZero(&mass);
    dMassSetCapsuleTotal(&mass,weight[i],3,r[i],length[i]);
    dBodySetMass(rlink[i].body, &mass);
    rlink[i].geom = dCreateCapsule(space,r[i],length[i]);
    dGeomSetBody(rlink[i].geom,rlink[i].body);
  }
  rlink[NUM-1].body = dBodyCreate(world);
  dBodySetPosition(rlink[NUM-1].body, x[NUM-1], y[NUM-1], z[NUM-1]);
  dMassSetZero(&mass);
  dMassSetBoxTotal(&mass,weight[NUM-1],0.4,0.4,0.4);
  dBodySetMass(rlink[NUM-1].body, &mass);
  rlink[NUM-1].geom = dCreateBox(space,0.4,0.4,0.4);
  dGeomSetBody(rlink[NUM-1].geom,rlink[NUM-1].body);

  // ジョイントの生成とリンクへの取り付け
  joint[0] = dJointCreateFixed(world, 0);  // 固定ジョイント
  dJointAttach(joint[0], rlink[0].body, 0);
  dJointSetFixed(joint[0]);
  for (int j = 1; j < NUM; j++) {
    joint[j] = dJointCreateHinge(world, 0); // ヒンジジョイント
    dJointAttach(joint[j], rlink[j].body, rlink[j-1].body);
    dJointSetHingeAnchor(joint[j], c_x[j], c_y[j], c_z[j]);
    dJointSetHingeAxis(joint[j], axis_x[j], axis_y[j],axis_z[j]);
  }
}
Пример #12
0
/**
 * Creates a new ODE_HingeJoint.
 *
 * @param body1 the first body to connect the joint to.
 * @param body2 the second body to connect the joint to.
 * @return the new ODE_HingeJoint.
 */
dJointID ODE_HingeJoint::createJoint(dBodyID body1, dBodyID body2) {
	if(mJointAxisPoint1->get().equals(mJointAxisPoint2->get())) {
		Core::log("Invalid axes for ODE_HingeJoint.");
		return 0;
	}
	//if one of the bodyIDs is null, the joint is connected to a static object.
	dJointID newJoint = dJointCreateHinge(mWorldID, mGeneralJointGroup);
	dJointAttach(newJoint, body1, body2);
	
	Vector3D anchor = mJointAxisPoint1->get() ;
	Vector3D axis = mJointAxisPoint2->get() ;
	
	axis.set(mJointAxisPoint2->getX() - mJointAxisPoint1->getX(), mJointAxisPoint2->getY() - mJointAxisPoint1->getY(), mJointAxisPoint2->getZ() - mJointAxisPoint1->getZ());
	
	dJointSetHingeAnchor(newJoint, anchor.getX(), anchor.getY(), anchor.getZ());
	dJointSetHingeAxis(newJoint, axis.getX(), axis.getY(), axis.getZ());
	return newJoint; 
}
Пример #13
0
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);
  world = dWorldCreate();

  dMass m;
  dMassSetBox (&m,1,SIDE,SIDE,SIDE);
  dMassAdjust (&m,MASS);

  dQuaternion q;
  dQFromAxisAndAngle (q,1,1,0,0.25*M_PI);

  body[0] = dBodyCreate (world);
  dBodySetMass (body[0],&m);
  dBodySetPosition (body[0],0.5*SIDE,0.5*SIDE,1);
  dBodySetQuaternion (body[0],q);

  body[1] = dBodyCreate (world);
  dBodySetMass (body[1],&m);
  dBodySetPosition (body[1],-0.5*SIDE,-0.5*SIDE,1);
  dBodySetQuaternion (body[1],q);

  hinge = dJointCreateHinge (world,0);
  dJointAttach (hinge,body[0],body[1]);
  dJointSetHingeAnchor (hinge,0,0,1);
  dJointSetHingeAxis (hinge,1,-1,1.41421356);

  // run simulation
  dsSimulationLoop (argc,argv,352,288,&fn);

  dWorldDestroy (world);
  dCloseODE();
  return 0;
}
Пример #14
0
void createHingeLeg(ODEObject &leg,
	ODEObject &bodyAttachedTo,
	dJointID &joint,
	dReal xPos, dReal yPos, dReal zPos,
	dReal xRot, dReal yRot, dReal zRot,
	dReal radius, dReal length,
	dReal maxAngle, dReal minAngle,
	dReal anchorXPos, dReal anchorYPos, dReal anchorZPos)
{
	dMatrix3 legOrient;
	dRFromEulerAngles(legOrient, xRot, yRot, zRot);

	//position and orientation
	leg.Body = dBodyCreate(World);
	dBodySetPosition(leg.Body, xPos, yPos, zPos);
	dBodySetRotation(leg.Body, legOrient);
	dBodySetLinearVel(leg.Body, 0, 0, 0);
	dBodySetData(leg.Body, (void *)0);

	//mass
	dMass legMass;
	dMassSetCapsule(&legMass, 1, 3, radius, length);
	//dBodySetMass(leg.Body, &legMass);

	//geometry
	leg.Geom = dCreateCapsule(Space, radius, length);
	dGeomSetBody(leg.Geom, leg.Body);

	//hinge joint
	joint = dJointCreateHinge(World, jointgroup);

	//attach and anchor
	dJointAttach(joint, bodyAttachedTo.Body, leg.Body);
	dJointSetHingeAnchor(joint, anchorXPos, anchorYPos, anchorZPos);

	//axes
	dJointSetHingeAxis(joint, 0, 1, 0);

	//Max and min angles
	dJointSetHingeParam(joint, dParamHiStop, maxAngle);
	dJointSetHingeParam(joint, dParamLoStop, minAngle);

}
Пример #15
0
sODEJoint* sODEJoint::AddHingeJoint( int id, dBodyID body1, dBodyID body2, float ax, float ay, float az, float x, float y, float z )
{
	sODEJoint *pJoint = new sODEJoint( );

	//modify prev and next pointers of any involved joints, make new joint the new head
	pJoint->pNextJoint = pODEJointList;
	if ( pODEJointList ) pODEJointList->pPrevJoint = pJoint;
	pODEJointList = pJoint;

	//create the ODE hinge joint
	pJoint->oJoint = dJointCreateHinge( g_ODEWorld, 0 );
	dJointAttach( pJoint->oJoint, body1, body2 );
	dJointSetHingeAnchor( pJoint->oJoint, x,y,z );
	dJointSetHingeAxis( pJoint->oJoint, ax,ay,az );

	pJoint->iID = id;

	return pJoint;
}
Пример #16
0
void SkidSteeringVehicle::create() {
    this->vehicleBody = dBodyCreate(this->environment->world);
    this->vehicleGeom = dCreateBox(this->environment->space, this->vehicleBodyLength, this->vehicleBodyWidth, this->vehicleBodyHeight);
    this->environment->setGeomName(this->vehicleGeom, name + ".vehicleGeom");
    dMassSetBox(&this->vehicleMass, this->density, this->vehicleBodyLength, this->vehicleBodyWidth, this->vehicleBodyHeight);
    dGeomSetCategoryBits(this->vehicleGeom, Category::OBSTACLE);
    dGeomSetCollideBits(this->vehicleGeom, Category::OBSTACLE | Category::TERRAIN);
    dBodySetMass(this->vehicleBody, &this->vehicleMass);
    dBodySetPosition(this->vehicleBody, this->xOffset, this->yOffset, this->zOffset);
    dGeomSetBody(this->vehicleGeom, this->vehicleBody);
    dGeomSetOffsetPosition(this->vehicleGeom, 0, 0, this->wheelRadius);

    dReal w = this->vehicleBodyWidth + this->wheelWidth + 2 * this->trackVehicleSpace;
    for(int fr = 0; fr < 2; fr++) {
        for(int lr = 0; lr < 2; lr++) {
            this->wheelGeom[fr][lr] = dCreateCylinder(this->environment->space, this->wheelRadius, this->wheelWidth);
            this->environment->setGeomName(this->wheelGeom[fr][lr], this->name + "." + (!fr ? "front" : "rear") + (!lr ? "Left" : "Right") + "Wheel");
            dGeomSetCategoryBits(this->wheelGeom[fr][lr], Category::TRACK_GROUSER);
            dGeomSetCollideBits(this->wheelGeom[fr][lr], Category::TERRAIN);
            dMassSetCylinder(&this->wheelMass[fr][lr], this->density, 3, this->wheelRadius, this->wheelWidth);
            this->wheelBody[fr][lr] = dBodyCreate(this->environment->world);
            dBodySetMass(this->wheelBody[fr][lr], &this->wheelMass[fr][lr]);
            dGeomSetBody(this->wheelGeom[fr][lr], this->wheelBody[fr][lr]);
            dBodySetPosition(this->wheelBody[fr][lr], this->xOffset + (fr - 0.5) * this->wheelBase, this->yOffset + w * (lr - 0.5), this->zOffset);
            dMatrix3 wheelR;
            dRFromZAxis(wheelR, 0, 2 * lr - 1, 0);
            dBodySetRotation(this->wheelBody[fr][lr], wheelR);
            this->wheelJoint[fr][lr] = dJointCreateHinge(this->environment->world, 0);
            dJointAttach(this->wheelJoint[fr][lr], this->vehicleBody, this->wheelBody[fr][lr]);
            dJointSetHingeAnchor(this->wheelJoint[fr][lr], this->xOffset + (fr - 0.5) * this->wheelBase, this->yOffset + this->vehicleBodyWidth * (lr - 0.5), this->zOffset);
            dJointSetHingeAxis(this->wheelJoint[fr][lr], 0, 1, 0);
            dJointSetHingeParam(this->wheelJoint[fr][lr], dParamFMax, 5.0);
        }
    }
    
    this->bodyArray = dRigidBodyArrayCreate(this->vehicleBody);
    for(int fr = 0; fr < 2; fr++) {
        for(int lr = 0; lr < 2; lr++) {
            dRigidBodyArrayAdd(this->bodyArray, this->wheelBody[fr][lr]);
        }
    }
}
Пример #17
0
int main(int argc, char *argv[]) {
  dsFunctions fn;
  double x[NUM] = {0.00}, y[NUM] = {0.00};  // Center of gravity
  double z[NUM]         = { 0.05, 0.50, 1.50, 2.55};
  double m[NUM] = {10.00, 2.00, 2.00, 2.00};       // mass
  double anchor_x[NUM]  = {0.00}, anchor_y[NUM] = {0.00};// anchors of joints  &nbsp;
  double anchor_z[NUM] = { 0.00, 0.10, 1.00, 2.00};
  double axis_x[NUM]  = { 0.00, 0.00, 0.00, 0.00};  // axises of joints
  double axis_y[NUM]  = { 0.00, 0.00, 1.00, 1.00};
  double axis_z[NUM]  = { 1.00, 1.00, 0.00, 0.00};
  fn.version = DS_VERSION;  fn.start   = &start;   fn.step   = &simLoop;
  fn.command = &command;
  fn.path_to_textures = "../../drawstuff/textures";

  dInitODE();  // Initialize ODE
  world = dWorldCreate();  // Create a world
  dWorldSetGravity(world, 0, 0, -9.8);

  for (int i = 0; i < NUM; i++) {
    dMass mass;
    link[i] = dBodyCreate(world);
    dBodySetPosition(link[i], x[i], y[i], z[i]); // Set a position
    dMassSetZero(&mass);      // Set mass parameter to zero
    dMassSetCapsuleTotal(&mass,m[i],3,r[i],l[i]);  // Calculate mass parameter
    dBodySetMass(link[i], &mass);  // Set mass
  }

  joint[0] = dJointCreateFixed(world, 0); // A fixed joint
  dJointAttach(joint[0], link[0], 0);     // Attach the joint between the ground and the base
  dJointSetFixed(joint[0]);               // Set the fixed joint

  for (int j = 1; j < NUM; j++) {
    joint[j] = dJointCreateHinge(world, 0); // Create a hinge joint
    dJointAttach(joint[j], link[j-1], link[j]); // Attach the joint
    dJointSetHingeAnchor(joint[j], anchor_x[j], anchor_y[j],anchor_z[j]);
    dJointSetHingeAxis(joint[j], axis_x[j], axis_y[j], axis_z[j]);
  }
  dsSimulationLoop(argc, argv, 640, 570, &fn); // Simulation loop
  dCloseODE();
  return 0;
}
Пример #18
0
    void createLeg( const dWorldID & _world ){
        // read leg parameters there 
        lower_part.mass = 0.5;
        lower_part.radius = 0.1;
        lower_part.length = 0.5;

        dReal lower_part_pos[3] =  {0.0, 0.0, lower_part.length};

        upper_part.mass = 0.5;
        upper_part.radius = 0.1;
        upper_part.length = 0.5;

        dReal upper_part_pos[3] =  {0.0, 0.0, upper_part.length + lower_part.length};

        createPart(_world,lower_part, lower_part_pos);
        createPart(_world,upper_part, upper_part_pos);

        jointHinge = dJointCreateHinge(_world, 0); // Create a hinge joint
        dJointAttach(jointHinge, lower_part.body, upper_part.body ); // Attach the joint
        dJointSetHingeAnchor(jointHinge, 1.0,0.0,0.0);//anchor_x[j], anchor_y[j],anchor_z[j]);
    }
Пример #19
0
bool ActiveWheelModel::initModel() {

   // Create the 4 components of the hinge
   wheelRoot_ = this->createBody(B_SLOT_ID);
   dBodyID servo = this->createBody(B_SERVO_ID);
   dBodyID wheel = this->createBody(B_WHEEL_ID);

   // Set the masses for the various boxes
   dMass mass;


   this->createBoxGeom(wheelRoot_, MASS_SLOT, osg::Vec3(0, 0, 0),
         SLOT_THICKNESS, SLOT_WIDTH, SLOT_WIDTH);

   dReal zServo = 0;//-SLOT_WIDTH / 2 + SERVO_Z_OFFSET + SERVO_HEIGHT / 2;
   this->createBoxGeom(servo, MASS_SERVO, osg::Vec3(X_SERVO, 0, zServo),
         SERVO_LENGTH, SERVO_WIDTH, SERVO_HEIGHT);

   this->createCylinderGeom(wheel, MASS_WHEEL, osg::Vec3(X_WHEEL, 0, 0), 1,
         getRadius(), WHEEL_THICKNESS);

   // Create joints to hold pieces in position

   // slot <slider> hinge
   this->fixBodies(wheelRoot_, servo, osg::Vec3(1, 0, 0));

   // servo <(hinge)> wheel
   dJointID joint = dJointCreateHinge(this->getPhysicsWorld(), 0);
   dJointAttach(joint, servo, wheel);
   dJointSetHingeAxis(joint, 1, 0, 0);
   dJointSetHingeAnchor(joint, X_WHEEL, 0, 0);

   // Create servo
   this->motor_.reset(
         new ServoMotor(joint, ServoMotor::DEFAULT_MAX_FORCE,
        		 ioPair(this->getId(),0)));

   return true;

}
Else::AcrobotArticulatedBody
::AcrobotArticulatedBody( dWorldID argWorldID,
                          dSpaceID argSpaceID,
                          double argScale )
: ArticulatedBody( argWorldID, argSpaceID ),
  myRadius( 0.1 * argScale ),
  myLength( argScale ),
  myJointGroupID( 0 )
{
    // create body
    gmtl::Vec3d dims( 2*myRadius, 2*myRadius, 2*myRadius + myLength );
    const double density = 1.0;
    const int zDirection = 3;
    const double extraSpace = 0.1*myLength;
    dMass mass;

    dBodyID body = dBodyCreate( myWorldID );
    dBodySetData( body, this );

    // create cylinder aligned to z-axis
    dGeomID geom = dCreateCapsule( mySpaceID, myRadius, myLength );
    dGeomSetBody( geom, body );
    dBodySetPosition( body, 0, 0, extraSpace + 0.5*myLength + myRadius );
    dMassSetCappedCylinder( &mass, density, zDirection, myRadius, myLength );
    dBodySetMass( body, &mass );

    dJointID joint = dJointCreateHinge( myWorldID, myJointGroupID );
    dJointAttach( joint, NULL, body );
    dJointSetHingeAnchor( joint, 0, 0, extraSpace + myLength + myRadius );
    dVector3 axis;
    axis[0] = 0; axis[1] = 1; axis[2] = 0;
    dJointSetHingeAxis( joint, axis[0], axis[1], axis[2] );

    myBodies[0] = body;
    myJoints[0] = joint;
    myBodyDims[0] = dims;
    myGeoms.push_back( geom );
}
Пример #21
0
//! A hinge requires a fixed anchor point and an axis
OscHingeODE::OscHingeODE(dWorldID odeWorld, dSpaceID odeSpace,
                         const char *name, OscBase* parent,
                         OscObject *object1, OscObject *object2,
                         double x, double y, double z, double ax, double ay, double az)
    : OscHinge(name, parent, object1, object2, x, y, z, ax, ay, az)
{
    m_response = new OscResponse("response",this);

	// create the constraint for object1
    cVector3d anchor(x,y,z);
    cVector3d axis(ax,ay,az);

    dJointID odeJoint = dJointCreateHinge(odeWorld,0);

    m_pSpecial = new ODEConstraint(this, odeJoint, odeWorld, odeSpace,
                                   object1, object2);

    dJointSetHingeAnchor(odeJoint, anchor.x, anchor.y, anchor.z);
    dJointSetHingeAxis(odeJoint, axis.x, axis.y, axis.z);

    printf("Hinge joint created between %s and %s at anchor (%f,%f,%f), axis (%f,%f,%f)\n",
        object1->c_name(), object2?object2->c_name():"world", x,y,z,ax,ay,az);
}
Пример #22
0
void  makeRobot_Nleg() 
{ 
	for(int segment = 0; segment < BODY_SEGMENTS; ++segment) {
		dReal segmentOffsetFromMiddle = segment - MIDDLE_BODY_SEGMENT;
		
		dReal torso_m = 50.0;                    // Mass of body
		//	torso_m[segment] = 10.0;
		
		dReal  l1m = 0.005,l2m = 0.5, l3m = 0.5; // Mass of leg segments 
		
		//for four legs
		//		dReal x[num_legs][num_links] = {{-cx1,-cx1,-cx1},// Position of each link (x coordinate)
		//																		{-cx1,-cx1,-cx1}};
		dReal x[num_legs][num_links] = {{0,0,0},// Position of each link (x coordinate)
			{0,0,0}};
		
		dReal y[num_legs][num_links] = {{ cy1, cy1, cy1},// Position of each link (y coordinate)
			{-cy1,-cy1,-cy1}};
		dReal z[num_legs][num_links] = {                                  // Position of each link (z coordinate)
			{c_z[0][0],(c_z[0][0]+c_z[0][2])/2,c_z[0][2]-l3/2},
			{c_z[0][0],(c_z[0][0]+c_z[0][2])/2,c_z[0][2]-l3/2} };
		
		dReal r[num_links]          =  { r1, r2, r3}; // radius of leg segment
		dReal length[num_links]     =  { l1, l2, l3}; // Length of leg segment
		dReal weight[num_links]     =  {l1m,l2m,l3m}; // Mass of leg segment
		
		//  //for one leg
		//  dReal axis_x[num_legs_pneat][num_links_pneat] = {{ 0,1, 0}};
		//  dReal axis_y[num_legs_pneat][num_links_pneat] = {{ 1,0, 1}};
		//  dReal axis_z[num_legs_pneat][num_links_pneat] = {{ 0,0, 0}};
		
		//for four legs
		dReal axis_x[num_legs][num_links];
		dReal axis_y[num_legs][num_links];
		dReal axis_z[num_legs][num_links];
		for(int i = 0; i < num_legs; ++i) {
			axis_x[i][0] = 0.0;
			axis_x[i][1] = 1.0;
			axis_x[i][2] = 0.0;
			
			axis_y[i][0] = 1.0;
			axis_y[i][1] = 0.0;
			axis_y[i][2] = 1.0;
			
			axis_z[i][0] = 0.0;
			axis_z[i][1] = 0.0;
			axis_z[i][2] = 0.0;
		}
		// For mation of the body
		dMass mass; 
		torso[segment].body  = dBodyCreate(world);
		dMassSetZero(&mass);
		dMassSetBoxTotal(&mass,torso_m, lx, ly, lz);
		dBodySetMass(torso[segment].body,&mass);
		torso[segment].geom = dCreateBox(space,lx, ly, lz); 
		dGeomSetBody(torso[segment].geom, torso[segment].body);
		dBodySetPosition(torso[segment].body, SX+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS), SY, SZ); 
		
		// Formation of leg
		dMatrix3 R;                          // Revolution queue
		dRFromAxisAndAngle(R,1,0,0,M_PI/2);  // 90 degrees to turn, parallel with the land
		for (int i = 0; i < num_legs; i++) { 
			for (int j = 0; j < num_links; j++) {
				THETA[segment][i][j] = 0;
				
				leg[segment][i][j].body = dBodyCreate(world);
				if (j == 0)
					dBodySetRotation(leg[segment][i][j].body,R);
				dBodySetPosition(leg[segment][i][j].body, SX+x[i][j]+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS), SY+y[i][j], SZ+z[i][j]);				
				dMassSetZero(&mass);
				dMassSetCapsuleTotal(&mass,weight[j],3,r[j],length[j]);
				dBodySetMass(leg[segment][i][j].body, &mass);
				//if(i==1 and j==2)                                         //to set the length of one leg differently
				//leg[i][j].geom = dCreateCapsule(space_pneat,r[j],length[j]+.5); //set the length of the leg
				//else
				leg[segment][i][j].geom = dCreateCapsule(space,r[j],length[j]); //set the length of the leg
				dGeomSetBody(leg[segment][i][j].geom,leg[segment][i][j].body);
			}
		}
		
		// Formation of joints (and connecting them up)
		for (int i = 0; i < num_legs; i++) { 
			for (int j = 0; j < num_links; j++) { 
				leg[segment][i][j].joint = dJointCreateHinge(world, 0);
				if (j == 0){ 
					dJointAttach(leg[segment][i][j].joint, torso[segment].body, leg[segment][i][j].body); //connects hip to the environment
					dJointSetHingeParam(leg[segment][i][j].joint, dParamLoStop, -.50*M_PI); //prevent the hip forward-back from going more than 90 degrees
					dJointSetHingeParam(leg[segment][i][j].joint, dParamHiStop,  .50*M_PI);
				}
				else
					dJointAttach(leg[segment][i][j].joint, leg[segment][i][j-1].body, leg[segment][i][j].body);
				
				dJointSetHingeAnchor(leg[segment][i][j].joint, SX+x[i][j]+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS), SY+c_y[i][j],SZ+c_z[i][j]);
				dJointSetHingeAxis(leg[segment][i][j].joint, axis_x[i][j], axis_y[i][j],axis_z[i][j]);
			} 
		}
	}
	
	
#ifdef USE_NLEG_ROBOT
	// link torsos
	for(int segment = 0; segment < BODY_SEGMENTS-1; ++segment) {
		dReal segmentOffsetFromMiddle = segment - MIDDLE_BODY_SEGMENT;
		
		switch (hingeType) {
			case 1: //Hinge Joint, X axis  (back-breaker)
				torso[segment].joint = dJointCreateHinge(world, 0);
				dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body);
				dJointSetHingeAnchor(torso[segment].joint, SX+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS)+(lx+DISTANCE_BETWEEN_SEGMENTS)/2, SY,SZ);
				dJointSetHingeAxis (torso[segment].joint, 1.0, 0.0, 0.0);
				break;
			case 2: //Hinge Joint, Y axis (???)
				torso[segment].joint = dJointCreateHinge(world, 0);
				dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body);
				dJointSetHingeAnchor(torso[segment].joint, SX+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS)+(lx+DISTANCE_BETWEEN_SEGMENTS)/2, SY,SZ);
				dJointSetHingeAxis (torso[segment].joint, 0.0, 1.0, 0.0);
				break;
			case 3: //Hinge Joint, Z axis (snake-like)
				torso[segment].joint = dJointCreateHinge(world, 0);
				dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body);
				dJointSetHingeAnchor(torso[segment].joint, SX+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS)+(lx+DISTANCE_BETWEEN_SEGMENTS)/2, SY,SZ);
				dJointSetHingeAxis (torso[segment].joint, 0.0, 0.0, 1.0);
				break;
			case 4: // Slider, Y axis  (??)
				torso[segment].joint = dJointCreateSlider(world, 0);
				dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body);
				dJointSetSliderAxis (torso[segment].joint, 0.0, 1.0, 0.0);
				break;
			case 5: // Slider, X axis  (extendo)
				torso[segment].joint = dJointCreateSlider(world, 0);
				dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body);
				dJointSetSliderAxis (torso[segment].joint, 1.0, 0.0, 0.0);				
				break;
			case 6: //Universal Joint
				torso[segment].joint = dJointCreateUniversal(world, 0);
				dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body);
				dJointSetUniversalAnchor(torso[segment].joint, SX+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS)+(lx+DISTANCE_BETWEEN_SEGMENTS)/2, SY,SZ);
				dJointSetUniversalAxis1(torso[segment].joint, 0.0, 1.0, 0.0);
				dJointSetUniversalAxis2(torso[segment].joint, 0.0, 0.0, 1.0);				
				break;
			case 7: //Ball Joint
				torso[segment].joint = dJointCreateBall(world, 0);
				dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body);
				dJointSetBallAnchor(torso[segment].joint, SX+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS)+(lx+DISTANCE_BETWEEN_SEGMENTS)/2, SY,SZ);
				break;
			case 8:  // Fixed
				torso[segment].joint = dJointCreateHinge(world, 0);
				dJointAttach(torso[segment].joint, torso[segment].body, torso[segment+1].body);
				dJointSetHingeAnchor(torso[segment].joint, SX+segmentOffsetFromMiddle*(lx+DISTANCE_BETWEEN_SEGMENTS)+(lx+DISTANCE_BETWEEN_SEGMENTS)/2, SY,SZ);
				dJointSetHingeAxis (torso[segment].joint, 1.0, 0.0, 0.0);				
				dJointSetHingeParam(torso[segment].joint, dParamLoStop, -0.00*M_PI); //prevent the hip forward-back from going more than 90 degrees
				dJointSetHingeParam(torso[segment].joint, dParamHiStop,  0.00*M_PI);
				break;
			default:
				assert(false); // not a valid hinge type
				break;
		}
	}
#endif
}
Пример #23
0
Файл: car.cpp Проект: alon/track
Car::Car(dWorldID world, dSpaceID space, CarDesignID n_car_design, AppDesignID n_app_design) :MyObject(world, space), car_design(n_car_design),
app_design(n_app_design)
{
	int i, j, ind;
	body_obj = 0;
	for (i = 0; i < 2; ++i)
		chain_obj[i] = 0;
	for (i = 0; i < 6; ++i)
		wheel_obj[i] = 0;
	max_f = 0;		// must be set with setMaxF
	TrackDesignID track_design = car_design->left_track_design;
	LinkDesignID link_design = track_design->link_design;
	WheelDesignID wheel_design = car_design->wheel_design;
	body_mass = car_design->getBodyMass();

	// create body
	createBody(car_design->getChassisPosition());
	
	/*
	for ( int i = 0 ; i < 6 ; ++i ) {
	CreateToothedWheel(world, space,
	wheel_obj[i], sprocket_teeth, R, t_sides, teeth_h_fuzz);
	}
	*/ 
	
	// create sprocket wheels in back wheels position
	for (j = 0, ind = FIRST_SPROCKET; j < 2; ++j, ++ind) {
		wheel_obj[ind] = wheel_design->create(world, space);
		sprocket[j] = wheel_obj[ind]->body[0];
		const dReal *sprocket_pos =
		    track_design->getBackWheelPos();
		PVEC(sprocket_pos);
		dReal y = sprocket_pos[YY];
		if (j == 1)
			y += car_design->getTrackToTrack();
		PEXP(y);
		dBodySetPosition(sprocket[j], sprocket_pos[XX], y,
				  sprocket_pos[ZZ]);
	}
	
	// create sprockets and set their position
	
	// create front wheels and set their position
	for (j = 0, ind = FIRST_FRONT; j < 2; ++j, ++ind) {
		wheel_obj[ind] = wheel_design->create(world, space);
		front[j] = wheel_obj[ind]->body[0];
		const dReal *front_pos = track_design->getFrontWheelPos();
		PVEC(front_pos);
		dReal y = front_pos[YY];
		if (j == 1)
			y += car_design->getTrackToTrack();
		PEXP(y);
		dBodySetPosition(front[j], front_pos[XX], y,
				  front_pos[ZZ]);
	}
	
	// create chains
	chain_obj[0] =
	new Chain(world, space, track_design, link_design);
	track_design->moveDesign(0, car_design->getTrackToTrack(), 0);	// instead of recreating design
	chain_obj[1] = new Chain(world, space, track_design, link_design);
	
	// create axle joints for sprockets
	for (j = 0, ind = FIRST_SPROCKET; j < 2; ++j, ++ind) {
		axle[ind] = dJointCreateHinge(world, 0);
		dBodyID second = (body_obj ? body_obj->body[0] : 0);
		dJointAttach(axle[ind], sprocket[j], second);
		const dReal *sp_v = dBodyGetPosition(sprocket[j]);
		const dReal *rot = dBodyGetRotation(sprocket[j]);
		dJointSetHingeAnchor(axle[ind], sp_v[XX], sp_v[YY],
				      sp_v[ZZ]);
		dJointSetHingeAxis(axle[ind], rot[1], rot[5], rot[9]);
	} 
/*
  for ( j = 0, ind = FIRST_BACK ; j < 2 ; ++j, ++ind ) {
  	axle[ind] = dJointCreateHinge(world, 0);			// no joint group (0==NULL)
  	dJointAttach(axle[ind], back[j], 0);			// attach to world
	const dReal* v = dBodyGetPosition(back[j]);
  	dJointSetHingeAnchor(axle[ind], v[XX], v[YY], v[ZZ]);
  	dJointSetHingeAxis(axle[ind], y_axis[XX], y_axis[YY], y_axis[ZZ]);   // axis is up (positive z)
  }
*/ 
	// create front wheels joints
	for (j = 0, ind = FIRST_FRONT; j < 2; ++j, ++ind) {
		axle[ind] = dJointCreateHinge(world, 0);
		dBodyID second = (body_obj ? body_obj->body[0] : 0);
		dJointAttach(axle[ind], front[j], second);
		const dReal *v = dBodyGetPosition(front[j]);
		const dReal *rot = dBodyGetRotation(sprocket[j]);
		dJointSetHingeAnchor(axle[ind], v[XX], v[YY], v[ZZ]);
		dJointSetHingeAxis(axle[ind], rot[1], rot[5], rot[9]);
	} if (body_obj)
		body_obj->show_forces = 1;
	
	// create appendage
	if (body_obj && app_design) {
		
		    //app_obj = new App(world, space, app_design, body_obj->body[0]);
		    app_obj = 0;
	};
	
	// lets see what we have, height wise
	if (chain_obj[0] != 0) {
		const dReal *pos =
		    dBodyGetPosition(chain_obj[0]->body[0]);
		std::cout << "debug: first link position, z wise\n";
		PEXP(pos[ZZ]);
	}

#ifdef ARE_YOU_NUTS_ABOUT_SPRINGS
	// now create the suspension - it is between the wheels and the body.
	Init(0, 2, 0);
	// initially lets make some springs between the wheels and the body.
	for (int i = 0 ; i < 2 ; ++i) {
		dBody a_wheel = wheel_obj[i + FIRST_SPROCKET]->body[0];
		
	};
#endif
}
Пример #24
0
void vmWishboneCar::buildWheelJoint(vmWheel *wnow, vmWishbone *snow, dReal shiftSign)
{
    // chassis pin joints
    const dReal *pos;
    snow->jChassisUp= dJointCreateHinge(world,0);
    dJointAttach(snow->jChassisUp,chassis.body,snow->uplink.body);
    dJointSetHingeAxis(snow->jChassisUp,1.0, 0.0, 0.0);
    pos= dBodyGetPosition(snow->uplink.body);
    dJointSetHingeAnchor(snow->jChassisUp,pos[0]
            ,pos[1]+0.5*shiftSign*snow->uplink.width, pos[2]);

    const dReal *pos1;
    snow->jChassisLow= dJointCreateHinge(world,0);
    dJointAttach(snow->jChassisLow, chassis.body, snow->lowlink.body);
    dJointSetHingeAxis(snow->jChassisLow,1.0, 0.0, 0.0);
    pos1= dBodyGetPosition(snow->lowlink.body);
    dJointSetHingeAnchor(snow->jChassisLow, pos1[0]
            , pos1[1]+0.5*shiftSign*snow->lowlink.width, pos1[2]);

    // rotor ball joint
    /*const dReal *p2;
    jRotorUp= dJointCreateBall(world,0);
    dJointAttach(jRotorUp, uplink.body, hlink.body);
    p2= dBodyGetPosition(uplink.body);
    dJointSetBallAnchor(jRotorUp, p2[0], p2[1]-0.5*uplink.width, p2[2]);

    const dReal *p3;
    jRotorLow= dJointCreateBall(world,0);
    dJointAttach(jRotorLow, lowlink.body,hlink.body);
    p3= dBodyGetPosition(lowlink.body);
    dJointSetBallAnchor(jRotorLow, p3[0], p3[1]-0.5*lowlink.width,p3[2]);*/

    const dReal *p2;
    snow->jRotorUp= dJointCreateHinge(world,0);
    dJointAttach(snow->jRotorUp, snow->uplink.body, snow->hlink.body);
    p2= dBodyGetPosition(snow->uplink.body);
    dJointSetHingeAnchor(snow->jRotorUp, p2[0]
            ,p2[1]-0.5*shiftSign*snow->uplink.width, p2[2]);
    dJointSetHingeAxis(snow->jRotorUp, 1.0, 0.0, 0.0);

    const dReal *p3;
    snow->jRotorLow= dJointCreateHinge(world,0);
    dJointAttach(snow->jRotorLow, snow->lowlink.body,snow->hlink.body);
    p3= dBodyGetPosition(snow->lowlink.body);
    dJointSetHingeAnchor(snow->jRotorLow, p3[0]
            ,p3[1]-0.5*shiftSign*snow->lowlink.width,p3[2]);
    dJointSetHingeAxis(snow->jRotorLow, 1.0, 0.0, 0.0);

    // rotor hinge joint
    const dReal *pw= dBodyGetPosition(wnow->body);
    snow->jRotorMid= dJointCreateHinge(world,0);
    dJointAttach(snow->jRotorMid, snow->hlink.body, wnow->body);
    dJointSetHingeAxis(snow->jRotorMid, 0.0,1.0,0.0);
    dJointSetHingeAnchor(snow->jRotorMid, pw[0],pw[1],pw[2]);


    // strut joint
    const dReal *ps1, *ps2;
    dReal angle= -shiftSign*strutAngle;

    ps1= dBodyGetPosition(snow->upstrut.body);
    ps2= dBodyGetPosition(snow->lowstrut.body);
    snow->jStrutUp= dJointCreateBall(world,0);
    dJointAttach(snow->jStrutUp, chassis.body, snow->upstrut.body);
    //dJointSetFixed(snow->jStrutUp);
    dJointSetBallAnchor(snow->jStrutUp, ps1[0]
            ,ps1[1]+0.5*shiftSign*snow->upstrut.width*fabs(sin(angle))
            ,ps1[2]+0.5*snow->upstrut.width*fabs(cos(angle)));

    snow->jStrutLow= dJointCreateBall(world,0);
    dJointAttach(snow->jStrutLow, snow->lowlink.body, snow->lowstrut.body);
    dJointSetBallAnchor(snow->jStrutLow, ps2[0]
            ,ps2[1]-0.5*shiftSign*snow->lowstrut.width*fabs(sin(angle))
            ,ps2[2]-0.5*snow->lowstrut.width*fabs(cos(angle)));

    // struct sliding joint
    snow->jStrutMid= dJointCreateSlider(world,0);
    dJointAttach(snow->jStrutMid, snow->upstrut.body, snow->lowstrut.body);
    dJointSetSliderAxis(snow->jStrutMid, 0.0,shiftSign*fabs(sin(angle)),fabs(cos(angle)));

    // set joint feedback
    wnow->feedback= new dJointFeedback;
    dJointSetFeedback(snow->jStrutMid,wnow->feedback);

    // suspension slider
    /*snow->jLowSpring= dJointCreateLMotor(world,0);
    dJointAttach(snow->jLowSpring, chassis.body, snow->lowlink.body);
    dJointSetLMotorNumAxes(snow->jLowSpring, 1);
    dJointSetLMotorAxis(snow->jLowSpring,0,0, 0.0, 0.0, 1.0);*/


}
bool ActiveCardanModel::initModel() {

    // Create the 4 components of the hinge
    cardanRoot_ = this->createBody(B_SLOT_A_ID);
    dBodyID connectionPartA = this->createBody(B_CONNECTION_A_ID);
    dBodyID crossPartA = this->createBody(B_CROSS_PART_A_ID);
    dBodyID crossPartB = this->createBody(B_CROSS_PART_B_ID);
    dBodyID connectionPartB = this->createBody(B_CONNECTION_B_ID);
    cardanTail_ = this->createBody(B_SLOT_B_ID);

    float separation = inMm(0.1);

    this->createBoxGeom(cardanRoot_, MASS_SLOT, osg::Vec3(0, 0, 0),
                        SLOT_THICKNESS, SLOT_WIDTH, SLOT_WIDTH);

    dReal xPartA = SLOT_THICKNESS / 2 + separation
                   + CONNNECTION_PART_LENGTH / 2;
    this->createBoxGeom(connectionPartA, MASS_SERVO, osg::Vec3(xPartA, 0, 0),
                        CONNNECTION_PART_LENGTH, CONNNECTION_PART_WIDTH,
                        CONNECTION_PART_HEIGHT);

    dReal xCrossPartA = xPartA + CONNECTION_PART_OFFSET;
    this->createBoxGeom(crossPartA, MASS_CROSS / 3,
                        osg::Vec3(xCrossPartA, 0, 0), CROSS_THICKNESS, CROSS_WIDTH,
                        CROSS_HEIGHT);
    this->createBoxGeom(crossPartB, MASS_CROSS / 3,
                        osg::Vec3(xCrossPartA, 0, 0), CROSS_THICKNESS, CROSS_HEIGHT,
                        CROSS_WIDTH);

    dReal xPartB = xCrossPartA + CONNECTION_PART_OFFSET;
    this->createBoxGeom(connectionPartB, MASS_SERVO, osg::Vec3(xPartB, 0, 0),
                        CONNNECTION_PART_LENGTH, CONNECTION_PART_HEIGHT,
                        CONNNECTION_PART_WIDTH);

    dReal xTail = xPartB + CONNNECTION_PART_LENGTH / 2 + separation
                  + SLOT_THICKNESS / 2;
    this->createBoxGeom(cardanTail_, MASS_SLOT, osg::Vec3(xTail, 0, 0),
                        SLOT_THICKNESS, SLOT_WIDTH, SLOT_WIDTH);

    // Cross Geometries
    dBodyID crossPartAedge1 = this->createBody(B_CROSS_PART_A_EDGE_1_ID);
    dBodyID crossPartAedge2 = this->createBody(B_CROSS_PART_A_EDGE_2_ID);

    dBodyID crossPartBedge1 = this->createBody(B_CROSS_PART_B_EDGE_1_ID);
    dBodyID crossPartBedge2 = this->createBody(B_CROSS_PART_B_EDGE_2_ID);

    this->createBoxGeom(crossPartAedge1, MASS_CROSS / 12,
                        osg::Vec3(xCrossPartA - (CROSS_WIDTH / 2 + CROSS_THICKNESS / 2), 0,
                                  CROSS_HEIGHT / 2 + (CROSS_THICKNESS / 2)),
                        CROSS_CENTER_OFFSET, CROSS_WIDTH, CROSS_THICKNESS);

    this->createBoxGeom(crossPartAedge2, MASS_CROSS / 12,
                        osg::Vec3(xCrossPartA - (CROSS_WIDTH / 2 + CROSS_THICKNESS / 2), 0,
                                  -CROSS_HEIGHT / 2 - (CROSS_THICKNESS / 2)),
                        CROSS_CENTER_OFFSET, CROSS_WIDTH, CROSS_THICKNESS);

    this->createBoxGeom(crossPartBedge1, MASS_CROSS / 12,
                        osg::Vec3(xCrossPartA + (CROSS_WIDTH / 2 + CROSS_THICKNESS / 2),
                                  CROSS_HEIGHT / 2 - (CROSS_THICKNESS / 2), 0),
                        CROSS_CENTER_OFFSET, CROSS_THICKNESS, CROSS_WIDTH);

    this->createBoxGeom(crossPartBedge2, MASS_CROSS / 12,
                        osg::Vec3(xCrossPartA + (CROSS_WIDTH / 2 + CROSS_THICKNESS / 2),
                                  -CROSS_HEIGHT / 2 + (CROSS_THICKNESS / 2), 0),
                        CROSS_CENTER_OFFSET, CROSS_THICKNESS, CROSS_WIDTH);

    // Create joints to hold pieces in position

    // root <-> connectionPartA
    this->fixBodies(cardanRoot_, connectionPartA, osg::Vec3(1, 0, 0));

    // connectionPartA <(hinge)> crossPartA
    dJointID joint = dJointCreateHinge(this->getPhysicsWorld(), 0);
    dJointAttach(joint, connectionPartA, crossPartA);
    dJointSetHingeAxis(joint, 0, 0, 1);
    dJointSetHingeAnchor(joint,
                         xPartA
                         + ((CONNNECTION_PART_LENGTH / 2)
                            - (CONNNECTION_PART_LENGTH
                               - CONNNECTION_PART_ROTATION_OFFSET)), 0, 0);

    // crossPartA <-> crossPartB
    this->fixBodies(crossPartA, crossPartB, osg::Vec3(1, 0, 0));

    // crossPartB <(hinge)> connectionPartB
    dJointID joint2 = dJointCreateHinge(this->getPhysicsWorld(), 0);
    dJointAttach(joint2, crossPartB, connectionPartB);
    dJointSetHingeAxis(joint2, 0, 1, 0);
    dJointSetHingeAnchor(joint2,
                         xPartB
                         - ((CONNNECTION_PART_LENGTH / 2)
                            - (CONNNECTION_PART_LENGTH
                               - CONNNECTION_PART_ROTATION_OFFSET)), 0, 0);

    // connectionPartB <-> tail
    this->fixBodies(connectionPartB, cardanTail_, osg::Vec3(1, 0, 0));

    // Fix cross Parts
    this->fixBodies(crossPartA, crossPartAedge1, osg::Vec3(1, 0, 0));
    this->fixBodies(crossPartA, crossPartAedge2, osg::Vec3(1, 0, 0));
    this->fixBodies(crossPartB, crossPartBedge1, osg::Vec3(1, 0, 0));
    this->fixBodies(crossPartB, crossPartBedge2, osg::Vec3(1, 0, 0));

    // Create motors
    motor1_.reset(
        new ServoMotor(joint, ServoMotor::DEFAULT_MAX_FORCE_SERVO,
                       ServoMotor::DEFAULT_GAIN,
                       ioPair(this->getId(),0)));
    motor2_.reset(
        new ServoMotor(joint2, ServoMotor::DEFAULT_MAX_FORCE_SERVO,
                       ServoMotor::DEFAULT_GAIN,
                       ioPair(this->getId(),1)));

    return true;

}
Пример #26
0
int main (int argc, char **argv)
{
  dMass m;

  // 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);
  world = dWorldCreate();
  space = dHashSpaceCreate (0);
  contactgroup = dJointGroupCreate (0);
  dWorldSetGravity (world,0,0,-9.8);
  dWorldSetQuickStepNumIterations (world, 20);

  int i;
  for (i=0; i<SEGMCNT; i++)
  {
    segbodies[i] = dBodyCreate (world);
    dBodySetPosition(segbodies[i], i - SEGMCNT/2.0, 0, 5);
    dMassSetBox (&m, 1, SEGMDIM[0], SEGMDIM[1], SEGMDIM[2]);
    dBodySetMass (segbodies[i], &m);
    seggeoms[i] = dCreateBox (0, SEGMDIM[0], SEGMDIM[1], SEGMDIM[2]);
    dGeomSetBody (seggeoms[i], segbodies[i]);
    dSpaceAdd (space, seggeoms[i]);
  }

  for (i=0; i<SEGMCNT-1; i++)
  {
    hinges[i] = dJointCreateHinge (world,0);
    dJointAttach (hinges[i], segbodies[i],segbodies[i+1]);
    dJointSetHingeAnchor (hinges[i], i + 0.5 - SEGMCNT/2.0, 0, 5);
    dJointSetHingeAxis (hinges[i], 0,1,0);
    dJointSetHingeParam (hinges[i],dParamFMax,  8000.0);
    // NOTE:
    // Here we tell ODE where to put the feedback on the forces for this hinge
    dJointSetFeedback (hinges[i], jfeedbacks+i);
    stress[i]=0;
  }

  for (i=0; i<STACKCNT; i++)
  {
    stackbodies[i] = dBodyCreate(world);
    dMassSetBox (&m, 2.0, 2, 2, 0.6);
    dBodySetMass(stackbodies[i],&m);

    stackgeoms[i] = dCreateBox(0, 2, 2, 0.6);
    dGeomSetBody(stackgeoms[i], stackbodies[i]);
    dBodySetPosition(stackbodies[i], 0,0,8+2*i);
    dSpaceAdd(space, stackgeoms[i]);
  }

  sliders[0] = dJointCreateSlider (world,0);
  dJointAttach(sliders[0], segbodies[0], 0);
  dJointSetSliderAxis (sliders[0], 1,0,0);
  dJointSetSliderParam (sliders[0],dParamFMax,  4000.0);
  dJointSetSliderParam (sliders[0],dParamLoStop,   0.0);
  dJointSetSliderParam (sliders[0],dParamHiStop,   0.2);

  sliders[1] = dJointCreateSlider (world,0);
  dJointAttach(sliders[1], segbodies[SEGMCNT-1], 0);
  dJointSetSliderAxis (sliders[1], 1,0,0);
  dJointSetSliderParam (sliders[1],dParamFMax,  4000.0);
  dJointSetSliderParam (sliders[1],dParamLoStop,   0.0);
  dJointSetSliderParam (sliders[1],dParamHiStop,  -0.2);

  groundgeom = dCreatePlane(space, 0,0,1,0);

  for (i=0; i<SEGMCNT; i++)
    colours[i]=0.0;

  // run simulation
  dsSimulationLoop (argc,argv,352,288,&fn);

  dJointGroupEmpty (contactgroup);
  dJointGroupDestroy (contactgroup);

  // First destroy seggeoms, then space, then the world.
  for (i=0; i<SEGMCNT; i++)
    dGeomDestroy (seggeoms[i]);
  for (i=0; i<STACKCNT; i++)
    dGeomDestroy (stackgeoms[i]);

  dSpaceDestroy(space);
  dWorldDestroy (world);
  dCloseODE();
  return 0;
}
void PhysicsHingeJoint::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 = dJointCreateHinge(getWorld()->getWorldID(), 0);
        }
        else
        {
            _JointID = dJointCreateHinge(getWorld()->getWorldID(), 0);
            if(!(whichField & HiStopFieldMask))
            {
                setHiStop(dJointGetHingeParam(_JointID,dParamHiStop));
            }
            if(!(whichField & LoStopFieldMask))
            {
                setLoStop(dJointGetHingeParam(_JointID,dParamLoStop));
            }
            if(!(whichField & BounceFieldMask))
            {
                setBounce(dJointGetHingeParam(_JointID,dParamBounce));
            }
            if(!(whichField & CFMFieldMask))
            {
                setCFM(dJointGetHingeParam(_JointID,dParamCFM));
            }
            if(!(whichField & StopCFMFieldMask))
            {
                setStopCFM(dJointGetHingeParam(_JointID,dParamStopCFM));
            }
            if(!(whichField & StopERPFieldMask))
            {
                setStopERP(dJointGetHingeParam(_JointID,dParamStopERP));
            }
        }
    }

    Inherited::changed(whichField, origin, details);

    if((whichField & AnchorFieldMask) || (whichField & WorldFieldMask))
    {
	    dJointSetHingeAnchor(_JointID, getAnchor().x(), getAnchor().y(), getAnchor().z());
    }
    if((whichField & AxisFieldMask) || (whichField & WorldFieldMask))
    {
	    dJointSetHingeAxis(_JointID, getAxis().x(), getAxis().y(), getAxis().z());
    }
    if((whichField & HiStopFieldMask) || (whichField & WorldFieldMask))
    {
        dJointSetHingeParam(_JointID,  dParamHiStop, getHiStop());
    }
    if((whichField & LoStopFieldMask) || (whichField & WorldFieldMask))
    {
        dJointSetHingeParam(_JointID,  dParamLoStop, getLoStop());
    }
    if((whichField & BounceFieldMask) || (whichField & WorldFieldMask))
    {
        dJointSetHingeParam(_JointID,  dParamBounce, getBounce());
    }
    if((whichField & CFMFieldMask) || (whichField & WorldFieldMask))
    {
        dJointSetHingeParam(_JointID,  dParamCFM, getCFM());
    }
    if((whichField & StopERPFieldMask) || (whichField & WorldFieldMask))
    {
        dJointSetHingeParam(_JointID,  dParamStopERP, getStopERP());
    }
    if((whichField & StopCFMFieldMask) || (whichField & WorldFieldMask))
    {
        dJointSetHingeParam(_JointID,  dParamStopCFM, getStopCFM());
    }
}
Пример #28
0
/*** Formation of robot ***/
void  makeRobot() 
{ 

  dReal torso_m = 10.0;                    // Mass of body
  dReal  l1m = 0.005,l2m = 0.5, l3m = 0.5; // Mass of leg segments 
	
  //for four legs
  dReal x[num_legs][num_links] = {{ cx1, cx1, cx1},{-cx1,-cx1,-cx1},// Position of each link (x coordinate)
		{-cx1,-cx1,-cx1},{ cx1, cx1, cx1}};
  dReal y[num_legs][num_links] = {{ cy1, cy1, cy1},{ cy1, cy1, cy1},// Position of each link (y coordinate)
		{-cy1,-cy1,-cy1},{-cy1,-cy1,-cy1}};
  dReal z[num_legs][num_links] = {                                  // Position of each link (z coordinate)
		{c_z[0][0],(c_z[0][0]+c_z[0][2])/2,c_z[0][2]-l3/2},
		{c_z[0][0],(c_z[0][0]+c_z[0][2])/2,c_z[0][2]-l3/2},
		{c_z[0][0],(c_z[0][0]+c_z[0][2])/2,c_z[0][2]-l3/2},
		{c_z[0][0],(c_z[0][0]+c_z[0][2])/2,c_z[0][2]-l3/2}};
	
	
  dReal r[num_links]          =  { r1, r2, r3}; // radius of leg segment
  dReal length[num_links]     =  { l1, l2, l3}; // Length of leg segment
  dReal weight[num_links]     =  {l1m,l2m,l3m}; // Mass of leg segment
	
	//  //for one leg
	//  dReal axis_x[num_legs_pneat][num_links_pneat] = {{ 0,1, 0}};
	//  dReal axis_y[num_legs_pneat][num_links_pneat] = {{ 1,0, 1}};
	//  dReal axis_z[num_legs_pneat][num_links_pneat] = {{ 0,0, 0}};
	
  //for four legs
  dReal axis_x[num_legs][num_links] = {{ 0,1, 0},{ 0,1,0},{ 0, 1, 0},{ 0, 1, 0}};
  dReal axis_y[num_legs][num_links] = {{ 1,0, 1},{ 1,0,1},{ 1, 0, 1},{ 1, 0, 1}};
  dReal axis_z[num_legs][num_links] = {{ 0,0, 0},{ 0,0,0},{ 0, 0, 0},{ 0, 0, 0}};
  
  // For mation of the body
  dMass mass; 
  torso[0].body  = dBodyCreate(world);
  dMassSetZero(&mass);
  dMassSetBoxTotal(&mass,torso_m, lx, ly, lz);
  dBodySetMass(torso[0].body,&mass);
  torso[0].geom = dCreateBox(space, lx, ly, lz); 
  dGeomSetBody(torso[0].geom, torso[0].body); 
  dBodySetPosition(torso[0].body, SX, SY, SZ); 
	
  // Formation of leg
  dMatrix3 R;                          // Revolution queue
  dRFromAxisAndAngle(R,1,0,0,M_PI/2);  // 90 degrees to turn, parallel with the land
  for (int i = 0; i < num_legs; i++) { 
    for (int j = 0; j < num_links; j++) { 
      leg[0][i][j].body = dBodyCreate(world);
      if (j == 0) dBodySetRotation(leg[0][i][j].body,R);
      dBodySetPosition(leg[0][i][j].body, SX+x[i][j], SY+y[i][j], SZ+z[i][j]);
      dMassSetZero(&mass);
      dMassSetCapsuleTotal(&mass,weight[j],3,r[j],length[j]);
      dBodySetMass(leg[0][i][j].body, &mass);
      //if(i==1 and j==2)                                         //to set the length of one leg differently
      //leg[i][j].geom = dCreateCapsule(space_pneat,r[j],length[j]+.5); //set the length of the leg
      //else
      leg[0][i][j].geom = dCreateCapsule(space,r[j],length[j]); //set the length of the leg
      dGeomSetBody(leg[0][i][j].geom,leg[0][i][j].body);
    }
  }
	
  // Formation of joints (and connecting them up)
  for (int i = 0; i < num_legs; i++) { 
    for (int j = 0; j < num_links; j++) { 
      leg[0][i][j].joint = dJointCreateHinge(world, 0);
      if (j == 0){ 
				dJointAttach(leg[0][i][j].joint, torso[0].body, leg[0][i][j].body); //connects hip to the environment
				dJointSetHingeParam(leg[0][i][j].joint, dParamLoStop, -.5*M_PI); //prevent the hip forward-back from going more than 90 degrees
				dJointSetHingeParam(leg[0][i][j].joint, dParamHiStop,  .5*M_PI);
      }
      else
        dJointAttach(leg[0][i][j].joint, leg[0][i][j-1].body, leg[0][i][j].body);
      dJointSetHingeAnchor(leg[0][i][j].joint, SX+c_x[i][j], SY+c_y[i][j],SZ+c_z[i][j]);
      dJointSetHingeAxis(leg[0][i][j].joint, axis_x[i][j], axis_y[i][j],axis_z[i][j]);
    } 
  } 
  
}
Пример #29
0
// ロボットの生成
void create() {

  // SHIELDの生成(空間に固定)
  rod[0].body = dBodyCreate(world);
  dBodySetPosition(rod[0].body, SHIELD_X, SHIELD_Y, SHIELD_Z);
  dMassSetZero(&mass);
  dMassSetCylinderTotal(&mass, SHIELD_WEIGHT, 2, SHIELD_RADIUS, SHIELD_LENGTH);
  dBodySetMass(rod[0].body, &mass);
  rod[0].geom = dCreateCylinder(space, SHIELD_RADIUS, SHIELD_LENGTH);
  dGeomSetBody(rod[0].geom, rod[0].body);
  dRFromAxisAndAngle(R, 1, 0, 0, 0.5 * M_PI);  // x軸に90度回転
  dBodySetRotation(rod[0].body, R);

  rod_joint[0] = dJointCreateFixed(world, 0);  // 固定ジョイント
  dJointAttach(rod_joint[0], rod[0].body, 0);
  dJointSetFixed(rod_joint[0]);

  // RODの生成(回転ジョイントy軸に回転軸)
  rod[1].body = dBodyCreate(world);
  dBodySetPosition(rod[1].body, SHIELD_X, SHIELD_Y, SHIELD_Z);
  dMassSetZero(&mass);
  dMassSetBoxTotal(&mass, ROD_WEIGHT, ROD_WIDTH, ROD_WIDTH, ROD_LENGTH);
  dBodySetMass(rod[1].body, &mass);
  rod[1].geom = dCreateBox(space, ROD_WIDTH, ROD_WIDTH, ROD_LENGTH);
  dGeomSetBody(rod[1].geom, rod[1].body);
  dRFromAxisAndAngle(R, 0, 0, 1, 0.25 * M_PI);  // z軸に45度回転
  dBodySetRotation(rod[1].body, R);

  rod_joint[1] = dJointCreateHinge(world, 0); // ヒンジジョイント
  dJointAttach(rod_joint[1], rod[1].body, rod[0].body);
  dJointSetHingeAnchor(rod_joint[1], SHIELD_X, SHIELD_Y, SHIELD_Z);
  dJointSetHingeAxis(rod_joint[1], 0, 1, 0);// y軸ジョイント

  // BODYの生成(たてておくだけ)
  rod[2].body = dBodyCreate(world);
  dBodySetPosition(rod[2].body, BODY_X, BODY_Y, BODY_Z);
  dMassSetZero(&mass);
  dMassSetBoxTotal(&mass, BODY_WEIGHT, BODY_WIDTH, BODY_LENGTH, BODY_HEIGHT);
  dBodySetMass(rod[2].body, &mass);
  rod[2].geom = dCreateBox(space, BODY_WIDTH, BODY_LENGTH, BODY_HEIGHT);
  dGeomSetBody(rod[2].geom, rod[2].body);

  // BULLETの生成(CANNON中心に初期座標)
  bullet.body = dBodyCreate(world);
  dMassSetZero(&mass);
  dMassSetSphereTotal(&mass, BULLET_WEIGHT, BULLET_RADIUS);
  dBodySetMass(bullet.body,&mass);
  dBodySetPosition(bullet.body, CANNON_X, CANNON_Y, CANNON_Z);
  bullet.geom = dCreateSphere(space, BULLET_RADIUS);
  dGeomSetBody(bullet.geom, bullet.body);

  // TARGETの生成
//  target.body = dBodyCreate(world);
//  dMassSetZero(&mass);
//  dMassSetSphereTotal(&mass, 0.0001, BULLET_RADIUS);
//  dBodySetMass(target.body,&mass);
//  dBodySetPosition(target.body, SHIELD_X, SHIELD_Y, SHIELD_Z);
//  target.geom = dCreateSphere(space, BULLET_RADIUS);
//  dGeomSetBody(target.geom, target.body);

}
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;
}