bandit.cc

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

  anchor.x -= shoulderSize.x*0.5 + upperArmSize.x*0.5 + 0.005;
  this->rightUpperArm->SetPosition(anchor);
  this->rightUpperArm->SetAngularVel(GzVectorSet(0,0,0));
  this->rightUpperArm->SetLinearVel(GzVectorSet(0,0,0));
  this->rightUpperArm->SetGravityMode(false);
  this->AddBody(this->rightUpperArm, false);

  geom = new BoxGeom(this->rightUpperArm, this->modelSpaceId, upperArmSize.x, upperArmSize.y, upperArmSize.z);
  geom->SetColor( GzColor(1, 0, 0) );
  geom->SetMass(0.1);
  geom->SetRelativePosition(GzVectorSet(0.0, 0.0, upperArmSize.z * 0.5) );

  // Create the right elbow
  this->rightElbow = new Body(this->world);
  anchor = this->rightUpperArm->GetPosition();
  anchor.x -= upperArmSize.x*0.5 + elbowSize.x*0.5 + 0.005;
  this->rightElbow->SetPosition(anchor);
  this->rightElbow->SetAngularVel(GzVectorSet(0,0,0));
  this->rightElbow->SetLinearVel(GzVectorSet(0,0,0));
  this->rightElbow->SetGravityMode(false);
  this->AddBody(this->rightElbow, false);
 
  geom = new BoxGeom(this->rightElbow, this->modelSpaceId, elbowSize.x, elbowSize.y, elbowSize.z);
  geom->SetColor( GzColor(0, 1, 0) );
  geom->SetMass(0.1);
  geom->SetRelativePosition(GzVectorSet(0, 0.0, elbowSize.z*0.5) );

  // Create the right lower arm
  this->rightLowerArm = new Body(this->world);
  anchor = this->rightElbow->GetPosition();
  anchor.x -= elbowSize.x*0.5 + lowerArmSize.x*0.5 + 0.005;
  this->rightLowerArm->SetPosition(anchor);
  this->rightLowerArm->SetAngularVel(GzVectorSet(0,0,0));
  this->rightLowerArm->SetLinearVel(GzVectorSet(0,0,0));
  this->rightLowerArm->SetGravityMode(false);
  this->AddBody(this->rightLowerArm, false);
 
  geom = new BoxGeom(this->rightLowerArm, this->modelSpaceId, lowerArmSize.x, lowerArmSize.y, lowerArmSize.z);
  geom->SetColor( GzColor(0, 1, 0) );
  geom->SetMass(0.1);
  geom->SetRelativePosition(GzVectorSet(0, 0.0, lowerArmSize.z*0.5) );
 

  // Create the right hand
  this->rightHand = new Body(this->world);
  anchor = this->rightLowerArm->GetPosition();
  anchor.x -= elbowSize.x*0.5 + handSize.x*0.5 + 0.005;
  this->rightHand->SetPosition(anchor);
  this->rightHand->SetAngularVel(GzVectorSet(0,0,0));
  this->rightHand->SetLinearVel(GzVectorSet(0,0,0));
  this->rightHand->SetGravityMode(false);
  this->AddBody(this->rightHand, false);

  geom = new BoxGeom(this->rightHand, this->modelSpaceId, handSize.x, handSize.y, handSize.z);
  geom->SetColor( GzColor(1, 0, 1) );
  geom->SetMass(0.1);
  geom->SetRelativePosition(GzVectorSet(0.0, 0.0, handSize.z * 0.5));


  // Right shoulder joint
  this->joints[R_SHOULDER] = new HingeJoint(this->world);
  this->joints[R_SHOULDER]->Attach(this->torso, this->rightShoulder);
  anchor = this->rightShoulder->GetCoMPose().pos;
  this->joints[R_SHOULDER]->SetAnchor(anchor);
  ((HingeJoint*)this->joints[R_SHOULDER])->SetAxis(GzVectorSet(0,1,0));
  this->joints[R_SHOULDER]->SetParam( dParamBounce, 0);
  this->joints[R_SHOULDER]->SetParam( dParamLoStop, DTOR(-45));
  this->joints[R_SHOULDER]->SetParam( dParamHiStop, DTOR(135.0) );
  this->joints[R_SHOULDER]->SetParam( dParamFMax, 1.1 );

  // Right upper arm joint
  this->joints[R_UPPERARM] = new HingeJoint( this->world );
  this->joints[R_UPPERARM]->Attach(this->rightShoulder, this->rightUpperArm);
  anchor = this->rightUpperArm->GetCoMPose().pos;
  this->joints[R_UPPERARM]->SetAnchor(anchor);
  ((HingeJoint*)this->joints[R_UPPERARM])->SetAxis(GzVectorSet(1,0,0));
  this->joints[R_UPPERARM]->SetParam( dParamBounce, 0);
  this->joints[R_UPPERARM]->SetParam( dParamLoStop, DTOR(-90));
  this->joints[R_UPPERARM]->SetParam( dParamHiStop, DTOR(90.0) );
  this->joints[R_UPPERARM]->SetParam( dParamFMax, 1.1 );
 
  // Right elbow joint
  this->joints[R_ELBOW] = new HingeJoint( this->world );
  this->joints[R_ELBOW]->Attach(this->rightUpperArm, this->rightElbow);
  anchor = this->rightElbow->GetCoMPose().pos;
  anchor.x += elbowSize.x * 0.5;
  this->joints[R_ELBOW]->SetAnchor(anchor);
  ((HingeJoint*)this->joints[R_ELBOW])->SetAxis(GzVectorSet(0,1,0));
  this->joints[R_ELBOW]->SetParam( dParamBounce, 0);
  this->joints[R_ELBOW]->SetParam( dParamLoStop, DTOR(0.0));
  this->joints[R_ELBOW]->SetParam( dParamHiStop, DTOR(90.0) );
  this->joints[R_ELBOW]->SetParam( dParamFMax, 1.1 );

  // Right lower arm joint
  this->joints[R_LOWERARM] = new HingeJoint( this->world );
  this->joints[R_LOWERARM]->Attach(this->rightElbow, this->rightLowerArm);
  anchor = this->rightLowerArm->GetCoMPose().pos;
  this->joints[R_LOWERARM]->SetAnchor(anchor);
  ((HingeJoint*)this->joints[R_LOWERARM])->SetAxis(GzVectorSet(1,0,0));
  this->joints[R_LOWERARM]->SetParam( dParamBounce, 0);
  this->joints[R_LOWERARM]->SetParam( dParamLoStop, DTOR(-90.0));
  this->joints[R_LOWERARM]->SetParam( dParamHiStop, DTOR(90.0) );
  this->joints[R_LOWERARM]->SetParam( dParamFMax, 1.1 );

  // Right wrist joint
  this->joints[R_WRIST] = new HingeJoint( this->world );
  this->joints[R_WRIST]->Attach( this->rightLowerArm, this->rightHand);
  anchor = this->rightHand->GetCoMPose().pos;
  anchor.x += handSize.x * 0.5;
  this->joints[R_WRIST]->SetAnchor(anchor);
  ((HingeJoint*)this->joints[R_WRIST])->SetAxis(GzVectorSet(0,1,0));
  this->joints[R_WRIST]->SetParam( dParamLoStop, DTOR(0));
  this->joints[R_WRIST]->SetParam( dParamHiStop, DTOR(90.0) );
  this->joints[R_WRIST]->SetParam( dParamFMax, 1.1 );


  this->head = new Body(this->world);
  anchor = this->torso->GetPosition();
  anchor.x += torsoSize.x*0.5 + headRadius + 0.005;
  this->head->SetPosition(anchor);
  this->head->SetAngularVel(GzVectorSet(0,0,0));
  this->head->SetLinearVel(GzVectorSet(0,0,0));
  this->head->SetGravityMode(false);
  this->AddBody(this->head, false);

  geom = new SphereGeom(this->head, this->modelSpaceId, headRadius);
  geom->SetColor( GzColor(1, 0, 1) );
  geom->SetMass(0.1);
  geom->SetRelativePosition(GzVectorSet(0.0, 0.0, headRadius));

  // Head joint
  this->joints[HEAD] = new UniversalJoint( this->world );
  this->joints[HEAD]->Attach( this->torso, this->head);
  anchor = this->torso->GetCoMPose().pos;
  anchor.x += torsoSize.x * 0.5;
  this->joints[HEAD]->SetAnchor(anchor);
  ((UniversalJoint*)this->joints[HEAD])->SetAxis1(GzVectorSet(1,0,0));
  ((UniversalJoint*)this->joints[HEAD])->SetAxis2(GzVectorSet(0,1,0));
  this->joints[HEAD]->SetParam( dParamLoStop, DTOR(-90));
  this->joints[HEAD]->SetParam( dParamLoStop2, DTOR(-90));
  this->joints[HEAD]->SetParam( dParamHiStop, DTOR(90.0) );
  this->joints[HEAD]->SetParam( dParamHiStop2, DTOR(90.0) );
  this->joints[HEAD]->SetParam( dParamFMax, 1.1 );

  return 0;
}


