Esempio n. 1
0
  void Line3D::jacobian(Matrix7x6d& Jp, Matrix7x6d& Jl, const Eigen::Isometry3d& x, const Line3D& l){
    Jp.setZero();
    Jl.setZero();
    Matrix6d A=Matrix6d::Zero();
    A.block<3,3>(0,0)=x.linear();
    A.block<3,3>(0,3)= _skew(x.translation())*x.linear();
    A.block<3,3>(3,3)=x.linear();
    Vector6d v=(Vector6d)l;
    Line3D lRemapped(v);

    Matrix6d D = Matrix6d::Zero();
    D.block<3,3>(0,0) = -_skew(l.d());
    D.block<3,3>(0,3) = -2*_skew(l.w());
    D.block<3,3>(3,3) = -2*_skew(l.d());
    Jp.block<6,6>(0,0) = A*D;

    Vector3d d = l.d();
    Vector3d w = l.w();
    double ln = d.norm();
    double iln = 1./ln;
    double iln3 = std::pow(iln,3);
    Matrix6d Jll;
    Jll <<
      iln,0,0,-(w.x()*d.x())*iln3,-(w.x()*d.y())*iln3,-(w.x()*d.z())*iln3,
      0,iln,0,-(w.y()*d.x())*iln3,-(w.y()*d.y())*iln3,-(w.y()*d.z())*iln3,
      0,0,iln,-(w.z()*d.x())*iln3,-(w.z()*d.y())*iln3,-(w.z()*d.z())*iln3,
      0,0,0,-(d.x()*d.x()-ln*ln)*iln3,-(d.x()*d.y())*iln3,-(d.x()*d.z())*iln3,
      0,0,0,-(d.x()*d.y())*iln3,-(d.y()*d.y()-ln*ln)*iln3,-(d.y()*d.z())*iln3,
      0,0,0,-(d.x()*d.z())*iln3,-(d.y()*d.z())*iln3,-(d.z()*d.z()-ln*ln)*iln3;
    Jl.block<6,6>(0,0) = A*Jll;
    Jl.block<1,6>(6,0) << 2*d.x(),2*d.y(),2*d.z();
  }
Esempio n. 2
0
/**
 * @function setRootTransform
 * @brief Set q (SCREW) from pose <x,y,z,r,p,y> 
 */
void InspectorTab::setRootTransform( dart::dynamics::Skeleton* robot, 
				     const Eigen::Matrix<double, 6, 1>& pose ) {
  dart::dynamics::Joint* joint = robot->getRootBodyNode()->getParentJoint();

  if(dynamic_cast<dart::dynamics::FreeJoint*>(joint)) {
    Matrix<double, 6, 1> q;
    Eigen::Isometry3d transform = Eigen::Isometry3d::Identity();
    transform.translation() = pose.head<3>();
    transform.linear() = dart::math::eulerXYZToMatrix(pose.tail<3>());
    q = dart::math::logMap(transform);
    joint->set_q( q );
  
  }
  else {
    Eigen::Isometry3d transform;
    transform.makeAffine();
    transform.linear() = dart::math::eulerXYZToMatrix(pose.tail<3>());
    transform.translation() = pose.head<3>();
    joint->setTransformFromParentBodyNode(transform);
  }
  
  for (int i = 0; i < robot->getNumBodyNodes(); ++i) {
    robot->getBodyNode(i)->updateTransform();
  }
}
Esempio n. 3
0
//==============================================================================
Eigen::Isometry3d State::getCOMFrame() const
{
  Eigen::Isometry3d T = Eigen::Isometry3d::Identity();

  // Y-axis
  Eigen::Vector3d yAxis = Eigen::Vector3d::UnitY();

  // X-axis
  Eigen::Vector3d xAxis = mPelvis->getTransform().linear().col(0);
  Eigen::Vector3d pelvisXAxis = mPelvis->getTransform().linear().col(0);
  double mag = yAxis.dot(pelvisXAxis);
  pelvisXAxis -= mag * yAxis;
  xAxis = pelvisXAxis.normalized();

  // Z-axis
  Eigen::Vector3d zAxis = xAxis.cross(yAxis);

  T.translation() = getCOM();

  T.linear().col(0) = xAxis;
  T.linear().col(1) = yAxis;
  T.linear().col(2) = zAxis;

  return T;
}
Esempio n. 4
0
    bool FLOATING_T::run(robot::robot_state_t& target, control_data_t* data)
    {
        // 1. Setup
        Eigen::Isometry3d body;
        target.get_body(body);

        // 1. Variables
        Eigen::Quaterniond save_rotation(data->joy_rotation);
        Eigen::Vector3d save_position = data->joy_position;
        
        Eigen::Quaterniond rotation(data->joy_rotation);
        Eigen::Vector3d& position = data->joy_position;

        // 2. Clamp to xyy
        position[2] = 0;
        rotation.y() = 0;
        rotation.x() = 0;

        body.linear() = body.linear() * rotation.matrix();
        body.translation() += position;

        // 3.
        target.set_body(body);

        return true;
    }    
