int main(void) {
char c;
init_robot();
fflush(stdin);
printf("Usage: %c to exit, %c to move forward, %c to move backward, %c to rotate to left, %c to rotate to right!\n\n", EXIT_S, MF_S, MB_S, RL_S, RR_S);
print_world();
scanf("%c", &c);
while(c != EXIT_S) {
switch(c) {
case MF_S:
move_robot(MOVE_FWD);
print_world();
break;
case MB_S:
move_robot(MOVE_BWD);
print_world();
break;
case RL_S:
rotate_90_left();
print_world();
break;
case RR_S:
rotate_90_right();
print_world();
break;
}
scanf("%c", &c);
}
return 0;
}
// Initialise the software
TICK_COUNT appInitSoftware(TICK_COUNT loopStart){
init_robot();
init_error_list();
init_map();
init_rct_set();
uartAttach(UART3, &lbRcv);
return 0;
}
/*!
@brief Program entry point
*/
int main (void) {
struct MagnetInit magnetInit;
struct ServoInit boomServoInit, crowdServoInit, swingServoInit;
struct RobotInit robotInit;
magnetInit.periph = RCC_AHB1Periph_GPIOD;
magnetInit.GPIOx = GPIOD;
magnetInit.pin = GPIO_Pin_15;
boomServoInit.CCR = 1;
boomServoInit.maxPosition = BOOM_ANGLE_MAX;
boomServoInit.minPosition = BOOM_ANGLE_MIN;
crowdServoInit.CCR = 2;
crowdServoInit.maxPosition = CROWD_ANGLE_MAX;
crowdServoInit.minPosition = CROWD_ANGLE_MIN;
swingServoInit.CCR = 3;
swingServoInit.maxPosition = SWING_ANGLE_MAX;
swingServoInit.minPosition = SWING_ANGLE_MIN;
robotInit.boomServo = &boomServo;
robotInit.crowdServo = &crowdServo;
robotInit.swingServo = &swingServo;
osThreadId tid_armThread;
osThreadId tid_ledThread;
init_TIM4(1 / SERVO_DUTY_CYCLE_STEP, SERVO_FREQUENCY);
init_LEDS_PWM();
init_LEDS();
init_servo(&boomServo, &boomServoInit);
init_servo(&crowdServo, &crowdServoInit);
init_servo(&swingServo, &swingServoInit);
init_robot(&robot, &robotInit);
init_magnet(&magnet, &magnetInit);
init_receiver(&receiver);
tid_armThread = osThreadCreate(osThread(armThread), NULL);
tid_ledThread = osThreadCreate(osThread(ledThread), NULL);
osDelay(osWaitForever);
}
void main_init() {
int i;
float starting_speed = 0.0;
init_robot();
flag = 0;
robot_state = malloc(sizeof (float) *3);
for (i = 0; i < LEN_PIC_BUFFER; i++) {
set_vel_2_array(pic_buffer[i], starting_speed, starting_speed);
}
write(pic_fd, pic_message_reset_steps_acc, PACKET_TIMING_LENGTH + 1);
tcflush(pic_fd, TCOFLUSH);
sync();
steps_anomaly = 0;
#ifdef TEST_KALMAN
float v=0;
float w=0;
set_robot_speed(&v,&w);
#endif
#ifdef OB_AV
init_probstavoid_module();
#endif
#ifdef CARTESIAN_REGULATOR
viapoints[0][0]=38;
viapoints[0][1]=119;
viapoints[0][2]=0;
viapoints[1][0]=98;
viapoints[1][1]=161;
viapoints[1][2]=0;
viapoints[2][0]=187;
viapoints[2][1]=179;
viapoints[2][2]=0;
viapoints[3][0]=158;
viapoints[3][1]=238;
viapoints[3][2]=0;
viapoints[4][0]=187;
viapoints[4][1]=268;
viapoints[4][2]=0;
curr_via=0;
via_done=0;
#endif
#ifdef HOKUYO
init_urg_laser(&urg,ON_DEMAND);
#endif
#ifdef JOYSTICK
init_joystick();
#endif
#ifdef EKF_LOC
load_map("ekf_map.txt");
init_ekf();
#ifdef HOKUYO_SENSOR
for (i=0; i< NUM_SENS; i++){
ANGLE_IDX[i]=urg_rad2index(urg,ANGLE_H[i]);
printf("angle: %f \tidx: %d\n",ANGLE_H[i],urg_rad2index(urg,ANGLE_H[i]));
}
#endif
xpost.x=50;
xpost.y=45;
xpost.th=M_PI/2;
xpred.x=0;
xpred.y=0;
xpred.th=0;
#endif
}
int main(int argc, char** argv)
{
init_robot(&robot);
struct coord *destination;
destination = new_coord(0, 0);
destination->x = 0;
destination->y = 0;
pthread_mutex_init(&state.frame_lock, NULL);
pthread_cond_init(&state.has_frame, NULL);
robot.dest = destination;
gpioSetMode(4, PI_OUTPUT);
gpioSetMode(24, PI_OUTPUT);
gpioSetMode(5, PI_OUTPUT);
gpioSetMode(14, PI_OUTPUT);
//870 for at the ground,
// gpioServo(24,1380);
// gpioServo(4, 2100);
pthread_t range_thread, render_thread, cv_thread;
pthread_create(&range_thread, NULL, &range_read, &robot);
pthread_create(&render_thread, NULL, &render_task, NULL);
// pthread_create(&cv_thread, NULL, &find_circles, &state);
gpioWrite(5, 0);
gpioWrite(14, 0);
start_control_panel(&robot, &state);
pthread_t this_thread = pthread_self();
//set_thread_priority(this_thread, 0);
//set_thread_priority(robot.server->serve_thread, 1);
//set_thread_priority(range_thread, 2);
read(robot.encoder_handle, robot.sensor_data, sizeof(robot.sensor_data));
getQuaternion(&robot.position.orientation, robot.sensor_data);
robot.position.last_left = *((int *) (robot.sensor_data + 42));
robot.position.last_right = *((int *) (robot.sensor_data + 46));
struct robot_task *position_task, *point_task, *remote_task,
*spin_task, *move_task, *adjust_task, *stand_up_task, *fly_task,
*avoid_task, *stay_task;
position_task = (struct robot_task *)malloc(sizeof(*position_task));
position_task->task_func = &update_position2;
fly_task = (struct robot_task *)malloc(sizeof(*fly_task));
fly_task->task_func = &buzz;
point_task = (struct robot_task *)malloc(sizeof(*point_task));
robot.turret.target.x = 500;
robot.turret.target.y = 0;
robot.turret.target.z = 0;
point_task->task_func = &pointer;
remote_task = (struct robot_task *)malloc(sizeof(*remote_task));
remote_task->task_func = &remote;
avoid_task = (struct robot_task *)malloc(sizeof(*avoid_task));
avoid_task->task_func = &stay_away;
stay_task = (struct robot_task *)malloc(sizeof(*stay_task));
stay_task->task_func = &stay_within;
move_task = (struct robot_task *)malloc(sizeof(*move_task));
move_task->task_func = &move;
struct waypoint the_point;
the_point.point.x = 1000;
the_point.point.y = 0;
the_point.point.z = -40;
INIT_LIST_HEAD(&the_point.list_entry);
// list_add(&the_point.list_entry, &robot.waypoints);
adjust_task = (struct robot_task *)malloc(sizeof(*adjust_task));
adjust_task->task_func = &adjust_speeds;
stand_up_task = (struct robot_task *)malloc(sizeof(*stand_up_task));
stand_up_task->task_func = &stand_up;
INIT_LIST_HEAD(&position_task->list_item);
INIT_LIST_HEAD(&fly_task->list_item);
INIT_LIST_HEAD(&point_task->list_item);
INIT_LIST_HEAD(&remote_task->list_item);
INIT_LIST_HEAD(&avoid_task->list_item);
INIT_LIST_HEAD(&stay_task->list_item);
INIT_LIST_HEAD(&move_task->list_item);
INIT_LIST_HEAD(&adjust_task->list_item);
INIT_LIST_HEAD(&stand_up_task->list_item);
list_add_tail(&position_task->list_item, &robot.task_list);
// list_add_tail(&fly_task->list_item, &robot.task_list);
list_add_tail(&point_task->list_item, &robot.task_list);
list_add_tail(&remote_task->list_item, &robot.task_list);
// list_add_tail(&avoid_task->list_item, &robot.task_list);
// list_add_tail(&stay_task->list_item, &robot.task_list);
list_add_tail(&move_task->list_item, &robot.task_list);
list_add_tail(&adjust_task->list_item, &robot.task_list);
list_add_tail(&stand_up_task->list_item, &robot.task_list);
get_sensor_data(&robot.position2, robot.sensor_data);
init_position(&robot.position2);
//Variables for Kalman filter
struct matrix_2x2 A, B, H, Q, R, current_prob_estimate;
float current_state_estimate[2];
current_state_estimate[0] = 0;
current_state_estimate[1] = 0;
A.elements[0][0] = 1;
A.elements[0][1] = 0;
A.elements[1][0] = 0;
A.elements[1][1] = 1;
Q.elements[0][0] = 0.01;
Q.elements[0][1] = 0;
Q.elements[1][0] = 0;
Q.elements[1][1] = 0.01;
R.elements[0][0] = 0.4;
R.elements[0][1] = 0;
R.elements[1][0] = 0;
R.elements[1][1] = 0.4;
current_prob_estimate.elements[0][0] = 1;
current_prob_estimate.elements[0][1] = 0;
current_prob_estimate.elements[1][0] = 0;
current_prob_estimate.elements[1][1] = 1;
static int look_wait = 0;
while(1)
{
read(robot.encoder_handle, robot.sensor_data, sizeof(robot.sensor_data));
get_sensor_data(&robot.position2, robot.sensor_data);
// fprintf(stderr, "%i, %i\n", robot.position2.left, robot.position2.right);
if (!((robot.position2.orientation.x == robot.position2.orientation.y) &&
(robot.position2.orientation.x == robot.position2.orientation.z) &&
(robot.position2.orientation.x == robot.position2.orientation.w)))
{
do_robot_tasks(&robot);
}
state.roll = robot.turret.roll * 180 / M_PI;
static struct vect sightings[3];
int has_dest = list_empty(&robot.waypoints);
int is_spinning = list_empty(&fly_task->list_item);
// fprintf(stderr, "%i, %i\n", has_dest, is_spinning);
//Prediction Step
float predicted_state_estimate[2];
mat_mult_2xv(predicted_state_estimate, &A, current_state_estimate);
struct matrix_2x2 predicted_prob_estimate;
mat_add_2x2(&predicted_prob_estimate, & current_prob_estimate, &Q);
if (!isnan(state.x_pos) && state.set_target >= 1)
{
float x_in_vid = (state.x_pos - .5) * -2;
float y_in_vid = (state.y_pos - .5) * -2;
// Get obervation.
screen_to_world_vect(&robot, &sightings[0], x_in_vid, y_in_vid);
float observation[2];
observation[0] = sightings[0].x;
observation[1] = sightings[0].y;
float innovation[2];
innovation[0] = observation[0] - predicted_state_estimate[0];
innovation[1] = observation[1] - predicted_state_estimate[1];
float innovation_magnitude = sqrtf((innovation[0] * innovation[0]) + (innovation [1] * innovation[1]));
//fprintf(stderr, "%f, %f, %f, %f, %f, %f, %f, %f\n", innovation_magnitude, robot.turret.yaw, robot.turret.pitch, x_in_vid, y_in_vid, sightings[0].x, sightings[0].y, sightings[0].z);
struct matrix_2x2 innovation_covariance;
mat_add_2x2(&innovation_covariance, &predicted_prob_estimate, &R);
// Update
struct matrix_2x2 kalman_gain, inv_innovation_covariance;
mat_inverse_2x2(&inv_innovation_covariance, &innovation_covariance);
mat_mult_2x2(&kalman_gain, &predicted_prob_estimate, &inv_innovation_covariance);
float step[2];
mat_mult_2xv(step, &kalman_gain, innovation);
current_state_estimate[0] = step[0] + predicted_state_estimate[0];
current_state_estimate[1] = step[1] + predicted_state_estimate[1];
struct matrix_2x2 intermediate;
intermediate.elements[0][0] = 1 - kalman_gain.elements[0][0];
intermediate.elements[0][1] = 0 - kalman_gain.elements[0][1];
intermediate.elements[1][0] = 0 - kalman_gain.elements[1][0];
intermediate.elements[1][1] = 1 - kalman_gain.elements[1][1];
mat_mult_2x2(¤t_prob_estimate, &intermediate, &predicted_prob_estimate);
//fprintf(stderr, "%f, %f\n", current_state_estimate[0], current_state_estimate[1]);
if (innovation_magnitude < 800)
{
look_wait = 300;
list_del_init(&fly_task->list_item);
if (innovation_magnitude < 100)
{
if (list_empty(&robot.waypoints))
{
list_add(&the_point.list_entry, &robot.waypoints);
}
robot.turret.target.x = current_state_estimate[0];
robot.turret.target.y = current_state_estimate[1];
robot.turret.target.z = -40;
the_point.point.x = current_state_estimate[0];
the_point.point.y = current_state_estimate[1];
the_point.point.z = -40;
//fprintf(stderr, "time to stop spinning\n");
}
//fprintf(stderr, "%f, %f, %f\n", current_state_estimate[0], current_state_estimate[1], innovation_magnitude);
}
/* if (fabs(state.x_pos) > .25 || fabs(state.y_pos) > .25)
{
robot.turret.target = temp;
the_point.point = temp;
}
else
{
the_point.point = temp;
}
if (list_empty(&robot.waypoints))
{
list_del_init(&fly_task->list_item);
list_add_tail(&the_point.list_entry, &robot.waypoints);
}*/
// fprintf(stderr, "%f, %f\n", state.x_pos, state.y_pos);
// fprintf(stderr, "%f, %f, %f\n", temp.x, temp.y, temp.z);
state.set_target = 0;
}
if (look_wait > 0)
{
look_wait--;
}
if (list_empty(&robot.waypoints) && list_empty(&fly_task->list_item) && look_wait == 0)
{
list_add_tail(&fly_task->list_item, &robot.task_list);
// fprintf(stderr, "I should spin\n");
}
// fprintf(stderr, "%i, %i\n", robot.position2.left, robot.position2.right);
float distance = get_expected_distance(&robot);
// fprintf(stderr, "%f, %f\n", distance, robot.range);
// fprintf(stderr, "%f, %f, %f\n", robot.turret.target.x, robot.turret.target.y, robot.turret.target.z);
// screen_to_world_vect(&robot, &robot.turret.target, 0, 0);
// fprintf(stderr, "%f, %f, %f\n", robot.turret.target.x, robot.turret.target.y, robot.turret.target.z);
struct timeval now, then;
// printf("%f, %f, %f, %f, %f, %f\n", robot.position2.position.x, robot.position2.position.y, robot.turret.target.x, robot.turret.target.y, robot.dest->x, robot.dest->y);
gettimeofday(&now, NULL);
/* fprintf(stderr, "%ld.%06ld, %f, %f, %f, %f, %i, %i, %f\n", now.tv_sec, now.tv_usec,
robot.position2.orientation.x, robot.position2.orientation.y,
robot.position2.orientation.z, robot.position2.orientation.w,
robot.position2.left, robot.position2.right, robot.position2.heading);
*/
//printf("%f\n", robot.range);
//printf("%f, %f, %f\n", robot.position2.position.x, robot.position2.position.y, robot.position2.tilt);
// fprintf(stderr, "%f, %f, %f, %i, %i\n", robot.position2.position.x, robot.position2.position.y, robot.position2.heading, robot.position2.left, robot.position2.right);
}
}
int main(void)
{
init_robot();
main_program_loop(user_program);
return 0;
}
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;
}
