void CODEUniversalJoint::setParams()
{
double anchorPos[3];
mpBodyAnchors[0]->getPosition(anchorPos);
dJointSetUniversalAnchor(mID, anchorPos[0], anchorPos[1], anchorPos[2]);
dJointSetUniversalAxis1(mID, mAxis1X, mAxis1Y, mAxis1Z);
dJointSetUniversalAxis1(mID, mAxis2X, mAxis2Y, mAxis2Z);
}
static void _soy_joints_universal_axis1_set (soyjointsUniversal* self, soyatomsAxis* value) {
soyatomsAxis* _tmp0_;
soyatomsAxis* _tmp1_;
soyatomsAxis* axis1;
soyscenesScene* _tmp2_;
struct dxJoint* _tmp3_;
gfloat _tmp4_;
gfloat _tmp5_;
gfloat _tmp6_;
gfloat _tmp7_;
gfloat _tmp8_;
gfloat _tmp9_;
soyscenesScene* _tmp10_;
g_return_if_fail (self != NULL);
g_return_if_fail (value != NULL);
_tmp0_ = value;
_tmp1_ = soy_atoms_axis_new_normalize (_tmp0_);
axis1 = _tmp1_;
_tmp2_ = ((soyjointsJoint*) self)->scene;
g_rw_lock_writer_lock (&_tmp2_->stepLock);
_tmp3_ = ((soyjointsJoint*) self)->joint;
_tmp4_ = soy_atoms_axis_get_x (axis1);
_tmp5_ = _tmp4_;
_tmp6_ = soy_atoms_axis_get_y (axis1);
_tmp7_ = _tmp6_;
_tmp8_ = soy_atoms_axis_get_z (axis1);
_tmp9_ = _tmp8_;
dJointSetUniversalAxis1 ((struct dxJoint*) _tmp3_, (dReal) _tmp5_, (dReal) _tmp7_, (dReal) _tmp9_);
_tmp10_ = ((soyjointsJoint*) self)->scene;
g_rw_lock_writer_unlock (&_tmp10_->stepLock);
_g_object_unref0 (axis1);
}
OscUniversalODE::OscUniversalODE(dWorldID odeWorld, dSpaceID odeSpace,
const char *name, OscBase *parent,
OscObject *object1, OscObject *object2,
double x, double y, double z, double a1x,
double a1y, double a1z, double a2x,
double a2y, double a2z)
: OscUniversal(name, parent, object1, object2, x, y, z,
a1x, a1y, a1z, a2x, a2y, a2z)
{
m_response = new OscResponse("response",this);
dJointID odeJoint = dJointCreateUniversal(odeWorld,0);
m_pSpecial = new ODEConstraint(this, odeJoint, odeWorld, odeSpace,
object1, object2);
dJointSetUniversalAnchor(odeJoint, x, y, z);
dJointSetUniversalAxis1(odeJoint, a1x, a1y, a1z);
dJointSetUniversalAxis2(odeJoint, a2x, a2y, a2z);
printf("[%s] Universal joint created between %s and %s at (%f, %f, %f) for axes (%f, %f, %f) and (%f,%f,%f)\n",
simulation()->type_str(),
object1->c_name(), object2?object2->c_name():"world",
x, y, z, a1x, a1y, a1z, a2x, a2y, a2z);
}
/**
* Creates a new ODE_UniversalJoint.
*
* @param body1 the first body to connect the joint to.
* @param body2 the second body to connect the joint to.
* @return the new ODE_UniversalJoint.
*/
dJointID ODE_UniversalJoint::createJoint(dBodyID body1, dBodyID body2) {
if(mJointAxis1Point1->get().equals(mJointAxis1Point2->get())
|| mJointAxis2Point1->get().equals(mJointAxis2Point2->get())) {
Core::log("Invalid axes for ODE_UniversalJoint.");
return 0;
}
//if one of the bodyIDs is null, the joint is connected to a static object.
dJointID newJoint = dJointCreateUniversal(mWorldID, mGeneralJointGroup);
dJointAttach(newJoint, body1, body2);
Vector3D anchor = mAnchorPoint->get() ;
Vector3D axis1;
Vector3D axis2;
axis1.set(mJointAxis1Point2->getX() - mJointAxis1Point1->getX(),
mJointAxis1Point2->getY() - mJointAxis1Point1->getY(),
mJointAxis1Point2->getZ() - mJointAxis1Point1->getZ());
axis2.set(mJointAxis2Point2->getX() - mJointAxis2Point1->getX(),
mJointAxis2Point2->getY() - mJointAxis2Point1->getY(),
mJointAxis2Point2->getZ() - mJointAxis2Point1->getZ());
dJointSetUniversalAnchor(newJoint, anchor.getX(), anchor.getY(), anchor.getZ());
dJointSetUniversalAxis1(newJoint, axis1.getX(), axis1.getY(), axis1.getZ());
dJointSetUniversalAxis2(newJoint, axis2.getX(), axis2.getY(), axis2.getZ());
return newJoint;
}
void CUniversalJoint::UpdateTransform(const matrix44& Tfm)
{
vector3 p = Tfm * Anchor;
dJointSetUniversalAnchor(ODEJointID, p.x, p.y, p.z);
matrix33 m33(Tfm.x_component(), Tfm.y_component(), Tfm.z_component());
vector3 a0 = m33 * AxisParams[0].Axis;
vector3 a1 = m33 * AxisParams[1].Axis;
dJointSetUniversalAxis1(ODEJointID, a0.x, a0.y, a0.z);
dJointSetUniversalAxis2(ODEJointID, a1.x, a1.y, a1.z);
}
static void set_phys_joint_axis_1(dJointID j, const float v[3])
{
switch (dJointGetType(j))
{
case dJointTypeHinge:
dJointSetHingeAxis (j, v[0], v[1], v[2]); break;
case dJointTypeSlider:
dJointSetSliderAxis (j, v[0], v[1], v[2]); break;
case dJointTypeHinge2:
dJointSetHinge2Axis1 (j, v[0], v[1], v[2]); break;
case dJointTypeUniversal:
dJointSetUniversalAxis1(j, v[0], v[1], v[2]); break;
default: break;
}
}
/*
=================================================================================
createUniversalLeg
Use parameters to create leg body/geom and attach to body with universal joint
**Warning**
mass is not set
=================================================================================
*/
void createUniversalLeg(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);
//universal joint
joint = dJointCreateUniversal(World, jointgroup);
//attach and anchor
dJointAttach(joint, bodyAttachedTo.Body, leg.Body);
dJointSetUniversalAnchor(joint, anchorXPos, anchorYPos, anchorZPos);
//axes
dJointSetUniversalAxis1(joint, 0, 0, 1);
dJointSetUniversalAxis2(joint, 0, 1, 0);
//Max and min angles
dJointSetUniversalParam(joint, dParamHiStop, maxAngle);
dJointSetUniversalParam(joint, dParamLoStop, minAngle);
dJointSetUniversalParam(joint, dParamHiStop2, maxAngle);
dJointSetUniversalParam(joint, dParamLoStop2, minAngle);
}
/**
* 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 UniversalJoint::attachJoint(dBodyID obj1, dBodyID obj2)
{
gmtl::Vec3f newAnchor, newAxis1, newAxis2, scaleVec;
gmtl::Quatf entityRot;
gmtl::AxisAnglef axAng;
TransformationData entityTrans;
joint = dJointCreateUniversal(world, 0);
dJointAttach(joint, obj1, obj2);
newAxis1 = axis1;
newAxis2 = axis2;
newAnchor = anchor;
if (mainEntity != NULL)
{
entityTrans = mainEntity->getEnvironmentTransformation();
// get the scale values of the mainEntity
// scaleVec[0] = mainEntity->getXScale();
// scaleVec[1] = mainEntity->getYScale();
// scaleVec[2] = mainEntity->getZScale();
scaleVec = entityTrans.scale;
// scale Anchor-offset by mainEntity-scale value
newAnchor[0] *= scaleVec[0];
newAnchor[1] *= scaleVec[1];
newAnchor[2] *= scaleVec[2];
// scale Axes by mainEntity-scale value because of possible distortion
newAxis1[0] *= scaleVec[0];
newAxis1[1] *= scaleVec[1];
newAxis1[2] *= scaleVec[2];
gmtl::normalize(newAxis1);
newAxis2[0] *= scaleVec[0];
newAxis2[1] *= scaleVec[1];
newAxis2[2] *= scaleVec[2];
gmtl::normalize(newAxis2);
// get the Rotation of the mainEntity
// axAng[0] = mainEntity->getRotAngle();
// axAng[1] = mainEntity->getXRot();
// axAng[2] = mainEntity->getYRot();
// axAng[3] = mainEntity->getZRot();
// gmtl::set(entityRot, axAng);
entityRot = entityTrans.orientation;
// rotate Axes by mainEntity-rotation
newAxis1 *= entityRot;
newAxis2 *= entityRot;
// rotate Anchor-offset by mainEntity-rotation
newAnchor *= entityRot;
// transform new Anchor to world coordinates
// newAnchor[0] += mainEntity->getXTrans();
// newAnchor[1] += mainEntity->getYTrans();
// newAnchor[2] += mainEntity->getZTrans();
newAnchor += entityTrans.position;
} // if
dJointSetUniversalAnchor(joint, newAnchor[0], newAnchor[1], newAnchor[2]);
dJointSetUniversalAxis1(joint, newAxis1[0], newAxis1[1], newAxis1[2]);
dJointSetUniversalAxis2(joint, newAxis2[0], newAxis2[1], newAxis2[2]);
} // attachJoint
static void soy_joints_universal_real_setup (soyjointsJoint* base, soyatomsPosition* anchor, soyatomsAxis* axis1, soyatomsAxis* axis2) {
soyjointsUniversal * self;
struct dxJoint* _tmp0_;
soyatomsPosition* _tmp1_;
gfloat _tmp2_;
gfloat _tmp3_;
soyatomsPosition* _tmp4_;
gfloat _tmp5_;
gfloat _tmp6_;
soyatomsPosition* _tmp7_;
gfloat _tmp8_;
gfloat _tmp9_;
struct dxJoint* _tmp10_;
soyatomsAxis* _tmp11_;
gfloat _tmp12_;
gfloat _tmp13_;
soyatomsAxis* _tmp14_;
gfloat _tmp15_;
gfloat _tmp16_;
soyatomsAxis* _tmp17_;
gfloat _tmp18_;
gfloat _tmp19_;
struct dxJoint* _tmp20_;
soyatomsAxis* _tmp21_;
gfloat _tmp22_;
gfloat _tmp23_;
soyatomsAxis* _tmp24_;
gfloat _tmp25_;
gfloat _tmp26_;
soyatomsAxis* _tmp27_;
gfloat _tmp28_;
gfloat _tmp29_;
self = (soyjointsUniversal*) base;
_tmp0_ = ((soyjointsJoint*) self)->joint;
_tmp1_ = anchor;
_tmp2_ = soy_atoms_position_get_x (_tmp1_);
_tmp3_ = _tmp2_;
_tmp4_ = anchor;
_tmp5_ = soy_atoms_position_get_y (_tmp4_);
_tmp6_ = _tmp5_;
_tmp7_ = anchor;
_tmp8_ = soy_atoms_position_get_z (_tmp7_);
_tmp9_ = _tmp8_;
dJointSetUniversalAnchor ((struct dxJoint*) _tmp0_, (dReal) _tmp3_, (dReal) _tmp6_, (dReal) _tmp9_);
_tmp10_ = ((soyjointsJoint*) self)->joint;
_tmp11_ = axis1;
_tmp12_ = soy_atoms_axis_get_x (_tmp11_);
_tmp13_ = _tmp12_;
_tmp14_ = axis1;
_tmp15_ = soy_atoms_axis_get_y (_tmp14_);
_tmp16_ = _tmp15_;
_tmp17_ = axis1;
_tmp18_ = soy_atoms_axis_get_z (_tmp17_);
_tmp19_ = _tmp18_;
dJointSetUniversalAxis1 ((struct dxJoint*) _tmp10_, (dReal) _tmp13_, (dReal) _tmp16_, (dReal) _tmp19_);
_tmp20_ = ((soyjointsJoint*) self)->joint;
_tmp21_ = axis2;
_tmp22_ = soy_atoms_axis_get_x (_tmp21_);
_tmp23_ = _tmp22_;
_tmp24_ = axis2;
_tmp25_ = soy_atoms_axis_get_y (_tmp24_);
_tmp26_ = _tmp25_;
_tmp27_ = axis2;
_tmp28_ = soy_atoms_axis_get_z (_tmp27_);
_tmp29_ = _tmp28_;
dJointSetUniversalAxis2 ((struct dxJoint*) _tmp20_, (dReal) _tmp23_, (dReal) _tmp26_, (dReal) _tmp29_);
}
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
}
// called by Webots at the beginning of the simulation
void webots_physics_init(dWorldID w, dSpaceID s, dJointGroupID j) {
int i;
// store global objects for later use
world = w;
space = s;
contact_joint_group = j;
// get floor geometry
floor_geom = getGeom(floor_name);
if (!floor_geom)
return;
// get foot geometry and body id's
for (i = 0; i < N_FEET; i++) {
foot_geom[i] = getGeom(foot_name[i]);
if (!foot_geom[i])
return;
foot_body[i] = dGeomGetBody(foot_geom[i]);
if (!foot_body[i])
return;
}
// create universal joints for linear actuators
for (i = 0; i < 10; i++) {
dBodyID upper_piston = getBody(upper_piston_name[i]);
dBodyID lower_piston = getBody(lower_piston_name[i]);
dBodyID upper_link = getBody(upper_link_name[i]);
dBodyID lower_link = getBody(lower_link_name[i]);
if (!upper_piston || !lower_piston || !upper_link || !lower_link)
return;
// create a ball and socket joint (3 DOFs) to attach the lower piston body to the lower link
// we don't need a universal joint here, because the piston's passive rotation is prevented
// by the universal joint at its upper end.
dJointID lower_balljoint = dJointCreateBall(world, 0);
dJointAttach(lower_balljoint, lower_piston, lower_link);
// transform attachement point from local to global coordinate system
// warning: this is a hard-coded translation
dVector3 lower_ball;
dBodyGetRelPointPos(lower_piston, 0, 0, -0.075, lower_ball);
// set attachement point (anchor)
dJointSetBallAnchor(lower_balljoint, lower_ball[0], lower_ball[1], lower_ball[2]);
// create a universal joint (2 DOFs) to attach upper piston body to upper link
// we need to use a universal joint to prevent the piston from passively rotating around its long axis
dJointID upper_ujoint = dJointCreateUniversal(world, 0);
dJointAttach(upper_ujoint, upper_piston, upper_link);
// transform attachement point from local to global coordinate system
// warning: this is a hard-coded translation
dVector3 upper_ball;
dBodyGetRelPointPos(upper_piston, 0, 0, 0, upper_ball);
// set attachement point (anchor)
dJointSetUniversalAnchor(upper_ujoint, upper_ball[0], upper_ball[1], upper_ball[2]);
// set the universal joint axes
dVector3 upper_xaxis;
dVector3 upper_yaxis;
dBodyVectorToWorld(upper_piston, 1, 0, 0, upper_xaxis);
dBodyVectorToWorld(upper_piston, 0, 1, 0, upper_yaxis);
dJointSetUniversalAxis1(upper_ujoint, upper_xaxis[0], upper_xaxis[1], upper_xaxis[2]);
dJointSetUniversalAxis2(upper_ujoint, upper_yaxis[0], upper_yaxis[1], upper_yaxis[2]);
}
}
int setupTest (int n)
{
switch (n) {
// ********** fixed joint
case 0: { // 2 body
constructWorldForTest (0,2,
0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1,
1,1,0, 1,1,0,
0.25*M_PI,0.25*M_PI);
joint = dJointCreateFixed (world,0);
dJointAttach (joint,body[0],body[1]);
dJointSetFixed (joint);
return 1;
}
case 1: { // 1 body to static env
constructWorldForTest (0,1,
0.5*SIDE,0.5*SIDE,1, 0,0,0,
1,0,0, 1,0,0,
0,0);
joint = dJointCreateFixed (world,0);
dJointAttach (joint,body[0],0);
dJointSetFixed (joint);
return 1;
}
case 2: { // 2 body with relative rotation
constructWorldForTest (0,2,
0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1,
1,1,0, 1,1,0,
0.25*M_PI,-0.25*M_PI);
joint = dJointCreateFixed (world,0);
dJointAttach (joint,body[0],body[1]);
dJointSetFixed (joint);
return 1;
}
case 3: { // 1 body to static env with relative rotation
constructWorldForTest (0,1,
0.5*SIDE,0.5*SIDE,1, 0,0,0,
1,0,0, 1,0,0,
0.25*M_PI,0);
joint = dJointCreateFixed (world,0);
dJointAttach (joint,body[0],0);
dJointSetFixed (joint);
return 1;
}
// ********** hinge joint
case 200: // 2 body
constructWorldForTest (0,2,
0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1,
1,1,0, 1,1,0, 0.25*M_PI,0.25*M_PI);
joint = dJointCreateHinge (world,0);
dJointAttach (joint,body[0],body[1]);
dJointSetHingeAnchor (joint,0,0,1);
dJointSetHingeAxis (joint,1,-1,1.41421356);
return 1;
case 220: // hinge angle polarity test
case 221: // hinge angle rate test
constructWorldForTest (0,2,
0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1,
1,0,0, 1,0,0, 0,0);
joint = dJointCreateHinge (world,0);
dJointAttach (joint,body[0],body[1]);
dJointSetHingeAnchor (joint,0,0,1);
dJointSetHingeAxis (joint,0,0,1);
max_iterations = 50;
return 1;
case 230: // hinge motor rate (and polarity) test
case 231: // ...with stops
constructWorldForTest (0,2,
0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1,
1,0,0, 1,0,0, 0,0);
joint = dJointCreateHinge (world,0);
dJointAttach (joint,body[0],body[1]);
dJointSetHingeAnchor (joint,0,0,1);
dJointSetHingeAxis (joint,0,0,1);
dJointSetHingeParam (joint,dParamFMax,1);
if (n==231) {
dJointSetHingeParam (joint,dParamLoStop,-0.5);
dJointSetHingeParam (joint,dParamHiStop,0.5);
}
return 1;
case 250: // limit bounce test (gravity down)
case 251: { // ...gravity up
constructWorldForTest ((n==251) ? 0.1 : -0.1, 2,
0.5*SIDE,0,1+0.5*SIDE, -0.5*SIDE,0,1-0.5*SIDE,
1,0,0, 1,0,0, 0,0);
joint = dJointCreateHinge (world,0);
dJointAttach (joint,body[0],body[1]);
dJointSetHingeAnchor (joint,0,0,1);
dJointSetHingeAxis (joint,0,1,0);
dJointSetHingeParam (joint,dParamLoStop,-0.9);
dJointSetHingeParam (joint,dParamHiStop,0.7854);
dJointSetHingeParam (joint,dParamBounce,0.5);
// anchor 2nd body with a fixed joint
dJointID j = dJointCreateFixed (world,0);
dJointAttach (j,body[1],0);
dJointSetFixed (j);
return 1;
}
// ********** slider
case 300: // 2 body
constructWorldForTest (0,2,
0,0,1, 0.2,0.2,1.2,
0,0,1, -1,1,0, 0,0.25*M_PI);
joint = dJointCreateSlider (world,0);
dJointAttach (joint,body[0],body[1]);
dJointSetSliderAxis (joint,1,1,1);
return 1;
case 320: // slider angle polarity test
case 321: // slider angle rate test
constructWorldForTest (0,2,
0,0,1, 0,0,1.2,
1,0,0, 1,0,0, 0,0);
joint = dJointCreateSlider (world,0);
dJointAttach (joint,body[0],body[1]);
dJointSetSliderAxis (joint,0,0,1);
max_iterations = 50;
return 1;
case 330: // slider motor rate (and polarity) test
case 331: // ...with stops
constructWorldForTest (0, 2,
0,0,1, 0,0,1.2,
1,0,0, 1,0,0, 0,0);
joint = dJointCreateSlider (world,0);
dJointAttach (joint,body[0],body[1]);
dJointSetSliderAxis (joint,0,0,1);
dJointSetSliderParam (joint,dParamFMax,100);
if (n==331) {
dJointSetSliderParam (joint,dParamLoStop,-0.4);
dJointSetSliderParam (joint,dParamHiStop,0.4);
}
return 1;
case 350: // limit bounce tests
case 351: {
constructWorldForTest ((n==351) ? 0.1 : -0.1, 2,
0,0,1, 0,0,1.2,
1,0,0, 1,0,0, 0,0);
joint = dJointCreateSlider (world,0);
dJointAttach (joint,body[0],body[1]);
dJointSetSliderAxis (joint,0,0,1);
dJointSetSliderParam (joint,dParamLoStop,-0.5);
dJointSetSliderParam (joint,dParamHiStop,0.5);
dJointSetSliderParam (joint,dParamBounce,0.5);
// anchor 2nd body with a fixed joint
dJointID j = dJointCreateFixed (world,0);
dJointAttach (j,body[1],0);
dJointSetFixed (j);
return 1;
}
// ********** hinge-2 joint
case 420: // hinge-2 steering angle polarity test
case 421: // hinge-2 steering angle rate test
constructWorldForTest (0,2,
0.5*SIDE,0,1, -0.5*SIDE,0,1,
1,0,0, 1,0,0, 0,0);
joint = dJointCreateHinge2 (world,0);
dJointAttach (joint,body[0],body[1]);
dJointSetHinge2Anchor (joint,-0.5*SIDE,0,1);
dJointSetHinge2Axis1 (joint,0,0,1);
dJointSetHinge2Axis2 (joint,1,0,0);
max_iterations = 50;
return 1;
case 430: // hinge 2 steering motor rate (+polarity) test
case 431: // ...with stops
case 432: // hinge 2 wheel motor rate (+polarity) test
constructWorldForTest (0,2,
0.5*SIDE,0,1, -0.5*SIDE,0,1,
1,0,0, 1,0,0, 0,0);
joint = dJointCreateHinge2 (world,0);
dJointAttach (joint,body[0],body[1]);
dJointSetHinge2Anchor (joint,-0.5*SIDE,0,1);
dJointSetHinge2Axis1 (joint,0,0,1);
dJointSetHinge2Axis2 (joint,1,0,0);
dJointSetHinge2Param (joint,dParamFMax,1);
dJointSetHinge2Param (joint,dParamFMax2,1);
if (n==431) {
dJointSetHinge2Param (joint,dParamLoStop,-0.5);
dJointSetHinge2Param (joint,dParamHiStop,0.5);
}
return 1;
// ********** angular motor joint
case 600: // test euler angle calculations
constructWorldForTest (0,2,
-SIDE*0.5,0,1, SIDE*0.5,0,1,
0,0,1, 0,0,1, 0,0);
joint = dJointCreateAMotor (world,0);
dJointAttach (joint,body[0],body[1]);
dJointSetAMotorNumAxes (joint,3);
dJointSetAMotorAxis (joint,0,1, 0,0,1);
dJointSetAMotorAxis (joint,2,2, 1,0,0);
dJointSetAMotorMode (joint,dAMotorEuler);
max_iterations = 200;
return 1;
// ********** universal joint
case 700: // 2 body
case 701:
case 702:
constructWorldForTest (0,2,
0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1,
1,1,0, 1,1,0, 0.25*M_PI,0.25*M_PI);
joint = dJointCreateUniversal (world,0);
dJointAttach (joint,body[0],body[1]);
dJointSetUniversalAnchor (joint,0,0,1);
dJointSetUniversalAxis1 (joint, 1, -1, 1.41421356);
dJointSetUniversalAxis2 (joint, 1, -1, -1.41421356);
return 1;
case 720: // universal transmit torque test
case 721:
case 722:
case 730: // universal torque about axis 1
case 731:
case 732:
case 740: // universal torque about axis 2
case 741:
case 742:
constructWorldForTest (0,2,
0.5*SIDE,0.5*SIDE,1, -0.5*SIDE,-0.5*SIDE,1,
1,0,0, 1,0,0, 0,0);
joint = dJointCreateUniversal (world,0);
dJointAttach (joint,body[0],body[1]);
dJointSetUniversalAnchor (joint,0,0,1);
dJointSetUniversalAxis1 (joint,0,0,1);
dJointSetUniversalAxis2 (joint, 1, -1,0);
max_iterations = 100;
return 1;
}
return 0;
}
void UniversalJoint::createJointPhysics()
{
physicalJoint = dJointCreateUniversal(*simulation->getPhysicalWorld(), 0);
PhysicalObject* o1 = getPhysicalParentObject();
PhysicalObject* o2 = getPhysicalChildObject(this);
assert (o1 && o2);
dJointAttach(physicalJoint, *(o2->getBody()), *(o1->getBody()));
//set joint parameter
dJointSetUniversalAnchor(physicalJoint, dReal(anchorPoint.v[0]), dReal(anchorPoint.v[1]), dReal(anchorPoint.v[2]));
Vector3d physAxis(axis1->direction);
physAxis.rotate(rotation);
dJointSetUniversalAxis1(physicalJoint, dReal(physAxis.v[0]), dReal(physAxis.v[1]), dReal(physAxis.v[2]));
physAxis = axis2->direction;
physAxis.rotate(rotation);
dJointSetUniversalAxis2(physicalJoint, dReal(physAxis.v[0]), dReal(physAxis.v[1]), dReal(physAxis.v[2]));
if(axis1->cfm != -1.0)
dJointSetUniversalParam(physicalJoint, dParamCFM, dReal(axis1->cfm));
if(axis2->cfm != -1.0)
dJointSetUniversalParam(physicalJoint, dParamCFM2, dReal(axis2->cfm));
if(axis1->limited || (axis1->motor != 0 && axis1->motor->getMotorType() == ANGULAR))
{
double minAxisLimit(Functions::toRad(axis1->minValue));
double maxAxisLimit(Functions::toRad(axis1->maxValue));
//Set physical limits to higher values (+10%) to avoid strange hinge effects.
//Otherwise, sometimes the motor exceeds the limits.
assert(maxAxisLimit >= minAxisLimit);
double totalAxisRange(maxAxisLimit - minAxisLimit);
double internalTolerance(totalAxisRange / 10);
minAxisLimit -= internalTolerance;
maxAxisLimit += internalTolerance;
dJointSetUniversalParam(physicalJoint, dParamLoStop, dReal(minAxisLimit));
dJointSetUniversalParam(physicalJoint, dParamHiStop, dReal(maxAxisLimit));
// this has to be done due to the way ODE sets joint stops
dJointSetUniversalParam(physicalJoint, dParamLoStop, dReal(minAxisLimit));
if(axis1->fullScaleDeflectionCFM != -1.0)
dJointSetUniversalParam(physicalJoint, dParamStopCFM, dReal(axis1->fullScaleDeflectionCFM));
if(axis1->fullScaleDeflectionERP != -1.0)
dJointSetUniversalParam(physicalJoint, dParamStopERP, dReal(axis1->fullScaleDeflectionERP));
}
if(axis2->limited || (axis2->motor != 0 && axis2->motor->getMotorType() == ANGULAR))
{
double minAxisLimit(Functions::toRad(axis2->minValue));
double maxAxisLimit(Functions::toRad(axis2->maxValue));
//Set physical limits to higher values (+10%) to avoid strange hinge effects.
//Otherwise, sometimes the motor exceeds the limits.
assert(maxAxisLimit >= minAxisLimit);
double totalAxisRange(maxAxisLimit - minAxisLimit);
double internalTolerance(totalAxisRange / 10);
minAxisLimit -= internalTolerance;
maxAxisLimit += internalTolerance;
dJointSetUniversalParam(physicalJoint, dParamLoStop2, dReal(minAxisLimit));
dJointSetUniversalParam(physicalJoint, dParamHiStop2, dReal(maxAxisLimit));
// this has to be done due to the way ODE sets joint stops
dJointSetUniversalParam(physicalJoint, dParamLoStop2, dReal(minAxisLimit));
if(axis2->fullScaleDeflectionCFM != -1.0)
dJointSetUniversalParam(physicalJoint, dParamStopCFM2, dReal(axis2->fullScaleDeflectionCFM));
if(axis2->fullScaleDeflectionERP != -1.0)
dJointSetUniversalParam(physicalJoint, dParamStopERP2, dReal(axis2->fullScaleDeflectionERP));
}
}
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;
}
