void controller::stanceLegTreatment(int leg_id) {
int other_leg_id;
if(leg_id == 0) {
other_leg_id = 1;
}
else if(leg_id == 1) {
other_leg_id = 0;
}
else if(leg_id == 2) {
other_leg_id = 3;
}
else if(leg_id == 3) {
other_leg_id = 2;
}
if(swingFlag[other_leg_id]) {
if(leg_id < 2) {
const dReal * torque = dBodyGetTorque(body_bag->getFootLinkBody(other_leg_id,0));
dBodyAddTorque(body_bag->getBackLink1Body(), -torque[0], -torque[1], -torque[2]);
}
else {
const dReal * torque = dBodyGetTorque(body_bag->getFootLinkBody(other_leg_id,0));
dBodyAddTorque(body_bag->getBackLink6Body(), -torque[0], -torque[1], -torque[2]);
}
}
}
void dJointAddUniversalTorques( dJointID j, dReal torque1, dReal torque2 )
{
dxJointUniversal* joint = ( dxJointUniversal* )j;
dVector3 axis1, axis2;
dAASSERT( joint );
checktype( joint, Universal );
if ( joint->flags & dJOINT_REVERSE )
{
dReal temp = torque1;
torque1 = - torque2;
torque2 = - temp;
}
getAxis( joint, axis1, joint->axis1 );
getAxis2( joint, axis2, joint->axis2 );
axis1[0] = axis1[0] * torque1 + axis2[0] * torque2;
axis1[1] = axis1[1] * torque1 + axis2[1] * torque2;
axis1[2] = axis1[2] * torque1 + axis2[2] * torque2;
if ( joint->node[0].body != 0 )
dBodyAddTorque( joint->node[0].body, axis1[0], axis1[1], axis1[2] );
if ( joint->node[1].body != 0 )
dBodyAddTorque( joint->node[1].body, -axis1[0], -axis1[1], -axis1[2] );
}
void dJointAddAMotorTorques( dJointID j, dReal torque1, dReal torque2, dReal torque3 )
{
dxJointAMotor* joint = ( dxJointAMotor* )j;
dVector3 axes[3];
dAASSERT( joint );
checktype( joint, AMotor );
if ( joint->num == 0 )
return;
dUASSERT(( joint->flags & dJOINT_REVERSE ) == 0, "dJointAddAMotorTorques not yet implemented for reverse AMotor joints" );
joint->computeGlobalAxes( axes );
axes[0][0] *= torque1;
axes[0][1] *= torque1;
axes[0][2] *= torque1;
if ( joint->num >= 2 )
{
axes[0][0] += axes[1][0] * torque2;
axes[0][1] += axes[1][1] * torque2;
axes[0][2] += axes[1][2] * torque2;
if ( joint->num >= 3 )
{
axes[0][0] += axes[2][0] * torque3;
axes[0][1] += axes[2][1] * torque3;
axes[0][2] += axes[2][2] * torque3;
}
}
if ( joint->node[0].body != 0 )
dBodyAddTorque( joint->node[0].body, axes[0][0], axes[0][1], axes[0][2] );
if ( joint->node[1].body != 0 )
dBodyAddTorque( joint->node[1].body, -axes[0][0], -axes[0][1], -axes[0][2] );
}
void addOscillatingTorqueAbout(dReal tscale, dReal x, dReal y, dReal z)
{
static dReal a=0;
dBodyAddTorque (body[0], tscale*cos(a) * x, tscale*cos(a) * y,
tscale * cos(a) * z);
a += 0.02;
}
void addOscillatingTorque (dReal tscale)
{
static dReal a=0;
dBodyAddTorque (body[0],tscale*cos(2*a),tscale*cos(2.7183*a),
tscale*cos(1.5708*a));
a += 0.01;
}
/* adds a torque given by the vector vec to the body b */
void body_add_torque (dBodyID b, const t_real *vec) {
/* let's check if we have the surface, in which case we exit doing nothing */
if (b == SURFACE_BODY_ID) return;
/* in all other cases, we add the torque to the body */
dBodyAddTorque (b, vec[0], vec[1], vec[2]);
}
static void simLoop (int pause)
{
if (!pause) {
// add random forces and torques to all bodies
int i;
const dReal scale1 = 5;
const dReal scale2 = 5;
for (i=0; i<NUM; i++) {
dBodyAddForce (body[i],
scale1*(dRandReal()*2-1),
scale1*(dRandReal()*2-1),
scale1*(dRandReal()*2-1));
dBodyAddTorque (body[i],
scale2*(dRandReal()*2-1),
scale2*(dRandReal()*2-1),
scale2*(dRandReal()*2-1));
}
dWorldStep (world,0.05);
createTest();
}
// float sides[3] = {SIDE,SIDE,SIDE};
dsSetColor (1,1,0);
for (int i=0; i<NUM; i++)
dsDrawSphere (dBodyGetPosition(body[i]), dBodyGetRotation(body[i]),RADIUS);
}
bool SimObjectRenderer::releaseDrag(int x, int y)
{
if(!dragging)
return false;
if(!dragSelection) // camera control
{
dragging = false;
return true;
}
else // object control
{
if(dragMode == adoptDynamics)
moveDrag(x, y);
else if(dragMode == applyDynamics)
{
Vector3<> projectedClick = projectClick(x, y);
Vector3<> currentPos;
if(intersectRayAndPlane(cameraPos, projectedClick - cameraPos, dragSelection->pose.translation, dragPlaneVector, currentPos))
{
if(dragType == dragRotate || dragType == dragRotateWorld)
{
Vector3<> oldv = dragStartPos - dragSelection->pose.translation;
Vector3<> newv = currentPos - dragSelection->pose.translation;
if(dragType != dragRotateWorld)
{
Matrix3x3<> invRotation = dragSelection->pose.rotation.transpose();
oldv = invRotation * oldv;
newv = invRotation * newv;
}
float angle = 0.f;
if(dragPlane == yzPlane)
angle = normalize(atan2f(newv.z, newv.y) - atan2f(oldv.z, oldv.y));
else if(dragPlane == xzPlane)
angle = normalize(atan2f(newv.x, newv.z) - atan2f(oldv.x, oldv.z));
else
angle = normalize(atan2f(newv.y, newv.x) - atan2f(oldv.y, oldv.x));
Vector3<> offset = dragPlaneVector * angle;
Vector3<> torque = offset * (float) dragSelection->mass.mass * 50.f;
dBodyAddTorque(dragSelection->body, torque.x, torque.y, torque.z);
}
else
{
const Vector3<> offset = currentPos - dragStartPos;
Vector3<> force = offset * (float) dragSelection->mass.mass * 500.f;
dBodyAddForce(dragSelection->body, force.x, force.y, force.z);
}
}
}
dragSelection->enablePhysics(true);
dragging = false;
return true;
}
}
void GameObject::Update()
{
double reward = 0;
if (!m_hasAgent) {
return;
}
// reorder the game states
if (m_oldGS != nullptr) {
delete m_oldGS;
m_oldGS = nullptr;
}
m_oldGS = m_newGS;
m_newGS = new GameState;
// construct the game state to pass to an agent
GetGameState(*m_newGS);
reward = 1 / std::max(1.d, m_newGS->m_distanceFromDestination);
// Discretize after reward calculation
m_newGS->discretize();
// get the agent for this object
QLearningAgent *agent = m_gw.m_game.GetAgent(m_id);
// Update the behaviors based on the last action
if (m_oldGS != 0) {
agent->update(*m_oldGS, m_lastAction, *m_newGS, reward);
}
if (m_pathToDest.size() == 0) {
Ogre::Vector3 a = GetLocation();
WorldPos curr = {a.x, a.z, 0};
m_gw.MakeMapPath(curr, m_pathToDest);
}
// here is where I'd pass the game state to an agent if we had one
// layout: turn, acceleration
m_lastAction = agent->getAction(*m_newGS);
std::cout << "Action: " << m_lastAction.m_accelerateMagnitude << ", " << m_lastAction.m_turnMagnitude << std::endl;
const dReal *quat = dBodyGetQuaternion(m_body);
// do physics things
if (quat[1] < 0.05f) {
dBodyAddRelForce(m_body, 0, 0, m_lastAction.m_accelerateMagnitude * m_maxForward);
dBodyAddTorque(m_body, 0, m_lastAction.m_turnMagnitude * m_maxTurn, 0);
}
// reset collision accumulator
m_totalDamage += m_collisionAccum;
m_collisionAccum = 0;
}
void add_phys_torque(dBodyID body, float x, float y, float z)
{
if (body)
{
dBodyEnable(body);
dBodyAddTorque(body, x, y, z);
}
}
void dJointAddHinge2Torques( dJointID j, dReal torque1, dReal torque2 )
{
dxJointHinge2* joint = ( dxJointHinge2* )j;
dVector3 axis1, axis2;
dUASSERT( joint, "bad joint argument" );
checktype( joint, Hinge2 );
if ( joint->node[0].body && joint->node[1].body )
{
dMultiply0_331( axis1, joint->node[0].body->posr.R, joint->axis1 );
dMultiply0_331( axis2, joint->node[1].body->posr.R, joint->axis2 );
axis1[0] = axis1[0] * torque1 + axis2[0] * torque2;
axis1[1] = axis1[1] * torque1 + axis2[1] * torque2;
axis1[2] = axis1[2] * torque1 + axis2[2] * torque2;
dBodyAddTorque( joint->node[0].body, axis1[0], axis1[1], axis1[2] );
dBodyAddTorque( joint->node[1].body, -axis1[0], -axis1[1], -axis1[2] );
}
}
IoObject *IoODEBody_addTorque(IoODEBody *self, IoObject *locals, IoMessage *m)
{
const double x = IoMessage_locals_doubleArgAt_(m, locals, 0);
const double y = IoMessage_locals_doubleArgAt_(m, locals, 1);
const double z = IoMessage_locals_doubleArgAt_(m, locals, 2);
IoODEBody_assertValidBody(self, locals, m);
dBodyAddTorque(BODYID, x, y, z);
return self;
}
static void simLoop (int pause)
{
const dReal kd = -0.3; // angular damping constant
const dReal ks = 0.5; // spring constant
if (!pause) {
// add an oscillating torque to body 0, and also damp its rotational motion
static dReal a=0;
const dReal *w = dBodyGetAngularVel (body[0]);
dBodyAddTorque (body[0],kd*w[0],kd*w[1]+0.1*cos(a),kd*w[2]+0.1*sin(a));
a += 0.01;
// add a spring force to keep the bodies together, otherwise they will
// fly apart along the slider axis.
const dReal *p1 = dBodyGetPosition (body[0]);
const dReal *p2 = dBodyGetPosition (body[1]);
dBodyAddForce (body[0],ks*(p2[0]-p1[0]),ks*(p2[1]-p1[1]),
ks*(p2[2]-p1[2]));
dBodyAddForce (body[1],ks*(p1[0]-p2[0]),ks*(p1[1]-p2[1]),
ks*(p1[2]-p2[2]));
// occasionally re-orient one of the bodies to create a deliberate error.
if (occasional_error) {
static int count = 0;
if ((count % 20)==0) {
// randomly adjust orientation of body[0]
const dReal *R1;
dMatrix3 R2,R3;
R1 = dBodyGetRotation (body[0]);
dRFromAxisAndAngle (R2,dRandReal()-0.5,dRandReal()-0.5,
dRandReal()-0.5,dRandReal()-0.5);
dMultiply0 (R3,R1,R2,3,3,3);
dBodySetRotation (body[0],R3);
// randomly adjust position of body[0]
const dReal *pos = dBodyGetPosition (body[0]);
dBodySetPosition (body[0],
pos[0]+0.2*(dRandReal()-0.5),
pos[1]+0.2*(dRandReal()-0.5),
pos[2]+0.2*(dRandReal()-0.5));
}
count++;
}
dWorldStep (world,0.05);
}
dReal sides1[3] = {SIDE,SIDE,SIDE};
dReal sides2[3] = {SIDE*0.8f,SIDE*0.8f,SIDE*2.0f};
dsSetTexture (DS_WOOD);
dsSetColor (1,1,0);
dsDrawBox (dBodyGetPosition(body[0]),dBodyGetRotation(body[0]),sides1);
dsSetColor (0,1,1);
dsDrawBox (dBodyGetPosition(body[1]),dBodyGetRotation(body[1]),sides2);
}
void dJointAddScrewTorque( dJointID j, dReal torque )
{
dxJointScrew* joint = ( dxJointScrew* )j;
dVector3 axis;
dAASSERT( joint );
checktype( joint, Screw );
if ( joint->flags & dJOINT_REVERSE )
torque = -torque;
getAxis( joint, axis, joint->axis1 );
axis[0] *= torque;
axis[1] *= torque;
axis[2] *= torque;
if ( joint->node[0].body != 0 )
dBodyAddTorque( joint->node[0].body, axis[0], axis[1], axis[2] );
if ( joint->node[1].body != 0 )
dBodyAddTorque( joint->node[1].body, -axis[0], -axis[1], -axis[2] );
}
void dJointAddScrewForce ( dJointID j, dReal force )
{
dxJointScrew* joint = ( dxJointScrew* ) j;
dVector3 axis;
dUASSERT ( joint, "bad joint argument" );
checktype ( joint, Screw );
if ( joint->flags & dJOINT_REVERSE )
force -= force;
getAxis ( joint, axis, joint->axis1 );
axis[0] *= force;
axis[1] *= force;
axis[2] *= force;
if ( joint->node[0].body != 0 )
dBodyAddForce ( joint->node[0].body, axis[0], axis[1], axis[2] );
if ( joint->node[1].body != 0 )
dBodyAddForce ( joint->node[1].body, -axis[0], -axis[1], -axis[2] );
if ( joint->node[0].body != 0 && joint->node[1].body != 0 )
{
// linear torque decoupling:
// we have to compensate the torque, that this screw force may generate
// if body centers are not aligned along the screw axis
dVector3 ltd; // Linear Torque Decoupling vector (a torque)
dVector3 cgdiff;
cgdiff[0] = REAL ( 0.5 ) * ( joint->node[1].body->posr.pos[0] -
joint->node[0].body->posr.pos[0] );
cgdiff[1] = REAL ( 0.5 ) * ( joint->node[1].body->posr.pos[1] -
joint->node[0].body->posr.pos[1] );
cgdiff[2] = REAL ( 0.5 ) * ( joint->node[1].body->posr.pos[2] -
joint->node[0].body->posr.pos[2] );
dCalcVectorCross3( ltd, cgdiff, axis );
dBodyAddTorque ( joint->node[0].body, ltd[0], ltd[1], ltd[2] );
dBodyAddTorque ( joint->node[1].body, ltd[0], ltd[1], ltd[2] );
}
}
static void simLoop (int pause)
{
if (!pause) {
dBodyAddTorque (anchor_body,torque[0],torque[1],torque[2]);
dBodyAddTorque (test_body,torque[0],torque[1],torque[2]);
dWorldStep (world,0.03);
iteration++;
if (iteration >= 100) {
// measure the difference between the anchor and test bodies
const dReal *w1 = dBodyGetAngularVel (anchor_body);
const dReal *w2 = dBodyGetAngularVel (test_body);
const dReal *q1 = dBodyGetQuaternion (anchor_body);
const dReal *q2 = dBodyGetQuaternion (test_body);
dReal maxdiff = dMaxDifference (w1,w2,1,3);
printf ("w-error = %.4e (%.2f,%.2f,%.2f) and (%.2f,%.2f,%.2f)\n",
maxdiff,w1[0],w1[1],w1[2],w2[0],w2[1],w2[2]);
maxdiff = dMaxDifference (q1,q2,1,4);
printf ("q-error = %.4e\n",maxdiff);
reset_test();
}
}
dReal sides[3] = {SIDE,SIDE,SIDE};
dReal sides2[3] = {6*SIDE,6*SIDE,6*SIDE};
dReal sides3[3] = {3*SIDE,3*SIDE,3*SIDE};
dsSetColor (1,1,1);
dsDrawBox (dBodyGetPosition(anchor_body), dBodyGetRotation(anchor_body),
sides3);
dsSetColor (1,0,0);
dsDrawBox (dBodyGetPosition(test_body), dBodyGetRotation(test_body), sides2);
dsSetColor (1,1,0);
for (int i=0; i<NUM; i++)
dsDrawBox (dBodyGetPosition (particle[i]),
dBodyGetRotation (particle[i]), sides);
}
void ODE_ForceHandle::updateActuators() {
if(mHostBody != 0 && mActivateForces->get()) {
dBodyID hostBodyId = mHostBody->getRigidBodyID();
if(hostBodyId != 0) {
if(mApplyRelativeForces->get()) {
dBodyAddForce(hostBodyId, mAppliedForce->getX(), mAppliedForce->getY(), mAppliedForce->getZ());
dBodyAddTorque(hostBodyId, mAppliedTorque->getX(), mAppliedTorque->getY(),mAppliedTorque->getZ());
}
else {
dBodyAddRelForce(hostBodyId, mAppliedForce->getX(), mAppliedForce->getY(), mAppliedForce->getZ());
dBodyAddRelTorque(hostBodyId, mAppliedTorque->getX(), mAppliedTorque->getY(),mAppliedTorque->getZ());
}
}
}
}
void odeBodyDamp( dBodyID dBody, float friction ) {
dReal *lv = (dReal *)dBodyGetLinearVel ( dBody );
dReal lv1[3];
lv1[0] = lv[0] * -friction;
lv1[1] = lv[1] * -friction;
lv1[2] = lv[2] * -friction;
dBodyAddForce( dBody, lv1[0], lv1[1], lv1[2] );
dReal *av = (dReal *)dBodyGetAngularVel( dBody );
dReal av1[3];
av1[0] = av[0] * -friction;
av1[1] = av[1] * -friction;
av1[2] = av[2] * -friction;
dBodyAddTorque( dBody, av1[0], av1[1], av1[2] );
}
void YGEBodyAsset::update(long delta){
if(!hasBody){
createBody();
} else {
double second = delta / 1000000.0f;
double m = 1; //second * 1000;
dBodySetAngularDamping(bodyId, angularDamping * second);
dBodySetLinearDamping(bodyId, linearDamping * second);
dBodyAddTorque(bodyId, torqueAccum.x * m, torqueAccum.y * m, torqueAccum.z * m);
dBodyAddForce(bodyId, forceAccum.x * m, forceAccum.y * m, forceAccum.z * m);
const dReal* pos = dBodyGetPosition (bodyId);
const dReal* rot = dBodyGetQuaternion(bodyId);
parent->setPosition(YGEMath::Vector3(pos[0], pos[1], pos[2]));
parent->setOrientation(YGEMath::Quaternion(rot[0], rot[1], rot[2], rot[3]));
}
}
void Robot::Kicker::step()
{
if (kicking)
{
box->setColor(1,0.3,0);
kickstate--;
if (kickstate<=0) kicking = false;
}
else if (rolling!=0)
{
box->setColor(1,0.7,0);
if (isTouchingBall())
{
float fx,fy,fz;
rob->chassis->getBodyDirection(fx,fy,fz);
fz = 0.6*sqrt(fx*fx + fy*fy); // Increasing the power of dribbler.
fx/=fz;fy/=fz;
if (rolling==-1) {fx=-fx;fy=-fy;}
rob->getBall()->tag = rob->getID();
float vx,vy,vz;
float bx,by,bz;
float kx,ky,kz;
rob->chassis->getBodyDirection(vx,vy,vz);
rob->getBall()->getBodyPosition(bx,by,bz);
box->getBodyPosition(kx,ky,kz);
float yy = -10*((-(kx-bx)*vy + (ky-by)*vx)) / rob->cfg->robotSettings.KickerWidth;
float dir = 1;
if (yy>0) dir = -1.0f;
dBodySetAngularVel(rob->getBall()->body,fy*rob->cfg->robotSettings.RollerTorqueFactor*1400,-fx*rob->cfg->robotSettings.RollerTorqueFactor*1400,0);
//dBodyAddTorque(rob->getBall()->body,fy*rob->cfg->ROLLERTORQUEFACTOR(),-fx*rob->cfg->ROLLERTORQUEFACTOR(),0);
dBodyAddTorque(rob->getBall()->body,yy*fx*rob->cfg->robotSettings.RollerPerpendicularTorqueFactor,yy*fy*rob->cfg->robotSettings.RollerPerpendicularTorqueFactor,0);
}
}
else box->setColor(0.9,0.9,0.9);
}
void RigidBody::addTorque(const ngl::Vec3 &_t)
{
dBodyAddTorque(m_id,_t.m_x,_t.m_y,_t.m_z);
}
void SParts::addTorque(dReal fx, dReal fy, dReal fz)
{
dBodyAddTorque(m_odeobj->body(), fx, fy, fz);
}
void controller::legController(int leg_id, int phase) {
cout << endl << "Leg Controller = " << leg_id << endl;
if(swingStart[leg_id] == phase) {
cout << "Starting swing phase"<< endl;
computePlacementLocation(leg_id, lfHeight[0]);
setFootLocation(leg_id, phase);
}
else if(isInSwing(leg_id)) {
cout << "Swing phase"<< endl;
swingFlag[leg_id] = true;
//Get target position
setFootLocation(leg_id, phase);
vector<float> targetPosition = getTargetPosition(leg_id);
//Get link lengths
vector<float> lengths(2);
for(int i = 0; i < 2; i++) {
lengths[i] = body_bag->getFootLinkLength(leg_id, i);
}
//Set axis perpendicular to the kinematic chain plane
Eigen::MatrixXf axis = Eigen::MatrixXf::Random(4, 1);
axis(0, 0) = 0;
axis(1, 0) = 0;
if(leg_id % 2 == 0) {
axis(2, 0) = 1;
}
else {
axis(2, 0) = -1;
}
axis(3, 0) = 1;
//Get transformation matrices
vector<Eigen::MatrixXf> transformationMatrices(2);
Eigen::MatrixXf alignment = Eigen::MatrixXf::Identity(4, 4);
alignment(1, 1) = 0;
alignment(1, 2) = 1;
alignment(2, 1) = -1;
alignment(2, 2) = 0;
Eigen::MatrixXf mr = Eigen::MatrixXf::Identity(4, 4);
const dReal *rotation_matrix_ode = dBodyGetRotation(body_bag->getFootLinkBody(leg_id, 3));
for(int i = 0; i < 3; i++) {
for(int j = 0; j < 4; j++) {
mr(i, j) = rotation_matrix_ode[i*4+j];
}
}
mr(3, 0) = 0;
mr(3, 1) = 0;
mr(3, 2) = 0;
mr(3, 3) = 1;
Eigen::MatrixXf mr2 = Eigen::MatrixXf::Identity(4, 4);
const dReal *rotation_matrix_ode2 = dBodyGetRotation(body_bag->getFootLinkBody(leg_id, 3));
for(int i = 0; i < 3; i++) {
for(int j = 0; j < 4; j++) {
mr2(i, j) = rotation_matrix_ode2[i*4+j];
}
}
mr2(3, 0) = 0;
mr2(3, 1) = 0;
mr2(3, 2) = 0;
mr2(3, 3) = 1;
transformationMatrices[0] = mr*alignment.inverse();
transformationMatrices[1] = mr2*alignment.inverse();
//Get translation matrix
const dReal *center_location = dBodyGetPosition(body_bag->getFootLinkBody(leg_id,0)); //get location of the center of the link
Eigen::MatrixXf mt = Eigen::MatrixXf::Identity(4, 4); //translate to the start of the link
mt(1, 3) = -body_bag->getFootLinkLength(leg_id, 0)/2;
mr = Eigen::MatrixXf::Identity(4, 4); //get orientation
rotation_matrix_ode = dBodyGetRotation(body_bag->getFootLinkBody(leg_id, 0));
for(int k = 0; k < 3; k++) {
for(int j = 0; j < 4; j++) {
mr(k, j) = rotation_matrix_ode[k*4+j];
}
}
mr(3, 0) = 0;
mr(3, 1) = 0;
mr(3, 2) = 0;
mr(3, 3) = 1;
Eigen::MatrixXf origin = Eigen::MatrixXf::Random(4, 1);
origin(0, 0) = 0;
origin(1, 0) = 0;
origin(2, 0) = 0;
origin(3, 0) = 1;
Eigen::MatrixXf addition = alignment.inverse()*mr.inverse()*mt*origin; //part to add to the center location
Eigen::MatrixXf translationMatrix = Eigen::MatrixXf::Identity(4, 4);
translationMatrix(0, 3) = center_location[0] + addition(0, 0);
translationMatrix(1, 3) = center_location[1] + addition(1, 0);
translationMatrix(2, 3) = center_location[2] + addition(2, 0);
vector<float> angles = body_bag->getAngles(0);
Eigen::MatrixXf jointAngleChange = applyIK(lengths, transformationMatrices, angles, targetPosition, translationMatrix, axis);
//Use PD controllers to get torque
//link 1
float torque = foot_link_gain_kp[leg_id][0]*jointAngleChange(0,0) + foot_link_gain_kd[leg_id][0]*(jointAngleChange(0,0) - foot_link_pd_error[leg_id][0]);
dBodyAddTorque(body_bag->getFootLinkBody(leg_id,0), axis(0, 0)*torque, axis(1, 0)*torque, axis(2, 0)*torque);
for(int i = 0; i < 3; i++) {
swing_torque[leg_id][i] = axis(i, 0)*torque;
}
foot_link_pd_error[leg_id][0] = jointAngleChange(0,0);
//link 2
torque = foot_link_gain_kp[leg_id][1]*jointAngleChange(1,0) + foot_link_gain_kd[leg_id][1]*(jointAngleChange(1,0) - foot_link_pd_error[leg_id][1]);
dBodyAddTorque(body_bag->getFootLinkBody(leg_id,1), axis(0, 0)*torque, axis(1, 0)*torque, axis(2, 0)*torque);
foot_link_pd_error[leg_id][1] = jointAngleChange(1,0);
//link 3
rotation_matrix_ode = dBodyGetRotation(body_bag->getFootLinkBody(leg_id, 2));
for(int k = 0; k < 3; k++) {
for(int j = 0; j < 4; j++) {
mr(k, j) = rotation_matrix_ode[k*4+j];
}
}
mr(3, 0) = 0;
mr(3, 1) = 0;
mr(3, 2) = 0;
mr(3, 3) = 1;
float swing_phase = computeSwingPhase(leg_id, phase);
float error = ((30*M_PI)/180)*(1 - swing_phase) - ((60*M_PI)/180)*swing_phase - acos(mr(0, 0));
torque = foot_link_gain_kp[leg_id][2]*error + foot_link_gain_kd[leg_id][2]*(error - foot_link_pd_error[leg_id][2]);
dBodyAddTorque(body_bag->getFootLinkBody(leg_id,2), axis(0, 0)*torque, axis(1, 0)*torque, axis(2, 0)*torque);
foot_link_pd_error[leg_id][2] = error;
//link 4
rotation_matrix_ode = dBodyGetRotation(body_bag->getFootLinkBody(leg_id, 3));
for(int k = 0; k < 3; k++) {
for(int j = 0; j < 4; j++) {
mr(k, j) = rotation_matrix_ode[k*4+j];
}
}
mr(3, 0) = 0;
mr(3, 1) = 0;
mr(3, 2) = 0;
mr(3, 3) = 1;
swing_phase = computeSwingPhase(leg_id, phase);
error = ((90*M_PI)/180)*(1 - swing_phase) - ((30*M_PI)/180)*swing_phase - acos(mr(0, 0));
torque = foot_link_gain_kp[leg_id][3]*error + foot_link_gain_kd[leg_id][3]*(error - foot_link_pd_error[leg_id][3]);
dBodyAddTorque(body_bag->getFootLinkBody(leg_id,3), axis(0, 0)*torque, axis(1, 0)*torque, axis(2, 0)*torque);
foot_link_pd_error[leg_id][3] = error;
//Apply gravity compensation
float force = 9.810*body_bag->getDensity()*(body_bag->getFootLinkLength(leg_id, 0) + body_bag->getFootLinkLength(leg_id, 1) + body_bag->getFootLinkLength(leg_id, 2) + body_bag->getFootLinkLength(leg_id, 3));
dBodyAddForce(body_bag->getFootLinkBody(leg_id, 2), 0.0, 9.810*body_bag->getDensity()*body_bag->getFootLinkLength(leg_id, 2), 0.0);
if(leg_id < 2) {
dBodyAddForce(body_bag->getBackLink1Body(), 0.0, force, 0.0);
}
else {
dBodyAddForce(body_bag->getBackLink6Body(), 0.0, force, 0.0);
}
}
else {
cout << "Stance phase"<< endl;
swingFlag[leg_id] = false;
/*for(int i = 0; i < 3; i++){
swing_torque[leg_id][i] = 0;
}*/
stanceLegTreatment(leg_id);
}
}
// called when a key pressed
static void command (int cmd)
{
switch (cmd)
{
case 'h' :
case 'H' :
case '?' :
printKeyBoardShortCut();
break;
// Force
case 'q' :
case 'Q' :
dBodyAddForce (body[BODY1],4,0,0);
break;
case 'w' :
case 'W' :
dBodyAddForce (body[BODY1],-4,0,0);
break;
case 'a' :
case 'A' :
dBodyAddForce (body[BODY1],0,40,0);
break;
case 's' :
case 'S' :
dBodyAddForce (body[BODY1],0,-40,0);
break;
case 'z' :
case 'Z' :
dBodyAddForce (body[BODY1],0,0,4);
break;
case 'x' :
case 'X' :
dBodyAddForce (body[BODY1],0,0,-4);
break;
// Torque
case 'e':
case 'E':
dBodyAddTorque (body[BODY1],0.1,0,0);
break;
case 'r':
case 'R':
dBodyAddTorque (body[BODY1],-0.1,0,0);
break;
case 'd':
case 'D':
dBodyAddTorque (body[BODY1],0, 0.1,0);
break;
case 'f':
case 'F':
dBodyAddTorque (body[BODY1],0,-0.1,0);
break;
case 'c':
case 'C':
dBodyAddTorque (body[BODY1],0.1,0,0);
break;
case 'v':
case 'V':
dBodyAddTorque (body[BODY1],-0.1,0,0);
break;
case 't':
case 'T':
if (joint->getType() == dJointTypePiston)
dJointAddPistonForce (joint->id(),1);
else
dJointAddSliderForce (joint->id(),1);
break;
case 'y':
case 'Y':
if (joint->getType() == dJointTypePiston)
dJointAddPistonForce (joint->id(),-1);
else
dJointAddSliderForce (joint->id(),-1);
break;
case '8' :
dJointAttach(joint->id(), body[0], 0);
break;
case '9' :
dJointAttach(joint->id(), 0, body[0]);
break;
case 'i':
case 'I' :
joint->setParam (dParamLoStop, 0);
joint->setParam (dParamHiStop, 0);
break;
case 'o':
case 'O' :
joint->setParam (dParamLoStop2, 0);
joint->setParam (dParamHiStop2, 0);
break;
case 'k':
case 'K':
joint->setParam (dParamLoStop2, -45.0*3.14159267/180.0);
joint->setParam (dParamHiStop2, 45.0*3.14159267/180.0);
break;
case 'l':
case 'L':
joint->setParam (dParamLoStop2, -dInfinity);
joint->setParam (dParamHiStop2, dInfinity);
break;
// Velocity of joint
case ',':
case '<' :
{
dReal vel = joint->getParam (dParamVel) - VEL_INC;
joint->setParam (dParamVel, vel);
std::cout<<"Velocity = "<<vel<<" FMax = 2"<<'\n';
}
break;
case '.':
case '>' :
{
dReal vel = joint->getParam (dParamVel) + VEL_INC;
joint->setParam (dParamVel, vel);
std::cout<<"Velocity = "<<vel<<" FMax = 2"<<'\n';
}
break;
case 'p' :
case 'P' :
{
switch (joint->getType() )
{
case dJointTypeSlider :
{
dSliderJoint *sj = reinterpret_cast<dSliderJoint *> (joint);
std::cout<<"Position ="<<sj->getPosition() <<"\n";
}
break;
case dJointTypePiston :
{
dPistonJoint *rj = reinterpret_cast<dPistonJoint *> (joint);
std::cout<<"Position ="<<rj->getPosition() <<"\n";
}
break;
default:
{} // keep the compiler happy
}
}
break;
case '+' :
(++tc) %= 4;
setPositionBodies (tc);
break;
case '-' :
(--tc) %= 4;
setPositionBodies (tc);
break;
}
}
void Cylinder::applySimpleRollingFriction(double rfc)
{
const dReal* curAngVel = dBodyGetAngularVel(body);
dBodyAddTorque(body, (curAngVel[0] * (dReal)(-rfc)), (curAngVel[1] * (dReal)(-rfc)), (curAngVel[2] * (dReal)(-rfc)));
}
int dxJointLimitMotor::addLimot( dxJoint *joint,
dxJoint::Info2 *info, int row,
const dVector3 ax1, int rotational )
{
int srow = row * info->rowskip;
// if the joint is powered, or has joint limits, add in the extra row
int powered = fmax > 0;
if ( powered || limit )
{
dReal *J1 = rotational ? info->J1a : info->J1l;
dReal *J2 = rotational ? info->J2a : info->J2l;
J1[srow+0] = ax1[0];
J1[srow+1] = ax1[1];
J1[srow+2] = ax1[2];
if ( joint->node[1].body )
{
J2[srow+0] = -ax1[0];
J2[srow+1] = -ax1[1];
J2[srow+2] = -ax1[2];
}
// linear limot torque decoupling step:
//
// if this is a linear limot (e.g. from a slider), we have to be careful
// that the linear constraint forces (+/- ax1) applied to the two bodies
// do not create a torque couple. in other words, the points that the
// constraint force is applied at must lie along the same ax1 axis.
// a torque couple will result in powered or limited slider-jointed free
// bodies from gaining angular momentum.
// the solution used here is to apply the constraint forces at the point
// halfway between the body centers. there is no penalty (other than an
// extra tiny bit of computation) in doing this adjustment. note that we
// only need to do this if the constraint connects two bodies.
dVector3 ltd = {0,0,0}; // Linear Torque Decoupling vector (a torque)
if ( !rotational && joint->node[1].body )
{
dVector3 c;
c[0] = REAL( 0.5 ) * ( joint->node[1].body->posr.pos[0] - joint->node[0].body->posr.pos[0] );
c[1] = REAL( 0.5 ) * ( joint->node[1].body->posr.pos[1] - joint->node[0].body->posr.pos[1] );
c[2] = REAL( 0.5 ) * ( joint->node[1].body->posr.pos[2] - joint->node[0].body->posr.pos[2] );
dCalcVectorCross3( ltd, c, ax1 );
info->J1a[srow+0] = ltd[0];
info->J1a[srow+1] = ltd[1];
info->J1a[srow+2] = ltd[2];
info->J2a[srow+0] = ltd[0];
info->J2a[srow+1] = ltd[1];
info->J2a[srow+2] = ltd[2];
}
// if we're limited low and high simultaneously, the joint motor is
// ineffective
if ( limit && ( lostop == histop ) ) powered = 0;
if ( powered )
{
info->cfm[row] = normal_cfm;
if ( ! limit )
{
info->c[row] = vel;
info->lo[row] = -fmax;
info->hi[row] = fmax;
}
else
{
// the joint is at a limit, AND is being powered. if the joint is
// being powered into the limit then we apply the maximum motor force
// in that direction, because the motor is working against the
// immovable limit. if the joint is being powered away from the limit
// then we have problems because actually we need *two* lcp
// constraints to handle this case. so we fake it and apply some
// fraction of the maximum force. the fraction to use can be set as
// a fudge factor.
dReal fm = fmax;
if (( vel > 0 ) || ( vel == 0 && limit == 2 ) ) fm = -fm;
// if we're powering away from the limit, apply the fudge factor
if (( limit == 1 && vel > 0 ) || ( limit == 2 && vel < 0 ) ) fm *= fudge_factor;
if ( rotational )
{
dBodyAddTorque( joint->node[0].body, -fm*ax1[0], -fm*ax1[1],
-fm*ax1[2] );
if ( joint->node[1].body )
dBodyAddTorque( joint->node[1].body, fm*ax1[0], fm*ax1[1], fm*ax1[2] );
}
else
{
dBodyAddForce( joint->node[0].body, -fm*ax1[0], -fm*ax1[1], -fm*ax1[2] );
if ( joint->node[1].body )
{
dBodyAddForce( joint->node[1].body, fm*ax1[0], fm*ax1[1], fm*ax1[2] );
// linear limot torque decoupling step: refer to above discussion
dBodyAddTorque( joint->node[0].body, -fm*ltd[0], -fm*ltd[1],
-fm*ltd[2] );
dBodyAddTorque( joint->node[1].body, -fm*ltd[0], -fm*ltd[1],
-fm*ltd[2] );
}
}
}
}
if ( limit )
{
dReal k = info->fps * stop_erp;
info->c[row] = -k * limit_err;
info->cfm[row] = stop_cfm;
if ( lostop == histop )
{
// limited low and high simultaneously
info->lo[row] = -dInfinity;
info->hi[row] = dInfinity;
}
else
{
if ( limit == 1 )
{
// low limit
info->lo[row] = 0;
info->hi[row] = dInfinity;
}
else
{
// high limit
info->lo[row] = -dInfinity;
info->hi[row] = 0;
}
// deal with bounce
if ( bounce > 0 )
{
// calculate joint velocity
dReal vel;
if ( rotational )
{
vel = dCalcVectorDot3( joint->node[0].body->avel, ax1 );
if ( joint->node[1].body )
vel -= dCalcVectorDot3( joint->node[1].body->avel, ax1 );
}
else
{
vel = dCalcVectorDot3( joint->node[0].body->lvel, ax1 );
if ( joint->node[1].body )
vel -= dCalcVectorDot3( joint->node[1].body->lvel, ax1 );
}
// only apply bounce if the velocity is incoming, and if the
// resulting c[] exceeds what we already have.
if ( limit == 1 )
{
// low limit
if ( vel < 0 )
{
dReal newc = -bounce * vel;
if ( newc > info->c[row] ) info->c[row] = newc;
}
}
else
{
// high limit - all those computations are reversed
if ( vel > 0 )
{
dReal newc = -bounce * vel;
if ( newc < info->c[row] ) info->c[row] = newc;
}
}
}
}
}
return 1;
}
else return 0;
}
void dampRotationalMotion (dReal kd)
{
const dReal *w = dBodyGetAngularVel (body[0]);
dBodyAddTorque (body[0],-kd*w[0],-kd*w[1],-kd*w[2]);
}
// called when a key pressed
static void command (int cmd)
{
switch (cmd) {
case 'h' : case 'H' : case '?' :
printKeyBoardShortCut();
break;
// Force
case 'q' : case 'Q' :
dBodyAddForce(body[D],40,0,0);
break;
case 'w' : case 'W' :
dBodyAddForce(body[D],-40,0,0);
break;
case 'a' : case 'A' :
dBodyAddForce(body[D],0,40,0);
break;
case 's' : case 'S' :
dBodyAddForce(body[D],0,-40,0);
break;
case 'z' : case 'Z' :
dBodyAddForce(body[D],0,0,40);
break;
case 'x' : case 'X' :
dBodyAddForce(body[D],0,0,-40);
break;
// Torque
case 'e': case 'E':
dBodyAddTorque(body[D],0.1,0,0);
break;
case 'r': case 'R':
dBodyAddTorque(body[D],-0.1,0,0);
break;
case 'd': case 'D':
dBodyAddTorque(body[D],0, 0.1,0);
break;
case 'f': case 'F':
dBodyAddTorque(body[D],0,-0.1,0);
break;
case 'c': case 'C':
dBodyAddTorque(body[D],0,0,0.1);
break;
case 'v': case 'V':
dBodyAddTorque(body[D],0,0,0.1);
break;
// Velocity of joint
case ',': case '<' : {
dReal vel = joint->getParam (dParamVel3) - VEL_INC;
joint->setParam (dParamVel3, vel);
std::cout<<"Velocity = "<<vel<<" FMax = 2"<<'\n';
}
break;
case '.': case '>' : {
dReal vel = joint->getParam (dParamVel3) + VEL_INC;
joint->setParam (dParamVel3, vel);
std::cout<<"Velocity = "<<vel<<" FMax = 2"<<'\n';
}
break;
case 'l': case 'L' : {
dReal aLimit, lLimit, fmax;
if ( joint->getParam (dParamFMax) ) {
aLimit = dInfinity;
lLimit = dInfinity;
fmax = 0;
}
else {
aLimit = 0.25*PI;
lLimit = 0.5*axDim[LENGTH];
fmax = 0.02;
}
joint->setParam (dParamFMax1, fmax);
joint->setParam (dParamFMax2, fmax);
joint->setParam (dParamFMax3, fmax);
switch (joint->getType() ) {
case dJointTypePR : {
dPRJoint *pr = reinterpret_cast<dPRJoint *> (joint);
pr->setParam (dParamLoStop, -lLimit);
pr->setParam (dParamHiStop, -lLimit);
pr->setParam (dParamLoStop2, aLimit);
pr->setParam (dParamHiStop2, -aLimit);
}
break;
case dJointTypePU : {
dPUJoint *pu = reinterpret_cast<dPUJoint *> (joint);
pu->setParam (dParamLoStop1, -aLimit);
pu->setParam (dParamHiStop1, aLimit);
pu->setParam (dParamLoStop2, -aLimit);
pu->setParam (dParamHiStop2, aLimit);
pu->setParam (dParamLoStop3, -lLimit);
pu->setParam (dParamHiStop3, lLimit);
}
break;
default: {} // keep the compiler happy
}
}
break;
case 'g': case 'G' : {
dVector3 g;
dWorldGetGravity(world, g);
if ( g[2]< -0.1 )
dWorldSetGravity(world, 0, 0, 0);
else
dWorldSetGravity(world, 0, 0, -0.5);
}
case 'p' :case 'P' : {
switch (joint->getType() ) {
case dJointTypeSlider : {
dSliderJoint *sj = reinterpret_cast<dSliderJoint *> (joint);
std::cout<<"Position ="<<sj->getPosition() <<"\n";
}
break;
case dJointTypePU : {
dPUJoint *pu = reinterpret_cast<dPUJoint *> (joint);
std::cout<<"Position ="<<pu->getPosition() <<"\n";
std::cout<<"Position Rate="<<pu->getPositionRate() <<"\n";
std::cout<<"Angle1 ="<<pu->getAngle1() <<"\n";
std::cout<<"Angle1 Rate="<<pu->getAngle1Rate() <<"\n";
std::cout<<"Angle2 ="<<pu->getAngle2() <<"\n";
std::cout<<"Angle2 Rate="<<pu->getAngle2Rate() <<"\n";
}
break;
default: {} // keep the compiler happy
}
}
break;
}
}
void CProtoHapticDoc::SimulationStep()
{
int elapsed= clock() - m_lastSimStep;
int steps= (int)((((float)elapsed)*m_simSpeed)*0.1f);
if(steps<1) {
m_lastSimStep= clock();
return;
}
// collision detection
for(int i= 0; i<m_shapeCount; i++) {
if(m_shapes[i]->isCollisionDynamic())
dGeomSetBody (m_geoms[i], bodies[i]);
else
dGeomSetBody (m_geoms[i], 0);
}
dSpaceCollide (m_spaceID,this,&nearCallbackStatic);
dWorldQuickStep (m_worldID, steps);
dJointGroupEmpty (m_jointGroup);
for( int i= 0; i<m_shapeCount; i++) {
//air resistance
if(m_shapes[i]->isCollisionDynamic()||m_shapes[i]->isProxyDynamic()) {
const dReal *vel;
const dReal *angvel;
vel= dBodyGetLinearVel (bodies[i]);
dBodyAddForce (bodies[i], -vel[0]*m_airResistance, -vel[1]*m_airResistance, -vel[2]*m_airResistance);
angvel= dBodyGetAngularVel (bodies[i]);
dBodyAddTorque (bodies[i], -angvel[0]*m_airResistance, -angvel[1]*m_airResistance, -angvel[2]*m_airResistance);
}
HHLRC rc= hlGetCurrentContext();
// proxy0
hlMakeCurrent(m_context);
if(m_shapes[i]->isProxyDynamic()&&m_shapes[i]->touching()) {
HLdouble force[3];
HLdouble pp[3];
hlGetDoublev(HL_DEVICE_FORCE,force);
hlGetDoublev(HL_PROXY_POSITION,pp);
dBodyAddForceAtPos (bodies[i], -force[0], -force[1], -force[2],
pp[0], pp[1], pp[2]);
}
if(((CProtoHapticApp*)AfxGetApp())->isTwoDevices()) {
// proxy1
hlMakeCurrent(m_context_1);
if(m_shapes[i]->isProxyDynamic()&&m_shapes[i]->touching1()) {
HLdouble force[3];
HLdouble pp[3];
hlGetDoublev(HL_DEVICE_FORCE,force);
hlGetDoublev(HL_PROXY_POSITION,pp);
dBodyAddForceAtPos (bodies[i], -force[0], -force[1], -force[2],
pp[0], pp[1], pp[2]);
}
}
hlMakeCurrent(rc);
// gravity
if(m_shapes[i]->isCollisionDynamic())
dBodyAddForce(bodies[i], 0.0, -m_shapes[i]->getMass()*(m_gravity/10.0f), 0.0);
const dReal *pos;
const dReal *rot;
pos= dBodyGetPosition (bodies[i]);
rot= dBodyGetRotation (bodies[i]);
float rotation[16];
rotation[0]= rot[0]; rotation[4]= rot[1]; rotation[8]= rot[2]; rotation[12]= rot[3];
rotation[1]= rot[4]; rotation[5]= rot[5]; rotation[9]= rot[6]; rotation[13]= rot[7];
rotation[2]= rot[8]; rotation[6]= rot[9]; rotation[10]= rot[10]; rotation[14]= rot[11];
rotation[3]= 0.0; rotation[7]= 0.0; rotation[11]= 0.0; rotation[15]= 1.0;
m_shapes[i]->setRotation(rotation);
m_shapes[i]->setLocation(pos[0], pos[1], pos[2]);
}
m_lastSimStep= clock();
}
dReal doStuffAndGetError (int n)
{
switch (n) {
// ********** fixed joint
case 0: { // 2 body
addOscillatingTorque (0.1);
dampRotationalMotion (0.1);
// check the orientations are the same
const dReal *R1 = dBodyGetRotation (body[0]);
const dReal *R2 = dBodyGetRotation (body[1]);
dReal err1 = dMaxDifference (R1,R2,3,3);
// check the body offset is correct
dVector3 p,pp;
const dReal *p1 = dBodyGetPosition (body[0]);
const dReal *p2 = dBodyGetPosition (body[1]);
for (int i=0; i<3; i++) p[i] = p2[i] - p1[i];
dMULTIPLY1_331 (pp,R1,p);
pp[0] += 0.5;
pp[1] += 0.5;
return (err1 + length (pp)) * 300;
}
case 1: { // 1 body to static env
addOscillatingTorque (0.1);
// check the orientation is the identity
dReal err1 = cmpIdentity (dBodyGetRotation (body[0]));
// check the body offset is correct
dVector3 p;
const dReal *p1 = dBodyGetPosition (body[0]);
for (int i=0; i<3; i++) p[i] = p1[i];
p[0] -= 0.25;
p[1] -= 0.25;
p[2] -= 1;
return (err1 + length (p)) * 1e6;
}
case 2: { // 2 body
addOscillatingTorque (0.1);
dampRotationalMotion (0.1);
// check the body offset is correct
// Should really check body rotation too. Oh well.
const dReal *R1 = dBodyGetRotation (body[0]);
dVector3 p,pp;
const dReal *p1 = dBodyGetPosition (body[0]);
const dReal *p2 = dBodyGetPosition (body[1]);
for (int i=0; i<3; i++) p[i] = p2[i] - p1[i];
dMULTIPLY1_331 (pp,R1,p);
pp[0] += 0.5;
pp[1] += 0.5;
return length(pp) * 300;
}
case 3: { // 1 body to static env with relative rotation
addOscillatingTorque (0.1);
// check the body offset is correct
dVector3 p;
const dReal *p1 = dBodyGetPosition (body[0]);
for (int i=0; i<3; i++) p[i] = p1[i];
p[0] -= 0.25;
p[1] -= 0.25;
p[2] -= 1;
return length (p) * 1e6;
}
// ********** hinge joint
case 200: // 2 body
addOscillatingTorque (0.1);
dampRotationalMotion (0.1);
return dInfinity;
case 220: // hinge angle polarity test
dBodyAddTorque (body[0],0,0,0.01);
dBodyAddTorque (body[1],0,0,-0.01);
if (iteration == 40) {
dReal a = dJointGetHingeAngle (joint);
if (a > 0.5 && a < 1) return 0; else return 10;
}
return 0;
case 221: { // hinge angle rate test
static dReal last_angle = 0;
dBodyAddTorque (body[0],0,0,0.01);
dBodyAddTorque (body[1],0,0,-0.01);
dReal a = dJointGetHingeAngle (joint);
dReal r = dJointGetHingeAngleRate (joint);
dReal er = (a-last_angle)/STEPSIZE; // estimated rate
last_angle = a;
return fabs(r-er) * 4e4;
}
case 230: // hinge motor rate (and polarity) test
case 231: { // ...with stops
static dReal a = 0;
dReal r = dJointGetHingeAngleRate (joint);
dReal err = fabs (cos(a) - r);
if (a==0) err = 0;
a += 0.03;
dJointSetHingeParam (joint,dParamVel,cos(a));
if (n==231) return dInfinity;
return err * 1e6;
}
// ********** slider joint
case 300: // 2 body
addOscillatingTorque (0.05);
dampRotationalMotion (0.1);
addSpringForce (0.5);
return dInfinity;
case 320: // slider angle polarity test
dBodyAddForce (body[0],0,0,0.1);
dBodyAddForce (body[1],0,0,-0.1);
if (iteration == 40) {
dReal a = dJointGetSliderPosition (joint);
if (a > 0.2 && a < 0.5) return 0; else return 10;
return a;
}
return 0;
case 321: { // slider angle rate test
static dReal last_pos = 0;
dBodyAddForce (body[0],0,0,0.1);
dBodyAddForce (body[1],0,0,-0.1);
dReal p = dJointGetSliderPosition (joint);
dReal r = dJointGetSliderPositionRate (joint);
dReal er = (p-last_pos)/STEPSIZE; // estimated rate (almost exact)
last_pos = p;
return fabs(r-er) * 1e9;
}
case 330: // slider motor rate (and polarity) test
case 331: { // ...with stops
static dReal a = 0;
dReal r = dJointGetSliderPositionRate (joint);
dReal err = fabs (0.7*cos(a) - r);
if (a < 0.04) err = 0;
a += 0.03;
dJointSetSliderParam (joint,dParamVel,0.7*cos(a));
if (n==331) return dInfinity;
return err * 1e6;
}
// ********** hinge-2 joint
case 420: // hinge-2 steering angle polarity test
dBodyAddTorque (body[0],0,0,0.01);
dBodyAddTorque (body[1],0,0,-0.01);
if (iteration == 40) {
dReal a = dJointGetHinge2Angle1 (joint);
if (a > 0.5 && a < 0.6) return 0; else return 10;
}
return 0;
case 421: { // hinge-2 steering angle rate test
static dReal last_angle = 0;
dBodyAddTorque (body[0],0,0,0.01);
dBodyAddTorque (body[1],0,0,-0.01);
dReal a = dJointGetHinge2Angle1 (joint);
dReal r = dJointGetHinge2Angle1Rate (joint);
dReal er = (a-last_angle)/STEPSIZE; // estimated rate
last_angle = a;
return fabs(r-er)*2e4;
}
case 430: // hinge 2 steering motor rate (+polarity) test
case 431: { // ...with stops
static dReal a = 0;
dReal r = dJointGetHinge2Angle1Rate (joint);
dReal err = fabs (cos(a) - r);
if (a==0) err = 0;
a += 0.03;
dJointSetHinge2Param (joint,dParamVel,cos(a));
if (n==431) return dInfinity;
return err * 1e6;
}
case 432: { // hinge 2 wheel motor rate (+polarity) test
static dReal a = 0;
dReal r = dJointGetHinge2Angle2Rate (joint);
dReal err = fabs (cos(a) - r);
if (a==0) err = 0;
a += 0.03;
dJointSetHinge2Param (joint,dParamVel2,cos(a));
return err * 1e6;
}
// ********** angular motor joint
case 600: { // test euler angle calculations
// desired euler angles from last iteration
static dReal a1,a2,a3;
// find actual euler angles
dReal aa1 = dJointGetAMotorAngle (joint,0);
dReal aa2 = dJointGetAMotorAngle (joint,1);
dReal aa3 = dJointGetAMotorAngle (joint,2);
// printf ("actual = %.4f %.4f %.4f\n\n",aa1,aa2,aa3);
dReal err = dInfinity;
if (iteration > 0) {
err = dFabs(aa1-a1) + dFabs(aa2-a2) + dFabs(aa3-a3);
err *= 1e10;
}
// get random base rotation for both bodies
dMatrix3 Rbase;
dRFromAxisAndAngle (Rbase, 3*(dRandReal()-0.5), 3*(dRandReal()-0.5),
3*(dRandReal()-0.5), 3*(dRandReal()-0.5));
dBodySetRotation (body[0],Rbase);
// rotate body 2 by random euler angles w.r.t. body 1
a1 = 3.14 * 2 * (dRandReal()-0.5);
a2 = 1.57 * 2 * (dRandReal()-0.5);
a3 = 3.14 * 2 * (dRandReal()-0.5);
dMatrix3 R1,R2,R3,Rtmp1,Rtmp2;
dRFromAxisAndAngle (R1,0,0,1,-a1);
dRFromAxisAndAngle (R2,0,1,0,a2);
dRFromAxisAndAngle (R3,1,0,0,-a3);
dMultiply0 (Rtmp1,R2,R3,3,3,3);
dMultiply0 (Rtmp2,R1,Rtmp1,3,3,3);
dMultiply0 (Rtmp1,Rbase,Rtmp2,3,3,3);
dBodySetRotation (body[1],Rtmp1);
// printf ("desired = %.4f %.4f %.4f\n",a1,a2,a3);
return err;
}
// ********** universal joint
case 700: { // 2 body: joint constraint
dVector3 ax1, ax2;
addOscillatingTorque (0.1);
dampRotationalMotion (0.1);
dJointGetUniversalAxis1(joint, ax1);
dJointGetUniversalAxis2(joint, ax2);
return fabs(10*dDOT(ax1, ax2));
}
case 701: { // 2 body: angle 1 rate
static dReal last_angle = 0;
addOscillatingTorque (0.1);
dampRotationalMotion (0.1);
dReal a = dJointGetUniversalAngle1(joint);
dReal r = dJointGetUniversalAngle1Rate(joint);
dReal diff = a - last_angle;
if (diff > M_PI) diff -= 2*M_PI;
if (diff < -M_PI) diff += 2*M_PI;
dReal er = diff / STEPSIZE; // estimated rate
last_angle = a;
// I'm not sure why the error is so large here.
return fabs(r - er) * 1e1;
}
case 702: { // 2 body: angle 2 rate
static dReal last_angle = 0;
addOscillatingTorque (0.1);
dampRotationalMotion (0.1);
dReal a = dJointGetUniversalAngle2(joint);
dReal r = dJointGetUniversalAngle2Rate(joint);
dReal diff = a - last_angle;
if (diff > M_PI) diff -= 2*M_PI;
if (diff < -M_PI) diff += 2*M_PI;
dReal er = diff / STEPSIZE; // estimated rate
last_angle = a;
// I'm not sure why the error is so large here.
return fabs(r - er) * 1e1;
}
case 720: { // universal transmit torque test: constraint error
dVector3 ax1, ax2;
addOscillatingTorqueAbout (0.1, 1, 1, 0);
dampRotationalMotion (0.1);
dJointGetUniversalAxis1(joint, ax1);
dJointGetUniversalAxis2(joint, ax2);
return fabs(10*dDOT(ax1, ax2));
}
case 721: { // universal transmit torque test: angle1 rate
static dReal last_angle = 0;
addOscillatingTorqueAbout (0.1, 1, 1, 0);
dampRotationalMotion (0.1);
dReal a = dJointGetUniversalAngle1(joint);
dReal r = dJointGetUniversalAngle1Rate(joint);
dReal diff = a - last_angle;
if (diff > M_PI) diff -= 2*M_PI;
if (diff < -M_PI) diff += 2*M_PI;
dReal er = diff / STEPSIZE; // estimated rate
last_angle = a;
return fabs(r - er) * 1e10;
}
case 722: { // universal transmit torque test: angle2 rate
static dReal last_angle = 0;
addOscillatingTorqueAbout (0.1, 1, 1, 0);
dampRotationalMotion (0.1);
dReal a = dJointGetUniversalAngle2(joint);
dReal r = dJointGetUniversalAngle2Rate(joint);
dReal diff = a - last_angle;
if (diff > M_PI) diff -= 2*M_PI;
if (diff < -M_PI) diff += 2*M_PI;
dReal er = diff / STEPSIZE; // estimated rate
last_angle = a;
return fabs(r - er) * 1e10;
}
case 730:{
dVector3 ax1, ax2;
dJointGetUniversalAxis1(joint, ax1);
dJointGetUniversalAxis2(joint, ax2);
addOscillatingTorqueAbout (0.1, ax1[0], ax1[1], ax1[2]);
dampRotationalMotion (0.1);
return fabs(10*dDOT(ax1, ax2));
}
case 731:{
dVector3 ax1;
static dReal last_angle = 0;
dJointGetUniversalAxis1(joint, ax1);
addOscillatingTorqueAbout (0.1, ax1[0], ax1[1], ax1[2]);
dampRotationalMotion (0.1);
dReal a = dJointGetUniversalAngle1(joint);
dReal r = dJointGetUniversalAngle1Rate(joint);
dReal diff = a - last_angle;
if (diff > M_PI) diff -= 2*M_PI;
if (diff < -M_PI) diff += 2*M_PI;
dReal er = diff / STEPSIZE; // estimated rate
last_angle = a;
return fabs(r - er) * 2e3;
}
case 732:{
dVector3 ax1;
static dReal last_angle = 0;
dJointGetUniversalAxis1(joint, ax1);
addOscillatingTorqueAbout (0.1, ax1[0], ax1[1], ax1[2]);
dampRotationalMotion (0.1);
dReal a = dJointGetUniversalAngle2(joint);
dReal r = dJointGetUniversalAngle2Rate(joint);
dReal diff = a - last_angle;
if (diff > M_PI) diff -= 2*M_PI;
if (diff < -M_PI) diff += 2*M_PI;
dReal er = diff / STEPSIZE; // estimated rate
last_angle = a;
return fabs(r - er) * 1e10;
}
case 740:{
dVector3 ax1, ax2;
dJointGetUniversalAxis1(joint, ax1);
dJointGetUniversalAxis2(joint, ax2);
addOscillatingTorqueAbout (0.1, ax2[0], ax2[1], ax2[2]);
dampRotationalMotion (0.1);
return fabs(10*dDOT(ax1, ax2));
}
case 741:{
dVector3 ax2;
static dReal last_angle = 0;
dJointGetUniversalAxis2(joint, ax2);
addOscillatingTorqueAbout (0.1, ax2[0], ax2[1], ax2[2]);
dampRotationalMotion (0.1);
dReal a = dJointGetUniversalAngle1(joint);
dReal r = dJointGetUniversalAngle1Rate(joint);
dReal diff = a - last_angle;
if (diff > M_PI) diff -= 2*M_PI;
if (diff < -M_PI) diff += 2*M_PI;
dReal er = diff / STEPSIZE; // estimated rate
last_angle = a;
return fabs(r - er) * 1e10;
}
case 742:{
dVector3 ax2;
static dReal last_angle = 0;
dJointGetUniversalAxis2(joint, ax2);
addOscillatingTorqueAbout (0.1, ax2[0], ax2[1], ax2[2]);
dampRotationalMotion (0.1);
dReal a = dJointGetUniversalAngle2(joint);
dReal r = dJointGetUniversalAngle2Rate(joint);
dReal diff = a - last_angle;
if (diff > M_PI) diff -= 2*M_PI;
if (diff < -M_PI) diff += 2*M_PI;
dReal er = diff / STEPSIZE; // estimated rate
last_angle = a;
return fabs(r - er) * 1e4;
}
}
return dInfinity;
}