Esempio n. 5
0
 Line3D operator*(const Eigen::Isometry3d& t, const Line3D& line){
   Matrix6d A=Matrix6d::Zero();
   A.block<3,3>(0,0)=t.linear();
   A.block<3,3>(0,3)= _skew(t.translation())*t.linear();
   A.block<3,3>(3,3)=t.linear();
   Vector6d v = (Vector6d)line;
   // cout << "v" << endl << v << endl;
   // cout << "A" << endl << A << endl;
   // cout << "Av" << endl << A*v << endl;
   return Line3D(A*v);
 }
BaseSensorDataNode* SyncSensorDataNodeMaker::makeNode(MapManager* manager, BaseSensorData* data) {
    SynchronizedSensorData* sdata = dynamic_cast<SynchronizedSensorData*>(data);
    if (! sdata)
        return 0;
    SyncSensorDataNode* snode = new SyncSensorDataNode(manager, sdata);
    for (size_t i = 0; i<sdata->sensorDatas.size(); i++) {
        IMUData* imu = dynamic_cast<IMUData*>(sdata->sensorDatas[i]);
        if (imu) {
            MapNodeUnaryRelation* imuRel = new MapNodeUnaryRelation(manager);
            imuRel->nodes()[0] = snode;
            Eigen::Isometry3d t;
            t.setIdentity();
            t.linear() = imu->orientation().toRotationMatrix();
            Eigen::Matrix<double, 6, 6> info;
            info.setZero();
            info.block<3,3>(3,3) = imu->orientationCovariance().inverse();
            //info.block<3,3>(3,3).setIdentity();
            imuRel->setTransform(t);
            imuRel->setInformationMatrix(info);
            snode->setImu(imuRel);
            break;
        }
    }
    return snode;
}
Esempio n. 7
0
Eigen::Isometry3d getRelativeTransform(TagMatch const& match, Eigen::Matrix3d const & camera_matrix, const Eigen::Vector4d &distortion_coefficients, double tag_size) 
{
  std::vector<cv::Point3f> objPts;
  std::vector<cv::Point2f> imgPts;
  double s = tag_size/2.;
  objPts.push_back(cv::Point3f(-s,-s, 0));
  objPts.push_back(cv::Point3f( s,-s, 0));
  objPts.push_back(cv::Point3f( s, s, 0));
  objPts.push_back(cv::Point3f(-s, s, 0));


  imgPts.push_back(match.p0);
  imgPts.push_back(match.p1);
  imgPts.push_back(match.p2);
  imgPts.push_back(match.p3);

  cv::Mat rvec, tvec;
  cv::Matx33f cameraMatrix(
                           camera_matrix(0,0), 0, camera_matrix(0,2),
                           0, camera_matrix(1,1), camera_matrix(1,2),
                           0,  0,  1);

  cv::Vec4f distParam(distortion_coefficients(0), distortion_coefficients(1), distortion_coefficients(2), distortion_coefficients(3)); 
  cv::solvePnP(objPts, imgPts, cameraMatrix, distParam, rvec, tvec);
  cv::Matx33d r;
  cv::Rodrigues(rvec, r);
  Eigen::Matrix3d wRo;
  wRo << r(0,0), r(0,1), r(0,2), r(1,0), r(1,1), r(1,2), r(2,0), r(2,1), r(2,2);

  Eigen::Isometry3d T; 
  T.linear() = wRo;
  T.translation() << tvec.at<double>(0), tvec.at<double>(1), tvec.at<double>(2);
  return T;
}
Esempio n. 8
0
//==============================================================================
Eigen::Vector6d FreeJoint::convertToPositions(const Eigen::Isometry3d& _tf)
{
  Eigen::Vector6d x;
  x.head<3>() = math::logMap(_tf.linear());
  x.tail<3>() = _tf.translation();
  return x;
}
Esempio n. 9
0
//==============================================================================
btTransform convertTransform(const Eigen::Isometry3d& _T)
{
  btTransform trans;
  trans.setOrigin(convertVector3(_T.translation()));
  trans.setBasis(convertMatrix3x3(_T.linear()));
  return trans;
}
Esempio n. 10
0
Eigen::Isometry3d toIsometry3d(const std::string& _str)
{
    Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
    Eigen::Vector6d elements = Eigen::Vector6d::Zero();
    std::vector<std::string> pieces;
    std::string trimedStr = boost::trim_copy(_str);
    boost::split(pieces, trimedStr, boost::is_any_of(" "), boost::token_compress_on);
    assert(pieces.size() == 6);

    for (size_t i = 0; i < pieces.size(); ++i)
    {
        if (pieces[i] != "")
        {
            try
            {
                elements(i) = boost::lexical_cast<double>(pieces[i].c_str());
            }
            catch(boost::bad_lexical_cast& e)
            {
                std::cerr << "value ["
                          << pieces[i]
                          << "] is not a valid double for SE3["
                          << i
                          << "]"
                          << std::endl;
            }
        }
    }

    T.linear() = math::eulerXYZToMatrix(elements.tail<3>());
    T.translation() = elements.head<3>();
    return T;
}
Esempio n. 11
0
//==============================================================================
void ShapeNode::setRelativeRotation(const Eigen::Matrix3d& rotation)
{
  Eigen::Isometry3d transform = getRelativeTransform();
  transform.linear() = rotation;

  setRelativeTransform(transform);
}
Esempio n. 12
0
bool RosMessageContext::getOdomPose(Eigen::Isometry3d& _trans, double time){
  bool transformFound = true;
  _tfListener->waitForTransform(_odomReferenceFrameId, _baseReferenceFrameId,
				ros::Time(time), ros::Duration(1.0));
  try{
    tf::StampedTransform t;
    _tfListener->lookupTransform(_odomReferenceFrameId, _baseReferenceFrameId,
				 ros::Time(time), t);
    Eigen::Isometry3d transform;
    transform.translation().x()=t.getOrigin().x();
    transform.translation().y()=t.getOrigin().y();
    transform.translation().z()=t.getOrigin().z();
    Eigen::Quaterniond rot;
    rot.x()=t.getRotation().x();
    rot.y()=t.getRotation().y();
    rot.z()=t.getRotation().z();
    rot.w()=t.getRotation().w();
    transform.linear()=rot.toRotationMatrix();
    _trans = transform;
    transformFound = true;
  }
  catch (tf::TransformException ex){
    ROS_ERROR("%s",ex.what());
    transformFound = false;
  }
  return transformFound;
}
Esempio n. 13
0
//==============================================================================
fcl::Transform3f FCLTypes::convertTransform(const Eigen::Isometry3d& _T)
{
  fcl::Transform3f trans;

  trans.setTranslation(convertVector3(_T.translation()));
  trans.setRotation(convertMatrix3x3(_T.linear()));

  return trans;
}
Esempio n. 14
0
Eigen::Vector3d RevoluteJoint::getWorldAxis() const
{
    Eigen::Isometry3d parentTransf = Eigen::Isometry3d::Identity();

    if (this->mParentBodyNode != NULL)
        parentTransf = mParentBodyNode->getWorldTransform();

    return parentTransf.linear() * mT_ParentBodyToJoint.linear() * mAxis;
}
Esempio n. 15
0
    bool LEG_AIK_T::run(robot::robot_state_t& target, control_data_t* data)
    {
        assert(data);
        
        if(!data->joystick_ok)
            return false;

        // 1. Set up
        kinematics::Skeleton* robot = data->robot;
        robot::robot_kinematics_t* robot_kin = data->kin;
        int ms = data->manip_side;
        int mi = ms ? robot::MANIP_L_FOOT : robot::MANIP_R_FOOT;
        Eigen::Isometry3d Twfoot = data->manip_xform[mi];
        
        Eigen::VectorXd save = target.dart_pose();

        assert(robot);
        assert(robot_kin);
        assert(ms == 1 || ms == 0);

        // 2. Target xform
        Twfoot.linear() = Twfoot.linear() * data->joy_rotation;
        Twfoot.translation() += data->joy_position;

        // 3. Run IK
        bool ok = robot_kin->leg_ik(Twfoot, ms, target);

        if(!ok)
            std::cout << "LEG_AIK: outside workspace\n";

        assert(target.check_limits());

        // 4. Save?
        if (!ok)
            target.set_dart_pose(save);
        else
            data->manip_xform[mi] = Twfoot;

        data->manip_target = Twfoot;

        return ok;
    }
