ForceSensor.cpp

Go to the documentation of this file.

/* 
 * Copyright (c) 2013 University of Jaume-I.
 * All rights reserved. This program and the accompanying materials
 * are made available under the terms of the GNU Public License v3.0
 * which accompanies this distribution, and is available at
 * http://www.gnu.org/licenses/gpl.html
 * 
 * Contributors:
 *     Mario Prats
 *     Javier Perez
 */

#include <pluginlib/class_list_macros.h>
#include <uwsim/ForceSensor.h>

ForceSensor::ForceSensor(ForceSensor_Config * cfg, osg::ref_ptr<osg::Node> target) :
    SimulatedDevice(cfg)
{
  this->target = target;
  this->offsetp[0]=cfg->offsetp[0];  this->offsetp[1]=cfg->offsetp[1];  this->offsetp[2]=cfg->offsetp[2];
  offset.makeRotate(osg::Quat(cfg->offsetr[0], osg::Vec3d(1, 0, 0), cfg->offsetr[1], osg::Vec3d(0, 1, 0), cfg->offsetr[2],osg::Vec3d(0, 0, 1)));
  physics=NULL;
}

static void forceSensorPreTickCallback(btDynamicsWorld *world, btScalar timeStep){
  ForceSensor *fs = static_cast<ForceSensor *>(world->getWorldUserInfo());
  fs->physicsInternalPreProcessCallback(timeStep);
}

static void forceSensorPostTickCallback(btDynamicsWorld *world, btScalar timeStep){
  ForceSensor *fs = static_cast<ForceSensor *>(world->getWorldUserInfo());
  fs->physicsInternalPostProcessCallback(timeStep);
}

void ForceSensor::applyPhysics(BulletPhysics * bulletPhysics)
{
  physics=bulletPhysics;
  osg::ref_ptr<NodeDataType> data = dynamic_cast<NodeDataType*>(target->getUserData());
  copy=physics->copyObject(data->rigidBody);
  copy->setGravity(btVector3(0,0,0));
  copy->setCenterOfMassTransform(btTransform(btQuaternion(0,0,0,1),btVector3(-0.2,0,0.75)));
  btTarget = data->rigidBody;
  btTarget->setCenterOfMassTransform(btTransform(btQuaternion(0,0,0,1),btVector3(-0.2,0,0.75)));

  physics->dynamicsWorld->setInternalTickCallback(forceSensorPreTickCallback, static_cast<void *>(this),true);
  physics->dynamicsWorld->setInternalTickCallback(forceSensorPostTickCallback, static_cast<void *>(this));
}

void ForceSensor::physicsInternalPreProcessCallback(btScalar timeStep)
{
  if (physics->physicsStep==1){
    copy->setCenterOfMassTransform(btTarget->getCenterOfMassTransform());
    copy->clearForces();
    copy->setLinearVelocity(btTarget->getLinearVelocity());
    copy->setAngularVelocity(btTarget->getAngularVelocity());
    linInitial=copy->getLinearVelocity();
    angInitial=copy->getAngularVelocity();
  }
  physics->physicsStep++;
}

void ForceSensor::physicsInternalPostProcessCallback(btScalar timeStep)
{
    linFinal=copy->getLinearVelocity();
    angFinal=copy->getAngularVelocity();
    lastTimeStep=timeStep;
}

void ForceSensor::getForceTorque(double force[3], double torque[3])
{
  if(physics->physicsStep ==0) //Check if physics is looping (data is not reliable)
  {

    osg::Matrix ObjectMat= offset * *getWorldCoords(target); //aply rotation offset to transform from world coords.

    osg::Vec3 res=ObjectMat.getRotate().inverse()*osg::Vec3(linFinal.x()-linInitial.x(),linFinal.y()-linInitial.y(),linFinal.z()-linInitial.z());

    force[0]=res.x();
    force[1]=res.y();
    force[2]=res.z();

    res=ObjectMat.getRotate().inverse()*osg::Vec3(angFinal.x()-angInitial.x(),angFinal.y()-angInitial.y(),angFinal.z()-angInitial.z());

    //torque forces are extremely high so they ar reduced.(some physics parameter must be wrong).
    torque[0]=res.x()/10;
    torque[1]=res.y()/10;
    torque[2]=res.z()/10;
  }
  else  //Maybe last reliable data should be sent
  {
    force[0]=0;
    force[1]=0;
    force[2]=0;

    torque[0]=0;
    torque[1]=0;
    torque[2]=0;

  }
}

