shrimp.cc

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

  double c = 0.125;
  double d = 0.051;
  double e = 0.114;
  double h = 0.140;


  double forkPart1length,forkPart1width,forkPart1height;
  forkPart1length = e;
  forkPart1width =  barWidth;
  forkPart1height = barWidth;


  GzVector chassisPos = chassis->GetPosition();
  double part1Anchor[3] = { chassisPos.x - bodylength/2, chassisPos.y, chassisPos.z + bodyheight/2 -barWidth/2 };
  double part1Pos[3] = { part1Anchor[0] - forkPart1length/2, part1Anchor[1], part1Anchor[2] };


  this->forkPart1 = new Body( this->world );
    geom = new BoxGeom(this->forkPart1, this->modelSpaceId, forkPart1length, forkPart1width, forkPart1height);
    geom->SetRelativePosition(GzVectorSet(0,0,0)); // part1Pos[0],part1Pos[1],part1Pos[2] );
    geom->SetMass( 0.05 );
    geom->SetColor( GzColor(0,1,0) );
  this->forkPart1->SetPosition(GzVectorSet(part1Pos[0],part1Pos[1],part1Pos[2]));
  //this->forkPart1->SetRotation(GzQuaternFromAxis(0.0,1.0,0.0,-3.14/2) );
  this->AddBody( this->forkPart1 );

  this->forkJoint = new HingeJoint( this->world );
  this->forkJoint->Attach( this->forkPart1, this->chassis );
  this->forkJoint->SetAnchor( GzVectorSet(part1Anchor[0],part1Anchor[1],part1Anchor[2] ));
  this->forkJoint->SetAxis( GzVectorSet(0, 1, 0 ) );
//  this->forkJoint->SetParam( dParamLoStop, -3.14/2 );
//  this->forkJoint->SetParam( dParamHiStop, -0.7 );
  //this->forkJoint->SetParam( dParamFMax, 0.0 );
  //this->forkJoint->SetParam( dParamBounce, 0.0 );
  //this->forkJoint->SetParam( dParamSuspensionERP, 0.0 );
  //this->forkJoint->SetParam( dParamCFM, 0.0 );


  /*
  std::cout << "part 1 positie =\n";
  std::cout << part1Pos[0];
  std::cout << " =x \n";
  std::cout << part1Pos[1];
  std::cout << " =y\n";
  std::cout << part1Pos[2];
  std::cout << " =z\n";

  std::cout << "part 1 anchor =\n";
  std::cout << part1Anchor[0];
  std::cout << " =x \n";
  std::cout << part1Anchor[1];
  std::cout << " =y\n";
  std::cout << part1Anchor[2];
  std::cout << " =z\n\n";
  */


  double forkPart1blength,forkPart1bwidth,forkPart1bheight;
  forkPart1blength = c;
  forkPart1bwidth =  barWidth;
  forkPart1bheight = barWidth;

  chassisPos = chassis->GetPosition();
  double part1bAnchor[3] = { chassisPos.x - bodylength/2, chassisPos.y , chassisPos.z + bodyheight/2 - b - barWidth/2 };
  double part1bPos[3] = { part1bAnchor[0] - forkPart1blength/2, part1bAnchor[1], part1bAnchor[2] };

  this->forkPart1b = new Body( this->world );
    geom = new BoxGeom(this->forkPart1b, this->modelSpaceId, forkPart1blength, forkPart1bwidth, forkPart1bheight);
    geom->SetRelativePosition(GzVectorSet(0,0,0));
    geom->SetMass( 0.05 );
    geom->SetColor( GzColor(0.7,0.2,0.2) );
  this->forkPart1b->SetPosition(GzVectorSet(part1bPos[0],part1bPos[1],part1bPos[2]));
  //this->forkPart1b->SetRotation(GzQuaternFromAxis(0.0,1.0,0.0,3.14/2) );
  this->AddBody( this->forkPart1b );

  this->forkJointb = new HingeJoint( this->world );
  this->forkJointb->Attach( this->forkPart1b, this->chassis );
  this->forkJointb->SetAnchor( GzVectorSet(part1bAnchor[0],part1bAnchor[1],part1bAnchor[2] ));
  this->forkJointb->SetAxis( GzVectorSet(0, 1, 0) );
 // this->forkJointb->SetParam( dParamLoStop, -3.14/2 );
 // this->forkJointb->SetParam( dParamHiStop, -0.7 );
  //this->forkJointb->SetParam( dParamFMax, 0.0 );
  //this->forkJointb->SetParam( dParamBounce, 0.0 );
  //this->forkJointb->SetParam( dParamSuspensionERP, 0.0 );
  //this->forkJointb->SetParam( dParamCFM, 0.0 );


  /*
  std::cout << "part 1b positie = \n";
  std::cout << part1bPos[0];
  std::cout << " =x \n";
  std::cout << part1bPos[1];
  std::cout << " =y\n";
  std::cout << part1bPos[2];
  std::cout << " =z\n";

  std::cout << "part 1b anchor =\n";
  std::cout << part1bAnchor[0];
  std::cout << " =x \n";
  std::cout << part1bAnchor[1];
  std::cout << " =y\n";
  std::cout << part1bAnchor[2];
  std::cout << " =z\n\n";
  */



  // FORK
  double forkPart2length,forkPart2width,forkPart2height;
  forkPart2length = d+h;
  forkPart2width =  barWidth; // 0.2*width;
  forkPart2height = barWidth; //1*height;


  GzVector part1Posi= forkPart1->GetPosition();
  GzVector part1bPosi= forkPart1b->GetPosition();
  //std::cout << part1Posi.x;
  //std::cout << " =x \n";
  //std::cout << part1Posi.y;
  //std::cout << " =y\n";
  //std::cout << part1Posi.z;
  //std::cout << " =z\n";
  double part2Pos[3] = { chassisPos.x - bodylength/2 - e - 0.025322444, part1Posi.y, chassisPos.z + bodyheight/2 - 0.092081615 };

  this->forkPart2 = new Body( this->world );
    geom = new BoxGeom(this->forkPart2, this->modelSpaceId, forkPart2length, forkPart2width, forkPart2height);
    geom->SetRelativePosition( GzVectorSet(0,0,0) );
    geom->SetMass( 0.05 );
    geom->SetColor( GzColor(0,1,1) );
  this->forkPart2->SetPosition(GzVectorSet(part2Pos[0],part2Pos[1],part2Pos[2]));
  this->forkPart2->SetRotation(GzQuaternFromAxis(0,1,0,-M_PI / 2 + 0.2683662) );
  this->AddBody( this->forkPart2 );
 

  // attach part2 to part1 and part1b
  double part2Anchor[3] = {part1Posi.x - forkPart1length/2, part1Posi.y, part1Posi.z  };
  double part2bAnchor[3] = {part1bPosi.x - forkPart1blength/2, chassisPos.y , part1bPosi.z};
  // double part2bAnchor[3] = {part1bPosi.x + forkPart1blength/2, part1bPosi.y + forkPart1bwidth/2, part1bPosi.z + forkPart1bheight/2};

  this->forkforkJoint = new HingeJoint( this->world );
  this->forkforkJoint->Attach( this->forkPart1, this->forkPart2 );
  this->forkforkJoint->SetAnchor ( GzVectorSet(part2Anchor[0], part2Anchor[1], part2Anchor[2] ));
  this->forkforkJoint->SetAxis( GzVectorSet(0, 1, 0) );
