Exemple #1
0
		void
		LeuzeRs4::getDistances(::rl::math::Vector& distances) const
		{
			assert(this->isConnected());
			assert(distances.size() >= this->getDistancesCount());
			
			const uint8_t* ptr;
			
			if (this->data[3] & 2)
			{
				ptr = this->data + 5;
				
				if ((this->data[3] & 3) && (this->data[4] & 128))
				{
					ptr = this->data + 6;
				}
			}
			else
			{
				ptr = this->data + 4;
			}
			
			const uint8_t& number1 = get(ptr);
			get(ptr);//bool motion = get(ptr) & 64 ? true : false;
			const uint8_t& number2 = get(ptr);
			get(ptr);
			const uint8_t& number3 = get(ptr);
			get(ptr);
			const uint8_t& number4 = get(ptr);
			get(ptr);
			
			/*int number = */hostEndianDoubleWord(hostEndianWord(number1, number2), hostEndianWord(number3, number4));
			
			int step = get(ptr);
			
			const uint8_t& startHigh = get(ptr);
			const uint8_t& startLow = get(ptr);
			int start = hostEndianWord(startHigh, startLow);
			
			const uint8_t& stopHigh = get(ptr);
			const uint8_t& stopLow = get(ptr);
			int stop = hostEndianWord(stopHigh, stopLow);
			
			for (int i = 0; i < (stop - start) / step + 1; ++i)
			{
				const uint8_t& dataHigh = get(ptr);
				const uint8_t& dataLow = get(ptr);
				distances(distances.size() - 1 - i) = hostEndianWord(dataHigh, dataLow & 254) / 1000.0f;
			}
		}
Exemple #2
0
		void
		Model::setPosition(const ::rl::math::Vector& q)
		{
			if (NULL != this->kin)
			{
				assert(q.size() == this->kin->getDof());
				this->kin->setPosition(q);
			}
			else
			{
				assert(q.size() == this->mdl->getDof());
				this->mdl->setPosition(q);
			}
		}
Exemple #3
0
		void
		Metric::step(const ::rl::math::Vector& q1, const ::rl::math::Vector& dq, ::rl::math::Vector& q2) const
		{
			assert(q1.size() == this->getDofPosition());
			assert(dq.size() == this->getDof());
			assert(q2.size() == this->getDofPosition());
			
			for (::std::size_t i = 0, j = 0, k = 0; i < this->joints.size(); j += this->joints[i]->getDofPosition(), k += this->joints[i]->getDof(), ++i)
			{
				this->joints[i]->step(
					q1.segment(j, this->joints[i]->getDofPosition()),
					dq.segment(k, this->joints[i]->getDof()),
					q2.segment(j, this->joints[i]->getDofPosition())
				);
			}
		}
		void
		SickS300::getDistances(::rl::math::Vector& distances) const
		{
			assert(this->isConnected());
			assert(distances.size() >= this->getDistancesCount());
			
			::rl::math::Real scale = 0.01f;
			
			for (::std::size_t i = 0; i < this->getDistancesCount(); ++i)
			{
				uint16_t value = Endian::hostEndianWord(this->data[24 + 1 + i * 2], this->data[24 + 0 + i * 2]) & 0x1FFF;
				
				switch (this->data[24 + 1 + i * 2] & 224)
				{
				case 128:
::std::cerr << "Measured value detected within warning field" << ::std::endl;
				case 64:
::std::cerr << "Measured value detected within protective field" << ::std::endl;
				case 32:
::std::cerr << "Glare (dazzling) detected" << ::std::endl;
					distances(i) = ::std::numeric_limits< ::rl::math::Real >::quiet_NaN();
					break;
				default:
					distances(i) = value * scale;
					break;
				}
			}
		}
Exemple #5
0
	DamaSupportSurface::DamaSupportSurface(::std::string _name, ::rl::math::Real _height, const ::rl::math::Vector& _min, const ::rl::math::Vector& _max)
	{
		this->name = _name;
		this->height = _height;
		this->dim = _max.size();
		this->min = _min;
		this->max = _max;
	}
Exemple #6
0
		::rl::math::Real
		Metric::distance(const ::rl::math::Vector& q1, const ::rl::math::Vector& q2) const
		{
			assert(q1.size() == this->getDofPosition());
			assert(q2.size() == this->getDofPosition());
			
			::rl::math::Real d = 0;
			
			for (::std::size_t i = 0, j = 0; i < this->joints.size(); j += this->joints[i]->getDofPosition(), ++i)
			{
				d += this->joints[i]->transformedDistance(
					q1.segment(j, this->joints[i]->getDofPosition()),
					q2.segment(j, this->joints[i]->getDofPosition())
				);
			}
			
			return this->inverseOfTransformedDistance(d);
		}
Exemple #7
0
		void
		Jr3::getTorques(::rl::math::Vector& torques) const
		{
			assert(torques.size() >= 3);
			
			for (::std::size_t i = 3; i < 6; ++i)
			{
				torques(i) = (this->values[i] - this->zeroes[i]) * 1000;
			}
		}
