Re: Problem with Judoka1 Reset Angle

/*****************************************************************************/
/* File:         judo_reset_supervisor.c                                     */
/* Version:      1.0                                                         */
/* Date:         10-Oct-03                                                   */
/* Description:  Supervisor for the Robot Judo Contest                       */
/* Author:       [email protected]                              */
/*                                                                           */
/* Copyright (c) 2003 Cyberbotics - www.cyberbotics.com                      */
/*****************************************************************************/

#include <device/robot.h>
#include <device/supervisor.h>
#include <device/receiver.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#define ROBOTS 2  /* number of robots */
#define SERVOS 22 /* number of servos in each robot */

static int verbose=1;
static NodeRef robot[ROBOTS];
static NodeRef servo[ROBOTS][SERVOS];
static float   robot_initial_position[ROBOTS][7];
static float   servo_initial_position[ROBOTS][SERVOS][7];
static float   robot_current_position[ROBOTS][7];
static float   servo_current_position[ROBOTS][SERVOS][7];
static char   *servo_name[SERVOS]={
  "back_1","back_2","left_hip_1","left_hip_2","left_hip_3","left_knee",
  "left_ankle_1","left_ankle_2","left_shoulder_1","left_shoulder_2",
  "left_elbow","neck","neck_tilt","right_hip_1","right_hip_2","right_hip_3",
  "right_knee","right_ankle_1","right_ankle_2","right_shoulder_1",
  "right_shoulder_2","right_elbow"};
static DeviceTag receiver;

void reset() {
  receiver=robot_get_device("receiver");
}

int main() {
  int k,l,m;
  char name[32];
  const char *text;
  float x,z,alpha;

  robot_live(reset);
  receiver_enable(receiver,64);
  for(k=0;k<ROBOTS;k++) {
    sprintf(name,"JUDOKA_%d",k);
    robot[k] = supervisor_node_get_from_def(name);
    supervisor_field_get(robot[k],
			 SUPERVISOR_FIELD_TRANSLATION_AND_ROTATION,
			 (void *)robot_current_position[k],128);
    for(l=0;l<SERVOS;l++) {
      sprintf(name,"%s_%d",servo_name[l],k);
      servo[k][l]=supervisor_node_get_from_def(name);
      supervisor_field_get(servo[k][l],
			   SUPERVISOR_FIELD_TRANSLATION_AND_ROTATION,
			   (void *)servo_current_position[k][l],128);
    }
  }
  robot_step(128); /* get and save initial positions of robots and servos */
  for(k=0;k<ROBOTS;k++) {
    if (verbose>1) printf("SUPERVISOR robot %d: T:%g,%g,%g R:%g,%g,%g,%g\n",k,
			  robot_current_position[k][0],
			  robot_current_position[k][1],
			  robot_current_position[k][2],
			  robot_current_position[k][3],
			  robot_current_position[k][4],
			  robot_current_position[k][5],
			  robot_current_position[k][6]);
    for(m=0;m<7;m++)
     robot_initial_position[k][m]=robot_current_position[k][m];
    for(l=0;l<SERVOS;l++) {
      if (!supervisor_node_was_found(servo[k][l]))
       fprintf(stderr,"SUPERVISOR: servo %s_%d not found\n",servo_name[l],k);
      if (verbose>1) printf("SUPERVISOR  servo %s: T:%g,%g,%g R:%g,%g,%g,%g\n",
			    servo_name[l],
			    servo_current_position[k][l][0],
			    servo_current_position[k][l][1],
			    servo_current_position[k][l][2],
			    servo_current_position[k][l][3],
			    servo_current_position[k][l][4],
			    servo_current_position[k][l][5],
			    servo_current_position[k][l][6]);
      for(m=0;m<7;m++)
       servo_initial_position[k][l][m]=servo_current_position[k][l][m];
      // we have to correct the rotation angle which may have changed due
      // to physics simulation before the supervisor process starts
      // (bug reported by Mike Jost)
      servo_initial_position[k][l][3]=0;
      servo_initial_position[k][l][4]=1;
      servo_initial_position[k][l][5]=0;
    }
  }
  for(;;) { /* never ending loop */
    l = receiver_get_buffer_size(receiver);
    if (l) {
      text = receiver_get_buffer(receiver);
      if (strncmp(text,"stand me up ",12)==0) {
	if (sscanf(text,"stand me up %d %f %f %f",&k,&x,&z,&alpha)==4) {
	  robot_initial_position[k][0]=x;
	  robot_initial_position[k][2]=z;
	  robot_initial_position[k][6]=alpha;
	}
	supervisor_field_set(robot[k],
			     SUPERVISOR_FIELD_TRANSLATION_AND_ROTATION,
			     robot_initial_position[k]);
	for(l=0;l<SERVOS;l++)
	  supervisor_field_set(servo[k][l],
			       SUPERVISOR_FIELD_TRANSLATION_AND_ROTATION,
			       servo_initial_position[k][l]);
      }
    }
    robot_step(64);
  }
  return 0;
}