//  this->forkforkJoint->SetParam( dParamLoStop, -3.14/2 );
 // this->forkforkJoint->SetParam( dParamHiStop, 0/2 );
  this->forkforkJoint->SetParam( dParamFMax, 0.0 );
  this->forkforkJoint->SetParam( dParamBounce, 0.0 );
  this->forkforkJoint->SetParam( dParamSuspensionERP, 0.2 );


  this->forkforkbJoint = new HingeJoint( this->world );
  this->forkforkbJoint->Attach( this->forkPart2, this->forkPart1b );
  this->forkforkbJoint->SetAnchor ( GzVectorSet(part2bAnchor[0], part2bAnchor[1], part2bAnchor[2]) );
  this->forkforkbJoint->SetAxis( GzVectorSet(0, 1, 0) );
//  this->forkforkbJoint->SetParam( dParamLoStop, -3.14/2 );
//  this->forkforkbJoint->SetParam( dParamHiStop, 0/2 );
  this->forkforkbJoint->SetParam( dParamFMax, 0.0 );
  this->forkforkbJoint->SetParam( dParamBounce, 0.0 );
  this->forkforkbJoint->SetParam( dParamSuspensionERP, 0.2 );



  // *************************************
  // **    create the frontwheel 	**
  // *************************************
  GzVector part2Posi = this->forkPart2->GetPosition();
  this->frontwheel = new Body( this->world );
    geom = new WheelGeom(this->frontwheel, this->modelSpaceId, 0.5 * wheelDiam,  wheelThick);
    //geom->SetBodyPosition( GzVectorSet(0,0,0) ); // length/2+forkPart1length, 0 , height/2);
    geom->SetMass( 0.2 );
    geom->SetColor( GzColor(0.1, 0.1, 0.1) );

    geom = new BoxGeom(this->frontwheel, this->modelSpaceId, 0.5 * wheelDiam, 0.5 * wheelDiam, 0.02);
    geom->SetRelativePosition( GzVectorSet(0, 0, wheelThick / 2) );
    geom->SetMass( 0.5 );
    geom->SetColor( GzColor(1.0, 1.0, 1.0) );

  this->AddBody( this-> frontwheel );
  this->frontwheel->SetPosition(GzVectorSet(part2Posi.x - 0.025322444, part2Posi.y + forkPart2width/2, part2Posi.z - 0.092081615 ));
  this->frontwheel->SetRotation(GzQuaternFromAxis(1.0,0.0,0.0,-M_PI / 2 ));
  
  this->frontwheelJoint = new HingeJoint( this->world );
  GzVector frontwheelPos = frontwheel->GetPosition();
  this->frontwheelJoint->Attach( this->frontwheel, this->forkPart2 );
  this->frontwheelJoint->SetAnchor( frontwheelPos );
  this->frontwheelJoint->SetAxis( GzVectorSet(0, 1, 0) );
  this->frontwheelJoint->SetParam( dParamSuspensionERP, 0.4 );
  this->frontwheelJoint->SetParam( dParamSuspensionCFM, 0.8 );

  return 0;
}