//////////////////////////////////////////////////////////////////////////////
// Initialize the model
int Bandit::Init( WorldFile *file, WorldFileNode *node )
{
  GzVector vec;
  HingeJoint *hjoint;
  UniversalJoint *ujoint;

  // Create and open a joint interface
  this->joint_iface = gz_joint_alloc();
  if (gz_joint_create(this->joint_iface, this->world->gz_server, this->GetId(), 
                     "Bandit", this->GetIntId(), this->GetParentIntId()) != 0)
  {
    return -1;
  }

  // Set the number of joints in this model
  this->joint_iface->data->joint_count = JOINT_COUNT;

  // Fill in the joint information
  for (int i=0; i<JOINT_COUNT; i++)
  {
    this->joint_iface->data->joints[i].type = this->joints[i]->GetType();
    
    vec = this->joints[i]->GetAnchor();
    this->joint_iface->data->joints[i].anchor[0] = vec.x;
    this->joint_iface->data->joints[i].anchor[1] = vec.y;
    this->joint_iface->data->joints[i].anchor[2] = vec.z;

    if (this->joints[i]->GetType() == dJointTypeHinge)
    {
      hjoint = ((HingeJoint*)this->joints[i]);

      vec = hjoint->GetAxis();
      this->joint_iface->data->joints[i].axis[0] = vec.x;
      this->joint_iface->data->joints[i].axis[1] = vec.y;
      this->joint_iface->data->joints[i].axis[2] = vec.z;

      this->joint_iface->data->joints[i].angular_velocity = hjoint->GetAngleRate();
      this->joint_iface->data->joints[i].angle = hjoint->GetAngle();

    }
    else if (this->joints[i]->GetType() == dJointTypeUniversal)
    {
      ujoint = ((UniversalJoint*)this->joints[i]);

      vec = ujoint->GetAxis1();
      this->joint_iface->data->joints[i].axis[0] = vec.x;
      this->joint_iface->data->joints[i].axis[1] = vec.y;
      this->joint_iface->data->joints[i].axis[2] = vec.z;

      vec = ujoint->GetAxis2();
      this->joint_iface->data->joints[i].axis2[0] = vec.x;
      this->joint_iface->data->joints[i].axis2[1] = vec.y;
      this->joint_iface->data->joints[i].axis2[2] = vec.z;

      this->joint_iface->data->joints[i].angular_velocity = ujoint->GetAngleRate1();
      this->joint_iface->data->joints[i].angle = ujoint->GetAngle1();

      this->joint_iface->data->joints[i].angular_velocity2 = ujoint->GetAngleRate2();
      this->joint_iface->data->joints[i].angle2 = ujoint->GetAngle2();
    }

  }
 
  return 0;
}


