/**
* This method checks the angle of the HingeJoint for correctness
* and corrects it if needed. This is necessary because ODE
* stores the hinge-angle between -180 degrees and +180 degrees.
* If the angle gets above +180 deg ODE swaps to -180 deg
* and vice versa.
* If a user wants to use a HingeJoint with a maximum angle above
* 180 deg or a minimum angle below 180 deg the method corrects
* the ODE-swapping.
**/
void HingeJoint::checkAngles()
{
if (active && joint && anglesSet)
{
float totalAngle;
dReal angle = dJointGetHingeAngle(joint);
// ODE swapped from -180� to +180� or from +180� to -180�
if ((angle - oldAngle) < -gmtl::Math::PI)
{
deltaAngleODE += 2*gmtl::Math::PI;
setODEAngles();
} else if ((angle - oldAngle) > gmtl::Math::PI)
{
deltaAngleODE -= 2*gmtl::Math::PI;
setODEAngles();
} // else if
totalAngle = angle + deltaAngle + deltaAngleODE;
if (totalAngle > 2*gmtl::Math::PI || totalAngle < -2*gmtl::Math::PI)
{
deltaAngleODE = 0;
setODEAngles();
} // if
// store currentAngle for test in next frame
oldAngle = angle;
} // if
} // checkAngles
/**
* This method is called if the joint should be detached.
* If the maximum and minimum angles are set the method
* stores the angle-offset for correct angles in later use.
**/
void HingeJoint::detachJoint()
{
if (active && joint)
{
dReal angle = dJointGetHingeAngle(joint);
if (!anglesSet)
{
deltaAngle += angle;
if (deltaAngle > gmtl::Math::PI)
deltaAngle -= 2*gmtl::Math::PI;
else if (deltaAngle < -gmtl::Math::PI)
deltaAngle += 2*gmtl::Math::PI;
} // if
else
{
deltaAngle += angle;
if (deltaAngle > gmtl::Math::PI)
{
deltaAngle -= 2*gmtl::Math::PI;
deltaAngleODE += 2*gmtl::Math::PI;
} // if
else if (deltaAngle < -gmtl::Math::PI)
{
deltaAngle += 2*gmtl::Math::PI;
deltaAngleODE -= 2*gmtl::Math::PI;
} // else if
} // else
} // if
} // detachJoint
/**
* This method copies the value of the field with the passed name
* to the result-pointer. It is a little helper to avoid type-casts
* when accessing joint-pointers.
* @param type name of the field
* @param result destination where the value should be stored
**/
void HingeJoint::getMemberValue(std::string type, void* result)
{
float angle = 0;
if (type == "axis1")
{
angle = deltaAngle + deltaAngleODE;
if (joint && active)
angle += dJointGetHingeAngle(joint);
if (!anglesSet)
{
if (angle > gmtl::Math::PI || angle < -gmtl::Math::PI)
printd(ERROR, "HingeJoint::GetMemberValue(): ERROR: THIS SHOULD NEVER HAPPEN!!!!!!\n");
if (angle > gmtl::Math::PI)
angle -= 2*gmtl::Math::PI;
else if (angle < -gmtl::Math::PI)
angle += 2*gmtl::Math::PI;
} // if
if (firstIsMain)
angle *= -1;
*((float*)result) = angle;
} // if
else if (type == "internalChanges")
{
JointInteractionEndEvent* evt = (JointInteractionEndEvent*)result;
evt->addJointID(jointID);
evt->addFloat(deltaAngle);
evt->addFloat(deltaAngleODE);
} // else if
} // getMemberValue
dReal ODE_Link::getAngle(){
if(jointType == ODE_Link::ROTATIONAL_JOINT)
return dJointGetHingeAngle(odeJointId);
else if(jointType == ODE_Link::SLIDE_JOINT)
return dJointGetSliderPosition(odeJointId);
else
return 0;
}
static float get_phys_joint_value(dJointID j)
{
switch (dJointGetType(j))
{
case dJointTypeHinge: return (float) DEG(dJointGetHingeAngle (j));
case dJointTypeSlider: return (float) dJointGetSliderPosition(j);
case dJointTypeHinge2: return (float) DEG(dJointGetHinge2Angle1 (j));
default: return 0.0f;
}
}
/*** P制御 ***/
void Pcontrol()
{
dReal k = 10.0, fMax = 100.0; // 比例ゲイン，最大トルク
for (int j = 1; j < NUM; j++) {
dReal tmp = dJointGetHingeAngle(joint[j]); // 関節角の取得
dReal z = THETA[j] - tmp; // 残差
dJointSetHingeParam(joint[j],dParamVel, k*z); // 角速度の設定
dJointSetHingeParam(joint[j],dParamFMax,fMax); // トルクの設定
}
}
void control() { /*** P control ****/
static int step = 0; // Steps of simulation
double k1 = 10.0, fMax = 100.0; // k1: proportional gain, fMax：Max torque[Nm]
printf("\r%6d:",step++);
for (int j = 1; j < NUM; j++) {
double tmpAngle = dJointGetHingeAngle(joint[j]); // Present angle[rad]
double z = THETA[j] - tmpAngle; // z: residual=target angle - present angle
dJointSetHingeParam(joint[j], dParamVel, k1*z); // Set angular velocity[m/s]
dJointSetHingeParam(joint[j], dParamFMax, fMax); // Set max torque[N/m]
}
}
void doDraw(){
if (!leg) return;
dsSetColor(0.0,0.0,1.0);
dsDrawCapsuleD(dBodyGetPosition(leg->lower_part.body), dBodyGetRotation(leg->lower_part.body), leg->lower_part.length, leg->lower_part.radius);
dsSetColor(1.0,0.0,0.0);
dsDrawCapsuleD(dBodyGetPosition(leg->upper_part.body), dBodyGetRotation(leg->upper_part.body), leg->upper_part.length, leg->upper_part.radius);
//TEST
double k1 = 1.0, fMax = 10.0; // k1: proportional gain, fMax：Max torque[Nm]
//dJointSetHingeParam(jointHinge, dParamVel, k1); // Set angular velocity[m/s]
//dJointSetHingeParam(jointHinge, dParamFMax, fMax); // Set max torque[N/m]
printf("\r %6d:",dJointGetHingeAngle(leg->jointHinge));
}
void control()
{
double k1 = 10.0, fMax = 100.0; // k1:gain, fMax: max torque [Nm]
for(int i=0;i<MAX_JOINTS;i++)
{
current_angleR[i] = dJointGetHingeAngle(jointR[i]); // current joint
//double z = target_angleR[i]*M_PI/180 - current_angleR[i]; // z = target – current
double z = target_angleR[i] - current_angleR[i]; // z = target – current
// Set angular velocity for the joint: w = k1(th_tarjet-th_curr)
dJointSetHingeParam(jointR[i],dParamVel,k1*z);
dJointSetHingeParam(jointR[i],dParamFMax,fMax); // max torque
}
for(int i=0;i<MAX_JOINTS;i++)
{
current_angleL[i] = dJointGetHingeAngle(jointL[i]); // current joint
//double z = target_angleL[i]*M_PI/180 - current_angleL[i]; // z = target – current
double z = target_angleL[i] - current_angleL[i]; // z = target – current
// Set angular velocity for the joint: w = k1(th_tarjet-th_curr)
dJointSetHingeParam(jointL[i],dParamVel,k1*z);
dJointSetHingeParam(jointL[i],dParamFMax,fMax); // max torque
}
}
void ODE_LinearSpringModel::updateInputValues() {
if(mOwnerSpringAdapter == 0 || mOwnerSpringAdapter->getCurrentTargetJoint() == 0) {
return;
}
//The first time an update is run the mJoint is located.
//This has to be done here instead of setup() to ensure that the joint is
//really available, which can not be guaranteed in setup().
if(mJoint == 0) {
SimJoint *simJoint = mOwnerSpringAdapter->getCurrentTargetJoint();
ODE_Joint *odeJoint = dynamic_cast<ODE_Joint*>(simJoint);
if(odeJoint == 0) {
MotorAdapter *adapter = dynamic_cast<MotorAdapter*>(simJoint);
if(adapter != 0) {
odeJoint = dynamic_cast<ODE_Joint*>(adapter->getActiveMotorModel());
}
}
if(simJoint != 0 && odeJoint != 0) {
if(getType() == MotorModel::HINGE_JOINT) {
if(dynamic_cast<HingeJoint*>(simJoint) != 0) {
mJoint = odeJoint->getJoint();
}
}
else if(getType() == MotorModel::SLIDER_JOINT) {
if(dynamic_cast<SliderJoint*>(simJoint) != 0) {
mJoint = odeJoint->getJoint();
}
}
}
}
if(mJoint != 0) {
if(getType() == MotorModel::HINGE_JOINT) {
double torque = calculateTorque(dJointGetHingeAngle(mJoint));
dJointAddHingeTorque(mJoint, torque);
mOwnerSpringAdapter->getCurrentTorqueValue()->set(torque);
}
else if(getType() == MotorModel::SLIDER_JOINT) {
double force = calculateForce(dJointGetSliderPosition(mJoint));
dJointAddSliderForce(mJoint, force);
mOwnerSpringAdapter->getCurrentTorqueValue()->set(force);
}
}
}
/**
*@brief ジョイントの位置、角度取得の関数
* @return ジョイントの位置、角度
*/
double ODEJointObj::GetJointPosition()
{
if(JointType == 0)
{
double a = dJointGetSliderPosition(joint);
return a;
}
else if(JointType == 2)
{
double a = dJointGetHingeAngle(joint);
return a;
}
return 0;
}
//! This function is called once per simulation step, allowing the
//! constraint to be "motorized" according to some response. It runs
//! in the physics thread.
void OscHingeODE::simulationCallback()
{
ODEConstraint& me = *static_cast<ODEConstraint*>(special());
dReal angle = dJointGetHingeAngle(me.joint());
dReal rate = dJointGetHingeAngleRate(me.joint());
dReal addtorque =
- m_response->m_stiffness.m_value*angle
- m_response->m_damping.m_value*rate;
// Limit the torque otherwise we get ODE assertions.
if (addtorque > 1000) addtorque = 1000;
if (addtorque < -1000) addtorque = -1000;
m_torque.m_value = addtorque;
dJointAddHingeTorque(me.joint(), addtorque);
}
void JOINT::Actuate(void) {
if ( motorNeuron == NULL )
return;
double motorNeuronValue = motorNeuron->Get_Value();
double zeroToOne = motorNeuronValue/2.0 + 0.5;
double desiredAngle = zeroToOne * ( highStop - lowStop ) + lowStop;
double currentAngle = dJointGetHingeAngle(joint);
double diff = desiredAngle - currentAngle;
dJointSetHingeParam(joint,dParamVel, speed * diff);
dJointSetHingeParam(joint,dParamFMax,1000000);
}
void ServoMotor::act()
{
const float currentPos = dJointGetType(joint->joint) == dJointTypeHinge
? dJointGetHingeAngle(joint->joint)
: dJointGetSliderPosition(joint->joint);
float setpoint = this->setpoint;
const float maxValueChange = maxVelocity * Simulation::simulation->scene->stepLength;
if(std::abs(setpoint - currentPos) > maxValueChange)
{
if(setpoint < currentPos)
setpoint = currentPos - maxValueChange;
else
setpoint = currentPos + maxValueChange;
}
const float newVel = controller.getOutput(currentPos, setpoint);
if(dJointGetType(joint->joint) == dJointTypeHinge)
dJointSetHingeParam(joint->joint, dParamVel, dReal(newVel));
else
dJointSetSliderParam(joint->joint, dParamVel, dReal(newVel));
}
void ServoMotor::step(float stepSize) {
if(!shouldStep_)
return;
if (isBurntOut_) {
dJointSetHingeParam(joint_->getJoint(), dParamVel, 0);
return;
}
if (isVelocityDriven_) {
if (fabs(desiredVelocity_) < 1e-5) {
dJointSetHingeParam(joint_->getJoint(), dParamVel, 0);
} else {
dJointSetHingeParam(joint_->getJoint(), dParamVel, desiredVelocity_);
}
} else {
dReal curPosition = dJointGetHingeAngle(joint_->getJoint());
dReal error = curPosition - desiredPosition_;
dReal velocity = -gain_ * error / stepSize;
if (velocity > MAX_VELOCITY) {
velocity = MAX_VELOCITY;
} else if (velocity < MIN_VELOCITY) {
velocity = MIN_VELOCITY;
}
if (fabs(velocity) > 1e-5) {
dJointSetHingeParam(joint_->getJoint(), dParamVel, velocity);
} else {
dJointSetHingeParam(joint_->getJoint(), dParamVel, 0);
}
}
}
double CODEHingeJoint::getAngle(int axisIndex) const
{
return dJointGetHingeAngle(mID);
}
int ToyControl::action() {
if (!_sim) {
return -1;
}
/*
_target[0] = -1;
_target[1] = s0.swh;
_target[2] = -s0.swk;
_target[3] = -s0.stk;
_target[4] = s0.swa;
_target[5] = s0.sta;
*/
// collision detection
dContactGeom contact[100];
dGeomID leftfootId = _toy.box[5].id();
dGeomID rightfootId = _toy.box[6].id();
dGeomID groundId = _toy._env.ground.id();
int left = dCollide(leftfootId, groundId, 100, &contact[0], sizeof(dContactGeom));
int right = dCollide(rightfootId, groundId, 100, &contact[1], sizeof(dContactGeom));
// end of collision detection
//target->joint
//0 left hip
//1 right hip
//2 left knee
//3 right knee
//4 left ankle
//5 right ankle
if (now.num==0){
if (diff >= now.deltaT){
now = s1;
before = clock();
std::cout<<"0->1"<<std::endl;
}else if (right > 0){
now = s2;
before = clock();
diff = 0;
std::cout<<"0->2"<<"\tright "<<right<<std::endl;
}else{
//left lift
_target[0] = -s0.swh;
_target[1] = s0.swh;
_target[2] = -s0.swk;//-
_target[3] = s0.stk;
_target[4] = s0.swa;
_target[5] = s0.sta;
}
}
if(now.num==1){
if (left>0){
std::cout<<"1->2"<<"\tleft "<<left<<std::endl;
now = s2;
before = clock();
diff = 0;
}else{
//left contact
_target[0] = -s1.swh;
_target[1] = s1.swh;
_target[2] = -s1.swk;//-
_target[3] = s1.stk;
_target[4] = s1.swa;
_target[5] = s1.sta;
}
}
if(now.num==2){
if (diff >= now.deltaT){
now = s3;
before = clock();
std::cout<<"2->3"<<std::endl;
}else if (left > 0){
now = s0;
before = clock();
diff = 0;
std::cout<<"2->0"<<"\tleft "<<left<<std::endl;
}else{
//right lift
_target[1] = -s2.swh;
_target[0] = s2.swh;
_target[3] = -s2.swk;//-
_target[2] = s2.stk;
_target[5] = s2.swa;
_target[4] = s2.sta;
}
}
if(now.num==3){
if (right>0){
std::cout<<"3->0"<<"\tright "<<right<<std::endl;
now = s0;
before = clock();
diff = 0;
}else{
_target[1] = -s3.swh;
_target[0] = s3.swh;
_target[3] = -s3.swk;//-
_target[2] = s3.stk;
_target[5] = s3.swa;
_target[4] = s3.sta;
}
}
if(now.num==4){
std::cout<<"starting..."<<std::endl;
before = clock();
_target[0] = -s0.swh;
_target[1] = s0.swh;
_target[2] = -s0.swk;//-
_target[3] = s0.stk;
_target[4] = s0.swa;
_target[5] = s0.sta;
now.num=0;
}
// Add joint torques to each DOF, pulling the body towards the
// desired state defined by _target.
for (int i = 0; i < NUM_BODY-1; i++) {
dJointID jt = _toy.joint[i].id();
double limit = _ks[i];
dReal theta = dJointGetHingeAngle(jt);
dReal thetav = dJointGetHingeAngleRate(jt);
dReal torque =
_ks[i]*(_target[i] - theta) - _kd[i]*thetav; // PD controler equation
if (torque > limit) torque = limit;
if (torque < -limit) torque = -limit; // a limit on torque
dJointAddHingeTorque(jt, torque);
//use this torque in the next step of simulation
}
after = clock();
diff = ((float)(after-before))/CLOCKS_PER_SEC;
return 0;
}
double
Else::AcrobotArticulatedBody
::currentAngle( void )
{
return dJointGetHingeAngle( myJoints[0] );
}
/*** P control tries to add a force that reduces the distance between
the current joint angles and the desired value in the lookup table THETA.
***/
void Pcontrol()
{
dReal kp = 2.0; //effects how quickly it tries to acheive the desired angle (original=2)
// dReal kp = 10.0; //effects how quickly it tries to acheive the desired angle
dReal fMax = 100.0; //fMax is the max for it can apply trying to acheive the desired angle
//#define ANG_VELOCITY
#ifndef ANG_VELOCITY
for(int segment = 0; segment < BODY_SEGMENTS; ++segment) {
for (int i = 0; i < num_legs; i++) {
for (int j = 0; j < num_links; j++) {
dReal tmp = dJointGetHingeAngle(leg[segment][i][j].joint);
dReal diff = THETA[segment][i][j] - tmp;
dReal u;
u = kp * diff;
//cout << "\n\n***" << u << "***\n\n\n\n" << endl;
dJointSetHingeParam(leg[segment][i][j].joint, dParamVel,
u);
dJointSetHingeParam(leg[segment][i][j].joint, dParamFMax,
fMax);
}
}
#else //when using angular velocity
for(int segment = 0; segment < BODY_SEGMENTS; ++segment) {
for (int i = 0; i < num_legs; i++) {
for (int j = 0; j < num_links; j++) {
//dReal tmp = dJointGetHingeAngle(leg[segment][i][j].joint);
//dReal diff = THETA[segment][i][j] - tmp;
//dReal u;
//u = kp * diff;
dReal desiredValue = THETA[segment][i][j];
desiredValue *= 2.25;
//cout << "\n\n***" << desiredValue << "***\n\n\n\n" << endl;
dJointSetHingeParam(leg[segment][i][j].joint, dParamVel,
desiredValue);
dJointSetHingeParam(leg[segment][i][j].joint, dParamFMax,
fMax);
}
}
#endif
#ifdef USE_NLEG_ROBOT
if(segment < BODY_SEGMENTS-1) {
// applying torso joint force
dReal tmp = dJointGetHingeAngle(torso[segment].joint); //joints might start in segment 2
dReal diff = torsoJointDesiredAngle[segment] - tmp;
dReal u;
u = kp * diff;
dJointSetHingeParam(torso[segment].joint, dParamVel, u);
dJointSetHingeParam(torso[segment].joint, dParamFMax, fMax);
}
#endif
}
}
/*** Control of walking ***/
void walk()
{
// int numSheets;
//
// #ifdef HIDDEN_LAYER
// numSheets = 3;
// #else
// numSheets = 2;
// #endif
timeSteps++;
//jmc added to print out joint angles
if(printJointAngles){
static bool doOnce = true;
if(doOnce){
doOnce=false;
//remove the file if it is there already
ofstream jangle_file;
jangle_file.open ("jointAngles.txt", ios::trunc );
jangle_file.close();
}
}
//clear out the network
//LHZ - When Recurrence is turned on, we multiply the input values by the recurrent values that were in
//the input nodes to begin with, if they are all 0, the network will never take a value. So we start it with
//all 1's, since this will act as if ther was no recurrence for the first update.
substrate_pointer->reinitialize();
if(experimentType != 33) //If it is a NEAT experiment, the substrate can have an arbitrary number of layers, so we need to update 1+ExtraActivationUpdates num times
{
substrate_pointer->dummyActivation(); //dummyActivation sets a flag so that the first update does not add on ExtraActivationUdates num updates
}
dReal roll, pitch, yaw;
for(int segment = 0; segment < BODY_SEGMENTS; ++segment) {
roll = pitch = yaw = 0.0;
const dReal * torsoRotation = dBodyGetRotation(torso[segment].body);
get_euler(torsoRotation, roll, pitch, yaw);
#if LEGSWING_ODE_DEBUG
printf("roll: %f, pitch: %f, yaw: %f\n", roll, pitch, yaw);
#endif
for (int leg_no = 0; leg_no < num_legs; leg_no++) {
for (int joint_no = 0; joint_no < num_joints; joint_no++) {
dReal jangle = dJointGetHingeAngle(leg[segment][leg_no][joint_no].joint) - jointError[segment][leg_no][joint_no]; // hidding joint error for sensor
int currDirectionPositive;
if(jangle-lastJangle[segment][leg_no][joint_no] > 0.00001) currDirectionPositive =1; //we use a small positive number to ignore tinsy values
else currDirectionPositive =0;
//printf("jangle: %e, lastJangle_pneat[leg_no][joint_no]: %e, currDir: %i\n", jangle, lastJangle_pneat[leg_no][joint_no], currDirectionPositive);
if(printJointAngles){
ofstream jangle_file;
jangle_file.open ("jointAngles.txt", ios::app );
jangle_file << jangle << " ";
if(leg_no==num_legs-1 and joint_no==num_joints-1) jangle_file << endl;
jangle_file.close();
}
if(currDirectionPositive and !lastJangleDirectionPositve[segment][leg_no][joint_no]) numDirectionChanges++;
if(!currDirectionPositive and lastJangleDirectionPositve[segment][leg_no][joint_no]) numDirectionChanges++;
//if(visualize_pneat) printf("numDirectionChanges_pneat %i \n", numDirectionChanges_pneat);
lastJangleDirectionPositve[segment][leg_no][joint_no] =currDirectionPositive;
lastJangle[segment][leg_no][joint_no] = jangle;
int xInt =joint_no; //which node we will be setting or getting. x & y are within the sheet, z specifies which sheet (0=input, 1=hidden, 2=output)
int yInt =leg_no+(segment*2);
int zInt =0;
#if LEGSWING_ODE_DEBUG
printf("About to set input (x: %i,y: %i,z: %i) to joint angle: %f \n", xInt, yInt, zInt, jangle);
#endif
substrate_pointer->setValue(nameLookupGlobal[HCUBE::Node(xInt,yInt,zInt)],jangle);
//<start> if inputting lastDesiredAngle
//xInt = xInt+1; //bump it one, cause the last desired angle goes into the input to the right of the current angle for that joint
//#if LEGSWING_ODE_DEBUG
//printf("about to set input (x: %i,y: %i,z: %i) to last desired joint angle: %f \n", xInt, yInt, zInt, newDesiredAngle[leg_no][joint_no]);
//#endif
//substrate_pointer->setValue(
// nameLookupGlobal[HCUBE::Node(xInt,yInt,zInt)],
// newDesiredAngle[segment][leg_no][joint_no]
// );
}
//set whether the leg is touching the ground
int xInt = 3; //putting touch just to the right of the 4th legs lastAngle
int yInt = leg_no+(segment*2);
int zInt = 0;
substrate_pointer->setValue(nameLookupGlobal[HCUBE::Node(xInt,yInt,zInt)], thisLegIsTouching[segment][leg_no]);
}
#ifndef LEGSWING_ODE_DEBUG
assert(num_legs==4); //warning: only works with four legs
printf("Legs touching: FrontLeft (%i), BackLeft (%i), BackRight (%i), FrontRight (%i)\n", thisLegIsTouching[segment][0],thisLegIsTouching[segment][1], thisLegIsTouching[segment][2], thisLegIsTouching[segment][3]);
#endif
//printf("timeStepDivisor_pneat: %f\n", timeStepDivisor_pneat);
// use if evolving sine period
// if(fabs(timeStepDivisor_pneat<1)) sineOfTimeStep =0; //prevent divide by zero errors, and allow them to silence the sine wave
// else sineOfTimeStep = sin(dReal(timeSteps_pneat)/timeStepDivisor_pneat)*M_PI;
dReal sineOfTimeStep = sin(dReal(timeSteps)/50.0)*M_PI;
#ifdef USE_EXTRA_NODE
dReal negSineOfTimeStep = (-1.0 * sineOfTimeStep);
#endif
if(visualize and sineOfTimeStep>.9999*M_PI) printf("****************************************************\n");
if(visualize and sineOfTimeStep<-.9999*M_PI) printf("---------------------------------------------------\n");
//printf("sineOfTimeStep: %f\n", sineOfTimeStep);
#ifndef USE_NLEG_ROBOT
//<start> use to spread the PRYS horizontally
//insert the roll, pitch and yaw to the 7th-9th columns of the 1st row
//int yInt = 0;
//int zInt = 0;
//dReal insertValue;
//
//for(int q=0;q<3;q++){
// int xInt = 7+q;
// if (q==0) insertValue = roll;
// else if (q==1) insertValue = pitch;
// else insertValue = yaw;
// substrate_pointer->setValue(
// nameLookupGlobal[HCUBE::Node(xInt,yInt,zInt)],
// insertValue
// );
//}
//<ent> use to spread the PRYS horizontally
//<start> to spread PRYS vertically
//insert the roll, pitch and yaw to the 1st/0th-4th/3rd row of the 5th/4th columns
int xInt = 4;
int zInt = 0;
dReal insertValue;
for(int q=0;q<3;q++){
int yInt = q;
if (q==0) insertValue = roll;
else if (q==1) insertValue = pitch;
else insertValue = yaw;
substrate_pointer->setValue(nameLookupGlobal[HCUBE::Node(xInt,yInt,zInt)], insertValue);
}
//<end>
//insert a sine wave based on timeSteps
xInt = 4;
int yInt = 3;
zInt = 0;
substrate_pointer->setValue(nameLookupGlobal[HCUBE::Node(xInt,yInt,zInt)], sineOfTimeStep);
#else // USE_NLEG_ROBOT
//<start> to spread PRYS vertically
//insert the roll, pitch and yaw to the 1st/0th-4th/3rd row of the 5th/4th columns
int xInt = 4;
int zInt = 0;
dReal insertValue;
//roll
int yInt = segment*2;
substrate_pointer->setValue(nameLookupGlobal[HCUBE::Node(xInt,yInt,zInt)], roll);
//pitch
yInt = (segment*2)+1;
substrate_pointer->setValue(nameLookupGlobal[HCUBE::Node(xInt,yInt,zInt)], pitch);
//yaw
xInt = 5;
yInt = segment*2;
substrate_pointer->setValue(nameLookupGlobal[HCUBE::Node(xInt,yInt,zInt)], yaw);
//<end>
if(segment < 1)
{
//insert a sine wave based on timeSteps
yInt = segment*2+1;
substrate_pointer->setValue(nameLookupGlobal[HCUBE::Node(xInt,yInt,zInt)], sineOfTimeStep);
} else
{
dReal torsoAngle = dJointGetHingeAngle(torso[segment-1].joint); //TODO: does torso zero have a valid joint?
yInt = segment*2+1;
substrate_pointer->setValue(nameLookupGlobal[HCUBE::Node(xInt,yInt,zInt)], torsoAngle);
}
#endif
#if SUBSTRATE_DEBUG
cout << "State of the nodes pre-update\n";
for (int z=0;z<numSheets;z++)
{
cout << "Printing Layer " << z << "************************************************" << endl;
for (int y1=0;y1<numNodesYglobal;y1++)
{
for (int x1=0;x1<numNodesXglobal;x1++)
{
//cout << "(x,y,z): (" << x1 << "," << y1 << "," << z << "): " ;
cout << setw(12) << setiosflags(ios::right) << fixed <<
double(substrate_pointer->getValue(
nameLookupGlobal[HCUBE::Node(x1,y1,z)]
));
}
cout << endl;
}
}
cout << "Updating Substrate\n";
//CREATE_PAUSE(string("Line Number: ")+toString(__LINE__));
#endif
//have to update the network once for each layer of *links* (or NumLayers -1)
#if SUBSTRATE_DEBUG
for (int a=0;a< numSheets - 1 ;a++)
{
substrate_pointer->update(1);
cout << "State of the nodes post-update: " << a << endl;
for (int z=0;z<numSheets;z++)
{
cout << "Printing Layer " << z << endl;
for (int y1=0;y1<numNodesYglobal;y1++)
{
for (int x1=0;x1<numNodesXglobal;x1++)
{
//cout << "(x,y,z): (" << x1 << "," << y1 << "," << z << "): " ;
cout << double(substrate_pointer->getValue(nameLookupGlobal[HCUBE::Node(x1,y1,z)]));
cout << " ";
}
cout << endl;
}
}
}
#else
substrate_pointer->update(numSheets-1);
#endif
}
for(int segment = 0; segment < BODY_SEGMENTS; ++segment){
for (int leg_no = 0; leg_no < num_legs; leg_no++) {
for (int joint_no = 0; joint_no < num_joints; joint_no++) {
int xInt =joint_no*2; //which node we will be setting or getting. x & y are within the sheet, z specifies which sheet (0=input, 1=hidden, 2=output)
int yInt =leg_no;
int zInt = numSheets-1; //2 (or numSheets - 1) to get the output
newDesiredAngle[segment][leg_no][joint_no] = M_PI*double(substrate_pointer->getValue(nameLookupGlobal[HCUBE::Node(xInt,yInt,zInt)]));
#if LEGSWING_ODE_DEBUG
printf("newDesiredAngle is: %f\n", newDesiredAngle[segment][leg_no][joint_no]);
#endif
THETA[segment][leg_no][joint_no] = newDesiredAngle[segment][leg_no][joint_no] + jointError[segment][leg_no][joint_no]; // adding joint error
}
}
#ifdef USE_NLEG_ROBOT
if(segment < BODY_SEGMENTS-1) {
//get torso joint angle
int xInt = 5;
int yInt = segment*2+1;
int zInt = numSheets - 1;
torsoJointDesiredAngle[segment] = M_PI*double(substrate_pointer->getValue(nameLookupGlobal[HCUBE::Node(xInt,yInt,zInt)]));
}
#endif
//get the timeStepDivisor
// xInt =10; //which node we will be setting or getting. x & y are within the sheet, z specifies which sheet (0=input, 1=hidden, 2=output)
// yInt =1;
// zInt = 2; //2 to get the output
// timeStepDivisor = 100.0*double(substrate_pointer->getValue(
// nameLookupGlobal[HCUBE::Node(xInt,yInt,zInt)]));
//if(visualize)printf("timeStepDivisor!: %f\n", timeStepDivisor);
const dReal * torsoPosition = dBodyGetPosition(torso[segment].body);
if(fabs(torsoPosition[0]) > maxXdistanceFrom0[segment]){
maxXdistanceFrom0[segment] = fabs(torsoPosition[0]);
#ifndef LEGSWING_ODE_DEBUG
printf("new maxX::::::::::::::::::::::::::::::::::::::::::::::: %f\n", maxXdistanceFrom0);
#endif
}
if(fabs(torsoPosition[1]) > maxYdistanceFrom0[segment]){
maxYdistanceFrom0[segment] = fabs(torsoPosition[1]);
#ifndef LEGSWING_ODE_DEBUG
printf("new maxY:!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! %f\n", maxYdistanceFrom0);
#endif
}
currentXdistanceFrom0[segment] = fabs(torsoPosition[0]);
currentYdistanceFrom0[segment] = fabs(torsoPosition[1]);
}
Pcontrol();
for(int segment = 0; segment < BODY_SEGMENTS; ++segment) {
for(int z=0;z<num_legs;z++) thisLegIsTouching[segment][z]=0; //reset these flags before checking the new state of things
}
if(fitness_function == 2) {
for(int segment = 0; segment < BODY_SEGMENTS; ++segment) {
for (int i = 0; i < num_legs; i++) {
for (int j = 0; j < num_links; j++) {
totalForce[0] += dBodyGetForce(leg[segment][i][j].body)[0];
totalForce[1] += dBodyGetForce(leg[segment][i][j].body)[1];
totalForce[2] += dBodyGetForce(leg[segment][i][j].body)[2];
}
}
totalForce[0] += dBodyGetForce(torso[segment].body)[0];
totalForce[1] += dBodyGetForce(torso[segment].body)[1];
totalForce[2] += dBodyGetForce(torso[segment].body)[2];
}
}
}
void
Else::AcrobotArticulatedBody
::acquireJointAngleData( std::vector< double >& out )
{
out.push_back( dJointGetHingeAngle( myJoints[0] ) );
}
// 制御式
static void control() {
mytime += 0.0001;
/* // ロッドのマニュアル制御
dReal rod_q = dJointGetHingeAngle(rod_joint[1]);
dReal rod_input = 5.0 * (rod_q_d - rod_q);
dJointSetHingeParam(rod_joint[1], dParamVel, rod_input);
dJointSetHingeParam(rod_joint[1], dParamFMax, 10000.0);
*/
// 弾丸の位置を取得
const dReal *pos, *vel; // 弾丸の位置と速度ベクトル
dReal tar[3] = {0.0}; // 計算後の着弾予想点
dReal rot[12] = {1.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,1.0,0.0};
//const dReal *tar_pos, *tar_rot; // 着弾予想地点
dReal q_p_d = 0.0; // 広域レーダーからの目標角度
dReal q_c_d = 0.0; // 近接センサからの偏差
dReal distance = 0.0; // SHIELD中心から弾丸までの距離
dReal rod_q = dJointGetHingeAngle(rod_joint[1]); // RODの現在角度
dReal rod_input = 0.0; // ROD制御入力
pos = dBodyGetPosition(bullet.body);
vel = dBodyGetLinearVel(bullet.body);
//pos = dBodyGetRelPointPos(bullet.body);
//vel = dBodyGetRelPointVel(bullet.body);
distance = sqrt(
(SHIELD_X-pos[0])*(SHIELD_X-pos[0]) +
(SHIELD_Y-pos[1])*(SHIELD_Y-pos[1]) +
(SHIELD_Z-pos[2])*(SHIELD_Z-pos[2])
);
if(abs(vel[1])>0.1){
tar[0] = pos[0] + pos[1]*vel[0]/abs(vel[1]);
tar[1] = 0.0;
tar[2] = pos[2] + pos[1]*vel[2]/abs(vel[1]);
// 着弾予想点の描画
dsSetColor(1 ,0 ,0);
dsDrawSphere(tar, rot, BULLET_RADIUS+0.1);
// 広域レーダー
if(distance < 50.0){
q_p_d = 1.0 * atan(tar[0]/(tar[2]-SHIELD_Z));
}
// 近接センサ
if(distance < 10.0){
q_c_d = -1.0 * atan(pos[0]/pos[2]);
diff_sum += q_c_d - rod_q;
}
}
// 入力トルク（疑似付加目標角度）を計算
if(act_flg){ // スイッチがオンの場合
rod_input = 500.0 * (q_p_d - rod_q); // 通常制御のみ
//rod_input = 50.0 * (q_p_d - rod_q) + 0.001 * diff_sum;
rod_q_d = rod_q;
//FILE *fp_0; fp_0 = fopen("dataA.csv","a");
//fprintf(fp_0,"%lf,%3.9lf,%3.9lf,%3.9lf,%3.9lf,%3.9lf,%3.9lf,%3.9lf,%3.9lf,%3.9lf,%3.9lf,%3.9lf,%3.9lf\n", mytime, pos[0], pos[1], pos[2], vel[0], vel[1], vel[2], tar[0], tar[1], tar[2], q_p_d, q_c_d, rod_q);
//fclose(fp_0);
}else{
rod_input = 500.0 * (rod_q_d - rod_q); // マニュアル操作
}
//rod_input = 500.0 * (rod_q_d - rod_q); // マニュアル操作
printf("%lf : %lf -> %lf = %lf\r", mytime, rod_q_d, rod_q, rod_input);
//printf("%lf, %lf, %lf\r", pos[0], pos[1], pos[2] );
//printf("%lf, %lf, %lf\r", vel[0], vel[1], vel[2] );
//printf("%lf, %lf, %lf\r", rod_q_d*180.0/M_PI, rod_q*180.0/M_PI, rod_input);
// 制御入力
dJointSetHingeParam(rod_joint[1], dParamVel, rod_input);
dJointSetHingeParam(rod_joint[1], dParamFMax, 100000.0);
}
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;
}
void ServoMotor::PositionSensor::updateValue()
{
data.floatValue = dJointGetType(joint->joint) == dJointTypeHinge
? dJointGetHingeAngle(joint->joint)
: dJointGetSliderPosition(joint->joint);
}
Real32 PhysicsHingeJoint::getAngle(void)
{
return dJointGetHingeAngle(_JointID);
}
dReal ServoMotor::getPosition() {
return dJointGetHingeAngle(joint_->getJoint());
}
void PROPRIOCEPTIVE_SENSOR::Poll(dJointID joint, int t) {
const dReal *pos;
angles[t] = dJointGetHingeAngle(joint);
}