//////////////////////////////////////////////////////////////////////////////
// Finalize the model
int Shrimp::Fini()
{
  // Finalize external interface
  gz_position_destroy( this->iface );
  gz_position_free( this->iface );
  this->iface = NULL;

  return 0;
}



//////////////////////////////////////////////////////////////////////////////
// Update model
void Shrimp::Update( double step )
{
  // Always update the odometry (better accuracy)
  this->UpdateOdometry( step );

  // Otherwise, update periodically
  if (this->world->GetSimTime() - this->updateTime > this->updatePeriod)
  {
    this->updateTime = this->world->GetSimTime();

    // Get commands from the external interface
    this->PositionGetCmd();

    this->wheelJoints[0]->SetParam( dParamVel,
                                    this->wheelSpeed[1] / this->wheelDiam * 2 );
    this->wheelJoints[1]->SetParam( dParamVel,
                                    this->wheelSpeed[0] / this->wheelDiam * 2 );
    this->wheelJoints[2]->SetParam( dParamVel,
                                    this->wheelSpeed[1] / this->wheelDiam * 2 );
    this->wheelJoints[3]->SetParam( dParamVel,
                                    this->wheelSpeed[0] / this->wheelDiam * 2 );
    this->frontwheelJoint->SetParam( dParamVel,
                                     (this->wheelSpeed[0] + this->wheelSpeed[1]) / 2 /
                                     this->wheelDiam * 2 );

    this->wheelJoints[0]->SetParam( dParamFMax, 1e2 );
    this->wheelJoints[1]->SetParam( dParamFMax, 1e2 );
    this->wheelJoints[2]->SetParam( dParamFMax, 1e2 );
    this->wheelJoints[3]->SetParam( dParamFMax, 1e2 );
    this->frontwheelJoint->SetParam( dParamFMax, 1e2 );

    // Update the interface
    this->PositionPutData();
  }

  return;
}


//////////////////////////////////////////////////////////////////////////////
// Update the odometry
void Shrimp::UpdateOdometry( double step )
{
  double wd, ws;
  double d1, d2;
  double dr, da;

  wd = this->wheelDiam;
  ws = this->wheelSep;

  // Average distance travelled by left and right wheels
  d1 = step * wd / 2 * (wheelJoints[0]->GetAngleRate() + wheelJoints[2]->GetAngleRate()) / 2;
  d2 = step * wd / 2 * (wheelJoints[1]->GetAngleRate() + wheelJoints[3]->GetAngleRate()) / 2;

  // Distance travelled by front wheels
  d1 = step * wd / 2 * wheelJoints[0]->GetAngleRate();
  d2 = step * wd / 2 * wheelJoints[1]->GetAngleRate();

  dr = (d1 + d2) / 2;
  da = (d2 - d1) / ws;
  
  // Compute odometric pose
  this->odomPose[0] += dr * cos( this->odomPose[2] );
  this->odomPose[1] += dr * sin( this->odomPose[2] );
  this->odomPose[2] += da;
  
  return;
}


//////////////////////////////////////////////////////////////////////////////
// Get commands from the external interface
void Shrimp::PositionGetCmd()
{
  double vr, va;

  gz_position_lock(this->iface, 1);
  vr = this->iface->data->cmd_vel_pos[0];
  va = this->iface->data->cmd_vel_rot[2];
  gz_position_unlock(this->iface);
  
  this->wheelSpeed[0] = vr + va * this->wheelSep;
  this->wheelSpeed[1] = vr - va * this->wheelSep;
  
  return;
}


//////////////////////////////////////////////////////////////////////////////
// Update the data in the erinterface
void Shrimp::PositionPutData()
{
  // Data timestamp
  /*this->iface->data->time = this->world->GetSimTime();

  this->iface->data->odom_pose[0] = this->odomPose[0];
  this->iface->data->odom_pose[1] = this->odomPose[1];
  this->iface->data->odom_pose[2] = this->odomPose[2];

  // TODO
  this->iface->data->odom_vel[0] = 0.0;
  this->iface->data->odom_vel[1] = 0.0; 
  this->iface->data->odom_vel[2] = 0.0; 
  */
  
  return;
}