Beispiel #1
0
void RobotHexa::position_zero()
{
  ROS_DEBUG_STREAM("initial position");
  enable();

  std::vector<int> pos(_actuators_ids.size());

  /*  for (size_t i = 0; i < _actuators_ids.size(); ++i)
      if(_actuators_ids[i]>=20)
      pos[i]= 2048;
      else if (_actuators_ids[i] >= 10) // mx28
      pos[i] = 1024;

      else
      pos[i] = 512;
      applyCorrection(pos);
      _controller.send(dynamixel::ax12::SetPositions(_actuators_ids, pos));
      _controller.recv(READ_DURATION, _status);
  */



  pos.clear();
  pos.resize(_actuators_ids.size());
  sleep(1);
  for (size_t i = 0; i < _actuators_ids.size(); ++i)
  {
	  pos[i] = 2048;
  }

  try
  {
    applyCorrection(pos);
    _controller.send(dynamixel::ax12::SetPositions(_actuators_ids, pos));
    _controller.recv(READ_DURATION, _status);
  }
  catch(dynamixel::Error e)
  {
    ROS_ERROR_STREAM(e.msg());
    sleep(10);
  }

  usleep(0.5e6);


  ROS_DEBUG_STREAM("done");


}
Beispiel #2
0
/** Make the hexapod properly stand up when it is laying on the ground.
    It does so by first raising its legs, folding them and then bringing them down.
**/
void RobotHexa::reset()
{
  try
  {
    if(_controller.isOpen()==false)
  	{
  	  ROS_INFO_STREAM("re-opening dynamixel's serial");
  	  // _controller.open_serial("/dev/ttyUSB0",B1000000);
  	  _controller.open_serial(_serial_port, _serial_baudrate);
  	}
    _controller.flush();
  }
  catch (dynamixel::Error e)
  {
    ROS_ERROR_STREAM("(dynamixel): " << e.msg());
  }

  enable();

  std::vector<int> pos(_actuators_ids.size());

  // Raise the robot "softly"

  for (size_t i = 0; i < _actuators_ids.size(); ++i)
    if(_actuators_ids[i]>=20) // last actuator of each leg
      pos[i]= 2048+256;
    else if (_actuators_ids[i] >= 10) // second actuator of each leg
      pos[i] = 3072;
    else // first actuator of each leg
      pos[i] = 2048;

  applyCorrection(pos);
  _controller.send(dynamixel::ax12::SetPositions(_actuators_ids, pos));
  _controller.recv(READ_DURATION, _status);

  pos.clear();
  pos.resize(_actuators_ids.size());
  usleep(0.5e6);
  for (size_t i = 0; i < _actuators_ids.size(); ++i)
    if(_actuators_ids[i]>=20)
      pos[i]= 2048+256;
    else if (_actuators_ids[i] >= 10)
      pos[i] = 2048;
    else
      pos[i] = 2048;

  applyCorrection(pos);
  _controller.send(dynamixel::ax12::SetPositions(_actuators_ids, pos));
  _controller.recv(READ_DURATION, _status);

  pos.clear();
  pos.resize(_actuators_ids.size());
  usleep(0.5e6);
  for (size_t i = 0; i < _actuators_ids.size(); ++i)
  {
	  pos[i] = 2048;
  }

  applyCorrection(pos);
  _controller.send(dynamixel::ax12::SetPositions(_actuators_ids, pos));
  _controller.recv(READ_DURATION, _status);

  sleep(1);
}
void PxSingleActor::unpackUpdate(NetConnection *con, BitStream *stream)
{
   Parent::unpackUpdate(con, stream);
   
   /*
   if ( stream->readFlag() ) // ForceSleepMask
   {
      bool forceSleep = stream->readFlag();

      if ( isProperlyAdded() )
         setForceSleep( forceSleep );
      else
         mForceSleep = forceSleep;
   }


   if ( stream->readFlag() ) // SleepMask
   {
      if ( mActor )
         mActor->putToSleep();

      // TODO: The sleep should start packing its own
      // position warp data and not rely on the WarpMask
      // below.  This will allow us to place the actor
      // in a way that is less prone to explosions.
   }
   */
   if ( stream->readFlag() ) // WarpMask
   {
      // If we set a warp mask,
      // we need to instantly move
      // the actor to the new position
      // without applying any corrections.
      MatrixF mat;
      stream->readAffineTransform( &mat );
      
      applyWarp( mat, true, false );
   }  

   if ( stream->readFlag() ) // MoveMask
   {
      MatrixF mat;
      stream->readAffineTransform( &mat );

      NxVec3 linVel, angVel;
      if ( stream->readFlag() )
      {
         stream->read( &linVel.x );
         stream->read( &linVel.y );
         stream->read( &linVel.z );

         stream->read( &angVel.x );
         stream->read( &angVel.y );
         stream->read( &angVel.z );

         if ( !mDataBlock->clientOnly )
            applyCorrection( mat, linVel, angVel );
      }
   }

   if ( stream->readFlag() ) // ImpulseMask
   {
      NxVec3 linVel;
      stream->read( &linVel.x );
      stream->read( &linVel.y );
      stream->read( &linVel.z );

      if ( mActor )
      {
         mWorld->releaseWriteLock();
         mActor->setLinearVelocity( linVel );
         mStartImpulse.zero();
      }
      else
         mStartImpulse.set( linVel.x, linVel.y, linVel.z );
   }

   if ( mLightPlugin )
      mLightPlugin->unpackUpdate( this, con, stream );
}