sonyvid30.cc

机器人人3D仿真工具,可以加入到Simbad仿真环境下应用。

  mass=0.4; //kg

  baseRadius = 0.071; //meters
  baseHeight = 0.028; //meters

  uprightLength = 0.057; //meters
  uprightWidth = 0.105; //meters
  uprightHeight = 0.025; //meters

  headLength = 0.120; //meters
  headWidth = 0.120; //meters
  headHeight = 0.055; //meters

  lensRadius = 0.024; //meters
  lensHeight = 0.01; //meters

  zOffset = 0.0;

  // The primary cylindrical base
  this->body = new Body( this->world );
  this->bodyGeoms[0] = new CylinderGeom( this->body, this->modelSpaceId, baseRadius, baseHeight);
  this->bodyGeoms[0]->SetRelativePosition( GzVectorSet(0, 0, 0) );
  this->bodyGeoms[0]->SetMass( mass*.5 );
  this->bodyGeoms[0]->SetColor( GzColor(0.3, 0.3, 0.3) );
  this->AddBody( this->body, true );
  zOffset += baseHeight;

  // Rectagular upright
  this->middle = new Body( this->world );
  this->bodyGeoms[1] = new BoxGeom( this->middle, this->modelSpaceId,
                                    uprightLength,uprightWidth, uprightHeight );
  this->bodyGeoms[1]->SetRelativePosition( GzVectorSet(0, 0, zOffset) );
  this->bodyGeoms[1]->SetMass( mass*.25 );
  this->bodyGeoms[1]->SetColor( GzColor(0.3, 0.3, 0.3) );
  this->AddBody( this->middle );
  zOffset += uprightHeight + uprightHeight * 0.5;

  // Rectangular head unit
  this->head = new Body( this->world, "head" );
  this->bodyGeoms[2] = new BoxGeom( this->head, this->modelSpaceId,
                                    headLength, headWidth, headHeight );
  this->bodyGeoms[2]->SetRelativePosition( GzVectorSet(0, 0, zOffset) );
  this->bodyGeoms[2]->SetMass( mass*0.25 );
  this->bodyGeoms[2]->SetColor( GzColor(0.3,0.3,0.3) );

  // The lens
  this->bodyGeoms[3] = new CylinderGeom( this->head, this->modelSpaceId,
                                         lensRadius, lensHeight );
  this->bodyGeoms[3]->SetRelativePosition( GzVectorSet(headWidth/2.0 + lensHeight/2, 0, zOffset) );
  //  this->bodyGeoms[3]->SetColor( GzColor(1, 1, 1) );
  //  this->bodyGeoms[3]->SetAmbientColor( GzColor(1, 1, 1) );
  this->bodyGeoms[3]->SetMass( 0 );
  this->AddBody( this->head );

  GzQuatern a,b,c;
  a = GzQuaternFromAxis(0,1,0,+M_PI/2.0);
  b = GzQuaternFromAxis(0,0,1,+M_PI/2.0);
  c = GzQuaternMul(a,b);
  this->bodyGeoms[3]->SetRelativeRotation( c );

  this->lensGeom = this->bodyGeoms[3];

  //pan joint
  this->panJoint = new HingeJoint( this->world );
  this->panJoint->Attach(this->middle, this->body);
  GzVector ab = this->middle->GetPosition();
  this->panJoint->SetAnchor( ab );
  this->panJoint->SetAxis(GzVectorSet(0,0,1));
  this->panJoint->SetParam( dParamFMax, 10.1 ); 

  //tilt joint
  this->tiltJoint = new HingeJoint( this->world );
  this->tiltJoint->Attach(this->head, this->middle);
  this->tiltJoint->SetAnchor( GzVectorSet(ab.x, ab.y, ab.z+uprightHeight));
  this->tiltJoint->SetAxis(GzVectorSet(0,1,0));
  this->tiltJoint->SetParam( dParamFMax, 10.1 ); 

  this->middle->SetFiniteRotationMode(1);
  this->head->SetFiniteRotationMode(1);

  return 0;
}

//////////////////////////////////////////////////////////////////////////////
// Initialize the model
int SonyVID30::Init( WorldFile *file, WorldFileNode *node )
{
  int width, height;
  RenderOptions *renderOpts;
  gz_camera_data_t *data;

  // Camera interface
  assert(this->cameraIface);
  if (gz_camera_create(this->cameraIface, this->world->gz_server, this->GetId(),
                       "SonyVID30", this->GetIntId(), this->GetParentIntId()) !=0)
    return -1;
  
  // PTZ interface
  assert(this->ptzIface);
  if (gz_ptz_create(this->ptzIface, this->world->gz_server, this->GetId(),
                    "SonyVID30", this->GetIntId(), this->GetParentIntId()) !=0)
    return -1;

  // Initialize PTZ interface to sensible values
  this->ptzIface->data->cmd_zoom = M_PI;

  // Set image dimensions (needed by GUI)
  gz_camera_lock(this->cameraIface, 1);
  data = this->cameraIface->data;
  this->camera->GetImageSize(&width, &height);
  data->width = width;
  data->height = height;
  gz_camera_unlock(this->cameraIface);

  // Set initial rendering options
  renderOpts = new RenderOptions();
  this->camera->GetRenderOptions(renderOpts);
  renderOpts->displaySkins = true;  
  this->camera->SetRenderOptions(renderOpts);

  return 0;
}


