void dJointSetDBallAnchor2( dJointID j, dReal x, dReal y, dReal z )
{
dxJointDBall* joint = dynamic_cast<dxJointDBall*>(j);
dUASSERT( joint, "bad joint argument" );
if ( joint->flags & dJOINT_REVERSE ) {
if (joint->node[0].body)
dBodyGetPosRelPoint(joint->node[0].body, x, y, z, joint->anchor1);
else {
joint->anchor1[0] = x;
joint->anchor1[1] = y;
joint->anchor1[2] = z;
}
} else {
if (joint->node[1].body)
dBodyGetPosRelPoint(joint->node[1].body, x, y, z, joint->anchor2);
else {
joint->anchor2[0] = x;
joint->anchor2[1] = y;
joint->anchor2[2] = z;
}
}
joint->updateTargetDistance();
}
void dJointSetTransmissionAnchor2( dJointID j, dReal x, dReal y, dReal z )
{
dxJointTransmission* joint = static_cast<dxJointTransmission*>(j);
dUASSERT( joint, "bad joint argument" );
if (joint->node[1].body) {
dBodyGetPosRelPoint(joint->node[1].body, x, y, z, joint->anchors[1]);
}
joint->update = 1;
}
void dGeomSetOffsetWorldPosition (dxGeom *g, dReal x, dReal y, dReal z)
{
dAASSERT (g);
dUASSERT (g->gflags & GEOM_PLACEABLE,"geom must be placeable");
dUASSERT (g->body, "geom must be on a body");
CHECK_NOT_LOCKED (g->parent_space);
if (!g->offset_posr)
{
dGeomCreateOffset(g);
}
dBodyGetPosRelPoint(g->body, x, y, z, g->offset_posr->pos);
dGeomMoved (g);
}
void dxPlanarJoint::updatePlane() {
dBodyID body1 = this->node[0].body;
if (body1)
{
// compute plane normal and anchor coordinates in the local coordinates of the first body
dBodyVectorFromWorld(body1, planeNormal[0], planeNormal[1], planeNormal[2], axis1);
dBodyGetPosRelPoint(body1, anchor[0], anchor[1], anchor[2], anchor1);
dBodyID body2 = this->node[1].body;
if (body2) // second body given, attach the plane to it
{
// compute plane normal and anchor coordinates in the local coordinates of the second body
dBodyVectorFromWorld(body2, planeNormal[0], planeNormal[1], planeNormal[2], axis2);
dBodyGetPosRelPoint(body2, anchor[0], anchor[1], anchor[2], anchor2);
}
else // second body not given, attach the plane to the world
{
// plane normal and anchor coordinates in the world world frame are the same as global coordinates
dCopyVector3(axis2, planeNormal);
dCopyVector3(anchor2, anchor);
}
}
}
/**
* @brief MainWindow::grabMarkPos Anchor a marker at its current position
* @param mark The marker to anchor
*
* Compute the location of the marker in body-relative
* coordinates. Set the joint anchor at that point.
*/
void MainWindow::grabMarkPos(int mark)
{
CapBody* body = ui->glWidget->getWorld()->getBody();
//LiveMarkerData* md = ui->glWidget->getWorld()->getMarkerData();
#if defined( BOARD_DATA )
BoardMarkerData* md = ui->glWidget->getWorld()->getMarkerData();
#elif defined( POKE_DATA )
PokeMarkerData* md = ui->glWidget->getWorld()->getMarkerData();
#else
MarkerData* md = ui->glWidget->getWorld()->getMarkerData();
#endif
const dReal* markPos = dBodyGetPosition(md->body[mark]);
dVector3 pos;
dBodyGetPosRelPoint(body->body_segments[body->marker_to_body[mark].id],
markPos[0],markPos[1],markPos[2],pos);
markerWidgetArray[mark]->setMarkPos(pos[0],pos[1],pos[2]);
}
void SimpleTrackedVehicleEnvironment::nearCallbackGrouserTerrain(dGeomID o1, dGeomID o2) {
dBodyID b1 = dGeomGetBody(o1);
dBodyID b2 = dGeomGetBody(o2);
if(b1 && b2 && dAreConnectedExcluding(b1, b2, dJointTypeContact)) return;
// body of the whole vehicle
dBodyID vehicleBody = ((SimpleTrackedVehicle*)this->v)->vehicleBody;
unsigned long geom1Categories = dGeomGetCategoryBits(o1);
// speeds of the belts
const dReal leftBeltSpeed = ((SimpleTrackedVehicle*)this->v)->leftTrack->getVelocity();
const dReal rightBeltSpeed = ((SimpleTrackedVehicle*)this->v)->rightTrack->getVelocity();
dReal beltSpeed = 0; // speed of the belt which is in collision and examined right now
if (geom1Categories & Category::LEFT) {
beltSpeed = leftBeltSpeed;
} else {
beltSpeed = rightBeltSpeed;
}
// the desired linear and angular speeds (set by desired track velocities)
const dReal linearSpeed = (leftBeltSpeed + rightBeltSpeed) / 2;
const dReal angularSpeed = (leftBeltSpeed - rightBeltSpeed) * steeringEfficiency / tracksDistance;
// radius of the turn the robot is doing
const dReal desiredRotationRadiusSigned = (fabs(angularSpeed) < 0.1) ?
dInfinity : // is driving straight
((fabs(linearSpeed) < 0.1) ?
0 : // is rotating about a single point
linearSpeed / angularSpeed // general movement
);
dVector3 yAxisGlobal; // vector pointing from vehicle body center in the direction of +y axis
dVector3 centerOfRotation; // at infinity if driving straight, so we need to distinguish the case
{ // compute the center of rotation
dBodyVectorToWorld(vehicleBody, 0, 1, 0, yAxisGlobal);
dCopyVector3(centerOfRotation, yAxisGlobal);
// make the unit vector as long as we need (and change orientation if needed; the radius is a signed number)
dScaleVector3(centerOfRotation, desiredRotationRadiusSigned);
const dReal *vehicleBodyPos = dBodyGetPosition(vehicleBody);
dAddVectors3(centerOfRotation, centerOfRotation, vehicleBodyPos);
}
int maxContacts = 20;
dContact contact[maxContacts];
int numContacts = dCollide(o1, o2, maxContacts, &contact[0].geom, sizeof(dContact));
for(size_t i = 0; i < numContacts; i++) {
dVector3 contactInVehiclePos; // position of the contact point relative to vehicle body
dBodyGetPosRelPoint(vehicleBody, contact[i].geom.pos[0], contact[i].geom.pos[1], contact[i].geom.pos[2], contactInVehiclePos);
dVector3 beltDirection; // vector tangent to the belt pointing in the belt's movement direction
dCalcVectorCross3(beltDirection, contact[i].geom.normal, yAxisGlobal);
if (beltSpeed > 0) {
dNegateVector3(beltDirection);
}
if (desiredRotationRadiusSigned != dInfinity) { // non-straight drive
dVector3 COR2Contact; // vector pointing from the center of rotation to the contact point
dSubtractVectors3(COR2Contact, contact[i].geom.pos, centerOfRotation);
// the friction force should be perpendicular to COR2Contact
dCalcVectorCross3(contact[i].fdir1, contact[i].geom.normal, COR2Contact);
const dReal linearSpeedSignum = (fabs(linearSpeed) > 0.1) ? sgn(linearSpeed) : 1;
// contactInVehiclePos[0] > 0 means the contact is in the front part of the track
if (sgn(angularSpeed) * sgn(dCalcVectorDot3(yAxisGlobal, contact[i].fdir1)) !=
sgn(contactInVehiclePos[0]) * linearSpeedSignum) {
dNegateVector3(contact[i].fdir1);
}
} else { // straight drive
dCalcVectorCross3(contact[i].fdir1, contact[i].geom.normal, yAxisGlobal);
if (dCalcVectorDot3(contact[i].fdir1, beltDirection) < 0) {
dNegateVector3(contact[i].fdir1);
}
}
// use friction direction and motion1 to simulate the track movement
contact[i].surface.mode = dContactFDir1 | dContactMotion1 | dContactMu2;
contact[i].surface.mu = 0.5;
contact[i].surface.mu2 = 10;
// the dot product <beltDirection,fdir1> is the cosine of the angle they form (because both are unit vectors)
contact[i].surface.motion1 = -dCalcVectorDot3(beltDirection, contact[i].fdir1) * fabs(beltSpeed) * 0.07;
// friction force visualization
dMatrix3 forceRotation;
dVector3 vec;
dBodyVectorToWorld(vehicleBody, 1, 0, 0, vec);
dRFrom2Axes(forceRotation, contact[i].fdir1[0], contact[i].fdir1[1], contact[i].fdir1[2], vec[0], vec[1], vec[2]);
posr data;
dCopyVector3(data.pos, contact[i].geom.pos);
dCopyMatrix4x3(data.R, forceRotation);
forces.push_back(data);
dJointID c = dJointCreateContact(this->world, this->contactGroup, &contact[i]);
dJointAttach(c, b1, b2);
if(!isValidCollision(o1, o2, contact[i]))
this->badCollision = true;
if(config.contact_grouser_terrain.debug)
this->contacts.push_back(contact[i].geom);
}
}
void Simulation::command(int cmd)
{
dMass mass;
static int view; // which view was set last
static std::string input = ""; // input string ("" initially)
//static dJointID tagging_joint = 0; // joint that keeps one link static
if (cmd >= 'A' && cmd <= 'B')
cmd = cmd + 'a' - 'A'; // lower case
if (mode == KeyMode) {
switch (cmd) {
case keyWriteFrame:
mywriteframes ^= 1;
break;
case keyCreateMainBody:
if (car->body_obj == 0) {
car->createBody(center);
objbin.Add(car->body_obj);
};
break;
// switch mode to input
case keyInputMode:
mode = InputMode;
std::cout << "input: ";
std::cout.flush();
break;
// abandon ship
case keyBreakWheel:
car->breakWheel();
break;
// change speed
case keyZeroSpeed:
s_speed[0] = 0;
s_speed[1] = 0;
break;
case keyRightSpeedIncrease:
s_speed[1] += 0.1;
break;
case keyRightSpeedDecrease:
s_speed[1] -= 0.1;
break;
case keyRightSpeedMax:
s_speed[1] = max_speed;
break;
case keyRightSpeedMin:
s_speed[1] = min_speed;
break;
case keyLeftSpeedIncrease:
s_speed[0] += 0.1;
break;
case keyLeftSpeedDecrease:
s_speed[0] -= 0.1;
break;
case keyLeftSpeedMax:
s_speed[0] = max_speed;
break;
case keyLeftSpeedMin:
s_speed[0] = min_speed;
break;
case keyBothSpeedIncrease:
s_speed[0] += 0.1;
s_speed[1] += 0.1;
break;
case keyBothSpeedDecrease:
s_speed[0] -= 0.1;
s_speed[1] -= 0.1;
break;
case keyBothSpeedMax:
s_speed[0] = max_speed;
s_speed[1] = max_speed;
break;
case keyBothSpeedMin:
s_speed[0] = min_speed;
s_speed[1] = min_speed;
break;
// do the twitch
case keyTwitchToggle:
twitch_flag = 1 - twitch_flag;
break;
case keyTwitchHertzHalf:
twitch_hertz /= 2.0;
break;
case keyTwitchHertzDouble:
twitch_hertz *= 2.0;
break;
// camera views change
case keyAimCamera:
aimCamera(camera_object);
break;
case keyLockCameraToVehicle: // only pos, not orientation so far
lockCamera = 1 - lockCamera;
float xyz[3], hpr[3];
dsGetViewpoint(xyz, hpr);
dBodyGetPosRelPoint(camera_object, xyz[0], xyz[1], xyz[2], cameraRelVec); // returns point in body relative coordinates
break;
case keyIncreaseViewByTwoMod:
view = (view + 2) % set_view(-1);
set_view(view);
break;
case keyIncreaseViewByOneMod:
view = (view + 1) % set_view(-1);
set_view(view);
break;
// show speed and wanted speed (uncorrect when in twitch mode)
case keyDisplayWheels:
for (int i = 0 ; i < 4 ; ++i)
car->wheel_obj[i]->toggleDrawFlag();
case keyPrintSprocketMass:
car->getSprocketMass(&mass);
printf("mass %3.3f %3.3f %3.3f %3.3f\n",
mass.mass, mass.I[0], mass.I[5],
mass.I[10]);
break;
case keyPrintManyVariables:
printf
("s_speed %3.3f | actual %3.3f | hertz %3.3f | time %3.3f | lv %d\n",
s_speed[0], car->getSprocketSpeed(),
twitch_hertz, dStopwatchTime(&timer), view);
break;
case keyEnterMovieMode:
enterMovieMode();
automat = new AutoEventFinish(simTime, event_time, m_myMachine, finish_time, m_exitMachine, camera_object); // create automat
break;
};
if (cmd >= '0' && cmd < ('0' + LINK_PARTS))
Chain::showbit[cmd - '0'] =
1 - Chain::showbit[cmd - '0'];
for (int i = 0; i < 2; ++i)
s_speed[i] =
BRACKET(s_speed[i], min_speed, max_speed);
} else if (mode == InputMode) {
// switch mode to single key
if (cmd == ' ') {
mode = KeyMode;
//process (input);
std::istringstream s(input);
int tagged_link;
s >> tagged_link;
/*
tagged_link = BRACKET(tagged_link, 0, chain_obj[0]->b_num - 1);
if (tagging_joint != 0) // delete old
dJointDestroy(tagging_joint);
tagging_joint = dJointCreateFixed(world, 0);
dJointAttach(tagging_joint, chain_obj[0]->body[tagged_link], 0);
dJointSetFixed(tagging_joint);
*/
std::cout << "\n";
// reset input
input = "";
} else {
void PhysicsBody::getPosRelPoint(const Vec3f &p, Vec3f &result)
{
dVector3 t;
dBodyGetPosRelPoint(_BodyID, p.x(), p.y(), p.z(), t);
result.setValue(Vec3f(t[0], t[1], t[2]));
}
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);
}