Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Port the imu of iCub_SIM to the MAS interfaces` #605

Merged
merged 2 commits into from
Sep 27, 2019
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
11 changes: 11 additions & 0 deletions src/simulators/iCubSimulation/odesdl/OdeInit.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -130,6 +130,17 @@ void OdeInit::setSimulationControl(iCubSimulationControl *control, int part)
}
}

void OdeInit::setSimulationIMU(iCubSimulationIMU *imu) {
if (imu)
{
_imu = imu;
}
}

void OdeInit::removeSimulationIMU() {
_imu = nullptr;
}

void OdeInit::sendHomePos()
{
double refs[16] = {0,0,0,0,0,0,0,0,0,0,0,10*M_PI/180,10*M_PI/180,10*M_PI/180,10*M_PI/180,10*M_PI/180};
Expand Down
4 changes: 4 additions & 0 deletions src/simulators/iCubSimulation/odesdl/OdeInit.h
Original file line number Diff line number Diff line change
Expand Up @@ -35,6 +35,7 @@
#include <yarp/os/Semaphore.h>
#include "iCubLogicalJoints.h"
#include "iCubSimulationControl.h"
#include "iCubSimulationIMU.h"
//#include <vector>

#include "RobotConfig.h"
Expand Down Expand Up @@ -71,6 +72,7 @@ class OdeInit {
int verbosity;
string name;
iCubSimulationControl **_controls;
iCubSimulationIMU * _imu{nullptr};
double contactFrictionCoefficient; //unlike the other ODE params fron .ini file that are used to intiialize the properties of the simulation (dWorldSet...),
//This parameter is employed on the run as contact joints are created (in OdeSdlSimulation::nearCallback() )
//for whole_body_skin_emul
Expand All @@ -95,6 +97,8 @@ class OdeInit {

void setSimulationControl(iCubSimulationControl *control, int part);
void removeSimulationControl(int part);
void setSimulationIMU(iCubSimulationIMU *imu);
void removeSimulationIMU();
static OdeInit& init(RobotConfig *config);
void sendHomePos();

Expand Down
10 changes: 10 additions & 0 deletions src/simulators/iCubSimulation/odesdl/iCub_Sim.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -1087,6 +1087,16 @@ Uint32 OdeSdlSimulation::ODE_process(Uint32 interval, void *param) {
odeinit._controls[ipart]->jointStep();
}
}

// UPDATE INERTIAL

if(odeinit._imu) {
Bottle inertialBot;
retreiveInertialData(inertialBot);
odeinit._imu->updateIMUData(inertialBot);
}


odeinit.sync = true;
odeinit.mutex.post();

Expand Down
5 changes: 5 additions & 0 deletions src/simulators/iCubSimulation/wrapper/SimulationRun.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -25,6 +25,7 @@

#include "SimulatorModule.h"
#include "iCubSimulationControl.h"
#include "iCubSimulationIMU.h"
#include "SimConfig.h"

using namespace yarp::dev;
Expand Down Expand Up @@ -68,6 +69,10 @@ bool SimulationRun::run(SimulationBundle *bundle, int argc, char *argv[]) {
"controlboard",
"iCubSimulationControl"));

Drivers::factory().add(new DriverCreatorOf<iCubSimulationIMU>("simulationIMU",
"multipleanalogsensorsserver",
"iCubSimulationIMU"));

SimulatorModule module(*world,config,bundle->createSimulation(config));

if (module.open()) {
Expand Down
26 changes: 26 additions & 0 deletions src/simulators/iCubSimulation/wrapper/SimulatorModule.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -284,6 +284,9 @@ bool SimulatorModule::interruptModule() {
delete iCubTorso;
iCubTorso = NULL;
}

masserver.close();
simImu.close();
world_manager.clear();

return true;
Expand Down Expand Up @@ -477,6 +480,29 @@ bool SimulatorModule::initSimulatorModule()
if (idesc) idesc->registerDevice(desc);
}

Property masServerConf {{"device", Value("multipleanalogsensorsserver")},
{"name",Value(moduleName+"/head/inertials")},
{"period",Value(10)}};
Property simImuConf {{"device",Value("simulationIMU")}};


if (!simImu.open(simImuConf)) {
return false;
}

polyList.push(&simImu,"simImu");

if (!masserver.open(masServerConf)){
return false;
}

if (!masserver.view(iMWrapper))
{
yError()<<"Failed to view the IMultipleWrapper";
return false;
}

iMWrapper->attachAll(polyList);
//dd_descClnt will be not used anymore.
dd_descClnt->close();
delete dd_descClnt;
Expand Down
4 changes: 4 additions & 0 deletions src/simulators/iCubSimulation/wrapper/SimulatorModule.h
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,7 @@
#include <yarp/sig/Image.h>
#include <yarp/dev/PolyDriver.h>
#include <yarp/dev/IRobotDescription.h>
#include <yarp/dev/IMultipleWrapper.h>

#include "RobotStreamer.h"
#include "RobotConfig.h"
Expand Down Expand Up @@ -124,6 +125,9 @@ class SimulatorModule : public yarp::os::PortReader, public RobotStreamer
yarp::sig::ImageOf<yarp::sig::PixelRgb> buffer;

yarp::dev::PolyDriver *iCubLArm, *iCubRArm, *iCubHead, *iCubLLeg ,*iCubRLeg, *iCubTorso;
yarp::dev::PolyDriver masserver, simImu;;
yarp::dev::PolyDriverList polyList;
yarp::dev::IMultipleWrapper* iMWrapper{nullptr};
yarp::os::BufferedPort<yarp::sig::ImageOf<yarp::sig::PixelRgb> > portLeft, portRight, portWide;

#ifndef OMIT_LOGPOLAR
Expand Down
261 changes: 261 additions & 0 deletions src/simulators/iCubSimulation/wrapper/iCubSimulationIMU.cpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,261 @@
/*
* Copyright (C) 2006-2019 Istituto Italiano di Tecnologia (IIT)
* All rights reserved.
*
* This software may be modified and distributed under the terms of the
* BSD-3-Clause license. See the accompanying LICENSE file for details.
*/

#include <string>
#include <yarp/os/Stamp.h>
#include <yarp/os/LogStream.h>

#include <iCubSimulationIMU.h>

#include "OdeInit.h"

using namespace yarp::os;
using namespace yarp::dev;
using namespace yarp::sig;


iCubSimulationIMU::iCubSimulationIMU()
{
gyro.resize(3,0.0);
rpy.resize(3,0.0);
accels.resize(3, 0.0);
magn.resize(3,0.0);
m_sensorName = "sensorName";
m_frameName = "frameName";
}

iCubSimulationIMU::~iCubSimulationIMU()
{
OdeInit& odeinit = OdeInit::get();
odeinit.mutex.wait();
odeinit.removeSimulationIMU();
odeinit.mutex.post();
}

bool iCubSimulationIMU::open(yarp::os::Searchable &config)
{
OdeInit& odeinit = OdeInit::get();
odeinit.mutex.wait();
odeinit.setSimulationIMU(this);
odeinit.mutex.post();
return true;
}

bool iCubSimulationIMU::close()
{
// TODO see what does simulation control
return true;
}



yarp::dev::MAS_status iCubSimulationIMU::genericGetStatus(size_t sens_index) const
{
if (sens_index!=0) {
return yarp::dev::MAS_status::MAS_ERROR;
}

return yarp::dev::MAS_status::MAS_OK;
}

bool iCubSimulationIMU::genericGetSensorName(size_t sens_index, std::string &name) const
{
if (sens_index!=0) {
return false;
}

name = m_sensorName;
return true;
}

bool iCubSimulationIMU::genericGetFrameName(size_t sens_index, std::string &frameName) const
{
if (sens_index!=0) {
return false;
}

frameName = m_frameName;
return true;
}

size_t iCubSimulationIMU::getNrOfThreeAxisGyroscopes() const
{
return 1;
}

yarp::dev::MAS_status iCubSimulationIMU::getThreeAxisGyroscopeStatus(size_t sens_index) const
{
return genericGetStatus(sens_index);
}

bool iCubSimulationIMU::getThreeAxisGyroscopeName(size_t sens_index, std::string &name) const
{
return genericGetSensorName(sens_index, name);
}

bool iCubSimulationIMU::getThreeAxisGyroscopeFrameName(size_t sens_index, std::string &frameName) const
{
return genericGetFrameName(sens_index, frameName);
}

bool iCubSimulationIMU::getThreeAxisGyroscopeMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const
{
if (sens_index!=0) {
return false;
}

out.resize(3);
m_mutex.lock();
out = gyro;
m_mutex.unlock();

// Workaround for https://github.com/robotology/yarp/issues/1610
yarp::os::Stamp copyStamp(lastStamp);
timestamp = copyStamp.getTime();

return true;
}

size_t iCubSimulationIMU::getNrOfThreeAxisLinearAccelerometers() const
{
return 1;
}

yarp::dev::MAS_status iCubSimulationIMU::getThreeAxisLinearAccelerometerStatus(size_t sens_index) const
{
return genericGetStatus(sens_index);
}

bool iCubSimulationIMU::getThreeAxisLinearAccelerometerName(size_t sens_index, std::string &name) const
{
return genericGetSensorName(sens_index, name);
}

bool iCubSimulationIMU::getThreeAxisLinearAccelerometerFrameName(size_t sens_index, std::string &frameName) const
{
return genericGetFrameName(sens_index, frameName);
}

bool iCubSimulationIMU::getThreeAxisLinearAccelerometerMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const
{
if (sens_index!=0) {
return false;
}

out.resize(3);
m_mutex.lock();
out = accels;
m_mutex.unlock();

// Workaround for https://github.com/robotology/yarp/issues/1610
yarp::os::Stamp copyStamp(lastStamp);
timestamp = copyStamp.getTime();

return true;
}

size_t iCubSimulationIMU::getNrOfThreeAxisMagnetometers() const
{
return 1;
}

yarp::dev::MAS_status iCubSimulationIMU::getThreeAxisMagnetometerStatus(size_t sens_index) const
{
return genericGetStatus(sens_index);
}

bool iCubSimulationIMU::getThreeAxisMagnetometerName(size_t sens_index, std::string &name) const
{
return genericGetSensorName(sens_index, name);
}

bool iCubSimulationIMU::getThreeAxisMagnetometerFrameName(size_t sens_index, std::string &frameName) const
{
return genericGetFrameName(sens_index, frameName);
}

bool iCubSimulationIMU::getThreeAxisMagnetometerMeasure(size_t sens_index, yarp::sig::Vector& out, double& timestamp) const
{
if (sens_index!=0) {
return false;
}

out.resize(3);
m_mutex.lock();
out = magn;
m_mutex.unlock();

// Workaround for https://github.com/robotology/yarp/issues/1610
yarp::os::Stamp copyStamp(lastStamp);
timestamp = copyStamp.getTime();

return true;
}

size_t iCubSimulationIMU::getNrOfOrientationSensors() const
{
return 1;
}

yarp::dev::MAS_status iCubSimulationIMU::getOrientationSensorStatus(size_t sens_index) const
{
return genericGetStatus(sens_index);
}

bool iCubSimulationIMU::getOrientationSensorName(size_t sens_index, std::string &name) const
{
return genericGetSensorName(sens_index, name);
}

bool iCubSimulationIMU::getOrientationSensorFrameName(size_t sens_index, std::string &frameName) const
{
return genericGetFrameName(sens_index, frameName);
}

bool iCubSimulationIMU::getOrientationSensorMeasureAsRollPitchYaw(size_t sens_index, yarp::sig::Vector& rpy_out, double& timestamp) const
{
if (sens_index!=0) {
return false;
}

rpy_out.resize(3);
m_mutex.lock();
rpy_out = rpy;
m_mutex.unlock();

// Workaround for https://github.com/robotology/yarp/issues/1610
yarp::os::Stamp copyStamp(lastStamp);
timestamp = copyStamp.getTime();

return true;
}


void iCubSimulationIMU::updateIMUData(const yarp::os::Bottle& imuData) {
m_mutex.lock();
if (imuData.size() < 12) {
return;
}
rpy[0] = imuData.get(0).asFloat64();
rpy[1] = imuData.get(1).asFloat64();
rpy[2] = imuData.get(2).asFloat64();

accels[0] = imuData.get(3).asFloat64();
accels[1] = imuData.get(4).asFloat64();
accels[2] = imuData.get(5).asFloat64();

gyro[0] = imuData.get(6).asFloat64();
gyro[1] = imuData.get(7).asFloat64();
gyro[2] = imuData.get(8).asFloat64();

magn[0] = imuData.get(9).asFloat64();
magn[1] = imuData.get(10).asFloat64();
magn[2] = imuData.get(11).asFloat64();

m_mutex.unlock();
}
Loading