Пример #1
0
 void drawDemo (Graphics& g) override
 {
     g.setColour (Colours::black.withAlpha (getAlpha()));
     glyphs.draw (g, getTransform());
 }
Пример #2
0
/**
 * @param rect the new size of the container
 * @param invalid the views to dirty
 */
void CViewContainer::setViewSize (const CRect &rect, bool invalid)
{
    if (rect == getViewSize ())
        return;

    CRect oldSize (getViewSize ());
    CView::setViewSize (rect, invalid);

    if (getAutosizingEnabled ())
    {
        CCoord widthDelta = rect.getWidth () - oldSize.getWidth ();
        CCoord heightDelta = rect.getHeight () - oldSize.getHeight ();
        getTransform ().inverse ().transform (widthDelta, heightDelta);

        if (widthDelta != 0 || heightDelta != 0)
        {
            uint32_t numSubviews = getNbViews ();
            uint32_t counter = 0;
            bool treatAsColumn = (getAutosizeFlags () & kAutosizeColumn) != 0;
            bool treatAsRow = (getAutosizeFlags () & kAutosizeRow) != 0;
            FOREACHSUBVIEW
            int32_t autosize = pV->getAutosizeFlags ();
            CRect viewSize (pV->getViewSize ());
            CRect mouseSize (pV->getMouseableArea ());
            if (treatAsColumn)
            {
                if (counter)
                {
                    viewSize.offset (counter * (widthDelta / (numSubviews)), 0);
                    mouseSize.offset (counter * (widthDelta / (numSubviews)), 0);
                }
                viewSize.setWidth (viewSize.getWidth () + (widthDelta / (numSubviews)));
                mouseSize.setWidth (mouseSize.getWidth () + (widthDelta / (numSubviews)));
            }
            else if (widthDelta != 0 && autosize & kAutosizeRight)
            {
                viewSize.right += widthDelta;
                mouseSize.right += widthDelta;
                if (!(autosize & kAutosizeLeft))
                {
                    viewSize.left += widthDelta;
                    mouseSize.left += widthDelta;
                }
            }
            if (treatAsRow)
            {
                if (counter)
                {
                    viewSize.offset (0, counter * (heightDelta / (numSubviews)));
                    mouseSize.offset (0, counter * (heightDelta / (numSubviews)));
                }
                viewSize.setHeight (viewSize.getHeight () + (heightDelta / (numSubviews)));
                mouseSize.setHeight (mouseSize.getHeight () + (heightDelta / (numSubviews)));
            }
            else if (heightDelta != 0 && autosize & kAutosizeBottom)
            {
                viewSize.bottom += heightDelta;
                mouseSize.bottom += heightDelta;
                if (!(autosize & kAutosizeTop))
                {
                    viewSize.top += heightDelta;
                    mouseSize.top += heightDelta;
                }
            }
            if (viewSize != pV->getViewSize ())
            {
                pV->setViewSize (viewSize);
                pV->setMouseableArea (mouseSize);
            }
            counter++;
            ENDFOREACHSUBVIEW
        }
    }
Пример #3
0
void scanCallback (const sensor_msgs::LaserScan::ConstPtr& scan_in)
{
    if (!we_have_a_map)
    {
        ROS_INFO("SnapMapICP waiting for map to be published");
        return;
    }

    ros::Time scan_in_time = scan_in->header.stamp;
    ros::Time time_received = ros::Time::now();

    if ( scan_in_time - last_processed_scan < ros::Duration(1.0f / SCAN_RATE) )
    {
        ROS_DEBUG("rejected scan, last %f , this %f", last_processed_scan.toSec() ,scan_in_time.toSec());
        return;
    }


    //projector_.transformLaserScanToPointCloud("base_link",*scan_in,cloud,listener_);
    if (!scan_callback_mutex.try_lock())
        return;

    ros::Duration scan_age = ros::Time::now() - scan_in_time;

    //check if we want to accept this scan, if its older than 1 sec, drop it
    if (!use_sim_time)
        if (scan_age.toSec() > AGE_THRESHOLD)
        {
            //ROS_WARN("SCAN SEEMS TOO OLD (%f seconds, %f threshold)", scan_age.toSec(), AGE_THRESHOLD);
            ROS_WARN("SCAN SEEMS TOO OLD (%f seconds, %f threshold) scan time: %f , now %f", scan_age.toSec(), AGE_THRESHOLD, scan_in_time.toSec(),ros::Time::now().toSec() );
            scan_callback_mutex.unlock();

            return;
        }

    count_sc_++;
    //ROS_DEBUG("count_sc %i MUTEX LOCKED", count_sc_);

    //if (count_sc_ > 10)
    //if (count_sc_ > 10)
    {
        count_sc_ = 0;

        tf::StampedTransform base_at_laser;
        if (!getTransform(base_at_laser, ODOM_FRAME, "base_link", scan_in_time))
        {
            ROS_WARN("Did not get base pose at laser scan time");
            scan_callback_mutex.unlock();

            return;
        }


        sensor_msgs::PointCloud cloud;
        sensor_msgs::PointCloud cloudInMap;

        projector_->projectLaser(*scan_in,cloud);

        we_have_a_scan = false;
        bool gotTransform = false;

        if (!listener_->waitForTransform("/map", cloud.header.frame_id, cloud.header.stamp, ros::Duration(0.05)))
        {
            scan_callback_mutex.unlock();
            ROS_WARN("SnapMapICP no map to cloud transform found MUTEX UNLOCKED");
            return;
        }

        if (!listener_->waitForTransform("/map", "/base_link", cloud.header.stamp, ros::Duration(0.05)))
        {
            scan_callback_mutex.unlock();
            ROS_WARN("SnapMapICP no map to base transform found MUTEX UNLOCKED");
            return;
        }


        while (!gotTransform && (ros::ok()))
        {
            try
            {
                gotTransform = true;
                listener_->transformPointCloud ("/map",cloud,cloudInMap);
            }
            catch (...)
            {
                gotTransform = false;
                ROS_WARN("DIDNT GET TRANSFORM IN A");
            }
        }

        for (size_t k =0; k < cloudInMap.points.size(); k++)
        {
            cloudInMap.points[k].z = 0;
        }


        gotTransform = false;
        tf::StampedTransform oldPose;
        while (!gotTransform && (ros::ok()))
        {
            try
            {
                gotTransform = true;
                listener_->lookupTransform("/map", "/base_link",
                                           cloud.header.stamp , oldPose);
            }
            catch (tf::TransformException ex)
            {
                gotTransform = false;
                ROS_WARN("DIDNT GET TRANSFORM IN B");
            }
        }
        if (we_have_a_map && gotTransform)
        {
            sensor_msgs::convertPointCloudToPointCloud2(cloudInMap,cloud2);
            we_have_a_scan = true;

            actScan++;

            //pcl::IterativeClosestPointNonLinear<pcl::PointXYZ, pcl::PointXYZ> reg;
            pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> reg;
            reg.setTransformationEpsilon (1e-6);
            // Set the maximum distance between two correspondences (src<->tgt) to 10cm
            // Note: adjust this based on the size of your datasets
            reg.setMaxCorrespondenceDistance(0.5);
            reg.setMaximumIterations (ICP_NUM_ITER);
            // Set the point representation

            //ros::Time bef = ros::Time::now();

            PointCloudT::Ptr myMapCloud (new PointCloudT());
            PointCloudT::Ptr myScanCloud (new PointCloudT());

            pcl::fromROSMsg(*output_cloud,*myMapCloud);
            pcl::fromROSMsg(cloud2,*myScanCloud);

            reg.setInputCloud(myScanCloud);
            reg.setInputTarget(myMapCloud);

            PointCloudT unused;
            int i = 0;

            reg.align (unused);

            const Eigen::Matrix4f &transf = reg.getFinalTransformation();
            tf::Transform t;
            matrixAsTransfrom(transf,t);

            //ROS_ERROR("proc time %f", (ros::Time::now() - bef).toSec());

            we_have_a_scan_transformed = false;
            PointCloudT transformedCloud;
            pcl::transformPointCloud (*myScanCloud, transformedCloud, reg.getFinalTransformation());

            double inlier_perc = 0;
            {
                // count inliers
                std::vector<int> nn_indices (1);
                std::vector<float> nn_sqr_dists (1);

                size_t numinliers = 0;

                for (size_t k = 0; k < transformedCloud.points.size(); ++k )
                {
                    if (mapTree->radiusSearch (transformedCloud.points[k], ICP_INLIER_DIST, nn_indices,nn_sqr_dists, 1) != 0)
                        numinliers += 1;
                }
                if (transformedCloud.points.size() > 0)
                {
                    //ROS_INFO("Inliers in dist %f: %zu of %zu percentage %f (%f)", ICP_INLIER_DIST, numinliers, transformedCloud.points.size(), (double) numinliers / (double) transformedCloud.points.size(), ICP_INLIER_THRESHOLD);
                    inlier_perc = (double) numinliers / (double) transformedCloud.points.size();
                }
            }

            last_processed_scan = scan_in_time;

            pcl::toROSMsg (transformedCloud, cloud2transformed);
            we_have_a_scan_transformed = true;

            double dist = sqrt((t.getOrigin().x() * t.getOrigin().x()) + (t.getOrigin().y() * t.getOrigin().y()));
            double angleDist = t.getRotation().getAngle();
            tf::Vector3 rotAxis  = t.getRotation().getAxis();
            t =  t * oldPose;

            tf::StampedTransform base_after_icp;
            if (!getTransform(base_after_icp, ODOM_FRAME, "base_link", ros::Time(0)))
            {
                ROS_WARN("Did not get base pose at now");
                scan_callback_mutex.unlock();

                return;
            }
            else
            {
                tf::Transform rel = base_at_laser.inverseTimes(base_after_icp);
                ROS_DEBUG("relative motion of robot while doing icp: %fcm %fdeg", rel.getOrigin().length(), rel.getRotation().getAngle() * 180 / M_PI);
                t= t * rel;
            }

            //ROS_DEBUG("dist %f angleDist %f",dist, angleDist);

            //ROS_DEBUG("SCAN_AGE seems to be %f", scan_age.toSec());
            char msg_c_str[2048];
            sprintf(msg_c_str,"INLIERS %f (%f) scan_age %f (%f age_threshold) dist %f angleDist %f axis(%f %f %f) fitting %f (icp_fitness_threshold %f)",inlier_perc, ICP_INLIER_THRESHOLD, scan_age.toSec(), AGE_THRESHOLD ,dist, angleDist, rotAxis.x(), rotAxis.y(), rotAxis.z(),reg.getFitnessScore(), ICP_FITNESS_THRESHOLD );
            std_msgs::String strmsg;
            strmsg.data = msg_c_str;

            //ROS_DEBUG("%s", msg_c_str);

            double cov = POSE_COVARIANCE_TRANS;

            //if ((actScan - lastTimeSent > UPDATE_AGE_THRESHOLD) && ((dist > DIST_THRESHOLD) || (angleDist > ANGLE_THRESHOLD)) && (angleDist < ANGLE_UPPER_THRESHOLD))
            //  if ( reg.getFitnessScore()  <= ICP_FITNESS_THRESHOLD )
            if ((actScan - lastTimeSent > UPDATE_AGE_THRESHOLD) && ((dist > DIST_THRESHOLD) || (angleDist > ANGLE_THRESHOLD)) && (inlier_perc > ICP_INLIER_THRESHOLD) && (angleDist < ANGLE_UPPER_THRESHOLD))
            {
                lastTimeSent = actScan;
                geometry_msgs::PoseWithCovarianceStamped pose;
                pose.header.frame_id = "map";
                pose.pose.pose.position.x = t.getOrigin().x();
                pose.pose.pose.position.y = t.getOrigin().y();

                tf::Quaternion quat = t.getRotation();
                //quat.setRPY(0.0, 0.0, theta);
                tf::quaternionTFToMsg(quat,pose.pose.pose.orientation);
                float factorPos = 0.03;
                float factorRot = 0.1;
                pose.pose.covariance[6*0+0] = (cov * cov) * factorPos;
                pose.pose.covariance[6*1+1] = (cov * cov) * factorPos;
                pose.pose.covariance[6*3+3] = (M_PI/12.0 * M_PI/12.0) * factorRot;
                ROS_DEBUG("i %i converged %i SCORE: %f", i,  reg.hasConverged (),  reg.getFitnessScore()  );
                ROS_DEBUG("PUBLISHING A NEW INITIAL POSE dist %f angleDist %f Setting pose: %.3f %.3f  [frame=%s]",dist, angleDist , pose.pose.pose.position.x  , pose.pose.pose.position.y , pose.header.frame_id.c_str());
                pub_pose.publish(pose);
                strmsg.data += " << SENT";
            }

            //ROS_INFO("processing time : %f", (ros::Time::now() - time_received).toSec());

            pub_info_.publish(strmsg);
            //ROS_DEBUG("map width %i height %i size %i, %s", myMapCloud.width, myMapCloud.height, (int)myMapCloud.points.size(), myMapCloud.header.frame_id.c_str());
            //ROS_DEBUG("scan width %i height %i size %i, %s", myScanCloud.width, myScanCloud.height, (int)myScanCloud.points.size(), myScanCloud.header.frame_id.c_str());
        }
    }
    scan_callback_mutex.unlock();
}
void ScanMatcher::getPoseDifference(const tf::Transform& other, float_t& position_difference, float_t& orientation_difference) const {
  position_difference = getTransform().getOrigin().distance(other.getOrigin());
  orientation_difference = getTransform().getRotation().angleShortestPath(other.getRotation());
}
Пример #5
0
 void Label::draw(sf::RenderTarget& target, sf::RenderStates states) const
 {
     states.transform *= getTransform();
     target.draw(mText, states);
 }
Пример #6
0
Point3F Etherform::_move( const F32 travelTime, Collision *outCol )
{
   // Try and move to new pos
   F32 totalMotion  = 0.0f;
   
   // TODO: not used?
   //F32 initialSpeed = mVelocity.len();

   Point3F start;
   Point3F initialPosition;
   getTransform().getColumn(3,&start);
   initialPosition = start;

   static CollisionList collisionList;
   static CollisionList physZoneCollisionList;

   collisionList.clear();
   physZoneCollisionList.clear();

   MatrixF collisionMatrix(true);
   collisionMatrix.setColumn(3, start);

   VectorF firstNormal(0.0f, 0.0f, 0.0f);
   F32 time = travelTime;
   U32 count = 0;

   static Polyhedron sBoxPolyhedron;
   static ExtrudedPolyList sExtrudedPolyList;
   static ExtrudedPolyList sPhysZonePolyList;

   for (; count < sMoveRetryCount; count++) {
      F32 speed = mVelocity.len();
      if(!speed)
         break;

      Point3F end = start + mVelocity * time;
      Point3F distance = end - start;

      if (mFabs(distance.x) < mObjBox.len_x() &&
          mFabs(distance.y) < mObjBox.len_y() &&
          mFabs(distance.z) < mObjBox.len_z())
      {
         // We can potentially early out of this.  If there are no polys in the clipped polylist at our
         //  end position, then we can bail, and just set start = end;
         Box3F wBox = mScaledBox;
         wBox.minExtents += end;
         wBox.maxExtents += end;

         static EarlyOutPolyList eaPolyList;
         eaPolyList.clear();
         eaPolyList.mNormal.set(0.0f, 0.0f, 0.0f);
         eaPolyList.mPlaneList.clear();
         eaPolyList.mPlaneList.setSize(6);
         eaPolyList.mPlaneList[0].set(wBox.minExtents,VectorF(-1.0f, 0.0f, 0.0f));
         eaPolyList.mPlaneList[1].set(wBox.maxExtents,VectorF(0.0f, 1.0f, 0.0f));
         eaPolyList.mPlaneList[2].set(wBox.maxExtents,VectorF(1.0f, 0.0f, 0.0f));
         eaPolyList.mPlaneList[3].set(wBox.minExtents,VectorF(0.0f, -1.0f, 0.0f));
         eaPolyList.mPlaneList[4].set(wBox.minExtents,VectorF(0.0f, 0.0f, -1.0f));
         eaPolyList.mPlaneList[5].set(wBox.maxExtents,VectorF(0.0f, 0.0f, 1.0f));

         // Build list from convex states here...
         CollisionWorkingList& rList = mConvex.getWorkingList();
         CollisionWorkingList* pList = rList.wLink.mNext;
         while (pList != &rList) {
            Convex* pConvex = pList->mConvex;
            if (pConvex->getObject()->getTypeMask() & sCollisionMoveMask) {
               Box3F convexBox = pConvex->getBoundingBox();
               if (wBox.isOverlapped(convexBox))
               {
                  // No need to separate out the physical zones here, we want those
                  //  to cause a fallthrough as well...
                  pConvex->getPolyList(&eaPolyList);
               }
            }
            pList = pList->wLink.mNext;
         }

         if (eaPolyList.isEmpty())
         {
            totalMotion += (end - start).len();
            start = end;
            break;
         }
      }

      collisionMatrix.setColumn(3, start);
      sBoxPolyhedron.buildBox(collisionMatrix, mScaledBox, true);

      // Setup the bounding box for the extrudedPolyList
      Box3F plistBox = mScaledBox;
      collisionMatrix.mul(plistBox);
      Point3F oldMin = plistBox.minExtents;
      Point3F oldMax = plistBox.maxExtents;
      plistBox.minExtents.setMin(oldMin + (mVelocity * time) - Point3F(0.1f, 0.1f, 0.1f));
      plistBox.maxExtents.setMax(oldMax + (mVelocity * time) + Point3F(0.1f, 0.1f, 0.1f));

      // Build extruded polyList...
      VectorF vector = end - start;
      sExtrudedPolyList.extrude(sBoxPolyhedron,vector);
      sExtrudedPolyList.setVelocity(mVelocity);
      sExtrudedPolyList.setCollisionList(&collisionList);

      sPhysZonePolyList.extrude(sBoxPolyhedron,vector);
      sPhysZonePolyList.setVelocity(mVelocity);
      sPhysZonePolyList.setCollisionList(&physZoneCollisionList);

      // Build list from convex states here...
      CollisionWorkingList& rList = mConvex.getWorkingList();
      CollisionWorkingList* pList = rList.wLink.mNext;
      while (pList != &rList) {
         Convex* pConvex = pList->mConvex;
         if (pConvex->getObject()->getTypeMask() & sCollisionMoveMask) {
            Box3F convexBox = pConvex->getBoundingBox();
            if (plistBox.isOverlapped(convexBox))
            {
               if (pConvex->getObject()->getTypeMask() & PhysicalZoneObjectType)
                  pConvex->getPolyList(&sPhysZonePolyList);
               else
                  pConvex->getPolyList(&sExtrudedPolyList);
            }
         }
         pList = pList->wLink.mNext;
      }

      // Take into account any physical zones...
      for (U32 j = 0; j < physZoneCollisionList.getCount(); j++) 
      {
         AssertFatal(dynamic_cast<PhysicalZone*>(physZoneCollisionList[j].object), "Bad phys zone!");
         const PhysicalZone* pZone = (PhysicalZone*)physZoneCollisionList[j].object;
         if (pZone->isActive())
            mVelocity *= pZone->getVelocityMod();
      }

      if (collisionList.getCount() != 0 && collisionList.getTime() < 1.0f) 
      {
         // Set to collision point
         F32 velLen = mVelocity.len();

         F32 dt = time * getMin(collisionList.getTime(), 1.0f);
         start += mVelocity * dt;
         time -= dt;

         totalMotion += velLen * dt;

         // Back off...
         if ( velLen > 0.f ) {
            F32 newT = getMin(0.01f / velLen, dt);
            start -= mVelocity * newT;
            totalMotion -= velLen * newT;
         }

         // Pick the surface most parallel to the face that was hit.
         const Collision *collision = &collisionList[0];
         const Collision *cp = collision + 1;
         const Collision *ep = collision + collisionList.getCount();
         for (; cp != ep; cp++)
         {
            if (cp->faceDot > collision->faceDot)
               collision = cp;
         }

         F32 bd = -mDot(mVelocity, collision->normal);

         // Copy this collision out so
         // we can use it to do impacts
         // and query collision.
         *outCol = *collision;

         // Subtract out velocity
         VectorF dv = collision->normal * (bd + sNormalElasticity);
         mVelocity += dv;
         if (count == 0)
         {
            firstNormal = collision->normal;
         }
         else
         {
            if (count == 1)
            {
               // Re-orient velocity along the crease.
               if (mDot(dv,firstNormal) < 0.0f &&
                   mDot(collision->normal,firstNormal) < 0.0f)
               {
                  VectorF nv;
                  mCross(collision->normal,firstNormal,&nv);
                  F32 nvl = nv.len();
                  if (nvl)
                  {
                     if (mDot(nv,mVelocity) < 0.0f)
                        nvl = -nvl;
                     nv *= mVelocity.len() / nvl;
                     mVelocity = nv;
                  }
               }
            }
         }
      }
      else
      {
         totalMotion += (end - start).len();
         start = end;
         break;
      }
   }

   if (count == sMoveRetryCount)
   {
      // Failed to move
      start = initialPosition;
      mVelocity.set(0.0f, 0.0f, 0.0f);
   }

   return start;
}
void Tile::draw(sf::RenderTarget& target, sf::RenderStates states) const{
	states.transform *= getTransform();
	target.draw(rect, states);
}
Пример #8
0
void Animation::draw(sf::RenderTarget& target, sf::RenderStates states) const
{
    states.transform *= getTransform();
	target.draw(mSprite, states);
}
Пример #9
0
 SpriteMovement::SpriteMovement() : data(SpriteMovementMotor::defaultSpriteData()) {
   state_machine = std::unique_ptr<SpriteMovementMotor::SpriteFSM>(new SpriteMovementMotor::SpriteFSM(data, SpriteMovementMotor::FACE_DOWN, this));
   data->transform = getTransform();
   data->current_level = 0;
 }
Пример #10
0
void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
{
	if(odometry_->getPose().isNull() &&
	   !groundTruthFrameId_.empty())
	{
		// sync with the first value of the ground truth
		Transform initialPose = getTransform(groundTruthFrameId_, frameId_, stamp);
		if(initialPose.isNull())
		{
			return;
		}

		ROS_INFO("Initializing odometry pose to %s (from \"%s\" -> \"%s\")",
				initialPose.prettyPrint().c_str(),
				groundTruthFrameId_.c_str(),
				frameId_.c_str());
		odometry_->reset(initialPose);
	}

	// process data
	ros::WallTime time = ros::WallTime::now();
	rtabmap::OdometryInfo info;
	SensorData dataCpy = data;
	rtabmap::Transform pose = odometry_->process(dataCpy, &info);
	if(!pose.isNull())
	{
		//*********************
		// Update odometry
		//*********************
		geometry_msgs::TransformStamped poseMsg;
		poseMsg.child_frame_id = frameId_;
		poseMsg.header.frame_id = odomFrameId_;
		poseMsg.header.stamp = stamp;
		rtabmap_ros::transformToGeometryMsg(pose, poseMsg.transform);

		if(publishTf_)
		{
			tfBroadcaster_.sendTransform(poseMsg);
		}

		if(odomPub_.getNumSubscribers())
		{
			//next, we'll publish the odometry message over ROS
			nav_msgs::Odometry odom;
			odom.header.stamp = stamp; // use corresponding time stamp to image
			odom.header.frame_id = odomFrameId_;
			odom.child_frame_id = frameId_;

			//set the position
			odom.pose.pose.position.x = poseMsg.transform.translation.x;
			odom.pose.pose.position.y = poseMsg.transform.translation.y;
			odom.pose.pose.position.z = poseMsg.transform.translation.z;
			odom.pose.pose.orientation = poseMsg.transform.rotation;

			//set covariance
			odom.pose.covariance.at(0) = info.variance;  // xx
			odom.pose.covariance.at(7) = info.variance;  // yy
			odom.pose.covariance.at(14) = info.variance; // zz
			odom.pose.covariance.at(21) = info.variance; // rr
			odom.pose.covariance.at(28) = info.variance; // pp
			odom.pose.covariance.at(35) = info.variance; // yawyaw

			//set velocity
			if(previousStamp_.isValid())
			{
				float x,y,z,roll,pitch,yaw;
				odometry_->previousVelocityTransform().getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
				odom.twist.twist.linear.x = x;
				odom.twist.twist.linear.y = y;
				odom.twist.twist.linear.z = z;
				odom.twist.twist.angular.x = roll;
				odom.twist.twist.angular.y = pitch;
				odom.twist.twist.angular.z = yaw;
			}
			previousStamp_ = stamp;

			//publish the message
			odomPub_.publish(odom);
		}

		// local map / reference frame
		if(odomLocalMap_.getNumSubscribers() && dynamic_cast<OdometryF2M*>(odometry_))
		{
			pcl::PointCloud<pcl::PointXYZ> cloud;
			const std::multimap<int, cv::Point3f> & map = ((OdometryF2M*)odometry_)->getMap().getWords3();
			for(std::multimap<int, cv::Point3f>::const_iterator iter=map.begin(); iter!=map.end(); ++iter)
			{
				cloud.push_back(pcl::PointXYZ(iter->second.x, iter->second.y, iter->second.z));
			}
			sensor_msgs::PointCloud2 cloudMsg;
			pcl::toROSMsg(cloud, cloudMsg);
			cloudMsg.header.stamp = stamp; // use corresponding time stamp to image
			cloudMsg.header.frame_id = odomFrameId_;
			odomLocalMap_.publish(cloudMsg);
		}

		if(odomLastFrame_.getNumSubscribers())
		{
			if(dynamic_cast<OdometryF2M*>(odometry_))
			{
				const std::multimap<int, cv::Point3f> & words3  = ((OdometryF2M*)odometry_)->getLastFrame().getWords3();
				if(words3.size())
				{
					pcl::PointCloud<pcl::PointXYZ> cloud;
					for(std::multimap<int, cv::Point3f>::const_iterator iter=words3.begin(); iter!=words3.end(); ++iter)
					{
						// transform to odom frame
						cv::Point3f pt = util3d::transformPoint(iter->second, pose);
						cloud.push_back(pcl::PointXYZ(pt.x, pt.y, pt.z));
					}

					sensor_msgs::PointCloud2 cloudMsg;
					pcl::toROSMsg(cloud, cloudMsg);
					cloudMsg.header.stamp = stamp; // use corresponding time stamp to image
					cloudMsg.header.frame_id = odomFrameId_;
					odomLastFrame_.publish(cloudMsg);
				}
			}
			else
			{
				//Frame to Frame
				const Signature & refFrame = ((OdometryF2F*)odometry_)->getRefFrame();
				if(refFrame.getWords3().size())
				{
					pcl::PointCloud<pcl::PointXYZ> cloud;
					for(std::multimap<int, cv::Point3f>::const_iterator iter=refFrame.getWords3().begin(); iter!=refFrame.getWords3().end(); ++iter)
					{
						// transform to odom frame
						cv::Point3f pt = util3d::transformPoint(iter->second, pose);
						cloud.push_back(pcl::PointXYZ(pt.x, pt.y, pt.z));
					}
					sensor_msgs::PointCloud2 cloudMsg;
					pcl::toROSMsg(cloud, cloudMsg);
					cloudMsg.header.stamp = stamp; // use corresponding time stamp to image
					cloudMsg.header.frame_id = odomFrameId_;
					odomLastFrame_.publish(cloudMsg);
				}
			}
		}
	}
	else
	{
		//ROS_WARN("Odometry lost!");

		//send null pose to notify that odometry is lost
		nav_msgs::Odometry odom;
		odom.header.stamp = stamp; // use corresponding time stamp to image
		odom.header.frame_id = odomFrameId_;
		odom.child_frame_id = frameId_;

		//publish the message
		odomPub_.publish(odom);
	}

	if(odomInfoPub_.getNumSubscribers())
	{
		rtabmap_ros::OdomInfo infoMsg;
		odomInfoToROS(info, infoMsg);
		infoMsg.header.stamp = stamp; // use corresponding time stamp to image
		infoMsg.header.frame_id = odomFrameId_;
		odomInfoPub_.publish(infoMsg);
	}

	ROS_INFO("Odom: quality=%d, std dev=%fm, update time=%fs", info.inliers, pose.isNull()?0.0f:std::sqrt(info.variance), (ros::WallTime::now()-time).toSec());
}
Пример #11
0
void TiledScreen::drawUpper(sf::RenderWindow *window) {
  this->renderStates.transform = getTransform();
  for (int16_t l = 0; l < UP_LAYERS; l++)
    window->draw(this->up[l], MAP_WIDTH*MAP_HEIGTH*VERS_PER_TILE, sf::Quads,this->renderStates);
}
void AnimatedSphere::draw(sf::RenderTarget& tgt, sf::RenderStates states) const noexcept
{
	states.transform *= getTransform();

	tgt.draw(circle, states);
}
Пример #13
0
void gui::MessageBox::draw(sf::RenderTarget& target,
						   sf::RenderStates states) const
{
	states.transform *= getTransform();
	mPages.at(mCurrentPage).draw(target, states);
}
Пример #14
0
void PrimitiveShader_PC::Flush()
{
    if(buffer_.empty())
        return;
    auto renderer = getRenderer();
    Use();
    CheckGL();

    // Send our transformation to the currently bound shader,
    // in the "MVP" uniform
    auto const &pv = renderer->getCamera().getPV();
    glEnable(GL_BLEND);
    if(hasTransform()){
        //m[0][2],m[1][2] is always 0
        auto const &m = getTransform();
        auto pv2 = pv * Mat4(Vec4(m[0][0],m[0][1],0,m[0][2]),
                Vec4(m[1][0],m[1][1],0,m[1][2]),
                Vec4(0,0,1,0),
                Vec4(m[2][0],m[2][1],0,m[2][2]));
        glUniformMatrix4fv(pv_, 1, GL_FALSE, &pv2[0][0]);
    }else{
        glUniformMatrix4fv(pv_, 1, GL_FALSE, &pv[0][0]);
    }
    CheckGL();

    glBindBuffer(GL_ARRAY_BUFFER, vbo_);
    glBufferData(GL_ARRAY_BUFFER, sizeof(Point)*buffer_.size(), (const void*)&buffer_[0], GL_DYNAMIC_DRAW);
    CheckGL();

    // 1rst attribute buffer : vertices
    glEnableVertexAttribArray(model_xy_);
    glVertexAttribPointer(
            model_xy_,                  // attribute. No particular reason for 0, but must match the layout in the shader.
            2,                  // size
            GL_FLOAT,           // type
            GL_FALSE,           // normalized?
            sizeof(Point),      // stride
            (void*)0            // array buffer offset
            );
    CheckGL();

    // 2nd attribute buffer : color
    glEnableVertexAttribArray(color_);
    glVertexAttribPointer(
            color_,
            4,
            GL_FLOAT,
            GL_FALSE,
            sizeof(Point),
            (void*)(2*sizeof(GLfloat))
            );
    CheckGL();
    glEnable(GL_PROGRAM_POINT_SIZE);
    glDrawArrays((GLenum)drawMode_, 0, buffer_.size());
    glDisable(GL_PROGRAM_POINT_SIZE);
    CheckGL();

    glDisableVertexAttribArray(model_xy_);
    glDisableVertexAttribArray(color_);

    glDisable(GL_BLEND);
    Unuse();
    buffer_.clear();
    renderer->addRenderCall();
}
Пример #15
0
bool Etherform::updatePos(const F32 travelTime)
{
   PROFILE_SCOPE(Etherform_UpdatePos);
   getTransform().getColumn(3, &delta.posVec);

   // When mounted to another object, only Z rotation used.
   if(isMounted()) 
	{
      mVelocity = mMount.object->getVelocity();
      setPosition(Point3F(0.0f, 0.0f, 0.0f), mRot);
      setMaskBits(MoveMask);
      return true;
   }

   Point3F newPos;

   Collision col;
   dMemset(&col, 0, sizeof(col));

   // DEBUG:
   //Point3F savedVelocity = mVelocity;

   if(mVelocity.isZero())
      newPos = delta.posVec;
   else
      newPos = _move(travelTime, &col);
   
   _handleCollision(col);

   // DEBUG:
   //if ( isClientObject() )
   //   Con::printf( "(client) vel: %g %g %g", mVelocity.x, mVelocity.y, mVelocity.z );
   //else
   //   Con::printf( "(server) vel: %g %g %g", mVelocity.x, mVelocity.y, mVelocity.z );

   // Set new position
	// If on the client, calc delta for backstepping
	if(isClientObject()) 
	{
		delta.pos = newPos;
		delta.rot = mRot;
		delta.posVec = delta.posVec - delta.pos;
		delta.rotVec = delta.rotVec - delta.rot;
		delta.dt = 1.0f;
	}

   this->setPosition(newPos, mRot);
   this->setMaskBits(MoveMask);
   this->updateContainer();

   if (!isGhost())  
   {
      // Collisions are only queued on the server and can be
      // generated by either updateMove or updatePos
      notifyCollision();

      // Do mission area callbacks on the server as well
      //checkMissionArea();
   }

   // Check the total distance moved.  If it is more than 1000th of the velocity, then
   //  we moved a fair amount...
   //if (totalMotion >= (0.001f * initialSpeed))
      return true;
   //else
      //return false;
}
Пример #16
0
  void SpriteMovement::onStart() {
    data->tile_location = glm::ivec2(getTransform()->getAbsoluteTranslation().x, -getTransform()->getAbsoluteTranslation().y);

    data->tile_location += glm::ivec2(data->collision_data->getWidth() / 2, data->collision_data->getHeight() / 2 - 1);
    state_machine->start();
  }
Пример #17
0
void Etherform::processTick(const Move* move)
{
   Parent::processTick(move);

   // Warp to catch up to server?
   if(delta.warpTicks > 0)
   {
		delta.warpTicks--;

		// Set new pos.
		getTransform().getColumn(3,&delta.pos);
		delta.pos += delta.warpOffset;
		delta.rot += delta.rotOffset;
		this->setPosition(delta.pos,delta.rot);
		this->setRenderPosition(delta.pos,delta.rot);

		// Backstepping
		delta.posVec.x = -delta.warpOffset.x;
		delta.posVec.y = -delta.warpOffset.y;
		delta.posVec.z = -delta.warpOffset.z;
		delta.rotVec.x = -delta.rotOffset.x;
		delta.rotVec.y = -delta.rotOffset.y;
		delta.rotVec.z = -delta.rotOffset.z;
	}
	else
	{
		// If there is no move, the we're either an
		// unattached etherform on the server, or another etherform's
		// client ghost.
		if(!move)
		{
			if(isGhost())
			{
				// If we haven't run out of prediction time,
				// predict using the last known move.
				if (mPredictionCount-- <= 0)
					return;

				move = &delta.move;
			}
			else
				move = &NullMove;
		}

		if(isServerObject() || (didRenderLastRender() || getControllingClient()))
		{
			delta.move = *move;

         if(isMounted())
         {
            // If we're mounted then do not perform any collision checks
            // and clear our previous working list.
            mConvex.clearWorkingList();
         }
         else
         {
            this->updateWorkingCollisionSet();
         }

			this->updateVelocity(move);
         VectorF contactNormal(0,0,0);
         this->findContact(&contactNormal );
			this->updatePos();
		}
	}

	// Add node to lasertrails.
	if(this->isClientObject())
	{
		GameConnection* conn = GameConnection::getConnectionToServer();
		if(conn && this == conn->getControlObject())
		{
			if(move->sendCount == 0)
				this->addLaserTrailNode(this->getPosition());
		}
		else
			this->addLaserTrailNode(this->getPosition());
	}
}
Пример #18
0
sf::FloatRect Text::getGlobalBounds() const
{
    return getTransform().transformRect(getLocalBounds());
}
Пример #19
0
void Etherform::_findContact( SceneObject **contactObject, 
                           VectorF *contactNormal, 
                           Vector<SceneObject*> *outOverlapObjects )
{
   Point3F pos;
   getTransform().getColumn(3,&pos);

   Box3F wBox;
   Point3F exp(0,0,sTractionDistance);
   wBox.minExtents = pos + mScaledBox.minExtents - exp;
   wBox.maxExtents.x = pos.x + mScaledBox.maxExtents.x;
   wBox.maxExtents.y = pos.y + mScaledBox.maxExtents.y;
   wBox.maxExtents.z = pos.z + mScaledBox.minExtents.z + sTractionDistance;

   static ClippedPolyList polyList;
   polyList.clear();
   polyList.doConstruct();
   polyList.mNormal.set(0.0f, 0.0f, 0.0f);
   polyList.setInterestNormal(Point3F(0.0f, 0.0f, -1.0f));

   polyList.mPlaneList.setSize(6);
   polyList.mPlaneList[0].setYZ(wBox.minExtents, -1.0f);
   polyList.mPlaneList[1].setXZ(wBox.maxExtents, 1.0f);
   polyList.mPlaneList[2].setYZ(wBox.maxExtents, 1.0f);
   polyList.mPlaneList[3].setXZ(wBox.minExtents, -1.0f);
   polyList.mPlaneList[4].setXY(wBox.minExtents, -1.0f);
   polyList.mPlaneList[5].setXY(wBox.maxExtents, 1.0f);
   Box3F plistBox = wBox;

   // Expand build box as it will be used to collide with items.
   // PickupRadius will be at least the size of the box.
   F32 pd = 0;
   wBox.minExtents.x -= pd; wBox.minExtents.y -= pd;
   wBox.maxExtents.x += pd; wBox.maxExtents.y += pd;
   wBox.maxExtents.z = pos.z + mScaledBox.maxExtents.z;

   // Build list from convex states here...
   CollisionWorkingList& rList = mConvex.getWorkingList();
   CollisionWorkingList* pList = rList.wLink.mNext;
   while (pList != &rList)
   {
      Convex* pConvex = pList->mConvex;

      U32 objectMask = pConvex->getObject()->getTypeMask();
      
      if (  ( objectMask & sCollisionMoveMask ) &&
            !( objectMask & PhysicalZoneObjectType ) )
      {
         Box3F convexBox = pConvex->getBoundingBox();
         if (plistBox.isOverlapped(convexBox))
            pConvex->getPolyList(&polyList);
      }
      else
         outOverlapObjects->push_back( pConvex->getObject() );

      pList = pList->wLink.mNext;
   }

   if (!polyList.isEmpty())
   {
      // Pick flattest surface
      F32 bestVd = -1.0f;
      ClippedPolyList::Poly* poly = polyList.mPolyList.begin();
      ClippedPolyList::Poly* end = polyList.mPolyList.end();
      for (; poly != end; poly++)
      {
         F32 vd = poly->plane.z;       // i.e.  mDot(Point3F(0,0,1), poly->plane);
         if (vd > bestVd)
         {
            bestVd = vd;
            *contactObject = poly->object;
            *contactNormal = poly->plane;
         }
      }
   }
}
Пример #20
0
sf::FloatRect AnimatedSprite::getGlobalBounds() const
{
	return getTransform().transformRect(getLocalBounds());
}
Пример #21
0
//private
void LineSegment::draw(sf::RenderTarget& rt, sf::RenderStates states) const
{
	states.transform *= getTransform();
	if(m_texture) states.texture = m_texture;
	rt.draw(m_vertexArray, states);
}
Пример #22
0
bool HoverVehicle::onNewDataBlock(GameBaseData* dptr, bool reload)
{
    mDataBlock = dynamic_cast<HoverVehicleData*>(dptr);
    if (!mDataBlock || !Parent::onNewDataBlock(dptr,reload))
        return false;

    if (isGhost())
    {
        // Create the sounds ahead of time.  This reduces runtime
        // costs and makes the system easier to understand.

        SFX_DELETE( mEngineSound );
        SFX_DELETE( mFloatSound );
        SFX_DELETE( mJetSound );

        if ( mDataBlock->sound[HoverVehicleData::EngineSound] )
            mEngineSound = SFX->createSource( mDataBlock->sound[HoverVehicleData::EngineSound], &getTransform() );

        if ( !mDataBlock->sound[HoverVehicleData::FloatSound] )
            mFloatSound = SFX->createSource( mDataBlock->sound[HoverVehicleData::FloatSound], &getTransform() );

        if ( mDataBlock->sound[HoverVehicleData::JetSound] )
            mJetSound = SFX->createSource( mDataBlock->sound[HoverVehicleData::JetSound], &getTransform() );
    }

    // Todo: Uncomment if this is a "leaf" class
    scriptOnNewDataBlock();

    return true;
}
Пример #23
0
void Firebug::draw(sf::RenderTarget& target, sf::RenderStates states) const
{
	states.transform *= getTransform();
//	target.draw(mSprite,states); 
	mAnimation.draw(target,states);
}
Пример #24
0
void Joystick::render(sf::RenderTarget& target, sf::RenderStates states)
{
	states.transform *= getTransform();
    target.draw(mBackground,states);
    target.draw(mButton,states);
}
void ParticleSkinnedModel::update(float dt)
{
	// Send update call to superclass
	Model::update(dt);

	const int PS_SUB_UPDATE_STEPS = 2;
	const int PS_CONSTRAINT_STEPS = 0;

	const float TARGET_TIME = (float)(1.0 / (60.0 * PS_SUB_UPDATE_STEPS));

	dt = TARGET_TIME;

	if(m_simulateOnGPU)
	{
		int loc;

		Shader& shader = m_ps->getShader();
		shader.bind();

		loc = shader.getUniformLocation("modelMatrix");
		glUniformMatrix4fv(loc, 1, false, glm::value_ptr(getTransform().getMat4()));

		loc = shader.getUniformLocation("invModelMatrix");
		glUniformMatrix4fv(loc, 1, false, glm::value_ptr(getTransform().getInvMat4()));

		loc = shader.getUniformLocation("boneMatrix");
		calculateAndSetBoneMatrices(loc);

		//loc = shader.getUniformLocation("randomForce");
		
		for(int i=0; i<PS_SUB_UPDATE_STEPS; ++i)
		{
			m_ps->update(dt);
		}
	}
	else
	{
		const size_t max_bones = 128;
		glm::mat4 finalMat[max_bones];
		// 16K on the stack thanks to this, perhaps allocate in heap?
		// The idéa is to make sure it is coherent in memory.

		std::vector<glm::vec3> targetPos(m_particles.size());

		size_t boneCount = m_body->getBoneCount();
		assert(boneCount < max_bones);

		calculateFinalBoneMatrices(finalMat, boneCount);

		for(size_t i=0; i<targetPos.size(); ++i)
		{
			const Body::sVertex& v = m_body->getVertexData()[i];

			glm::vec4 finalPos(0.0f);
			for(size_t u=0; u<MAX_WEIGHTS; ++u)
			{
				if(v.weight[u].unused())
					break;

				int index =		v.weight[u].getIndex();
				float weight =	v.weight[u].getWeight();
				finalPos += weight * finalMat[index] * glm::vec4(v.position, 1);
			}
			targetPos[i] = glm::vec3(finalPos);
		}

		const glm::vec3 externalForce(0);
		const glm::vec3 externalAcc(0,-9.82,0);
		const float maxDist = 0.1;

		for(int u=0; u<PS_SUB_UPDATE_STEPS; ++u)
		{
			// Simulate timesteps
			for(size_t i=0; i<m_particles.size(); ++i)
			{
				sParticle& p = m_particles[i]; 
				glm::vec3& target = targetPos[i];
				float mass 	= p.mass_k_d.x;
				float k 	= p.mass_k_d.y;
				float d 	= p.mass_k_d.z * 0.4f;

				//float len 	= glm::length(target - p.position);

				glm::vec3 attrForce = (target - p.position) * k;

				glm::vec3 force = externalForce + attrForce;
				glm::vec3 acc = externalAcc + force / mass;
				glm::vec3 pos = p.position;

				glm::vec3 vel = (1.0f - d) * (p.position - p.oldPosition) + acc * dt * dt;

				// This part can probably be optimized further
				// This is to make a soft clamp of the particle pos to the max allowed distance
				float dist = glm::distance(pos, target);
				glm::vec3 goPath = (pos-target) / dist;
	
				pos += vel;

				if(dist < maxDist) {
					p.oldPosition = p.position;
					p.position = pos;
				}else{
					glm::vec3 maxDistPos = target + goPath * maxDist;
					p.position = glm::mix(pos, maxDistPos, glm::clamp(glm::distance(maxDistPos,pos), 0.0f, 1.0f));
					p.oldPosition = p.position - vel;
				}
			}
		}

		
		const std::vector<Body::sVertex>& vertexData = m_body->getVertexData();
		const float stiffness = 0.1f;

		for(int u=0; u<PS_CONSTRAINT_STEPS; ++u)
		{
			// Update constraints
			for(size_t i=0; i<m_constraints.size(); ++i)
			{
				size_t i0 = m_constraints[i].index[0];
				size_t i1 = m_constraints[i].index[1];

				sParticle& p0 = m_particles[i0];
				sParticle& p1 = m_particles[i1];

				float m0 = p0.mass_k_d.x;
				float m1 = p1.mass_k_d.x;

				glm::vec3 delta = p1.position - p0.position;

				glm::vec3 restDelta = vertexData[i1].position - vertexData[i0].position;
				float restLength2 = glm::dot(restDelta, restDelta);
            
				delta *= (1.0f - 2.0f * restLength2 / (restLength2 + glm::dot(delta,delta)) ) * (m0 + m1);
            
				glm::vec3 val = stiffness * delta;
            
				p0.position += val / m0;
				p1.position -= val / m1;
			}

			// Merge seems and maintain velocity
			for(size_t i=0; i<m_postVertexMerge.size(); ++i)
			{
				const unsigned int i0 = m_postVertexMerge[i].index[0];
				const unsigned int i1 = m_postVertexMerge[i].index[1];

				glm::vec3 v0 = m_particles[i0].position - m_particles[i0].oldPosition;
				glm::vec3 v1 = m_particles[i1].position - m_particles[i1].oldPosition;

				glm::vec3 p = m_particles[i0].position * 0.5f + m_particles[i1].position * 0.5f;

				m_particles[i0].position = p;
				m_particles[i0].oldPosition = p - v0;

				m_particles[i1].position = p;
				m_particles[i1].oldPosition = p - v1;
			}
		}

		// Update bufferdata!
		m_particleBuffer.bind();

		glBufferSubData(GL_ARRAY_BUFFER, 0, sizeof(sParticle) * m_particles.size(), &m_particles[0]);

		m_particleBuffer.unbind();
	}
}
void GuiComponent::render(const Eigen::Affine3f& parentTrans)
{
	Eigen::Affine3f trans = parentTrans * getTransform();
	renderChildren(trans);
}
Пример #27
0
FloatRect Shape::getGlobalBounds() const
{
    return getTransform().transformRect(getLocalBounds());
}
Пример #28
0
Rectf Sprite::getGlobalBounds() const {
	return getTransform().transformRect(getLocalBounds());
}
Пример #29
0
void CosmoButton::draw(sf::RenderTarget& target, sf::RenderStates states) const
{
	states.transform *= getTransform();
	target.draw(m_background, states);
	target.draw((gui::Button) *this);
}
/*!
  Sets up the chain given a XML DOM from a currently open robot
  configuration file.  It reads the number of joints and the number of links
  and allocates space for those vectors, then it reads in the base transform
  of the chain.  Next, it reads a node for each joint and creates a
  prismatic or revolute joint which is initialized with the kinematic data
  in that node. \a linkDir should be the path to the directory where the link
  body files are kept (usually rootPath/iv).
*/ 
int
KinematicChain::initChainFromXml(const TiXmlElement* root,QString &linkDir)
{
  numJoints = countXmlElements(root,"joint");
  if (numJoints < 1) {
    DBGA("number of joints < 1");
    return FAILURE;
  }
  
  numLinks = countXmlElements(root,"link");
  if (numLinks < 1) {
    DBGA("Number of links < 1");
    return FAILURE;
  }
  
  jointVec.resize(numJoints, NULL);
  linkVec.resize(numLinks, NULL);  
  
  lastJoint = new int[numLinks];
  
  IVRoot = new SoSeparator;
  IVTran = new SoTransform;
  IVTran->ref();
  
  /* read in the finger transformation */
  const TiXmlElement* element = findXmlElement(root,"transform");
  if(element){
    if(!getTransform(element,tran)){
      QTWARNING("Fail to perform transformation");
      return FAILURE;
    }
  }
  tran.toSoTransform(IVTran);
  
  DBGA("  Creating joints");
  numDOF = 0;
  std::list<const TiXmlElement*> elementList = findAllXmlElements(root, "joint");
  std::list<const TiXmlElement*>::iterator p;
  int j;
  for(p = elementList.begin(), j=0; p!=elementList.end(); p++,j++){
    DBGA("   Joint " << j);
    QString type = (*p)->Attribute("type");
    if(type.isNull()){
      QTWARNING("No Joint Type Specified");
      return FAILURE;
    }
    if(type == "Revolute"){
      jointVec[j] = new RevoluteJoint(this);
    } else if(type == "Prismatic"){
      jointVec[j] = new PrismaticJoint (this);
    } else {
      DBGA("Unknown joint type requested");
      return FAILURE;
    }
    if (jointVec[j]->initJointFromXml(*p, firstJointNum+j) == FAILURE) {
      DBGA("Failed to initialize joint");
      return FAILURE;
    }
  }
  
  DBGA("  Creating links");
  std::vector<int> dynJointTypes;
  int lastJointNum = -1;
  elementList = findAllXmlElements(root, "link");
  int l;
  for (l=0, p=elementList.begin(); p!=elementList.end(); p++,l++){
    DBGA("   Link " << l);
    QString jointType=(*p)->Attribute("dynamicJointType");
    if(jointType.isNull()){
      QTWARNING("No Dynamic Joint Type Specified");
      return FAILURE;
    }
    jointType = jointType.stripWhiteSpace();
    if (jointType == "Revolute") {
      dynJointTypes.push_back(DynJoint::REVOLUTE);
      lastJointNum += 1;
    } else if (jointType == "Ball") {
      dynJointTypes.push_back(DynJoint::BALL);
      lastJointNum += 3;
    } else if (jointType == "Universal") {
      dynJointTypes.push_back(DynJoint::UNIVERSAL);
      lastJointNum += 2;
    } else if (jointType == "Prismatic") {
      dynJointTypes.push_back(DynJoint::PRISMATIC);
      lastJointNum += 1;
    } else if (jointType == "Fixed") {
      dynJointTypes.push_back(DynJoint::FIXED);
    } else {
      DBGA("Unknown joint type requested");
      return FAILURE;
    }
    
    QString linkFilename = (*p)->GetText();
    linkFilename = linkFilename.stripWhiteSpace();
    QString linkName = QString(owner->name()) + QString("_chain%1_link%2").arg(chainNum).arg(l);
    linkVec[l] = new Link(owner,chainNum,l,owner->getWorld(),linkName.latin1());
    if (linkVec[l]->load(linkDir + linkFilename)==FAILURE) {
      delete linkVec[l]; linkVec[l] = NULL;
      DBGA("Failed to load file for link " << l);
      return FAILURE;
    }
    /*Handle collision rule settings:
     *Common case is NULL - collisions are on except for adjacent links.
     *Off - turn off collisions for this link globally, do not enter this body
     *into the collision engine at all.
     *OverlappingPair - turn off collisions between two particular links
     */
    QString collisionRule=(*p)->Attribute("collisionRule");
    
    if(!collisionRule.isNull()){
      collisionRule = collisionRule.stripWhiteSpace();
      if (collisionRule == "Off"){
	linkVec[l]->addToIvc(true);
	linkVec[l]->myWorld->toggleCollisions(false, linkVec[l],NULL);
	DBGA("Collisions off for link " << l);
      }else if (collisionRule == "OverlappingPair"){
	/*targetChain - specifies the chain of the target link to disable
	 *collisions for.  No attribute implies current chain.  
	 *Base implies robot base.
	 *targetLink - specifies link number in target chain
	 */
	linkVec[l]->addToIvc();
	QString targetChain=(*p)->Attribute("targetChain");
	targetChain = targetChain.stripWhiteSpace();
	if (targetChain == "base")
	  linkVec[l]->myWorld->toggleCollisions(false, linkVec[l],owner->getBase());
	else{
	  
	  QString targetLink = (*p)->Attribute("targetLink");
	  if (!targetLink.isNull()){
	    bool ok = TRUE;
	    int linkNum = targetLink.toInt(&ok);
	    if (!ok){
	      DBGA("targetLink not a valid input");
	      return FAILURE;
	    }
	    
	    if(targetChain.isNull())
	      linkVec[l]->myWorld->toggleCollisions(false, linkVec[l],linkVec[linkNum]);
	    else{
	      int chainNum = targetChain.toInt(&ok);
	      if (!ok){
		DBGA("targetChain not a valid input");
		return FAILURE;
	      }
	      linkVec[l]->myWorld->toggleCollisions(false, linkVec[l],owner->getChain(chainNum)->linkVec[linkNum]);	
	    }
	  }
	}				
      }
      else{
	DBGA("Unknown Collision Rule");
	return FAILURE;
      }
    }else{
      linkVec[l]->addToIvc();
    }
    
    
    lastJoint[l] = lastJointNum;
    if (lastJoint[l] >= numJoints) {
      DBGA("Wrong last joint value: " << lastJoint[l]);
      return FAILURE;
    }
    
    IVRoot->addChild(linkVec[l]->getIVRoot());
  }
  
  DBGA("  Creating dynamic joints");
  if (createDynamicJoints(dynJointTypes) == FAILURE) {
    DBGA("Failed to create dynamic joints");
    return FAILURE;
  }
  
  jointsMoved = true;
  updateLinkPoses();
  owner->getWorld()->tendonChange();
  owner->getIVRoot()->addChild(IVRoot);
  
  return SUCCESS;
}