<commit_before>/**
* <File comment goes here!!>
* 
* Copyright (c) 200x by <your name/ organization here>
*/
#ifndef HYBRID_AUTOMATON_CONTROLLER_H_
#define HYBRID_AUTOMATON_CONTROLLER_H_

#include "hybrid_automaton/Serializable.h"
#include "hybrid_automaton/System.h"
#include "hybrid_automaton/HybridAutomatonRegistration.h"
#include "hybrid_automaton/error_handling.h"

#include "hybrid_automaton/HybridAutomatonOStringStream.h"

#include <boost/shared_ptr.hpp>
#include <Eigen/Dense>

namespace ha {

	class Controller;
	typedef boost::shared_ptr<Controller> ControllerPtr;
	typedef boost::shared_ptr<const Controller> ControllerConstPtr;

	class Controller : public Serializable {

	public:
		typedef boost::shared_ptr<Controller> Ptr;
		typedef boost::shared_ptr<const Controller> ConstPtr;

        /**
		* Constructor
		*/
		Controller();

        /**
		* Destructor
		*/
		virtual ~Controller();

        /**
		* Copy constructor
		*/
		Controller(const Controller& controller);

        /**
		* @brief Activate the controller for execution
	    */
		virtual void initialize() {
			HA_THROW_ERROR("Controller.initialize", "not implemented");
		}

        /**
		* @brief Deactivate the controller for execution
	    */
		virtual void terminate() {
			HA_THROW_ERROR("Controller.terminate", "not implemented");
		}

        /**
        * @brief Step function
		*/
		virtual ::Eigen::MatrixXd step(const double& t) {
			HA_THROW_ERROR("Controller.step", "not implemented");
		}

        /**
		* @brief Transform a goal in relative coordinates into absolute coordinates.
	    */
		virtual ::Eigen::MatrixXd relativeGoalToAbsolute(const Eigen::MatrixXd& goalRel) const{
			HA_THROW_ERROR("Controller.relativeGoalToAbsolute", "not implemented - You can not use a relative goal with this type of controller!: "<<this->_type);
		}

		/*!
		*/
		virtual DescriptionTreeNode::Ptr serialize(const DescriptionTree::ConstPtr& factory) const;

        /**
        * @brief Deserialize
		*/
		virtual void deserialize(const DescriptionTreeNode::ConstPtr& tree, const System::ConstPtr& system, const HybridAutomaton* ha);

        /**
        * @brief Clone function
		*/
		ControllerPtr clone() const {
			return ControllerPtr(_doClone());
		}

		/**
		 * @deprecated
		 */
		virtual int getDimensionality() const;

		virtual void setSystem(const System::ConstPtr& system);

		virtual Eigen::MatrixXd getGoal() const;
		virtual void setGoal(const Eigen::MatrixXd& new_goal);
	
		virtual bool getGoalIsRelative() const;
		virtual void setGoalIsRelative(bool is_goal_relative);

		virtual Eigen::MatrixXd getKp() const;
		virtual void setKp(const Eigen::MatrixXd& new_kp);

		virtual Eigen::MatrixXd getKv() const;
		virtual void setKv(const Eigen::MatrixXd& new_kv);

		/**
		 * @brief Completion times to reach goal and intermediate points
		 *
		 * Optional attribute, e.g. makes sense for interpolated
		 * controllers/
		 */
		virtual void setCompletionTimes(const Eigen::MatrixXd& new_times);
		virtual void setCompletionTime(const double& t); //for convenience - creates 1x1 matrix	
		virtual Eigen::MatrixXd getCompletionTimes() const;

		virtual void setMaximumVelocity(const Eigen::MatrixXd& max_vel);
		virtual	void setMaximumAcceleration(const Eigen::MatrixXd& max_acc);

		virtual Eigen::MatrixXd	getMaximumVelocity() const;
		virtual Eigen::MatrixXd	getMaximumAcceleration() const;
		/**
		 * @brief Priority of this controller in a ControlSet
		 *
		 * Optional attribute.
		 */
		virtual double getPriority() const;
		virtual void setPriority(double priority);
		
		virtual std::string getName() const;
		virtual void setName(const std::string& new_name);

		virtual const std::string getType() const;
		virtual void setType(const std::string& new_type);

		/**
		 * @brief Pass any additional argument for your controller
		 */
		template <typename T> void setArgument(const std::string& field_name, const T& field_value)
		{
			ha_ostringstream ha_oss;
			ha_oss << field_value;
			this->setArgumentString(field_name, ha_oss.str());
		}

		template <typename T> bool getArgument(const std::string& field_name, T& return_value) const
		{
			std::string val;
			bool ret = this->getArgumentString(field_name, val);
			if (ret)
			{
				std::istringstream ss(val);
				ss >> return_value;
				return true;
			}
			else
			{
				return false;
			}
		}

		/**
		 * @brief Computes a default interpolation time for a trajectory from x0 to xf
		 * under velocity and acceleration limits. Currrently assumes linear interpolation!
		 * 
	     * @return minimal interpolation time.
		 */
		virtual double computeInterpolationTime
			(const Eigen::MatrixXd& x0, const Eigen::MatrixXd& xf,
			const Eigen::MatrixXd& xdot0 = Eigen::MatrixXd(), const Eigen::MatrixXd& xdotf = Eigen::MatrixXd()) const;


        const std::map<std::string, std::string>& getAdditionalArgumentsString() {
            return _additional_arguments;
        }