Exemple #8
0
		void
		Metric::clamp(::rl::math::Vector& q) const
		{
			assert(q.size() == this->getDofPosition());
			
			for (::std::size_t i = 0, j = 0; i < this->joints.size(); j += this->joints[i]->getDofPosition(), ++i)
			{
				this->joints[i]->clamp(q.segment(j, this->joints[i]->getDofPosition()));
			}
		}
Exemple #9
0
		void
		Jr3::getForcesTorques(::rl::math::Vector& forcesTorques) const
		{
			assert(forcesTorques.size() >= 6);
			
			for (::std::size_t i = 0; i < 6; ++i)
			{
				forcesTorques(i) = (this->values[i] - this->zeroes[i]) * 1000;
			}
		}
Exemple #10
0
		void
		Metric::interpolate(const ::rl::math::Vector& q1, const ::rl::math::Vector& q2, const ::rl::math::Real& alpha, ::rl::math::Vector& q) const
		{
			assert(q1.size() == this->getDofPosition());
			assert(q2.size() == this->getDofPosition());
			assert(alpha >= 0);
			assert(alpha <= 1);
			assert(q.size() == this->getDofPosition());
			
			for (::std::size_t i = 0, j = 0; i < this->joints.size(); j += this->joints[i]->getDofPosition(), ++i)
			{
				this->joints[i]->interpolate(
					q1.segment(j, this->joints[i]->getDofPosition()),
					q2.segment(j, this->joints[i]->getDofPosition()),
					alpha,
					q.segment(j, this->joints[i]->getDofPosition())
				);
			}
		}
Exemple #11
0
		void
		Metric::step(const ::rl::math::Vector& q1, const ::rl::math::Vector& qdot, ::rl::math::Vector& q2) const
		{
			assert(q1.size() == this->getDofPosition());
			assert(qdot.size() == this->getDof());
			assert(q2.size() == this->getDofPosition());
			
			for (::std::size_t i = 0, j = 0, k = 0; i < this->joints.size(); j += this->joints[i]->getDofPosition(), k += this->joints[i]->getDof(), ++i)
			{
				::rl::math::Vector q2i = q2.segment(j, this->joints[i]->getDofPosition()); // TODO
				
				this->joints[i]->step(
					q1.segment(j, this->joints[i]->getDofPosition()),
					qdot.segment(k, this->joints[i]->getDof()),
					q2i
				);
				
				q2.segment(j, this->joints[i]->getDofPosition()) = q2i; // TODO
			}
		}
Exemple #12
0
		void
		Metric::clip(::rl::math::Vector& q) const
		{
			assert(q.size() == this->getDofPosition());
			
			for (::std::size_t i = 0, j = 0; i < this->joints.size(); j += this->joints[i]->getDofPosition(), ++i)
			{
				::rl::math::Vector qi = q.segment(j, this->joints[i]->getDofPosition()); // TODO
				this->joints[i]->clip(qi);
				q.segment(j, this->joints[i]->getDofPosition()) = qi; // TODO
			}
		}
Exemple #13
0
		void
		Sampler::generateCollisionFree(::rl::math::Vector& q)
		{
			assert(q.size() == this->model->getDof());
			
			do
			{
				this->generate(q);
				this->model->setPosition(q);
				this->model->updateFrames();
			}
			while (this->model->isColliding());
		}
Exemple #14
0
		bool
		Metric::isValid(const ::rl::math::Vector& q) const
		{
			assert(q.size() == this->getDofPosition());
			
			for (::std::size_t i = 0, j = 0; i < this->joints.size(); j += this->joints[i]->getDofPosition(), ++i)
			{
				if (!this->joints[i]->isValid(q.segment(j, this->joints[i]->getDofPosition())))
				{
					return false;
				}
			}
			
			return true;
		}
Exemple #15
0
		void
		Coach::setJointPosition(const ::rl::math::Vector& q)
		{
			assert(this->getDof() >= q.size());
			
			this->text.clear();
			this->text.str("");
			
			this->text << 2;
			
			this->text << " " << this->i;
			
			for (::std::size_t i = 0; i < this->getDof(); ++i)
			{
				this->text << " " << q(i);
			}
			
			this->text << ::std::endl;
		}