//////////////////////////////////////////////////////////////////////////////
// Finalize the model
int Bandit::Fini()
{
  gz_joint_destroy(this->joint_iface);
  gz_joint_free(this->joint_iface);
  this->joint_iface = NULL;

  return 0;
}


//////////////////////////////////////////////////////////////////////////////
// Get commands from the external interface
void Bandit::IfaceGetCmd()
{
  double angle;
  double cmd_angle;

  // Process all joints
  for (int i=0; i<JOINT_COUNT; i++)
  {
    if (this->joints[i]->GetType() == dJointTypeHinge)
    {
      angle = ((HingeJoint*)this->joints[i])->GetAngle();
      cmd_angle = this->joint_iface->data->joints[i].cmd_angle;

      this->SetJointVelocity(this->joints[i], dParamVel, angle, cmd_angle);
    }

    else if (this->joints[i]->GetType() == dJointTypeUniversal)
    {
      angle = ((UniversalJoint*)this->joints[i])->GetAngle1();
      cmd_angle = this->joint_iface->data->joints[i].cmd_angle;

      this->SetJointVelocity(this->joints[i], dParamVel, angle, cmd_angle);

      angle = ((UniversalJoint*)this->joints[i])->GetAngle2();
      cmd_angle = this->joint_iface->data->joints[i].cmd_angle2;

      this->SetJointVelocity(this->joints[i], dParamVel2, angle, cmd_angle);
    }


  }

  return;
}


