Esempio n. 1
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
			}
		}
Esempio n. 2
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())
				);
			}
		}
Esempio n. 3
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);
		}
Esempio n. 4
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())
				);
			}
		}
Esempio n. 5
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()));
			}
		}
Esempio n. 6
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
			}
		}
Esempio n. 7
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;
		}