Esempio n. 16
0
/**
 * @function param2Transf 
 * @brief Read a SQ_params instance and get the Tf 
 */
Eigen::Isometry3d param2Transf( const SQ_params &_par ) {

    Eigen::Isometry3d Tf = Eigen::Isometry3d::Identity();
    Tf.translation() << _par.px, _par.py, _par.pz;
    Eigen::Matrix3d rot;
    rot = Eigen::AngleAxisd(_par.ya, Eigen::Vector3d::UnitZ() )*
	Eigen::AngleAxisd( _par.pa, Eigen::Vector3d::UnitY() )*
	Eigen::AngleAxisd( _par.ra, Eigen::Vector3d::UnitX() );
    Tf.linear() = rot;
    return Tf;
}
Esempio n. 17
0
int main() {
    
    Eigen::Isometry3d pose;
    
    while(1) {
        fastrak.getPose(pose, 1);
        std::cout << "pose = \n" << Eigen::Quaterniond(pose.linear()).vec().transpose() << std::endl;
    }
    

}
Esempio n. 18
0
const Eigen::Matrix3d CMiniVisionToolbox::getEssential( const Eigen::Isometry3d& p_matTransformationFrom, const Eigen::Isometry3d& p_matTransformationTo )
{
    //ds compute essential matrix: http://en.wikipedia.org/wiki/Essential_matrix TODO check math!
    const Eigen::Isometry3d matTransformation( p_matTransformationTo.inverse( )*p_matTransformationFrom );
    //const Eigen::Isometry3d matTransformation( p_matTransformationFrom.inverse( )*p_matTransformationTo );
    //const Eigen::Isometry3d matTransformation( p_matTransformationTo*p_matTransformationFrom.inverse( ) );
    const Eigen::Matrix3d matSkewTranslation( CMiniVisionToolbox::getSkew( matTransformation.translation( ) ) );

    //ds compute essential matrix
    return matTransformation.linear( )*matSkewTranslation;
}
Esempio n. 19
0
//==============================================================================
dart::collision::fcl::Transform3 FCLTypes::convertTransform(const Eigen::Isometry3d& _T)
{
#if FCL_VERSION_AT_LEAST(0,6,0)
  return _T;
#else
  dart::collision::fcl::Transform3 trans;

  trans.setTranslation(convertVector3(_T.translation()));
  trans.setRotation(convertMatrix3x3(_T.linear()));

  return trans;
#endif
}
Esempio n. 20
0
void G2oOptimizer::addFeatureEdge(const SlamEdge &edge)
{
    if (node_id_to_g2o_map_.left.find(edge.id_from_) == node_id_to_g2o_map_.left.end() ||
            node_id_to_g2o_map_.left.find(edge.id_to_) == node_id_to_g2o_map_.left.end()) {
        ROS_DEBUG("did not find a node of edge %s", edge.id_.c_str());
        edge_filter_->remove(edge.id_);
        return;
    }

    if (static_cast<g2o::VertexSE3*>(node_id_to_g2o_map_.left.at(edge.id_from_))->fixed() &&
            static_cast<g2o::VertexSE3*>(node_id_to_g2o_map_.left.at(edge.id_to_))->fixed()) {
        edge_id_to_g2o_map_.insert(g2o_edge_bimap::value_type(edge.id_, NULL));
        return;
    }

    g2o::EdgeSE3* edge_sensor = new g2o::EdgeSE3();
    edge_sensor->setId(optimizer_count_);
    edge_sensor->vertices()[0] = node_id_to_g2o_map_.left.at(edge.id_from_);
    edge_sensor->vertices()[1] = node_id_to_g2o_map_.left.at(edge.id_to_);

    Eigen::Isometry3d transform = edge.displacement_from_ * sensor_transforms_[edge.sensor_from_] * edge.transform_ * sensor_transforms_[edge.sensor_to_].inverse() * edge.displacement_to_.inverse();
    if (config_.optimize_xy_only) {
        Eigen::Vector3d rpy = g2o::internal::toEuler(transform.linear());
        rpy(0) = 0;
        rpy(1) = 0;
        transform.linear() = g2o::internal::fromEuler(rpy);
        transform.translation()(2) = 0;
    }
    edge_sensor->setMeasurement(transform);

    edge_sensor->information() = edge.information_;
    g2o::RobustKernelHuber *rk = new g2o::RobustKernelHuber();
    rk->setDelta(1.);
    edge_sensor->setRobustKernel(rk);
    optimizer_.addEdge(edge_sensor);
    edge_id_to_g2o_map_.insert(g2o_edge_bimap::value_type(edge.id_, dynamic_cast<g2o::HyperGraph::Edge*>(edge_sensor)));

    optimizer_count_++;
}
Esempio n. 21
0
GTEST_TEST(TestLcmUtil, testPose) {
  default_random_engine generator;
  generator.seed(0);
  Eigen::Isometry3d pose;
  pose.linear() = drake::math::UniformlyRandomRotmat(generator);
  pose.translation().setLinSpaced(0, drake::kSpaceDimension);
  pose.makeAffine();
  const Eigen::Isometry3d& const_pose = pose;
  bot_core::position_3d_t msg;
  EncodePose(const_pose, msg);
  Eigen::Isometry3d pose_back = DecodePose(msg);
  EXPECT_TRUE(CompareMatrices(pose.matrix(), pose_back.matrix(), 1e-12,
                              MatrixCompareType::absolute));
}
Esempio n. 22
0
std::string toString(const Eigen::Isometry3d& _v)
{
    std::ostringstream ostr;
    ostr.precision(6);

    Eigen::Vector3d xyz = math::matrixToEulerXYZ(_v.linear());

    ostr << _v.translation()(0) << " "
         << _v.translation()(1) << " "
         << _v.translation()(2) << " ";
    ostr << xyz[0] << " " << xyz[1] << " " << xyz[2];

    return ostr.str();
}
Esempio n. 23
0
geometry_msgs::Pose GraphGridMapper::affineTransformToPoseMsg(const Eigen::Isometry3d &pose)
{
    Eigen::Vector3d position = pose.translation();
    Eigen::Quaterniond orientation(pose.linear());
    geometry_msgs::Pose poseMsg;
    poseMsg.position.x = position(0);
    poseMsg.position.y = position(1);
    poseMsg.position.z = position(2);
    poseMsg.orientation.w = orientation.w();
    poseMsg.orientation.x = orientation.x();
    poseMsg.orientation.y = orientation.y();
    poseMsg.orientation.z = orientation.z();
    return poseMsg;
}
Esempio n. 24
0
bool pronto_vis::interpolateScan(const std::vector<float>& iRanges,
                const double iTheta0, const double iThetaStep,
                const Eigen::Isometry3d& iPose0,
                const Eigen::Isometry3d& iPose1,
                std::vector<Eigen::Vector3f>& oPoints) {
  const int n = iRanges.size();
  if (n < 2) return false;
  const double tStep = 1.0/(n-1);
  Eigen::Quaterniond q0(iPose0.linear());
  Eigen::Quaterniond q1(iPose1.linear());
  Eigen::Vector3d pos0(iPose0.translation());
  Eigen::Vector3d pos1(iPose1.translation());
  oPoints.resize(n);
  double t = 0;
  double theta = iTheta0;
  for (int i = 0; i < n; ++i, t += tStep, theta += iThetaStep) {
    Eigen::Quaterniond q = q0.slerp(t,q1);
    Eigen::Vector3d pos = (1-t)*pos0 + t*pos1;
    Eigen::Vector3d pt = iRanges[i]*Eigen::Vector3d(cos(theta), sin(theta), 0);
    oPoints[i] = (q*pt + pos).cast<float>();
  }
  return true;
}
Esempio n. 25
0
Eigen::Isometry3d fromStampedTransform(const tf::StampedTransform& transform) {
  geometry_msgs::TransformStamped msg;
  tf::transformStampedTFToMsg(transform, msg);
  Eigen::Isometry3d t;
  Eigen::Quaterniond q(msg.transform.rotation.w, 
		       msg.transform.rotation.x, 
		       msg.transform.rotation.y, 
		       msg.transform.rotation.z);
  t.linear()=q.matrix();
  t.translation() = Eigen::Vector3d(msg.transform.translation.x,
				    msg.transform.translation.y,
				    msg.transform.translation.z);
  return t;
}
Esempio n. 26
0
math::Jacobian
TemplatedJacobianNode<NodeType>::getJacobianSpatialDeriv(
    const Eigen::Vector3d& _offset,
    const Frame* _inCoordinatesOf) const
{
  if(this == _inCoordinatesOf)
    return getJacobianSpatialDeriv(_offset);

  Eigen::Isometry3d T = getTransform(_inCoordinatesOf);
  T.translation() = T.linear() * -_offset;

  return math::AdTJac(
        T, static_cast<const NodeType*>(this)->getJacobianSpatialDeriv());
}
Esempio n. 27
0
geometry_msgs::Pose Conversions::toMsg(const Eigen::Isometry3d &pose)
{
    // Store the pose in a ROS pose message.
    Eigen::Vector3d position = pose.translation();
    Eigen::Quaterniond orientation(pose.linear());
    geometry_msgs::Pose poseMsg;
    poseMsg.position.x = position(0);
    poseMsg.position.y = position(1);
    poseMsg.position.z = position(2);
    poseMsg.orientation.w = orientation.w();
    poseMsg.orientation.x = orientation.x();
    poseMsg.orientation.y = orientation.y();
    poseMsg.orientation.z = orientation.z();
    return poseMsg;
}
Esempio n. 28
0
/**
 * @function transf2Params
 * @brief Fill par arguments for rot and trans from a _Tf
 */
