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 ); }