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");
}
/** 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 );
}