void transf2Params( const Eigen::Isometry3d &_Tf,
		    SQ_params &_par ) {

    _par.px = _Tf.translation().x();
    _par.py = _Tf.translation().y();
    _par.pz = _Tf.translation().z();

    double r31, r11, r21, r32, r33;
    double r, p, y;

    r31 = _Tf.linear()(2,0);
    r11 = _Tf.linear()(0,0);
    r21 = _Tf.linear()(1,0);
    r32 = _Tf.linear()(2,1);
    r33 = _Tf.linear()(2,2);

    p = atan2( -r31, sqrt(r11*r11 + r21*r21) );
    y = atan2( r21 / cos(p), r11 / cos(p) );
    r = atan2( r32 / cos(p), r33 / cos(p) );

    _par.ra = r;
    _par.pa = p;
    _par.ya = y;
}
Esempio n. 29
0
/**
 * @function getRootTransform
 * @brief Return <x,y,z, r, p, y> Remember that get_q() gives you the screw so 
 * do NOT use it directly
 */
Eigen::Matrix<double, 6, 1> InspectorTab::getRootTransform(dart::dynamics::Skeleton* robot) {
  dart::dynamics::Joint *joint = robot->getRootBodyNode()->getParentJoint();
  Eigen::Matrix<double, 6, 1> pose;

  if(joint->getJointType() == dart::dynamics::Joint::FREE) {
    Matrix<double, 6, 1> q = joint->get_q();
    Eigen::Isometry3d Tf = dart::math::expMap( joint->get_q() );
    pose.head<3>() = Tf.translation();
    pose.tail<3>() = dart::math::matrixToEulerXYZ( Tf.linear() );
  }
  else {
    pose = getPoseFromTransform(joint->getTransformFromParentBodyNode());
  }

  return pose;
}
Esempio n. 30
0
/**
 * @function parseWorld
 */