//////////////////////////////////////////////////////////////////////////////
// Update external interface
void Bandit::IfacePutData()
{

  // Fill in the joint information
  for (int i=0; i<JOINT_COUNT; i++)
  {
    if (this->joints[i]->GetType() == dJointTypeHinge)
    {
      this->joint_iface->data->joints[i].angular_velocity = ((HingeJoint*)this->joints[i])->GetAngleRate();
      this->joint_iface->data->joints[i].angle = ((HingeJoint*)this->joints[i])->GetAngle();
    }

    else if (this->joints[i]->GetType() == dJointTypeUniversal)
    {
      this->joint_iface->data->joints[i].angular_velocity = ((UniversalJoint*)this->joints[i])->GetAngleRate1();
      this->joint_iface->data->joints[i].angular_velocity2 = ((UniversalJoint*)this->joints[i])->GetAngleRate2();

      this->joint_iface->data->joints[i].angle = ((UniversalJoint*)this->joints[i])->GetAngle1();
      this->joint_iface->data->joints[i].angle2 = ((UniversalJoint*)this->joints[i])->GetAngle2();
    }

  }
 
  return;
}


//////////////////////////////////////////////////////////////////////////////
// Update the model information; returns non-zero if sensor data has
// been updated
void Bandit::Update( double step )
{
  // Get commands from the external interface
  this->IfaceGetCmd();

  // Update the external interface with the new data
  this->IfacePutData();

  return;
}

//////////////////////////////////////////////////////////////////////////////
// Set the velocity of a joint
void Bandit::SetJointVelocity( Joint *joint, int param, double angle, double cmd_angle)
{
  double velocity = 0.0;

  // Move the joint if the commanded angle doesn't match the current angle
  if (fabs(angle - cmd_angle) > 1e-2)
  {
    if (angle < cmd_angle)
      velocity = 1.0;
    else
      velocity = -1.0;
  }

  if (joint->GetType() == dJointTypeHinge)
    ((HingeJoint*)joint)->SetParam(param, velocity);
  else
    ((UniversalJoint*)joint)->SetParam(param, velocity);
}