SimulatedDeviceConfig::Ptr ForceSensor_Factory::processConfig(const xmlpp::Node* node, ConfigFile * config)
{
  ForceSensor_Config * cfg = new ForceSensor_Config(getType());
  xmlpp::Node::NodeList list = node->get_children();
  for (xmlpp::Node::NodeList::iterator iter = list.begin(); iter != list.end(); ++iter)
  {
    xmlpp::Node* child = dynamic_cast<const xmlpp::Node*>(*iter);
    if(child->get_name() == "target")
      config->extractStringChar(child, cfg->target);
    else if(child->get_name() == "offsetp")
      config->extractPositionOrColor(child, cfg->offsetp);
    else if(child->get_name() == "offsetr")
      config->extractPositionOrColor(child, cfg->offsetr);
  }
  return SimulatedDeviceConfig::Ptr(cfg);
}

bool ForceSensor_Factory::applyConfig(SimulatedIAUV * auv, Vehicle &vehicleChars, SceneBuilder *sceneBuilder,
                                      size_t iteration)
{
  if (iteration > 0)
    return true;
  for (size_t i = 0; i < vehicleChars.simulated_devices.size(); ++i)
    if (vehicleChars.simulated_devices[i]->getType() == this->getType())
    {
      ForceSensor_Config * cfg = dynamic_cast<ForceSensor_Config *>(vehicleChars.simulated_devices[i].get());
      osg::ref_ptr<osg::Node> target;
      for(i=0;i<auv->urdf->link.size();i++)
      {
        if(auv->urdf->link[i]->getName()==cfg->target)
        {
          target=auv->urdf->link[i];
        }
      }
      auv->devices->all.push_back(ForceSensor::Ptr(new ForceSensor(cfg,target)));
    }
  return true;
}

std::vector<boost::shared_ptr<ROSInterface> > ForceSensor_Factory::getInterface(
    ROSInterfaceInfo & rosInterface, std::vector<boost::shared_ptr<SimulatedIAUV> > & iauvFile)
{
  std::vector < boost::shared_ptr<ROSInterface> > ifaces;
  for (size_t i = 0; i < iauvFile.size(); ++i)
    for (size_t d = 0; d < iauvFile[i]->devices->all.size(); ++d)
      if (iauvFile[i]->devices->all[d]->getType() == this->getType()
          && iauvFile[i]->devices->all[d]->name == rosInterface.targetName)
      {
        ifaces.push_back(
            boost::shared_ptr < ROSInterface
                > (new ForceSensor_ROSPublisher(dynamic_cast<ForceSensor*>(iauvFile[i]->devices->all[d].get()),
                                                rosInterface.topic, rosInterface.rate)));
        //rosInterface.values are for new and non-standard xml configurations, but it looks like currently existing rosInterface fields are enough...
        //below is just an example how to get values, alternatively you can use rosInterface.values["name"]
        //for(std::map<std::string,std::string>::iterator it = rosInterface.values.begin() ;it != rosInterface.values.end();++it)
        //      ROS_INFO("rosInterface.values[%s]='%s'",  it->first.c_str(), it->second.c_str());
      }
  if (ifaces.size() == 0)
    ROS_WARN("Returning empty ROS interface for device %s...", rosInterface.targetName.c_str());
  return ifaces;
}

void ForceSensor_ROSPublisher::createPublisher(ros::NodeHandle &nh)
{
  ROS_INFO("ForceSensor_ROSPublisher on topic %s", topic.c_str());
  pub_ = nh.advertise < geometry_msgs::WrenchStamped > (topic, 1);
}

void ForceSensor_ROSPublisher::publish()
{

  double force[3], torque[3], elapsed;

  dev->getForceTorque(force,torque);

  ros::WallDuration t_diff = ros::WallTime::now() - last;
  elapsed = t_diff.toSec();

  geometry_msgs::WrenchStamped msg;
  msg.header.stamp = getROSTime();
  msg.header.frame_id =dev->target->getName();

  msg.wrench.force.x=force[0]/elapsed*(1/dev->btTarget->getInvMass());
  msg.wrench.force.y=force[1]/elapsed*(1/dev->btTarget->getInvMass());
  msg.wrench.force.z=force[2]/elapsed*(1/dev->btTarget->getInvMass());


  msg.wrench.torque.x=torque[0]/elapsed*(1/dev->btTarget->getInvMass());
  msg.wrench.torque.y=torque[1]/elapsed*(1/dev->btTarget->getInvMass());
  msg.wrench.torque.z=torque[2]/elapsed*(1/dev->btTarget->getInvMass());
  pub_.publish(msg);

  last = ros::WallTime::now();
}

#if ROS_VERSION_MINIMUM(1, 9, 0)
// new pluginlib API in Groovy and Hydro
PLUGINLIB_EXPORT_CLASS(ForceSensor_Factory, uwsim::SimulatedDeviceFactory)
#else
PLUGINLIB_REGISTER_CLASS(ForceSensor_Factory, ForceSensor_Factory, uwsim::SimulatedDeviceFactory)
#endif