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]);
}
}