	protected:
<commit_msg>implemented computeInterpolationTime for orientation controllers<commit_after>/**
* <File comment goes here!!>
* 
* Copyright (c) 200x by <your name/ organization here>
*/
#ifndef HYBRID_AUTOMATON_CONTROLLER_H_
#define HYBRID_AUTOMATON_CONTROLLER_H_

#include "hybrid_automaton/Serializable.h"
#include "hybrid_automaton/System.h"
#include "hybrid_automaton/HybridAutomatonRegistration.h"
#include "hybrid_automaton/error_handling.h"

#include "hybrid_automaton/HybridAutomatonOStringStream.h"

#include <boost/shared_ptr.hpp>
#include <Eigen/Dense>

namespace ha {

	class Controller;
	typedef boost::shared_ptr<Controller> ControllerPtr;
	typedef boost::shared_ptr<const Controller> ControllerConstPtr;

	class Controller : public Serializable {

	public:
		typedef boost::shared_ptr<Controller> Ptr;
		typedef boost::shared_ptr<const Controller> ConstPtr;

        /**
		* Constructor
		*/
		Controller();

        /**
		* Destructor
		*/
		virtual ~Controller();

        /**
		* Copy constructor
		*/
		Controller(const Controller& controller);

        /**
		* @brief Activate the controller for execution
	    */
		virtual void initialize() {
			HA_THROW_ERROR("Controller.initialize", "not implemented");
		}

        /**
		* @brief Deactivate the controller for execution
	    */
		virtual void terminate() {
			HA_THROW_ERROR("Controller.terminate", "not implemented");
		}

        /**
        * @brief Step function
		*/
		virtual ::Eigen::MatrixXd step(const double& t) {
			HA_THROW_ERROR("Controller.step", "not implemented");
		}

        /**
		* @brief Transform a goal in relative coordinates into absolute coordinates.
	    */
		virtual ::Eigen::MatrixXd relativeGoalToAbsolute(const Eigen::MatrixXd& goalRel) const{
			HA_THROW_ERROR("Controller.relativeGoalToAbsolute", "not implemented - You can not use a relative goal with this type of controller!: "<<this->_type);
		}

		/*!
		*/
		virtual DescriptionTreeNode::Ptr serialize(const DescriptionTree::ConstPtr& factory) const;

        /**
        * @brief Deserialize
		*/
		virtual void deserialize(const DescriptionTreeNode::ConstPtr& tree, const System::ConstPtr& system, const HybridAutomaton* ha);

        /**
        * @brief Clone function
		*/
		ControllerPtr clone() const {
			return ControllerPtr(_doClone());
		}

		/**
		 * @deprecated
		 */
		virtual int getDimensionality() const;

		virtual void setSystem(const System::ConstPtr& system);

		virtual Eigen::MatrixXd getGoal() const;
		virtual void setGoal(const Eigen::MatrixXd& new_goal);
	
		virtual bool getGoalIsRelative() const;
		virtual void setGoalIsRelative(bool is_goal_relative);

		virtual Eigen::MatrixXd getKp() const;
		virtual void setKp(const Eigen::MatrixXd& new_kp);

		virtual Eigen::MatrixXd getKv() const;
		virtual void setKv(const Eigen::MatrixXd& new_kv);

		/**
		 * @brief Completion times to reach goal and intermediate points
		 *
		 * Optional attribute, e.g. makes sense for interpolated
		 * controllers/
		 */
		virtual void setCompletionTimes(const Eigen::MatrixXd& new_times);
		virtual void setCompletionTime(const double& t); //for convenience - creates 1x1 matrix	
		virtual Eigen::MatrixXd getCompletionTimes() const;

		virtual void setMaximumVelocity(const Eigen::MatrixXd& max_vel);
		virtual	void setMaximumAcceleration(const Eigen::MatrixXd& max_acc);

		virtual Eigen::MatrixXd	getMaximumVelocity() const;
		virtual Eigen::MatrixXd	getMaximumAcceleration() const;
		/**
		 * @brief Priority of this controller in a ControlSet
		 *
		 * Optional attribute.
		 */
		virtual double getPriority() const;
		virtual void setPriority(double priority);
		
		virtual std::string getName() const;
		virtual void setName(const std::string& new_name);

		virtual const std::string getType() const;
		virtual void setType(const std::string& new_type);

		/**
		 * @brief Pass any additional argument for your controller
		 */
		template <typename T> void setArgument(const std::string& field_name, const T& field_value)
		{
			ha_ostringstream ha_oss;
			ha_oss << field_value;
			this->setArgumentString(field_name, ha_oss.str());
		}

		template <typename T> bool getArgument(const std::string& field_name, T& return_value) const
		{
			std::string val;
			bool ret = this->getArgumentString(field_name, val);
			if (ret)
			{
				std::istringstream ss(val);
				ss >> return_value;
				return true;
			}
			else
			{
				return false;
			}
		}

		/**
		 * @brief Computes a default interpolation time for a trajectory from x0 to xf
		 * under velocity and acceleration limits. Currrently assumes linear interpolation!
		 * 
		 * The implementation in Controller computes element-wise linear interpolation.
		 * If you want to have more complex behaviour (i.e. rotational interpolation) overload this function.
		 *
	     * @return minimal interpolation time.
		 */
		virtual double computeInterpolationTime
			(const Eigen::MatrixXd& x0, const Eigen::MatrixXd& xf,
			const Eigen::MatrixXd& xdot0 = Eigen::MatrixXd(), const Eigen::MatrixXd& xdotf = Eigen::MatrixXd()) const;


        const std::map<std::string, std::string>& getAdditionalArgumentsString() {
            return _additional_arguments;
        }

	protected:

{'fraction_non_alphanumeric': 0.10551181102362205, 'fraction_numerical': 0.0015748031496062992, 'mean_word_length': 4.020256916996048, 'pattern_counts': {'":': 0, '<': 33, '<?xml version=': 0, '>': 35, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '1993887', 'n_tokens_mistral': 3422, 'n_tokens_neox': 2925, 'n_words': 818}