static dJointID find_shared_joint(dBodyID body1, dBodyID body2)
{
dJointID joint = 0;
if (body1 == 0)
{
if (body2 == 0)
return 0;
else
return find_shared_joint(body2, body1);
}
else
{
int i, n = dBodyGetNumJoints(body1);
for (i = 0; i < n; ++i)
if ((joint = dBodyGetJoint(body1, i)))
{
if (dJointGetBody(joint, 0) == body2 ||
dJointGetBody(joint, 1) == body2) return joint;
}
return 0;
}
}
dReal ServoMotor::getTorque() {
// code from Jeff Shim
osg::Vec3 torque1(fback_.t1[0], fback_.t1[1], fback_.t1[2] );
osg::Vec3 torque2(fback_.t2[0], fback_.t2[1], fback_.t2[2] );
osg::Vec3 force1(fback_.f1[0], fback_.f1[1], fback_.f1[2] );
osg::Vec3 force2(fback_.f2[0], fback_.f2[1], fback_.f2[2] );
const double* p1 = dBodyGetPosition( dJointGetBody(joint_->getJoint(),0) );
const double* p2 = dBodyGetPosition( dJointGetBody(joint_->getJoint(),1) );
osg::Vec3 pos1(p1[0], p1[1], p1[2]);
osg::Vec3 pos2(p2[0], p2[1], p2[2]);
dVector3 odeAnchor;
dJointGetHingeAnchor ( joint_->getJoint(), odeAnchor );
osg::Vec3 anchor(odeAnchor[0], odeAnchor[1], odeAnchor[2]);
osg::Vec3 ftorque1 = torque1 - (force1^(pos1-anchor));// torq by motor = total torq - constraint torq
osg::Vec3 ftorque2 = torque2 - (force2^(pos2-anchor));// opposite direction - use if this is necessary
dVector3 odeAxis;
dJointGetHingeAxis ( joint_->getJoint(), odeAxis);
osg::Vec3 axis(odeAxis[0], odeAxis[1], odeAxis[2] );
axis.normalize();
double torque = ftorque1 * axis;
//printf ("torque: % 1.10f\n", torque);
return torque;
}
Joint* internal_getCommonJoint(dBodyID body0, dBodyID body1)
{
// First we need to find the ODE joint connecting the bodies
// (it would be ideal if ODE included this functionality...).
// We only need to check one of the bodies' ODE joints
// because it is assumed here that the two bodies are
// connected, thus they should have a common ODE joint.
int numJoints0 = dBodyGetNumJoints(body0);
dJointID theJoint = 0;
// Loop through body0's ODE joints.
int i = 0;
for (i = 0; i < numJoints0; ++i)
{
dJointID currentJoint = dBodyGetJoint(body0, i);
dBodyID jointBody0 = dJointGetBody(currentJoint, 0);
dBodyID jointBody1 = dJointGetBody(currentJoint, 1);
if ((jointBody0 == body0 && jointBody1 == body1) ||
(jointBody0 == body1 && jointBody1 == body0))
{
// This is the ODE joint connecting the two bodies.
// Note that if the two bodies are connected by multiple
// Joints, the behavior is undefined.
theJoint = currentJoint;
}
}
// Make sure the ODE joint was actually found. This should
// be guaranteed.
assert(theJoint);
// Now return the associated OPAL Joint.
return (Joint*) dJointGetData(theJoint);
}
void BallJoint::createMotorPhysics()
{
for(unsigned int i=0; i<this->motors.size(); i++)
{
dJointID physMotor = dJointCreateAMotor(*(this->simulation->getPhysicalWorld()), 0);
this->motors[i]->setPhysicalMotor(physMotor);
dJointAttach(physMotor, dJointGetBody(this->physicalJoint, 0), dJointGetBody(this->physicalJoint, 1));
if(this->motors[i]->getKind() == "eulermotor")
dJointSetAMotorMode(physMotor, dAMotorEuler);
else
dJointSetAMotorMode(physMotor, dAMotorUser);
//user defined motor
if(this->motors[i]->getKind() == "userdefinedmotor")
{
unsigned short numOfAxes = this->motors[i]->getNumberOfParsedAxes();
dJointSetAMotorNumAxes(physMotor, numOfAxes);
dBodyID b1= dJointGetBody(this->physicalJoint, 0);
if(b1 != NULL)
{
for(int j=0; j < numOfAxes; j++)
{
MotorAxisDescription* ax = this->motors[i]->getAxis(j);
dJointSetAMotorAxis(physMotor, j, 1, dReal(ax->direction.v[0]), dReal(ax->direction.v[1]), dReal(ax->direction.v[2]));
}
}
else
{
for(int j=0; j < numOfAxes; j++)
{
MotorAxisDescription* ax = this->motors[i]->getAxis(j);
dJointSetAMotorAxis(physMotor, j, 0, dReal(ax->direction.v[0]), dReal(ax->direction.v[1]), dReal(ax->direction.v[2]));
}
}
//maxForce and maxVelocity are stored in each axis of the motor
dJointSetAMotorParam(physMotor, dParamFMax, dReal(this->motors[i]->getAxis(0)->maxForce));
if(numOfAxes > 1)
{
dJointSetAMotorParam(physMotor, dParamFMax2, dReal(this->motors[i]->getAxis(1)->maxForce));
if(numOfAxes > 2)
{
dJointSetAMotorParam(physMotor, dParamFMax3, dReal(this->motors[i]->getAxis(2)->maxForce));
}
}
//stops are useless for balljoint
}
}
}
bool HasContact(dBodyID a,dBodyID b)
{
if(a == 0 && b == 0) return false; //two environments
if(a == 0) Swap(a,b);
int n = dBodyGetNumJoints (a);
for(int i=0;i<n;i++) {
dJointID j=dBodyGetJoint (a,i);
if(dJointGetType(j)==dJointTypeContact) {
dBodyID j0=dJointGetBody(j,0);
dBodyID j1=dJointGetBody(j,1);
if(j0 == b || j1 == b) return true;
}
}
return false;
}
static void inspectJoints(void)
{
const dReal forcelimit = 2000.0;
int i;
for (i=0; i<SEGMCNT-1; i++)
{
if (dJointGetBody(hinges[i], 0))
{
// This joint has not snapped already... inspect it.
dReal l0 = dCalcVectorLength3(jfeedbacks[i].f1);
dReal l1 = dCalcVectorLength3(jfeedbacks[i].f2);
colours[i+0] = 0.95*colours[i+0] + 0.05 * l0/forcelimit;
colours[i+1] = 0.95*colours[i+1] + 0.05 * l1/forcelimit;
if (l0 > forcelimit || l1 > forcelimit)
stress[i]++;
else
stress[i]=0;
if (stress[i]>4)
{
// Low-pass filter the noisy feedback data.
// Only after 4 consecutive timesteps with excessive load, snap.
fprintf(stderr,"SNAP! (that was the sound of joint %d breaking)\n", i);
dJointAttach (hinges[i], 0, 0);
}
}
}
}
static void simLoop (int pause)
{
int i, j;
dsSetTexture (DS_WOOD);
if (!pause) {
#ifdef BOX
dBodyAddForce(body[bodies-1],lspeed,0,0);
#endif
for (j = 0; j < joints; j++)
{
dReal curturn = dJointGetHinge2Angle1 (joint[j]);
//dMessage (0,"curturn %e, turn %e, vel %e", curturn, turn, (turn-curturn)*1.0);
dJointSetHinge2Param(joint[j],dParamVel,(turn-curturn)*1.0);
dJointSetHinge2Param(joint[j],dParamFMax,dInfinity);
dJointSetHinge2Param(joint[j],dParamVel2,speed);
dJointSetHinge2Param(joint[j],dParamFMax2,FMAX);
dBodyEnable(dJointGetBody(joint[j],0));
dBodyEnable(dJointGetBody(joint[j],1));
}
if (doFast)
{
dSpaceCollide (space,0,&nearCallback);
#if defined(QUICKSTEP)
dWorldQuickStep (world,0.05);
#elif defined(STEPFAST)
dWorldStepFast1 (world,0.05,ITERS);
#endif
dJointGroupEmpty (contactgroup);
}
else
{
dSpaceCollide (space,0,&nearCallback);
dWorldStep (world,0.05);
dJointGroupEmpty (contactgroup);
}
for (i = 0; i < wb; i++)
{
b = dGeomGetBody(wall_boxes[i]);
if (dBodyIsEnabled(b))
{
bool disable = true;
const dReal *lvel = dBodyGetLinearVel(b);
dReal lspeed = lvel[0]*lvel[0]+lvel[1]*lvel[1]+lvel[2]*lvel[2];
if (lspeed > DISABLE_THRESHOLD)
disable = false;
const dReal *avel = dBodyGetAngularVel(b);
dReal aspeed = avel[0]*avel[0]+avel[1]*avel[1]+avel[2]*avel[2];
if (aspeed > DISABLE_THRESHOLD)
disable = false;
if (disable)
wb_stepsdis[i]++;
else
wb_stepsdis[i] = 0;
if (wb_stepsdis[i] > DISABLE_STEPS)
{
dBodyDisable(b);
dsSetColor(0.5,0.5,1);
}
else
dsSetColor(1,1,1);
}
else
dsSetColor(0.4,0.4,0.4);
dVector3 ss;
dGeomBoxGetLengths (wall_boxes[i], ss);
dsDrawBox(dGeomGetPosition(wall_boxes[i]), dGeomGetRotation(wall_boxes[i]), ss);
}
}
else
{
for (i = 0; i < wb; i++)
{
b = dGeomGetBody(wall_boxes[i]);
if (dBodyIsEnabled(b))
dsSetColor(1,1,1);
else
dsSetColor(0.4,0.4,0.4);
dVector3 ss;
dGeomBoxGetLengths (wall_boxes[i], ss);
dsDrawBox(dGeomGetPosition(wall_boxes[i]), dGeomGetRotation(wall_boxes[i]), ss);
}
}
dsSetColor (0,1,1);
dReal sides[3] = {LENGTH,WIDTH,HEIGHT};
for (i = 0; i < boxes; i++)
dsDrawBox (dGeomGetPosition(box[i]),dGeomGetRotation(box[i]),sides);
dsSetColor (1,1,1);
for (i=0; i< spheres; i++) dsDrawSphere (dGeomGetPosition(sphere[i]),
dGeomGetRotation(sphere[i]),RADIUS);
// draw the cannon
dsSetColor (1,1,0);
dMatrix3 R2,R3,R4;
dRFromAxisAndAngle (R2,0,0,1,cannon_angle);
dRFromAxisAndAngle (R3,0,1,0,cannon_elevation);
dMultiply0 (R4,R2,R3,3,3,3);
dReal cpos[3] = {CANNON_X,CANNON_Y,1};
dReal csides[3] = {2,2,2};
dsDrawBox (cpos,R2,csides);
for (i=0; i<3; i++) cpos[i] += 1.5*R4[i*4+2];
dsDrawCylinder (cpos,R4,3,0.5);
// draw the cannon ball
dsDrawSphere (dBodyGetPosition(cannon_ball_body),dBodyGetRotation(cannon_ball_body),
CANNON_BALL_RADIUS);
}
static void drawGeom(dGeomID geomID)
{
// printf("1%lu", (uint64_t)geomID);
int gclass = dGeomGetClass(geomID);
// printf("2\n");
const dReal *pos = NULL;
const dReal *rot = NULL;
bool canDrawJoints = false;
ODEUserObject* userObj = (ODEUserObject*)dGeomGetData(geomID);
if (nullptr != userObj) {
dsSetColorAlpha(1, 1, 1, 1);
// printf("%f %f %f %f\n", userObj->colorVec[0], userObj->colorVec[1], userObj->colorVec[2], userObj->colorVec[3]);
dsSetTexture (userObj->textureNum);
dsSetColorAlpha(userObj->m_colorVec[0], userObj->m_colorVec[1], userObj->m_colorVec[2], userObj->m_colorVec[3]);
} else {
dsSetColorAlpha(1, 1, 0, 1);
dsSetTexture (DS_WOOD);
}
switch (gclass) {
case dSphereClass:
if (nullptr != userObj && userObj->visible) {
pos = dGeomGetPosition(geomID);
rot = dGeomGetRotation(geomID);
dsDrawSphere(pos, rot, dGeomSphereGetRadius(geomID));
}
canDrawJoints = true;
break;
case dBoxClass:
{
if (nullptr != userObj && userObj->visible) {
pos = dGeomGetPosition(geomID);
rot = dGeomGetRotation(geomID);
dVector3 lengths;
dGeomBoxGetLengths(geomID, lengths);
dsDrawBox(pos, rot, lengths);
}
canDrawJoints = true;
break;
}
case dCylinderClass:
{
if (nullptr != userObj && userObj->visible) {
pos = dGeomGetPosition(geomID);
rot = dGeomGetRotation(geomID);
dReal length;
dReal radius;
dGeomCylinderGetParams(geomID, &radius, &length);
dsDrawCylinder(pos, rot, length, radius);
}
canDrawJoints = true;
break;
}
default:
break;
}
// printf("class: %d\n", gclass);
if (canDrawJoints) {
#ifdef DRAW_JOINTS_TOO
dBodyID body = dGeomGetBody(geomID);
int numJoints = dBodyGetNumJoints(body);
for (int i = 0; i < numJoints; ++i)
{
dJointID joint = dBodyGetJoint(body, i);
int jointClass = dJointGetType(joint);
switch (jointClass)
{
case dJointTypeHinge:
{
dVector3 a11;
dBodyID body1 = dJointGetBody(joint, 0);
dBodyID body2 = dJointGetBody(joint, 1);
if (body1 && body2) {
const dReal* bodyPos1 = dBodyGetPosition(body1);
const dReal* bodyPos2 = dBodyGetPosition(body2);
dJointGetHingeAnchor(joint, a11);
dsSetColor(1, 0, 0);
dsDrawLine(a11, bodyPos1);
dsDrawLine(a11, bodyPos2);
dsSetColor(0, 1, 0);
dsDrawLine(bodyPos1, bodyPos2);
}
}
}
}
#endif
}
}
dBodyID PhysicsJoint::getBody( Int32 i)
{
return dJointGetBody(_JointID, i);
}
IoObject *IoODEJoint_attachedBody2(IoODEJoint *self, IoObject *locals, IoMessage *m)
{
IoODEJoint_assertValidJoint(self, locals, m);
return IoODEBody_bodyFromId(IOSTATE, dJointGetBody(JOINTID, 1));
}
void SParts::calcPosition(Joint *currj, Joint *nextj, const Vector3d &anchorv, const Rotation &R)
{
if (currj) {
DUMP(("currj = %s\n", currj->name()));
}
DUMP(("name = %s\n", name()));
if (nextj) {
DUMP(("nextj = %s\n", nextj->name()));
}
DUMP(("anchorv = (%f, %f, %f)\n", anchorv.x(), anchorv.y(), anchorv.z()));
{
Vector3d v(m_pos.x(), m_pos.y(), m_pos.z());
// Calculate shift vector from positoin/anchor/orientation
if (currj) {
v -= currj->getAnchor();
DUMP(("parent anchor = %s (%f, %f, %f)\n",
currj->name(),
currj->getAnchor().x(),
currj->getAnchor().y(), currj->getAnchor().z()));
}
DUMP(("v1 = (%f, %f, %f)\n", v.x(), v.y(), v.z()));
Rotation rr(R);
if (currj) {
rr *= currj->getRotation();
}
v.rotate(rr);
DUMP(("v2 = (%f, %f, %f)\n", v.x(), v.y(), v.z()));
v += anchorv;
DUMP(("v3 = (%f, %f, %f)\n", v.x(), v.y(), v.z()));
//DUMP(("v4 = (%f, %f, %f)\n", v.x(), v.y(), v.z()));
setPosition(v);
if(m_onGrasp) {
dBodyID body = odeobj().body();
//int num = dBodyGetNumJoints(body);
// get grasp joint
dJointID joint = dBodyGetJoint(body, 0);
// get target body from joint
dBodyID targetBody = dJointGetBody(joint, 1);
// get position of grasp target and parts
const dReal *pos = dBodyGetPosition(body);
const dReal *tpos = dBodyGetPosition(targetBody);
// Set if it is moved
if(pos[0] != tpos[0] || pos[1] != tpos[1] || pos[2] != tpos[2]) {
dBodySetPosition(targetBody, pos[0], pos[1], pos[2]);
}
// get position of grasp target and parts
const dReal *qua = dBodyGetQuaternion(body);
const dReal *tqua = dBodyGetQuaternion(targetBody);
// rotation from initial orientation
dQuaternion rot;
dQMultiply2(rot, qua, m_gini);
// Set if it is rotated
if(rot[0] != tqua[0] || rot[1] != tqua[1] || rot[2] != tqua[2] || rot[3] != tqua[3]) {
dBodySetQuaternion(targetBody, rot);
}
//LOG_MSG(("target2 %d", targetBody));
//LOG_MSG(("(%f, %f, %f)", tpos[0], tpos[1], tpos[2]));
//LOG_MSG(("onGrasp!! num = %d, joint = %d", num));
}
}
Rotation rr(R);
rr *= m_rot;
if (currj) {
rr *= currj->getRotation();
}
setRotation(rr);
if (!nextj) { return; }
for (ChildC::iterator i=m_children.begin(); i!=m_children.end(); i++) {
Child *child = *i;
Rotation R_(R);
if (currj) {
R_ *= currj->getRotation();
}
Vector3d nextv;
nextv = nextj->getAnchor();
if (currj) {
nextv -= currj->getAnchor();
}
nextv.rotate(R_);
nextv += anchorv;
DUMP(("nextv1 : (%f, %f, %f)\n", nextv.x(), nextv.y(), nextv.z()));
// act in a case that multiple JOINTs are connected to one link
if(strcmp(nextj->name(),child->currj->name()) == 0) {
child->nextp->calcPosition(child->currj, child->nextj, nextv, R_);
}
}
}
void doStuff(const std::string & prefix,amarsi::Limbs limb){
std::ostringstream name;
name<<prefix<<"_TOE";
dBodyID toeBody=dWebotsGetBodyFromDEF(name.str().c_str());
dGeomID toe=dWebotsGetGeomFromDEF(name.str().c_str());
//std::cerr<<"Get Geom of value "<<toe<<std::endl;
if(!toe || !toeBody){
std::cerr<<"Did not found "<<name<<" "<<toe<<" "<<toeBody<<std::endl;
s_data->disablePhysics();
return;
}
std::ostringstream nameTibia;
nameTibia<<prefix<<"_FRONT_KNEE";
dBodyID tibia = dWebotsGetBodyFromDEF(nameTibia.str().c_str());
if(!tibia){
std::cerr<<"Did not found "<<nameTibia<<std::endl;
s_data->disablePhysics();
return;
}
// std::cerr<<nameTibia<<" "<<tibia<<std::endl;
int numJoint=dBodyGetNumJoints(tibia);
std::ostringstream nameFemur;
nameFemur<<prefix<<"_HIP_SERVO";
dBodyID femur = dWebotsGetBodyFromDEF(nameFemur.str().c_str());
if(!femur){
std::cerr<<"Did not found "<<nameFemur<<std::endl;
s_data->disablePhysics();
return;
}
for(int i=0;i<numJoint;++i){
dJointID j=dBodyGetJoint(tibia,i);
dBodyID b1= dJointGetBody(j,0);
dBodyID b2= dJointGetBody(j,1);
dReal pos[4];
if(b1==femur || b2==femur){
if(b1==femur)
dJointGetHingeAnchor2(j,pos);
else
dJointGetHingeAnchor(j,pos);
dVector3 posRel;
dBodyGetPosRelPoint(tibia,pos[0],pos[1],pos[3],posRel);
// std::cerr<<"pos : "<<posRel[0]<<" "<<posRel[1]<<" "<<posRel[2]<<std::endl;
// s_data->addKneeJoint(j,limb);
}
}
std::ostringstream nameAnkle;
nameAnkle<<prefix<<"_ANKLE"<<std::flush;
dBodyID ankleBody = dWebotsGetBodyFromDEF(nameAnkle.str().c_str());
dGeomID ankle=dWebotsGetGeomFromDEF(nameAnkle.str().c_str());
if(!ankle || !ankleBody){
std::cerr<<"Did not found "<<nameAnkle<<" "<<ankle<<" "
<<ankleBody<<std::endl;
s_data->disablePhysics();
return;
}
insertGeomInMonitored(ankle,limb);
insertGeomInMonitored(toe,limb);
s_data->addResettableBody(femur);
s_data->addResettableBody(tibia);
s_data->addResettableBody(ankleBody);
s_data->addResettableBody(toeBody);
}
