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)); }
// Simulation loop void simLoop(int pause) { control(); dWorldStep(world, 0.02); const double* var; // Draw a robot dsSetColor(1.0,1.0,1.0); // Set color (r, g, b), In this case white is set for (int i = 0; i < NUM; i++ ){// Draw capsules for links dsDrawCapsuleD(var, var , l[i], r[i]); } }