Exemple #16
0
		void
		MitsubishiH7::setJointPosition(const ::rl::math::Vector& q)
		{
			assert(q.size() >= this->getDof());
			
			this->out.dat.jnt.j1 = 0;
			this->out.dat.jnt.j2 = 0;
			this->out.dat.jnt.j3 = 0;
			this->out.dat.jnt.j4 = 0;
			this->out.dat.jnt.j5 = 0;
			this->out.dat.jnt.j6 = 0;
			this->out.dat.jnt.j7 = 0;
			this->out.dat.jnt.j8 = 0;
			
			switch (this->getDof())
			{
			case 8:
				this->out.dat.jnt.j8 = static_cast<float>(q(7));
			case 7:
				this->out.dat.jnt.j7 = static_cast<float>(q(6));
			case 6:
				this->out.dat.jnt.j6 = static_cast<float>(q(5));
			case 5:
				this->out.dat.jnt.j5 = static_cast<float>(q(4));
			case 4:
				this->out.dat.jnt.j4 = static_cast<float>(q(3));
			case 3:
				this->out.dat.jnt.j3 = static_cast<float>(q(2));
			case 2:
				this->out.dat.jnt.j2 = static_cast<float>(q(1));
			case 1:
				this->out.dat.jnt.j1 = static_cast<float>(q(0));
			default:
				break;
			}
			
			this->out.command = MXT_COMMAND_MOVE;
			this->out.sendType = MXT_TYPE_JOINT;
		}
		void
		SickLms200::getDistances(::rl::math::Vector& distances) const
		{
			assert(this->isConnected());
			assert(distances.size() >= this->getDistancesCount());
			
			if (this->data[6] & 32)
			{
				throw DeviceException("partial scan");
			}
			
			::rl::math::Real scale;
			
			switch (this->data[6] & 192)
			{
			case 0:
				scale = 0.01f;
				break;
			case 64:
				scale = 0.001f;
				break;
			default:
				throw DeviceException("unknown scale");
				break;
			}
			
			uint16_t count = Endian::hostEndianWord(this->data[6] & 11, this->data[5]);
			
			uint8_t mask;
			
			switch (this->configuration)
			{
			case 0x00:
			case 0x01:
			case 0x02:
				mask = 0x1F;
				break;
			case 0x03:
			case 0x04:
			case 0x05:
				mask = 0x3F;
				break;
			case 0x06:
				mask = 0x7F;
				break;
			default:
				mask = 0x00;
				break;
			}
			
			uint16_t value;
			
			for (::std::size_t i = 0; i < count; ++i)
			{
				value = Endian::hostEndianWord(this->data[8 + i * 2] & mask, this->data[7 + i * 2]);
				
				switch (this->configuration)
				{
				case 0x00:
				case 0x01:
				case 0x02:
					switch (value)
					{
					case 0x1FFF:
::std::cerr << "Measured value not valid" << ::std::endl;
					case 0x1FFE:
::std::cerr << "Dazzling" << ::std::endl;
					case 0x1FFD:
::std::cerr << "Operation overflow" << ::std::endl;
					case 0x1FFB:
::std::cerr << "Signal-to-noise ratio too small" << ::std::endl;
					case 0x1FFA:
::std::cerr << "Error when reading channel 1" << ::std::endl;
						distances(i) = ::std::numeric_limits< ::rl::math::Real >::quiet_NaN();
						break;
					case 0x1FF7:
::std::cerr << "Measured value > Maximum value" << ::std::endl;
						distances(i) = ::std::numeric_limits< ::rl::math::Real >::infinity();
						break;
					default:
						distances(i) = value;
						distances(i) *= scale;
						break;
					}
					break;
				case 0x03:
				case 0x04:
				case 0x05:
					switch (value)
					{
					case 0x3FFF:
::std::cerr << "Measured value not valid" << ::std::endl;
					case 0x3FFE:
::std::cerr << "Dazzling" << ::std::endl;
					case 0x3FFD:
::std::cerr << "Operation overflow" << ::std::endl;
					case 0x3FFB:
::std::cerr << "Signal-to-noise ratio too small" << ::std::endl;
					case 0x3FFA:
::std::cerr << "Error when reading channel 1" << ::std::endl;
						distances(i) = ::std::numeric_limits< ::rl::math::Real >::quiet_NaN();
						break;
					case 0x3FF7:
::std::cerr << "Measured value > Maximum value" << ::std::endl;
						distances(i) = ::std::numeric_limits< ::rl::math::Real >::infinity();
						break;
					default:
						distances(i) = value;
						distances(i) *= scale;
						break;
					}
					break;
				case 0x06:
					switch (value)
					{
					case 0x7FFF:
::std::cerr << "Measured value not valid" << ::std::endl;
					case 0x7FFE:
::std::cerr << "Dazzling" << ::std::endl;
					case 0x7FFD:
::std::cerr << "Operation overflow" << ::std::endl;
					case 0x7FFB:
::std::cerr << "Signal-to-noise ratio too small" << ::std::endl;
					case 0x7FFA:
::std::cerr << "Error when reading channel 1" << ::std::endl;
						distances(i) = ::std::numeric_limits< ::rl::math::Real >::quiet_NaN();
						break;
					case 0x7FF7:
::std::cerr << "Measured value > Maximum value" << ::std::endl;
						distances(i) = ::std::numeric_limits< ::rl::math::Real >::infinity();
						break;
					default:
						distances(i) = value;
						distances(i) *= scale;
						break;
					}
				default:
					distances(i) = value;
					distances(i) *= scale;
					break;
				}
			}
		}