GLvoid draw_scene(GLvoid) {
u_int i;
float rotfv[3];
rotfv[0] = -rot_x * 3.14159 / 180;
rotfv[1] = -rot_y * 3.14159 / 180;
rotfv[2] = -rot_z * 3.14159 / 180;
v3_del(gr);
gr = v3_copy(grav_vector);
v3_rot(gr, rotfv);
glTranslatef(0.0f, 0.0f, -20.0f);
gl_v3(gr);
glRotatef(rot_x, 1.0f, 0.0f, 0.0f);
glRotatef(rot_y, 0.0f, 1.0f, 0.0f);
glRotatef(rot_z, 0.0f, 0.0f, 1.0f);
glScalef(zoom, zoom, zoom);
glutWireCube(2 * B);
for (i = 0; i < PARTICLES; i++) {
glPushMatrix();
gl_translate_v3(ps[i].c);
glutSolidSphere(RADIUS, 15, 15);
gl_v3(ps[i].v);
glPopMatrix();
}
}
GLvoid init_scene(GLvoid) {
u_int i;
// u_int d;
grav_vector = v3_new(0,-g,0);
gr = v3_copy(grav_vector);
glutTimerFunc(100, update_world, 0);
view_main = view_new(0,
VIEW_NONE,
GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT,
0, 0,
1, 1);
view_main->draw_func = draw_scene;
for (i = 0; i < PARTICLES; i++) {
ps[i].c = v3_new(2 * B * frandom() - B,
2 * B * frandom() - B,
2 * B * frandom() - B);
ps[i].v = v3_new((frandom() * 2 - 1),
(frandom() * 2 - 1),
(frandom() * 2 - 1));
// ps[i].v = v3_new(0,0,0);
// ps[i].c = v3_new(0,//frandom() * 0.001 - 0.0005,
// B - RADIUS - i * RADIUS * 2 * 1.01,
// 0);//frandom() * 0.001 - 0.0005);
//
}
/*
ps[0].v = v3_new(3,0,0);
ps[1].v = v3_new(-3,0,0);
ps[0].c = v3_new(-B + RADIUS, -B * 0 + RADIUS, 0);
ps[1].c = v3_new(B - RADIUS, -B * 0 + RADIUS, 0);
*/
timer_frames = timer_new();
timer_bounce = timer_new();
mutex_frames = 0;
timer_start(timer_frames);
timer_start(timer_bounce);
}
GLvoid sample_world() {
// int timing;
u_int i,j,d;
// float t0,t1,t2;
v3* temp;
v3* disp;
// v3* disp2;
float dot;
float dm;
char* temp_s1;
char* temp_s2;
for (i = 0; i < PARTICLES; i++) {
// printf("adding g %d,%f\n", i,ps[i].v[1]);
v3_add(ps[i].v, gr);
// ps[i].v->v[1] -= g;
v3_mult(ps[i].v, LOSS_V);
for (d = 0; d < D; d++) {
v3_mult_add(ps[i].c, ps[i].v, 1 / (float) SPS); // ps[i].c[d] += ps[i].v[d] / (float) SPS;
if (ps[i].c->v[d] < -B + RADIUS) {
disp = v3_new(0,0,0);
disp->v[d] = ps[i].c->v[d] + B;
dm = 1 / (1 - (RADIUS - v3_mag(disp)) / RADIUS);
v3_unit(disp);
v3_mult_add(ps[i].v, disp, dm * LOSS_WALL * dt);
// ps[i].c->v[d] = -B + RADIUS;
// ps[i].v->v[d] *= -1 * LOSS_WALL;
}
if (ps[i].c->v[d] > B - RADIUS) {
disp = v3_new(0,0,0);
disp->v[d] = ps[i].c->v[d] - B;
// dm = (RADIUS - v3_mag(disp)) / RADIUS;
dm = 1 / (1 - (RADIUS - v3_mag(disp)) / RADIUS);
v3_unit(disp);
v3_mult_add(ps[i].v, disp, dm * LOSS_WALL * dt);
// ps[i].c->v[d] = B - RADIUS;
// ps[i].v->v[d] *= -1 * LOSS_WALL;
}
}
}
for (i = 0; i < PARTICLES; i++) {
for (j = 0; j < PARTICLES; j++) {
if (i == j) continue;
if (v3_dist(ps[i].c, ps[j].c) >= RADIUS * 2) continue;
/* temp_s1 = v3_str(ps[i].c);
temp_s2 = v3_str(ps[j].c);
printf("collision pos: %s %s\n", temp_s1, temp_s2);
free(temp_s1);
free(temp_s2);*/
/*
temp_s1 = v3_str(ps[i].v);
temp_s2 = v3_str(ps[j].v);
printf("collision vel: %s %s\n", temp_s1, temp_s2);
free(temp_s1);
free(temp_s2);
*/
temp = v3_copy(ps[i].v);
v3_sub(temp, ps[j].v);
/*
temp_s1 = v3_str(temp);
printf("temp = %s\n", temp_s1);
free(temp_s1);
*/
disp = v3_copy(ps[i].c);
v3_sub(disp, ps[j].c);
// disp2 = v3_copy(disp);
dm = RADIUS - v3_mag(disp) / 2;
/*
temp_s1 = v3_str(disp);
printf("disp = %s\n", temp_s1);
free(temp_s1);
*/
v3_unit(disp);
/*
temp_s1 = v3_str(disp);
printf("disp unit = %s\n", temp_s1);
free(temp_s1);
*/
dot = v3_dot(temp, disp);
/*
printf("dot = %f\n", dot);
*/
v3_mult_add(ps[i].v, disp, dm*LOSS_COL*dt);
v3_mult_sub(ps[j].v, disp, dm*LOSS_COL*dt);
// v3_mult_add(ps[i].c, disp, dm);
// v3_mult_sub(ps[j].c, disp, dm);
v3_del(disp);
v3_del(temp);
}
}
}
void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[] )
{
if(nrhs!=3) {
mexErrMsgIdAndTxt("kindyn:nrhs","3 inputs required: angles[28], anglesd[28], anglesdd[28] ");
}
int j, i;
KIN_DYN r[ N_LINKS ];
KIN_DYN lleg[ LLEG_LINKS ];
KIN_DYN rleg[ RLEG_LINKS ];
double q[4];
double g_vector[3] = { 0.0, 0.0, 9.81 };
double root_force_w[3];
double root_torque_w[3];
double v1[3];
double w[3];
init_kin_dyn();
init_robot( r );
init_lleg( lleg );
init_rleg( rleg );
double *input_angle;
double *input_angled;
double *input_angledd;
input_angle = mxGetPr(prhs[0]);
input_angled = mxGetPr(prhs[1]);
input_angledd = mxGetPr(prhs[2]);
/*
double *pelvis_pos;
double *pelvis_quat;
pelvis_pos = mxGetPr(prhs[3]);
pelvis_quat = mxGetPr(prhs[4]);
*/
for ( i = 0; i < 3; i++ )
{
r[0].position_w[i] = 0;
r[0].angular_velocity_b[i] = 0;
r[0].angular_acceleration_b[i] = 0;
r[0].joint_acceleration_b[i] = 0;
r[0].link_acceleration_b[i] = 0;
}
// default state for rest of robot
for ( i = 0; i < N_LINKS; i++ )
{
r[i].angle = input_angle[i];
r[i].angled = input_angled[i];
r[i].angledd = input_angledd[i];
}
// set up ground joint
//r[0].position_w[XX] = -0.0484;
//r[0].position_w[YY] = 0;
//r[0].position_w[ZZ] = 0.9271;
// Put left foot on the ground and go up to find the pelvis
lleg[0].angle = 0;
lleg[0].angle = 0;
lleg[0].sine = 0;
lleg[0].cosine = 1;
m3_identity( lleg[0].orientation );
lleg[0].angled = 0;
lleg[0].angledd = 0;
lleg[0].position_w[XX] = 0;
lleg[0].position_w[YY] = 0.089;
lleg[0].position_w[ZZ] = 0;
for(i=1; i < LLEG_JOINTS; i++){
lleg[i].angle = r[11-i].angle;
lleg[i].angled = r[11-i].angled;
lleg[i].angled = r[11-i].angled;
lleg[i].sine = sin( lleg[i].angle );
lleg[i].cosine = cos( lleg[i].angle );
}
//FK
forward_kinematics(lleg, LLEG_LINKS);
double pelvis_offset[3];
pelvis_offset[0] = 0;
pelvis_offset[1] = -0.089;
pelvis_offset[2] = 0;
mv3_multiply(lleg[6].orientation,pelvis_offset,r[0].position_w);
for(i=0;i<3;i++)
r[0].position_w[i] += lleg[6].position_w[i];
for(i=0;i<3;i++)
for(j=0;j<3;j++)
r[0].orientation[i][j] = lleg[6].orientation[i][j];
r[0].angle = 0;
r[0].sine = 0;
r[0].cosine = 1;
for ( i = 1; i < N_LINKS; i++ ){
r[i].sine = sin( r[i].angle );
r[i].cosine = cos( r[i].angle );
}
forward_kinematics( r, N_LINKS );
//printf( "COM\n" );
double total_com[3];
double total_mass;
total_com[XX] = 0; total_com[YY] = 0; total_com[ZZ] = 0;
total_mass = 0;
for ( i = 0; i < N_LINKS; i++ ){
//printf( "%d: %g %g %g\n", i, r[i].com_w[XX], r[i].com_w[YY],r[i].com_w[ZZ] );
total_com[XX] += (r[i].mass*r[i].com_w[XX]);
total_com[YY] += (r[i].mass*r[i].com_w[YY]);
total_com[ZZ] += (r[i].mass*r[i].com_w[ZZ]);
total_mass += r[i].mass;
}
total_com[XX] = total_com[XX]/total_mass;
total_com[YY] = total_com[YY]/total_mass;
total_com[ZZ] = total_com[ZZ]/total_mass;
/*
for ( i = 0; i < N_LINKS; i++ )
{
printf( "%d: %g %g %g\n", i, r[i].orientation[0][0],
r[i].orientation[0][1], r[i].orientation[0][2] );
printf( " %g %g %g\n", r[i].orientation[1][0],
r[i].orientation[1][1], r[i].orientation[1][2] );
printf( " %g %g %g\n", r[i].orientation[2][0],
r[i].orientation[2][1], r[i].orientation[2][2] );
}
*/
// implement gravity
mtv3_multiply( r[0].orientation, g_vector, r[0].joint_acceleration_b );
v3_copy( r[0].joint_acceleration_b, r[0].link_acceleration_b );
inverse_dynamics( r );
/*
printf( "Angular Velocity\n" );
for ( i = 0; i < N_LINKS; i++ )
{
v3_copy( r[i].angular_velocity_b, v1 );
// mv3_multiply( r[i].orientation, r[i].angular_velocity_b, v1 );
printf( "%d: %g %g %g\n", i, v1[XX], v1[YY], v1[ZZ] );
}
printf( "Angular Acceleration\n" );
for ( i = 0; i < N_LINKS; i++ )
{
v3_copy( r[i].angular_acceleration_b, v1 );
// mv3_multiply( r[i].orientation, r[i].angular_acceleration_b, v1 );
printf( "%d: %g %g %g\n", i, v1[XX], v1[YY], v1[ZZ] );
}
*/
mv3_multiply( r[0].orientation, r[0].joint_force_b, root_force_w );
mv3_multiply( r[0].orientation, r[0].joint_torque_b, root_torque_w );
/*
printf( "joint_force: %g %g %g\n", r[0].joint_force_b[0], r[0].joint_force_b[1],
r[0].joint_force_b[2] );
printf( "joint torque: %g %g %g\n", r[0].joint_torque_b[0], r[0].joint_torque_b[1],
r[0].joint_torque_b[2] );
printf( "root_force: %g %g %g\n", root_force_w[0], root_force_w[1],
root_force_w[2] );
printf( "root torque: %g %g %g\n", root_torque_w[0], root_torque_w[1],
root_torque_w[2] );
*/
/*
printf( "Torques\n" );
for ( j = 1; j < N_JOINTS; j++ )
printf( "%d: %g\n", j, r[j].the_joint_torque );
*/
// Each leg inv dyn w/ added pelvis forces
//r/l leg:
//0 = ground
//6 = l_leg_uhz
//rleg:
//11 = r_leg_uhz
//16 = r_leg_lax
//body:
//5 = l_leg_uhz
//10 = l_leg_lax
lleg[0].angle = 0;
lleg[0].angle = 0;
lleg[0].sine = 0;
lleg[0].cosine = 1;
m3_identity( lleg[0].orientation );
lleg[0].angled = 0;
lleg[0].angledd = 0;
lleg[0].position_w[YY] = 0.089;
for(i=0;i<3;i++){
lleg[0].angular_velocity_b[i] = 0.0;
lleg[0].angular_acceleration_b[i] = 0.0;
lleg[0].joint_acceleration_b[i] = 0.0;
lleg[0].link_acceleration_b[i] = 0.0;
}
for(i=1; i < LLEG_JOINTS; i++){
lleg[i].angle = r[11-i].angle;
lleg[i].angled = r[11-i].angled;
lleg[i].angled = r[11-i].angled;
lleg[i].sine = sin( lleg[i].angle );
lleg[i].cosine = cos( lleg[i].angle );
}
rleg[0].angle = 0;
rleg[0].angle = 0;
rleg[0].sine = 0;
rleg[0].cosine = 1;
m3_identity( rleg[0].orientation );
rleg[0].angled = 0;
rleg[0].angledd = 0;
rleg[0].position_w[YY] = -0.089;
for(i=0;i<3;i++){
rleg[0].angular_velocity_b[i] = 0.0;
rleg[0].angular_acceleration_b[i] = 0.0;
rleg[0].joint_acceleration_b[i] = 0.0;
rleg[0].link_acceleration_b[i] = 0.0;
}
for(i=1; i < RLEG_JOINTS; i++){
rleg[i].angle = r[17-i].angle;
rleg[i].angled = r[17-i].angled;
rleg[i].angled = r[17-i].angled;
rleg[i].sine = sin( rleg[i].angle );
rleg[i].cosine = cos( rleg[i].angle );
}
//FK
forward_kinematics(lleg, LLEG_LINKS);
forward_kinematics(rleg, RLEG_LINKS);
/*
printf("\nlleg[6].position:\n %f %f %f\n\n",lleg[6].position_w[0],lleg[6].position_w[1],lleg[6].position_w[2]);
printf("lleg[6].orientation:\n%f %f %f\n%f %f %f\n%f %f %f\n",lleg[6].orientation[0][0],lleg[6].orientation[0][1],lleg[6].orientation[0][2],lleg[6].orientation[1][0],lleg[6].orientation[1][1],lleg[6].orientation[1][2],lleg[6].orientation[2][0],lleg[6].orientation[2][1],lleg[6].orientation[2][2]);
printf("\nrleg[6].position:\n %f %f %f\n\n",rleg[6].position_w[0],rleg[6].position_w[1],rleg[6].position_w[2]);
printf("rleg[6].orientation:\n%f %f %f\n%f %f %f\n%f %f %f\n\n",rleg[6].orientation[0][0],rleg[6].orientation[0][1],rleg[6].orientation[0][2],rleg[6].orientation[1][0],rleg[6].orientation[1][1],rleg[6].orientation[1][2],rleg[6].orientation[2][0],rleg[6].orientation[2][1],rleg[6].orientation[2][2]);
*/
// put force on the pelvis - for now we'll just try an equal split
// no gravity because these legs are massless
double split_force[3];
double split_torque[3];
for (i=0;i<3;i++){
split_force[i] = root_force_w[i]/2.0;
split_torque[i] = root_torque_w[i]/2.0;
}
lleg[0].link_acceleration_b[ZZ] = 9.81;
lleg[0].joint_acceleration_b[ZZ] = 9.81;
rleg[0].link_acceleration_b[ZZ] = 9.81;
rleg[0].joint_acceleration_b[ZZ] = 9.81;
lleg_id(lleg,split_force,split_torque);
rleg_id(rleg,split_force,split_torque);
for(i=1; i < RLEG_JOINTS; i++){
r[17-i].the_joint_torque = rleg[i].the_joint_torque;
}
for(i=1; i < LLEG_JOINTS; i++){
r[11-i].the_joint_torque = lleg[i].the_joint_torque;
}
double *output_torques;
plhs[0] = mxCreateDoubleMatrix(1,N_LINKS,mxREAL);
output_torques = mxGetPr(plhs[0]);
for (i=0; i<N_LINKS;i++){
output_torques[i] = r[i].the_joint_torque;
}
double *output_pos;
plhs[1] = mxCreateDoubleMatrix(3,N_LINKS,mxREAL);
output_pos = mxGetPr(plhs[1]);
for (i=0; i<N_LINKS;i++){
for (j=0; j<3; j++){
output_pos[(3*i)+j] = r[i].position_w[j];
}
}
double *output_l_force;
plhs[2] = mxCreateDoubleMatrix(1,3,mxREAL);
output_l_force = mxGetPr(plhs[2]);
mv3_multiply( lleg[0].orientation, lleg[0].joint_force_b, output_l_force );
double *output_l_torque;
plhs[3] = mxCreateDoubleMatrix(1,3,mxREAL);
output_l_torque = mxGetPr(plhs[3]);
mv3_multiply( lleg[0].orientation, lleg[0].joint_torque_b, output_l_torque );
double *output_r_force;
plhs[4] = mxCreateDoubleMatrix(1,3,mxREAL);
output_r_force = mxGetPr(plhs[4]);
mv3_multiply( rleg[0].orientation, rleg[0].joint_force_b, output_r_force );
double *output_r_torque;
plhs[5] = mxCreateDoubleMatrix(1,3,mxREAL);
output_r_torque = mxGetPr(plhs[5]);
mv3_multiply( rleg[0].orientation, rleg[0].joint_torque_b, output_r_torque );
/*
double *l_torques;
plhs[6] = mxCreateDoubleMatrix(1,7,mxREAL);
l_torques = mxGetPr(plhs[6]);
for (i=0; i < LLEG_JOINTS ;i++){
l_torques[i] = lleg[i].the_joint_torque;
}
double *r_torques;
plhs[7] = mxCreateDoubleMatrix(1,7,mxREAL);
r_torques = mxGetPr(plhs[7]);
for (i=0; i < RLEG_JOINTS ;i++){
r_torques[i] = rleg[i].the_joint_torque;
}
*/
double *r_orientation;
plhs[6] = mxCreateDoubleMatrix(3,3,mxREAL);
r_orientation = mxGetPr(plhs[6]);
for (i=0;i<3;i++){
for(j=0;j<3;j++){
r_orientation[(3*i)+j] = r[16].orientation[i][j];
}
}
double *l_orientation;
plhs[7] = mxCreateDoubleMatrix(3,3,mxREAL);
l_orientation = mxGetPr(plhs[7]);
for (i=0;i<3;i++){
for(j=0;j<3;j++){
l_orientation[(3*i)+j] = r[10].orientation[i][j];
}
}
double *com_ret;
plhs[8] = mxCreateDoubleMatrix(1,3,mxREAL);
com_ret = mxGetPr(plhs[8]);
for(j=0;j<3;j++){
com_ret[j] = total_com[j];
}
// return 0;
}