simulation::World* DartLoader::parseWorld(std::string _urdfFileName) {

    std::string xmlString = readFileToString(_urdfFileName);

    // Change path to a Unix-style path if given a Windows one
    // Windows can handle Unix-style paths (apparently)
    std::replace(_urdfFileName.begin(), _urdfFileName.end(), '\\' , '/');
    std::string worldDirectory = _urdfFileName.substr(0, _urdfFileName.rfind("/") + 1);
    
    urdf::World* worldInterface = urdf::parseWorldURDF(xmlString, worldDirectory);
    if(!worldInterface)
        return NULL;

    // Store paths from world to entities
    parseWorldToEntityPaths(xmlString);

    simulation::World* world = new simulation::World();

    for(unsigned int i = 0; i < worldInterface->models.size(); ++i) {
      std::string skeletonDirectory = worldDirectory + mWorld_To_Entity_Paths.find(worldInterface->models[i].model->getName())->second;
      dynamics::Skeleton* skeleton = modelInterfaceToSkeleton(worldInterface->models[i].model.get(), skeletonDirectory);

      if(!skeleton) {
        std::cout << "[ERROR] Robot " << worldInterface->models[i].model->getName() << " was not correctly parsed. World is not loaded. Exiting!" << std::endl;
        return NULL; 
      }

      // Initialize position and RPY
      dynamics::Joint* rootJoint = skeleton->getRootBodyNode()->getParentJoint();
      Eigen::Isometry3d transform = toEigen(worldInterface->models[i].origin);

      if(dynamic_cast<dynamics::FreeJoint*>(rootJoint)) {
          Eigen::Vector6d coordinates;
          coordinates << math::logMap(transform.linear()), transform.translation();
          rootJoint->set_q(coordinates);
      }
      else {
          rootJoint->setTransformFromParentBodyNode(transform);
      }

      world->addSkeleton(skeleton);
    }
    return world;
}