//////////////////////////////////////////////////////////////////////////////
// Finalize the model
int SonyVID30::Fini()
{
  // Finalize external interface  
  gz_camera_destroy(this->cameraIface);
  gz_ptz_destroy(this->ptzIface);

  this->camera->Fini();
    
  return 0;
}


//////////////////////////////////////////////////////////////////////////////
// Update the model
void SonyVID30::Update( double step )
{
  double pan, tilt;    

  if (this->world->GetSimTime() - this->ptzUpdateTime >= this->ptzUpdatePeriod)
  {
    step = this->world->GetSimTime() - this->ptzUpdateTime;
    
    this->ptzUpdateTime = this->world->GetSimTime();
  
    // Get new commands
    this->GetPtzCmd();

    // Apply joint limits to commanded pan/tilt angles
    if (this->cmdTilt > M_PI*0.5)
      this->cmdTilt = M_PI*0.5;
    else if (this->cmdTilt < -M_PI*0.5)
      this->cmdTilt = -M_PI*0.5;

    if (this->cmdPan > M_PI*0.3)
      this->cmdPan = M_PI*0.3;
    else if (this->cmdPan < -M_PI*0.3)
      this->cmdPan = -M_PI*0.3;

    // Apply limits on commanded zoom
    if (this->cmdZoom < this->zoomMin)
      this->cmdZoom = this->zoomMin;
    if (this->cmdZoom > this->zoomMax)
      this->cmdZoom = this->zoomMax;
  
    // Set the pan and tilt motors; can't set angles so track cmds with
    // a proportional control
    tilt = this->cmdTilt - this->tiltJoint->GetAngle();
    pan = this->cmdPan - this->panJoint->GetAngle();
    this->tiltJoint->SetParam( dParamVel, this->motionGain * tilt);
    this->panJoint->SetParam( dParamVel, this->motionGain * pan);

    // Set the zoom using a proportional controller
    this->zoom += (this->cmdZoom - this->zoom) * this->zoomGain * step;

    // Update the ptz interface
    this->PutPtzData();
  }

  if (this->world->GetSimTime() - this->cameraUpdateTime >= this->cameraUpdatePeriod)
  {
    this->cameraUpdateTime = this->world->GetSimTime();
            
    // Update the image; there is an ugly little hack here to set the
    // imaging plane slightly in front of the camera (so the camera
    // doesnt image itself)
    GzVector pos;
    GzQuatern rot;
    rot = this->head->GetRotation();
    pos = this->head->GetPosition();
    pos = GzCoordPositionAdd(GzVectorSet(0.10, 0, 0), pos, rot);

    this->camera->SetFOV(this->hfov / this->zoom);
    this->camera->SetPose(GzPoseSet(pos, rot));

    // Update the GUI
    if (this->frustrum)
    {
      int width, height;
      const uint8_t *data;
      this->camera->GetImageSize(&width, &height);
      data = this->camera->GetImageData();
      this->frustrum->SetFOV(this->camera->GetFOV());
      this->frustrum->SetTexture2D(width, height, data);
    }

    // Update the camera
    this->camera->Update();
      
    // Update the camera interface
    this->PutCameraData();
  }
  
  return;
}


//////////////////////////////////////////////////////////////////////////////
// Send PTZ data
void SonyVID30::PutPtzData()
{
  // Update the PTZ interface
  gz_ptz_lock(this->ptzIface, 1);
  this->ptzIface->data->time = this->world->GetSimTime();
  this->ptzIface->data->pan = this->panJoint->GetAngle();
  this->ptzIface->data->tilt = this->tiltJoint->GetAngle();
  this->ptzIface->data->zoom = this->camera->GetFOV();  
  gz_ptz_unlock(this->ptzIface);  
  gz_ptz_post(this->ptzIface);
  
  return;
}


//////////////////////////////////////////////////////////////////////////////
// Send data camera data
void SonyVID30::PutCameraData()
{
  int i;
  int len;
  const unsigned char *src;
  unsigned char *dst;
  int width, height;
  gz_camera_data_t *data;

  gz_camera_lock(this->cameraIface, 1);

  data = this->cameraIface->data;

  // Get image dimensions
  this->camera->GetImageSize(&width, &height);
  
  data->time = this->world->GetSimTime();
  data->width = width;
  data->height = height;
  data->image_size = width * height * 3;

  // Make sure there is room to store the image
  assert(data->image_size <= sizeof(data->image));
  
  // Copy the pixel data to the interface, but flip the y axis
  len = width * 3;
  src = this->camera->GetImageData() + (height - 1) * len;
  dst = data->image;    
  for (i = 0; i < height; i++, src -= len, dst += len)
    memcpy(dst, src, len);

  gz_camera_unlock(this->cameraIface);
  gz_camera_post(this->cameraIface);

  return;
}


//////////////////////////////////////////////////////////////////////////////
// Get commands
void SonyVID30::GetPtzCmd()
{
  gz_ptz_lock(this->ptzIface, 1);
  
  this->cmdPan = this->ptzIface->data->cmd_pan;
  this->cmdTilt = this->ptzIface->data->cmd_tilt;
  this->cmdZoom = this->hfov / this->ptzIface->data->cmd_zoom;
  
  gz_ptz_unlock(this->ptzIface);
  gz_ptz_post(this->ptzIface);
  
  return;
}