OpenRAVE: headers diff between 0.6.6 and 0.8.0 versions

	config.h	config.h

	skipping to change at line 39	skipping to change at line 39
	#else	#else
	#define OPENRAVE_API OPENRAVE_HELPER_DLL_IMPORT	#define OPENRAVE_API OPENRAVE_HELPER_DLL_IMPORT
	#endif // OPENRAVE_DLL_EXPORTS	#endif // OPENRAVE_DLL_EXPORTS
	#define OPENRAVE_LOCAL OPENRAVE_HELPER_DLL_LOCAL	#define OPENRAVE_LOCAL OPENRAVE_HELPER_DLL_LOCAL
	#else // OPENRAVE_DLL is not defined: this means OpenRAVE is a static lib.	#else // OPENRAVE_DLL is not defined: this means OpenRAVE is a static lib.
	#define OPENRAVE_API	#define OPENRAVE_API
	#define OPENRAVE_LOCAL	#define OPENRAVE_LOCAL
	#endif // OPENRAVE_DLL	#endif // OPENRAVE_DLL

	#define OPENRAVE_VERSION_MAJOR 0	#define OPENRAVE_VERSION_MAJOR 0

	#define OPENRAVE_VERSION_MINOR 6	#define OPENRAVE_VERSION_MINOR 8
	#define OPENRAVE_VERSION_PATCH 6	#define OPENRAVE_VERSION_PATCH 0
	#define OPENRAVE_VERSION_COMBINED(major, minor, patch) (((major) << 16) \| ( (minor) << 8) \| (patch))	#define OPENRAVE_VERSION_COMBINED(major, minor, patch) (((major) << 16) \| ( (minor) << 8) \| (patch))
	#define OPENRAVE_VERSION OPENRAVE_VERSION_COMBINED(OPENRAVE_VERSION_MAJOR, OPENRAVE_VERSION_MINOR, OPENRAVE_VERSION_PATCH)	#define OPENRAVE_VERSION OPENRAVE_VERSION_COMBINED(OPENRAVE_VERSION_MAJOR, OPENRAVE_VERSION_MINOR, OPENRAVE_VERSION_PATCH)
	#define OPENRAVE_VERSION_EXTRACT_MAJOR(version) (((version)>>16)&0xff)	#define OPENRAVE_VERSION_EXTRACT_MAJOR(version) (((version)>>16)&0xff)
	#define OPENRAVE_VERSION_EXTRACT_MINOR(version) (((version)>>8)&0xff)	#define OPENRAVE_VERSION_EXTRACT_MINOR(version) (((version)>>8)&0xff)
	#define OPENRAVE_VERSION_EXTRACT_PATCH(version) (((version))&0xff)	#define OPENRAVE_VERSION_EXTRACT_PATCH(version) (((version))&0xff)

	#define OPENRAVE_VERSION_STRING "0.6.6"	#define OPENRAVE_VERSION_STRING "0.8.0"
	#define OPENRAVE_VERSION_STRING_FORMAT(version) boost::str(boost::format("% s.%s.%s")%(OPENRAVE_VERSION_EXTRACT_MAJOR(version))%(OPENRAVE_VERSION_EXTRA CT_MINOR(version))%(OPENRAVE_VERSION_EXTRACT_PATCH(version)))	#define OPENRAVE_VERSION_STRING_FORMAT(version) boost::str(boost::format("% s.%s.%s")%(OPENRAVE_VERSION_EXTRACT_MAJOR(version))%(OPENRAVE_VERSION_EXTRA CT_MINOR(version))%(OPENRAVE_VERSION_EXTRACT_PATCH(version)))

	#define OPENRAVE_VERSION_GE(major1, minor1, patch1, major2, minor2, patch2) (OPENRAVE_VERSION_COMBINED(major1, minor1, patch1) >= OPENRAVE_VERSION_COM BINED(major2, minor2, patch2))	#define OPENRAVE_VERSION_GE(major1, minor1, patch1, major2, minor2, patch2) (OPENRAVE_VERSION_COMBINED(major1, minor1, patch1) >= OPENRAVE_VERSION_COM BINED(major2, minor2, patch2))
	#define OPENRAVE_VERSION_MINIMUM(major, minor, patch) OPENRAVE_VERSION_GE(O PENRAVE_VERSION_MAJOR, OPENRAVE_VERSION_MINOR, OPENRAVE_VERSION_PATCH, majo r, minor, patch)	#define OPENRAVE_VERSION_MINIMUM(major, minor, patch) OPENRAVE_VERSION_GE(O PENRAVE_VERSION_MAJOR, OPENRAVE_VERSION_MINOR, OPENRAVE_VERSION_PATCH, majo r, minor, patch)

	// if 0, single precision	// if 0, single precision
	// if 1, double precision	// if 1, double precision
	#define OPENRAVE_PRECISION 1	#define OPENRAVE_PRECISION 1


	#define OPENRAVE_PLUGINS_INSTALL_DIR "/home/ut/testing/openrave/0.6.6/share	#define OPENRAVE_PLUGINS_INSTALL_DIR "/home/ut/testing/openrave/0.8.0/share
	/openrave-0.6/plugins"	/openrave-0.8/plugins"
	#define OPENRAVE_DATA_INSTALL_DIR "/home/ut/testing/openrave/0.6.6/share/op	#define OPENRAVE_DATA_INSTALL_DIR "/home/ut/testing/openrave/0.8.0/share/op
	enrave-0.6"	enrave-0.8"
	#define OPENRAVE_PYTHON_INSTALL_DIR "/home/ut/testing/openrave/0.6.6/lib/py	#define OPENRAVE_PYTHON_INSTALL_DIR "/home/ut/testing/openrave/0.8.0/lib/py
	thon2.7/site-packages"	thon2.7/site-packages"

	#endif	#endif

End of changes. 3 change blocks.
	9 lines changed or deleted	9 lines changed or added

	environment.h	environment.h
	// -- coding: utf-8 --	// -- coding: utf-8 --

	// Copyright (C) 2006-2011 Rosen Diankov <rosen.diankov@gmail.com>	// Copyright (C) 2006-2012 Rosen Diankov <rosen.diankov@gmail.com>
	//	//
	// This file is part of OpenRAVE.	// This file is part of OpenRAVE.
	// OpenRAVE is free software: you can redistribute it and/or modify	// OpenRAVE is free software: you can redistribute it and/or modify
	// it under the terms of the GNU Lesser General Public License as published by	// it under the terms of the GNU Lesser General Public License as published by
	// the Free Software Foundation, either version 3 of the License, or	// the Free Software Foundation, either version 3 of the License, or
	// at your option) any later version.	// at your option) any later version.
	//	//
	// This program is distributed in the hope that it will be useful,	// This program is distributed in the hope that it will be useful,
	// but WITHOUT ANY WARRANTY; without even the implied warranty of	// but WITHOUT ANY WARRANTY; without even the implied warranty of
	// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the	// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the

	skipping to change at line 76	skipping to change at line 76
	///	///
	/// Clones do not share any memory or resource between each other.	/// Clones do not share any memory or resource between each other.
	/// or their parent making them ideal for performing separte planning e xperiments while keeping	/// or their parent making them ideal for performing separte planning e xperiments while keeping
	/// the parent environment unchanged.	/// the parent environment unchanged.
	/// By default a clone only copies the collision checkers and physics e ngine.	/// By default a clone only copies the collision checkers and physics e ngine.
	/// When bodies are cloned, the unique ids are preserved across environ ments (each body can be referenced with its id in both environments). The a ttached and grabbed bodies of each body/robot are also copied to the new en vironment.	/// When bodies are cloned, the unique ids are preserved across environ ments (each body can be referenced with its id in both environments). The a ttached and grabbed bodies of each body/robot are also copied to the new en vironment.
	/// \param options A set of \ref CloningOptions describing what is actu ally cloned.	/// \param options A set of \ref CloningOptions describing what is actu ally cloned.
	/// \return An environment of the same type as this environment contain ing the copied information.	/// \return An environment of the same type as this environment contain ing the copied information.
	virtual EnvironmentBasePtr CloneSelf(int options) = 0;	virtual EnvironmentBasePtr CloneSelf(int options) = 0;


		/// \brief Clones the reference environment into the current environmen
		t
		///
		/// Tries to preserve computation by re-using bodies/interfaces that ar
		e already similar between the current and reference environments.
		/// \param[in] cloningoptions The parts of the environment to clone. Pa
		rts not specified are left as is.
		virtual void Clone(EnvironmentBaseConstPtr preference, int cloningoptio
		ns) = 0;

	/// \brief Each function takes an optional pointer to a CollisionReport structure and returns true if collision occurs. <b>[multi-thread safe]</b>	/// \brief Each function takes an optional pointer to a CollisionReport structure and returns true if collision occurs. <b>[multi-thread safe]</b>
	///	///
	/// \name Collision specific functions.	/// \name Collision specific functions.
	/// \anchor env_collision_checking	/// \anchor env_collision_checking
	//@{	//@{
	/// set the global environment collision checker	/// set the global environment collision checker
	virtual bool SetCollisionChecker(CollisionCheckerBasePtr pchecker)=0;	virtual bool SetCollisionChecker(CollisionCheckerBasePtr pchecker)=0;
	virtual CollisionCheckerBasePtr GetCollisionChecker() const =0;	virtual CollisionCheckerBasePtr GetCollisionChecker() const =0;

	/// \see CollisionCheckerBase::CheckCollision(KinBodyConstPtr,Collision ReportPtr)	/// \see CollisionCheckerBase::CheckCollision(KinBodyConstPtr,Collision ReportPtr)

	skipping to change at line 126	skipping to change at line 132
	virtual bool CheckSelfCollision(KinBodyConstPtr pbody, CollisionReportP tr report = CollisionReportPtr()) = 0;	virtual bool CheckSelfCollision(KinBodyConstPtr pbody, CollisionReportP tr report = CollisionReportPtr()) = 0;

	typedef boost::function<CollisionAction(CollisionReportPtr,bool)> Colli sionCallbackFn;	typedef boost::function<CollisionAction(CollisionReportPtr,bool)> Colli sionCallbackFn;

	/// Register a collision callback.	/// Register a collision callback.
	///	///
	/// Whenever a collision is detected between between bodies during a Ch eckCollision call or physics simulation, the callback is called.	/// Whenever a collision is detected between between bodies during a Ch eckCollision call or physics simulation, the callback is called.
	/// The callback should return an action specifying how the collision s hould be handled:	/// The callback should return an action specifying how the collision s hould be handled:
	/// <b>action = callback(CollisionReport,bool IsCalledFromPhysicsEngine )</b>	/// <b>action = callback(CollisionReport,bool IsCalledFromPhysicsEngine )</b>
	/// \return a handle to the registration, once the handle loses scope, the callback is unregistered	/// \return a handle to the registration, once the handle loses scope, the callback is unregistered

	virtual boost::shared_ptr<void> RegisterCollisionCallback(const Collisi onCallbackFn& callback) = 0;	virtual UserDataPtr RegisterCollisionCallback(const CollisionCallbackFn & callback) = 0;
	virtual bool HasRegisteredCollisionCallbacks() const = 0;	virtual bool HasRegisteredCollisionCallbacks() const = 0;


		/// \brief return all the collision callbacks, the environment must be
		locked!
		///
		/// \param listcallbacks filled with the user callbacks. Once the envir
		onment is unlocked, the list becomes invalid.
	virtual void GetRegisteredCollisionCallbacks(std::list<CollisionCallbac kFn>&) const = 0;	virtual void GetRegisteredCollisionCallbacks(std::list<CollisionCallbac kFn>&) const = 0;


	//@}	//@}

	/// \name Physics and Simulation	/// \name Physics and Simulation
	//@{	//@{
	/// set the physics engine, disabled by default	/// set the physics engine, disabled by default
	/// \param physics the engine to set, if NULL, environment sets an dumm y physics engine	/// \param physics the engine to set, if NULL, environment sets an dumm y physics engine
	virtual bool SetPhysicsEngine(PhysicsEngineBasePtr physics) = 0;	virtual bool SetPhysicsEngine(PhysicsEngineBasePtr physics) = 0;
	virtual PhysicsEngineBasePtr GetPhysicsEngine() const = 0;	virtual PhysicsEngineBasePtr GetPhysicsEngine() const = 0;

	/// \brief Makes one simulation time step. <b>[multi-thread safe]</b>	/// \brief Makes one simulation time step. <b>[multi-thread safe]</b>

	skipping to change at line 176	skipping to change at line 187
	//@}	//@}

	/// \name File Loading and Parsing	/// \name File Loading and Parsing
	/// \anchor env_loading	/// \anchor env_loading
	//@{	//@{

	/// \brief A set of options used to select particular parts of the scen e	/// \brief A set of options used to select particular parts of the scen e
	enum SelectionOptions	enum SelectionOptions
	{	{
	SO_NoRobots = 1, ///< everything but robots	SO_NoRobots = 1, ///< everything but robots

	TO_Obstacles = 1, ///< everything but robots	TO_Obstacles = 1,
	SO_Robots = 2, ///< all robots	SO_Robots = 2, ///< all robots

	TO_Robots = 2, ///< all robots	TO_Robots = 2,
	SO_Everything = 3, ///< all bodies and robots everythi ng	SO_Everything = 3, ///< all bodies and robots everythi ng

	TO_Everything = 3, ///< all bodies and robots everythi	TO_Everything = 3,
	ng	SO_Body = 4, ///< robot/kinbody. atts 'target' k
	SO_Body = 4, ///< only triangulate robot/kinbody	ey contains the body name. If multiple targets are specified, then will sav
	TO_Body = 4, ///< only triangulate robot/kinbody	e all related bodies.
	SO_AllExceptBody = 5, ///< select everything but the ro	TO_Body = 4,
	bot/kinbody	SO_AllExceptBody = 5, ///< save everything but the robo
	TO_AllExceptBody = 5, ///< select everything but the ro	t/kinbody objects specified through the atts 'target' key. Multiple targets
	bot/kinbody	can be specified.
	SO_BodyList = 6, ///< provide a list of body names	TO_AllExceptBody = 5,
	};	};
	typedef SelectionOptions TriangulateOptions;	typedef SelectionOptions TriangulateOptions;


	/// \brief Loads a scene from a file and adds all objects in the enviro	/** \brief Loads a scene from a file and adds all objects in the enviro
	nment. <b>[multi-thread safe]</b>	nment. <b>[multi-thread safe]</b>

		For collada readers, the options are passed through to
		\code
		DAE::getIOPlugin()->setOption(key,value).
		\endcode
		*/
	virtual bool Load(const std::string& filename, const AttributesList& at ts = AttributesList()) = 0;	virtual bool Load(const std::string& filename, const AttributesList& at ts = AttributesList()) = 0;


		/** \brief Loads a scene from a URI and adds all objects in the environ
		ment. <b>[multi-thread safe]</b>

		Currently only collada files are supported. Options are passed thro
		ugh to
		\code
		DAE::getIOPlugin()->setOption(key,value).
		\endcode
		*/
		virtual bool LoadURI(const std::string& filename, const AttributesList&
		atts = AttributesList()) = 0;

	/// \brief Loads a scene from in-memory data and adds all objects in th e environment. <b>[multi-thread safe]</b>	/// \brief Loads a scene from in-memory data and adds all objects in th e environment. <b>[multi-thread safe]</b>
	virtual bool LoadData(const std::string& data, const AttributesList& at ts = AttributesList()) = 0;	virtual bool LoadData(const std::string& data, const AttributesList& at ts = AttributesList()) = 0;

	virtual bool LoadXMLData(const std::string& data, const AttributesList& atts = AttributesList()) {	virtual bool LoadXMLData(const std::string& data, const AttributesList& atts = AttributesList()) {
	return LoadData(data,atts);	return LoadData(data,atts);
	}	}


	/// \brief Saves a scene depending on the filename extension. Default i	/** \brief Saves a scene depending on the filename extension. Default i
	s in COLLADA format	s in COLLADA format
	///
	/// \param filename the filename to save the results at	\param filename the filename to save the results at
	/// \param options controls what to save	\param options controls what to save
	/// \param selectname	\param atts attributes that refine further options. For collada par
	/// \throw openrave_exception Throw if failed to save anything	sing, the options are passed through
	virtual void Save(const std::string& filename, SelectionOptions options	\code
	=SO_Everything, const std::string& selectname="") = 0;	DAE::getIOPlugin()->setOption(key,value).
		\endcode
		Several default options are:
		- 'target' - the target body name of the options, if relevant
		- 'password' - the password/key to encrypt the data with, collada s
		upports this through zae zip archives
		\throw openrave_exception Throw if failed to save anything
		*/
		virtual void Save(const std::string& filename, SelectionOptions options
		=SO_Everything, const AttributesList& atts = AttributesList()) = 0;

		/// \deprecated (12/08/15)
		virtual void Save(const std::string& filename, SelectionOptions options
		, const std::string& selectname) RAVE_DEPRECATED {
		AttributesList atts;
		atts.push_back(std::make_pair(std::string("target"),selectname));
		Save(filename,options,atts);
		}

	/** \brief Initializes a robot from a resource file. The robot is not a dded to the environment when calling this function. <b>[multi-thread safe]< /b>	/** \brief Initializes a robot from a resource file. The robot is not a dded to the environment when calling this function. <b>[multi-thread safe]< /b>

	\param robot If a null pointer is passed, a new robot will be creat ed, otherwise an existing robot will be filled	\param robot If a null pointer is passed, a new robot will be creat ed, otherwise an existing robot will be filled
	\param filename the name of the resource file, its extension determ ines the format of the file. See \ref supported_formats.	\param filename the name of the resource file, its extension determ ines the format of the file. See \ref supported_formats.
	\param atts The attribute/value pair specifying loading options. De fined in \ref arch_robot.	\param atts The attribute/value pair specifying loading options. De fined in \ref arch_robot.
	*/	*/
	virtual RobotBasePtr ReadRobotURI(RobotBasePtr robot, const std::string & filename, const AttributesList& atts = AttributesList()) = 0;	virtual RobotBasePtr ReadRobotURI(RobotBasePtr robot, const std::string & filename, const AttributesList& atts = AttributesList()) = 0;
	virtual RobotBasePtr ReadRobotXMLFile(RobotBasePtr robot, const std::st ring& filename, const AttributesList& atts = AttributesList()) {	virtual RobotBasePtr ReadRobotXMLFile(RobotBasePtr robot, const std::st ring& filename, const AttributesList& atts = AttributesList()) {
	return ReadRobotURI(robot,filename,atts);	return ReadRobotURI(robot,filename,atts);

	skipping to change at line 304	skipping to change at line 343
	/** \brief reads in the rigid geometry of a resource file into a TRIMES H structure	/** \brief reads in the rigid geometry of a resource file into a TRIMES H structure

	\param filename the name of the resource file, its extension determ ines the format of the file. Complex meshes and articulated meshes are all triangulated appropriately. See \ref supported_formats.	\param filename the name of the resource file, its extension determ ines the format of the file. Complex meshes and articulated meshes are all triangulated appropriately. See \ref supported_formats.
	\param options Options to control the parsing process.	\param options Options to control the parsing process.
	*/	*/
	virtual boost::shared_ptr<KinBody::Link::TRIMESH> ReadTrimeshURI(boost: :shared_ptr<KinBody::Link::TRIMESH> ptrimesh, const std::string& filename, const AttributesList& atts = AttributesList()) = 0;	virtual boost::shared_ptr<KinBody::Link::TRIMESH> ReadTrimeshURI(boost: :shared_ptr<KinBody::Link::TRIMESH> ptrimesh, const std::string& filename, const AttributesList& atts = AttributesList()) = 0;
	virtual boost::shared_ptr<KinBody::Link::TRIMESH> ReadTrimeshFile(boost ::shared_ptr<KinBody::Link::TRIMESH> ptrimesh, const std::string& filename, const AttributesList& atts = AttributesList()) {	virtual boost::shared_ptr<KinBody::Link::TRIMESH> ReadTrimeshFile(boost ::shared_ptr<KinBody::Link::TRIMESH> ptrimesh, const std::string& filename, const AttributesList& atts = AttributesList()) {
	return ReadTrimeshURI(ptrimesh,filename,atts);	return ReadTrimeshURI(ptrimesh,filename,atts);
	}	}


	/// \deprecated (10/09/30) see \ref RaveRegisterXMLReader
	virtual UserDataPtr RegisterXMLReader(InterfaceType type, const std::st
	ring& xmltag, const CreateXMLReaderFn& fn) RAVE_DEPRECATED = 0;

	/// \brief Parses a file for OpenRAVE XML formatted data.
	virtual bool ParseXMLFile(BaseXMLReaderPtr preader, const std::string&
	filename) RAVE_DEPRECATED = 0;

	/** \brief Parses a data file for XML data.

	\param pdata The data of the buffer
	\param len the number of bytes valid in pdata
	*/
	virtual bool ParseXMLData(BaseXMLReaderPtr preader, const std::string&
	data) RAVE_DEPRECATED = 0;
	//@}	//@}

	/// \name Object Setting and Querying	/// \name Object Setting and Querying
	/// \anchor env_objects	/// \anchor env_objects
	//@{	//@{


	/// \brief Add a body to the environment	/** \brief Add an interface to the environment
	///
	/// \param[in] body the pointer to an initialized body	Depending on the type of interface, the addition behaves differentl
	/// \param[in] bAnonymous if true and there exists a body/robot with th	y. For bodies/robots, will add them to visual/geometric environment. For mo
	e same name, will make body's name unique	dules, will call their main() method and add them officially. For viewers,
	/// \throw openrave_exception Throw if body is invalid or already added	will attach a viewer to the environment and start sending it data.
	virtual void AddKinBody(KinBodyPtr body, bool bAnonymous=false) = 0;	For interfaces that don't make sense to add, will throw an exceptio
		n.
	/// \brief add a robot to the environment	\param[in] pinterface the pointer to an initialized interface
	///	\param[in] bAnonymous if true and there exists a body/robot with th
	/// \param[in] robot the pointer to an initialized robot	e same name, will make body's name unique
	/// \param[in] bAnonymous if true and there exists a body/robot with th	\param[in] cmdargs The command-line arguments for the module.
	e same name, will make robot's name unique	\throw openrave_exception Throw if interface is invalid or already
	/// \throw openrave_exception Throw if robot is invalid or already adde	added
	d	*/
	virtual void AddRobot(RobotBasePtr robot, bool bAnonymous=false) = 0;	virtual void Add(InterfaceBasePtr pinterface, bool bAnonymous=false, co
		nst std::string& cmdargs="") = 0;
	/// \brief registers the sensor with the environment and turns its powe
	r on.	/// \deprecated (12/04/18)
	///	virtual void AddKinBody(KinBodyPtr body, bool bAnonymous=false) RAVE_DE
	/// \param[in] sensor the pointer to an initialized sensor	PRECATED {
	/// \param[in] bAnonymous if true and there exists a sensor with the sa	RAVELOG_WARN("EnvironmentBase::AddKinBody deprecated, please use En
	me name, will make sensor's name unique	vironmentBase::Add\n");
	/// \throw openrave_exception Throw if sensor is invalid or already add	Add(body,bAnonymous);
	ed	}
	virtual void AddSensor(SensorBasePtr sensor, bool bAnonymous=false) = 0	/// \deprecated (12/04/18)
	;	virtual void AddRobot(RobotBasePtr robot, bool bAnonymous=false) RAVE_D
		EPRECATED {
		RAVELOG_WARN("EnvironmentBase::AddRobot deprecated, please use Envi
		ronmentBase::Add\n");
		Add(robot,bAnonymous);
		}

		/// \deprecated (12/04/18)
		virtual void AddSensor(SensorBasePtr sensor, bool bAnonymous=false) RAV
		E_DEPRECATED {
		RAVELOG_WARN("EnvironmentBase::AddSensor deprecated, please use Env
		ironmentBase::Add\n");
		Add(sensor,bAnonymous);
		}

	/// \brief Fill an array with all sensors loaded in the environment. <b >[multi-thread safe]</b>	/// \brief Fill an array with all sensors loaded in the environment. <b >[multi-thread safe]</b>
	///	///
	/// The sensors come from the currently loaded robots and the explicitl y added sensors	/// The sensors come from the currently loaded robots and the explicitl y added sensors

	virtual void GetSensors(std::vector<SensorBasePtr>& sensors) const = 0;	/// \param timeout microseconds to wait before throwing an exception, i
		f 0, will block indefinitely.
	/// \deprecated (10/09/15) see \ref EnvironmentBase::Remove	/// \throw openrave_exception with ORE_Timeout error code
	virtual bool RemoveKinBody(KinBodyPtr body) RAVE_DEPRECATED = 0;	virtual void GetSensors(std::vector<SensorBasePtr>& sensors, uint64_t t
		imeout=0) const = 0;

	/// \brief Removes a currently loaded interface from the environment. < b>[multi-thread safe]</b>	/// \brief Removes a currently loaded interface from the environment. < b>[multi-thread safe]</b>
	///	///
	/// The function removes currently loaded bodies, robots, sensors, prob lems from the actively used	/// The function removes currently loaded bodies, robots, sensors, prob lems from the actively used
	/// interfaces of the environment. This does not destroy the interface, but it does remove all	/// interfaces of the environment. This does not destroy the interface, but it does remove all
	/// references managed. Some interfaces like problems have destroy meth ods that are called to signal	/// references managed. Some interfaces like problems have destroy meth ods that are called to signal
	/// unloading. Note that the active interfaces are different from the o wned interfaces.	/// unloading. Note that the active interfaces are different from the o wned interfaces.
	/// \param[in] obj interface to remove	/// \param[in] obj interface to remove
	/// \return true if the interface was successfully removed from the env ironment.	/// \return true if the interface was successfully removed from the env ironment.
	virtual bool Remove(InterfaceBasePtr obj) = 0;	virtual bool Remove(InterfaceBasePtr obj) = 0;

	skipping to change at line 374	skipping to change at line 407

	/// \brief Query a sensor from its name. <b>[multi-thread safe]</b>	/// \brief Query a sensor from its name. <b>[multi-thread safe]</b>
	/// \return first sensor that matches with name, note that sensors atta ched to robots have the robot name as a prefix.	/// \return first sensor that matches with name, note that sensors atta ched to robots have the robot name as a prefix.
	virtual SensorBasePtr GetSensor(const std::string& name) const =0;	virtual SensorBasePtr GetSensor(const std::string& name) const =0;

	/// \brief Query a robot from its name. <b>[multi-thread safe]</b>	/// \brief Query a robot from its name. <b>[multi-thread safe]</b>
	/// \return first Robot that matches the name	/// \return first Robot that matches the name
	virtual RobotBasePtr GetRobot(const std::string& name) const =0;	virtual RobotBasePtr GetRobot(const std::string& name) const =0;

	/// \brief Get all bodies loaded in the environment (including robots). <b>[multi-thread safe]</b>	/// \brief Get all bodies loaded in the environment (including robots). <b>[multi-thread safe]</b>

		///
		/// A separate interface mutex is locked for reading the bodies.
	/// \param[out] bodies filled with all the bodies	/// \param[out] bodies filled with all the bodies

	virtual void GetBodies(std::vector<KinBodyPtr>& bodies) const = 0;	/// \param timeout microseconds to wait before throwing an exception, i
		f 0, will block indefinitely.
		/// \throw openrave_exception with ORE_Timeout error code
		virtual void GetBodies(std::vector<KinBodyPtr>& bodies, uint64_t timeou
		t=0) const = 0;

	/// \brief Fill an array with all robots loaded in the environment. <b> [multi-thread safe]</b>	/// \brief Fill an array with all robots loaded in the environment. <b> [multi-thread safe]</b>

	virtual void GetRobots(std::vector<RobotBasePtr>& robots) const = 0;	///
		/// A separate interface mutex is locked for reading the bodies.
		/// \param timeout microseconds to wait before throwing an exception, i
		f 0, will block indefinitely.
		/// \throw openrave_exception with ORE_Timeout error code
		virtual void GetRobots(std::vector<RobotBasePtr>& robots, uint64_t time
		out=0) const = 0;

	/// \brief Retrieve published bodies, completes even if environment is locked. <b>[multi-thread safe]</b>	/// \brief Retrieve published bodies, completes even if environment is locked. <b>[multi-thread safe]</b>
	///	///

		/// A separate interface mutex is locked for reading the modules.
	/// Note that the pbody pointer might become invalid as soon as GetPubl ishedBodies returns.	/// Note that the pbody pointer might become invalid as soon as GetPubl ishedBodies returns.

	virtual void GetPublishedBodies(std::vector<KinBody::BodyState>& vbodie	/// \param timeout microseconds to wait before throwing an exception, i
	s) = 0;	f 0, will block indefinitely.
		/// \throw openrave_exception with ORE_Timeout error code
		virtual void GetPublishedBodies(std::vector<KinBody::BodyState>& vbodie
		s, uint64_t timeout=0) = 0;


	/// updates the published bodies that viewers and other programs listen	/// \brief Updates the published bodies that viewers and other programs
	ing in on the environment see.	listening in on the environment see.
		///
	/// For example, calling this function inside a planning loop allows th e viewer to update the environment	/// For example, calling this function inside a planning loop allows th e viewer to update the environment
	/// reflecting the status of the planner.	/// reflecting the status of the planner.
	/// Assumes that the physics are locked.	/// Assumes that the physics are locked.

	virtual void UpdatePublishedBodies() = 0;	/// \param timeout microseconds to wait before throwing an exception, i
		f 0, will block indefinitely.
		/// \throw openrave_exception with ORE_Timeout error code
		virtual void UpdatePublishedBodies(uint64_t timeout=0) = 0;

	/// Get the corresponding body from its unique network id	/// Get the corresponding body from its unique network id
	virtual KinBodyPtr GetBodyFromEnvironmentId(int id) = 0;	virtual KinBodyPtr GetBodyFromEnvironmentId(int id) = 0;

	/// \brief Triangulation of the body including its current transformati on. trimesh will be appended the new data. <b>[multi-thread safe]</b>	/// \brief Triangulation of the body including its current transformati on. trimesh will be appended the new data. <b>[multi-thread safe]</b>
	///	///
	/// \param[out] trimesh - The output triangle mesh	/// \param[out] trimesh - The output triangle mesh
	/// \param[in] body body the triangulate	/// \param[in] body body the triangulate
	/// \throw openrave_exception Throw if failed to add anything	/// \throw openrave_exception Throw if failed to add anything
	virtual void Triangulate(KinBody::Link::TRIMESH& trimesh, KinBodyConstP tr pbody) = 0;	virtual void Triangulate(KinBody::Link::TRIMESH& trimesh, KinBodyConstP tr pbody) = 0;

	skipping to change at line 418	skipping to change at line 465
	//@}	//@}

	/// \brief Load a new module, need to Lock if calling outside simulatio n thread	/// \brief Load a new module, need to Lock if calling outside simulatio n thread
	virtual int AddModule(ModuleBasePtr module, const std::string& cmdargs) = 0;	virtual int AddModule(ModuleBasePtr module, const std::string& cmdargs) = 0;

	/// \deprecated (12/01/30) see \ref EnvironmentBase::AddModule	/// \deprecated (12/01/30) see \ref EnvironmentBase::AddModule
	virtual int LoadProblem(ModuleBasePtr module, const std::string& cmdarg s) RAVE_DEPRECATED {	virtual int LoadProblem(ModuleBasePtr module, const std::string& cmdarg s) RAVE_DEPRECATED {
	return AddModule(module,cmdargs);	return AddModule(module,cmdargs);
	}	}


	/// \deprecated (10/09/15) see \ref EnvironmentBase::Remove	/// \brief Fills a list with the loaded modules in the environment.
	virtual bool RemoveProblem(ModuleBasePtr prob) RAVE_DEPRECATED = 0;

	/// \brief Returns a list of loaded problems with a pointer to a lock p
	reventing the list from being modified.
	///	///

	/// As long as the lock is held, the problems are guaranteed to stay lo	/// A separate interface mutex is locked for reading the modules.
	aded in the environment.	/// If the environment is locked, the modules are guaranteed to stay lo
	/// \return returns a pointer to a Lock. Destroying the shared_ptr will	aded in the environment.
	release the lock	/// \param timeout microseconds to wait before throwing an exception, i
	virtual boost::shared_ptr<void> GetModules(std::list<ModuleBasePtr>& li	f 0, will block indefinitely.
	stModules) const = 0;	/// \throw openrave_exception with ORE_Timeout error code
		virtual void GetModules(std::list<ModuleBasePtr>& listModules, uint64_t
	virtual boost::shared_ptr<void> GetLoadedProblems(std::list<ModuleBaseP	timeout=0) const = 0;
	tr>& listModules) const {
	return GetModules(listModules);	/// \deprecated (12/01/30)
		virtual void GetLoadedProblems(std::list<ModuleBasePtr>& listModules) c
		onst {
		GetModules(listModules);
	}	}

	/// \brief Return the global environment mutex used to protect environm ent information access in multi-threaded environments.	/// \brief Return the global environment mutex used to protect environm ent information access in multi-threaded environments.
	///	///
	/// Accessing environment body information and adding/removing bodies	/// Accessing environment body information and adding/removing bodies
	/// or changing any type of scene property should have the environment lock acquired. Once the environment	/// or changing any type of scene property should have the environment lock acquired. Once the environment
	/// is locked, the user is guaranteed that nnothing will change in the environment.	/// is locked, the user is guaranteed that nnothing will change in the environment.
	virtual EnvironmentMutex& GetMutex() const = 0;	virtual EnvironmentMutex& GetMutex() const = 0;

	/// \name 3D plotting methods.	/// \name 3D plotting methods.
	/// \anchor env_plotting	/// \anchor env_plotting
	//@{	//@{

	/// \deprecated (10/11/05)	/// \deprecated (10/11/05)
	typedef OpenRAVE::GraphHandlePtr GraphHandlePtr RAVE_DEPRECATED;	typedef OpenRAVE::GraphHandlePtr GraphHandlePtr RAVE_DEPRECATED;


	/// \brief adds a viewer to the environment	/// \deprecated (12/04/18)
	///	virtual void AddViewer(ViewerBasePtr pviewer) {
	/// \throw openrave_exception Throw if body is invalid or already added	Add(pviewer);
	virtual void AddViewer(ViewerBasePtr pviewer) = 0;	}

	/// \deprecated (11/06/13) see AddViewer	/// \deprecated (11/06/13) see AddViewer
	virtual bool AttachViewer(ViewerBasePtr pnewviewer) RAVE_DEPRECATED {	virtual bool AttachViewer(ViewerBasePtr pnewviewer) RAVE_DEPRECATED {

	AddViewer(pnewviewer); return true;	Add(pnewviewer);
		return true;
	}	}

	/// \brief Return a viewer with a particular name.	/// \brief Return a viewer with a particular name.
	///	///
	/// When no name is specified, the first loaded viewer is returned.	/// When no name is specified, the first loaded viewer is returned.
	virtual ViewerBasePtr GetViewer(const std::string& name="") const = 0;	virtual ViewerBasePtr GetViewer(const std::string& name="") const = 0;

	/// \brief Returns a list of loaded viewers with a pointer to a lock pr eventing the list from being modified.	/// \brief Returns a list of loaded viewers with a pointer to a lock pr eventing the list from being modified.
	///	///

	/// As long as the lock is held, the problems are guaranteed to stay lo	/// If the environment is locked, the viewers are guaranteed to stay lo
	aded in the environment.	aded in the environment.
	/// \return returns a pointer to a Lock. Destroying the shared_ptr will	virtual void GetViewers(std::list<ViewerBasePtr>& listViewers) const =
	release the lock	0;
	virtual boost::shared_ptr<boost::mutex::scoped_lock> GetViewers(std::li
	st<ViewerBasePtr>& listViewers) const = 0;

	/// \brief Plot a point cloud with one color. <b>[multi-thread safe]</b >	/// \brief Plot a point cloud with one color. <b>[multi-thread safe]</b >
	///	///
	/// \param ppoints array of points	/// \param ppoints array of points
	/// \param numPoints number of points to plot	/// \param numPoints number of points to plot
	/// \param stride stride in bytes to next point, ie: nextpoint = (float )((char)ppoints+stride)	/// \param stride stride in bytes to next point, ie: nextpoint = (float )((char)ppoints+stride)
	/// \param fPointSize size of a point in pixels	/// \param fPointSize size of a point in pixels
	/// \param color the rgb color of the point. The last component of the color is used for alpha blending	/// \param color the rgb color of the point. The last component of the color is used for alpha blending
	/// \param drawstyle if 0 will draw pixels. if 1, will draw 3D spheres	/// \param drawstyle if 0 will draw pixels. if 1, will draw 3D spheres
	/// \return handle to plotted points, graph is removed when handle is d estroyed (goes out of scope). This requires the user to always store the ha ndle in a persistent variable if the plotted graphics are to remain on the viewer.	/// \return handle to plotted points, graph is removed when handle is d estroyed (goes out of scope). This requires the user to always store the ha ndle in a persistent variable if the plotted graphics are to remain on the viewer.

	skipping to change at line 548	skipping to change at line 595
	/// \param ppoints - array of 3D points	/// \param ppoints - array of 3D points
	/// \param stride stride in bytes to next point, ie: nextpoint = (float )((char)ppoints+stride)	/// \param stride stride in bytes to next point, ie: nextpoint = (float )((char)ppoints+stride)
	/// \param pIndices If not NULL, zero-based indices into the points for every triangle. pIndices	/// \param pIndices If not NULL, zero-based indices into the points for every triangle. pIndices
	/// should be of size numTriangles. If pIndices is NULL, ppoints is ass umed to contain numTriangles*3	/// should be of size numTriangles. If pIndices is NULL, ppoints is ass umed to contain numTriangles*3
	/// points and triangles will be rendered in list order.	/// points and triangles will be rendered in list order.
	/// \param color The color of the triangle. The last component of the c olor is used for alpha blending	/// \param color The color of the triangle. The last component of the c olor is used for alpha blending
	/// \return handle to plotted points, graph is removed when handle is d estroyed (goes out of scope). This requires the user to always store the ha ndle in a persistent variable if the plotted graphics are to remain on the viewer.	/// \return handle to plotted points, graph is removed when handle is d estroyed (goes out of scope). This requires the user to always store the ha ndle in a persistent variable if the plotted graphics are to remain on the viewer.
	virtual OpenRAVE::GraphHandlePtr drawtrimesh(const float* ppoints, int stride, const int* pIndices, int numTriangles, const boost::multi_array<flo at,2>& colors) = 0;	virtual OpenRAVE::GraphHandlePtr drawtrimesh(const float* ppoints, int stride, const int* pIndices, int numTriangles, const boost::multi_array<flo at,2>& colors) = 0;
	//@}	//@}


	/// \deprecated (10/09/23) see \ref RaveGetHomeDirectory
	virtual const std::string& GetHomeDirectory() const RAVE_DEPRECATED = 0
	;

	//@{ debug/global commands	//@{ debug/global commands

	/// sets the debug level	/// sets the debug level
	/// \param level 0 for no debug, 1 - to print all debug messeges. Defau lt	/// \param level 0 for no debug, 1 - to print all debug messeges. Defau lt
	/// value for release builds is 0, for debug builds it is 1	/// value for release builds is 0, for debug builds it is 1
	/// declaring variables with stdcall can be a little complex	/// declaring variables with stdcall can be a little complex

	virtual void SetDebugLevel(uint32_t level) = 0;	virtual void SetDebugLevel(int level) = 0;
	virtual uint32_t GetDebugLevel() const = 0;	virtual int GetDebugLevel() const = 0;
	//@}	//@}

	protected:	protected:
	virtual const char* GetHash() const {	virtual const char* GetHash() const {
	return OPENRAVE_ENVIRONMENT_HASH;	return OPENRAVE_ENVIRONMENT_HASH;
	}	}
	private:	private:
	UserDataPtr __pUserData; ///< \see GetUserData	UserDataPtr __pUserData; ///< \see GetUserData
	int __nUniqueId; ///< \see GetId	int __nUniqueId; ///< \see GetId
	};	};

End of changes. 28 change blocks.
	109 lines changed or deleted	174 lines changed or added

	geometry.h	geometry.h

	skipping to change at line 50	skipping to change at line 50
	#endif	#endif

	namespace OpenRAVE {	namespace OpenRAVE {

	/// Templated math and geometric functions	/// Templated math and geometric functions
	namespace geometry {	namespace geometry {

	template <typename T> class RaveTransform;	template <typename T> class RaveTransform;
	template <typename T> class RaveTransformMatrix;	template <typename T> class RaveTransformMatrix;


	#ifndef MATH_SQRT	#ifdef OPENRAVE_MATH_SQRT_FLOAT
		inline float MATH_SQRT(float f) {
		return OPENRAVE_MATH_SQRT_FLOAT(f);
		}
		#else
	inline float MATH_SQRT(float f) {	inline float MATH_SQRT(float f) {
	return sqrtf(f);	return sqrtf(f);
	}	}

		#endif
		#ifdef OPENRAVE_MATH_SQRT_DOUBLE
		inline double MATH_SQRT(double f) {
		return OPENRAVE_MATH_SQRT_DOUBLE(f);
		}
		#else
	inline double MATH_SQRT(double f) {	inline double MATH_SQRT(double f) {
	return sqrt(f);	return sqrt(f);
	}	}
	#endif	#endif

	#ifndef MATH_SIN
		#ifdef OPENRAVE_MATH_SIN_FLOAT
		inline float MATH_SIN(float f) {
		return OPENRAVE_MATH_SIN_FLOAT(f);
		}
		#else
	inline float MATH_SIN(float f) {	inline float MATH_SIN(float f) {
	return sinf(f);	return sinf(f);
	}	}

		#endif

		#ifdef OPENRAVE_MATH_SIN_DOUBLE
		inline double MATH_SIN(double f) {
		return OPENRAVE_MATH_SIN_DOUBLE(f);
		}
		#else
	inline double MATH_SIN(double f) {	inline double MATH_SIN(double f) {
	return sin(f);	return sin(f);
	}	}
	#endif	#endif

	#ifndef MATH_COS
		#ifdef OPENRAVE_MATH_COS_FLOAT
		inline float MATH_COS(float f) {
		return OPENRAVE_MATH_COS_FLOAT(f);
		}
		#else
	inline float MATH_COS(float f) {	inline float MATH_COS(float f) {
	return cosf(f);	return cosf(f);
	}	}

		#endif

		#ifdef OPENRAVE_MATH_COS_DOUBLE
		inline double MATH_COS(double f) {
		return OPENRAVE_MATH_COS_DOUBLE(f);
		}
		#else
	inline double MATH_COS(double f) {	inline double MATH_COS(double f) {
	return cos(f);	return cos(f);
	}	}
	#endif	#endif

	#ifndef MATH_FABS
		#ifdef OPENRAVE_MATH_FABS_FLOAT
		inline float MATH_FABS(float f) {
		return OPENRAVE_MATH_FABS_FLOAT(f);
		}
		#else
	inline float MATH_FABS(float f) {	inline float MATH_FABS(float f) {
	return fabsf(f);	return fabsf(f);
	}	}

		#endif

		#ifdef OPENRAVE_MATH_FABS_DOUBLE
		inline double MATH_FABS(double f) {
		return OPENRAVE_MATH_FABS_DOUBLE(f);
		}
		#else
	inline double MATH_FABS(double f) {	inline double MATH_FABS(double f) {
	return fabs(f);	return fabs(f);
	}	}
	#endif	#endif

	#ifndef MATH_ACOS
		#ifdef OPENRAVE_MATH_ACOS_FLOAT
		inline float MATH_ACOS(float f) {
		return OPENRAVE_MATH_ACOS_FLOAT(f);
		}
		#else
	inline float MATH_ACOS(float f) {	inline float MATH_ACOS(float f) {
	return acosf(f);	return acosf(f);
	}	}

		#endif

		#ifdef OPENRAVE_MATH_ACOS_DOUBLE
		inline double MATH_ACOS(double f) {
		return OPENRAVE_MATH_ACOS_DOUBLE(f);
		}
		#else
	inline double MATH_ACOS(double f) {	inline double MATH_ACOS(double f) {
	return acos(f);	return acos(f);
	}	}
	#endif	#endif

	#ifndef MATH_ASIN
		#ifdef OPENRAVE_MATH_ASIN_FLOAT
		inline float MATH_ASIN(float f) {
		return OPENRAVE_MATH_ASIN_FLOAT(f);
		}
		#else
	inline float MATH_ASIN(float f) {	inline float MATH_ASIN(float f) {
	return asinf(f);	return asinf(f);
	}	}

		#endif

		#ifdef OPENRAVE_MATH_ASIN_DOUBLE
		inline double MATH_ASIN(double f) {
		return OPENRAVE_MATH_ASIN_DOUBLE(f);
		}
		#else
	inline double MATH_ASIN(double f) {	inline double MATH_ASIN(double f) {
	return asin(f);	return asin(f);
	}	}
	#endif	#endif

	#ifndef MATH_ATAN2
		#ifdef OPENRAVE_MATH_ATAN2_FLOAT
		inline float MATH_ATAN2(float fy, float fx) {
		return OPENRAVE_MATH_ATAN2_FLOAT(fy,fx);
		}
		#else
	inline float MATH_ATAN2(float fy, float fx) {	inline float MATH_ATAN2(float fy, float fx) {
	return atan2f(fy,fx);	return atan2f(fy,fx);
	}	}

		#endif

		#ifdef OPENRAVE_MATH_ATAN2_DOUBLE
		inline double MATH_ATAN2(double fy, double fx) {
		return OPENRAVE_MATH_ATAN2_DOUBLE(fy,fx);
		}
		#else
	inline double MATH_ATAN2(double fy, double fx) {	inline double MATH_ATAN2(double fy, double fx) {
	return atan2(fy,fx);	return atan2(fy,fx);
	}	}
	#endif	#endif

	/** \brief Vector class containing 4 dimensions.	/** \brief Vector class containing 4 dimensions.

	\ingroup affine_math	\ingroup affine_math
	It is better to use this for a 3 dim vector because it is 16byte align ed and SIMD instructions can be used	It is better to use this for a 3 dim vector because it is 16byte align ed and SIMD instructions can be used
	*/	*/

	skipping to change at line 223	skipping to change at line 305
	ucrossv[1] = u[2] * v[0] - u[0] * v[2];	ucrossv[1] = u[2] * v[0] - u[0] * v[2];
	ucrossv[2] = u[0] * v[1] - u[1] * v[0];	ucrossv[2] = u[0] * v[1] - u[1] * v[0];
	*this = ucrossv;	*this = ucrossv;
	return *this;	return *this;
	}	}

	inline RaveVector<T> operator-() const {	inline RaveVector<T> operator-() const {
	RaveVector<T> v; v.x = -x; v.y = -y; v.z = -z; v.w = -w; return v;	RaveVector<T> v; v.x = -x; v.y = -y; v.z = -z; v.w = -w; return v;
	}	}
	template <typename U> inline RaveVector<T> operator+(const RaveVector<U > &r) const {	template <typename U> inline RaveVector<T> operator+(const RaveVector<U > &r) const {

	RaveVector<T> v; v.x = x+r.x; v.y = y+r.y; v.z = z+r.z; v.w = w+r.w ; return v;	RaveVector<T> v; v.x = x+T(r.x); v.y = y+T(r.y); v.z = z+T(r.z); v. w = w+T(r.w); return v;
	}	}
	template <typename U> inline RaveVector<T> operator-(const RaveVector<U > &r) const {	template <typename U> inline RaveVector<T> operator-(const RaveVector<U > &r) const {

	RaveVector<T> v; v.x = x-r.x; v.y = y-r.y; v.z = z-r.z; v.w = w-r.w ; return v;	RaveVector<T> v; v.x = x-T(r.x); v.y = y-T(r.y); v.z = z-T(r.z); v. w = w-T(r.w); return v;
	}	}
	template <typename U> inline RaveVector<T> operator*(const RaveVector<U > &r) const {	template <typename U> inline RaveVector<T> operator*(const RaveVector<U > &r) const {

	RaveVector<T> v; v.x = r.xx; v.y = r.yy; v.z = r.zz; v.w = r.ww ; return v;	RaveVector<T> v; v.x = T(r.x)x; v.y = T(r.y)y; v.z = T(r.z)z; v. w = T(r.w)w; return v;
	}	}
	inline RaveVector<T> operator*(T k) const {	inline RaveVector<T> operator*(T k) const {
	RaveVector<T> v; v.x = kx; v.y = ky; v.z = kz; v.w = kw; return v;	RaveVector<T> v; v.x = kx; v.y = ky; v.z = kz; v.w = kw; return v;
	}	}

	template <typename U> inline RaveVector<T>& operator += (const RaveVect or<U>&r) {	template <typename U> inline RaveVector<T>& operator += (const RaveVect or<U>&r) {

	x += r.x; y += r.y; z += r.z; w += r.w; return *this;	x += T(r.x); y += T(r.y); z += T(r.z); w += T(r.w); return *this;
	}	}
	template <typename U> inline RaveVector<T>& operator -= (const RaveVect or<U>&r) {	template <typename U> inline RaveVector<T>& operator -= (const RaveVect or<U>&r) {

	x -= r.x; y -= r.y; z -= r.z; w -= r.w; return *this;	x -= T(r.x); y -= T(r.y); z -= T(r.z); w -= T(r.w); return *this;
	}	}
	template <typename U> inline RaveVector<T>& operator *= (const RaveVect or<U>&r) {	template <typename U> inline RaveVector<T>& operator *= (const RaveVect or<U>&r) {

	x = r.x; y = r.y; z = r.z; w = r.w; return *this;	x = T(r.x); y = T(r.y); z = T(r.z); w = T(r.w); return *this;
	}	}

	inline RaveVector<T>& operator *= (const T k) {	inline RaveVector<T>& operator *= (const T k) {
	x = k; y = k; z = k; w = k; return *this;	x = k; y = k; z = k; w = k; return *this;
	}	}
	inline RaveVector<T>& operator /= (const T _k) {	inline RaveVector<T>& operator /= (const T _k) {
	T k=1/_k; x = k; y = k; z = k; w = k; return *this;	T k=1/_k; x = k; y = k; z = k; w = k; return *this;
	}	}

	template <typename U> friend RaveVector<U> operator* (float f, const Ra veVector<U>&v);	template <typename U> friend RaveVector<U> operator* (float f, const Ra veVector<U>&v);

	skipping to change at line 572	skipping to change at line 654
	aabb() {	aabb() {
	}	}
	aabb(const RaveVector<T>&vpos, const RaveVector<T>&vextents) : pos(vpos ), extents(vextents) {	aabb(const RaveVector<T>&vpos, const RaveVector<T>&vextents) : pos(vpos ), extents(vextents) {
	}	}
	RaveVector<T> pos, extents;	RaveVector<T> pos, extents;
	};	};

	/// \brief An oriented bounding box.	/// \brief An oriented bounding box.
	/// \ingroup geometric_primitives	/// \ingroup geometric_primitives
	template <typename T>	template <typename T>

		class OrientedBox
		{
		public:
		RaveTransform<T> transform;
		RaveVector<T> extents;
		};

		/// \brief An oriented bounding box.
		/// \ingroup geometric_primitives
		template <typename T>
	class obb	class obb
	{	{
	public:	public:
	RaveVector<T> right, up, dir, pos, extents;	RaveVector<T> right, up, dir, pos, extents;
	};	};

	/// \brief A triangle defined by 3 points.	/// \brief A triangle defined by 3 points.
	/// \ingroup geometric_primitives	/// \ingroup geometric_primitives
	template <typename T>	template <typename T>
	class triangle	class triangle

End of changes. 21 change blocks.
	13 lines changed or deleted	105 lines changed or added

	iksolver.h	iksolver.h
	// -- coding: utf-8 --	// -- coding: utf-8 --

	// Copyright (C) 2006-2011 Rosen Diankov <rosen.diankov@gmail.com>	// Copyright (C) 2006-2012 Rosen Diankov <rosen.diankov@gmail.com>
	//	//
	// This file is part of OpenRAVE.	// This file is part of OpenRAVE.
	// OpenRAVE is free software: you can redistribute it and/or modify	// OpenRAVE is free software: you can redistribute it and/or modify
	// it under the terms of the GNU Lesser General Public License as published by	// it under the terms of the GNU Lesser General Public License as published by
	// the Free Software Foundation, either version 3 of the License, or	// the Free Software Foundation, either version 3 of the License, or
	// at your option) any later version.	// at your option) any later version.
	//	//
	// This program is distributed in the hope that it will be useful,	// This program is distributed in the hope that it will be useful,
	// but WITHOUT ANY WARRANTY; without even the implied warranty of	// but WITHOUT ANY WARRANTY; without even the implied warranty of
	// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the	// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the

	skipping to change at line 27	skipping to change at line 27
	/** \file iksolver.h	/** \file iksolver.h
	\brief Inverse kinematics related definitions.	\brief Inverse kinematics related definitions.

	Automatically included with \ref openrave.h	Automatically included with \ref openrave.h
	*/	*/
	#ifndef OPENRAVE_IKSOLVER_H	#ifndef OPENRAVE_IKSOLVER_H
	#define OPENRAVE_IKSOLVER_H	#define OPENRAVE_IKSOLVER_H

	namespace OpenRAVE {	namespace OpenRAVE {


	/// \brief Return value for the ik filter that can be optionally set on an
	ik solver.
	enum IkFilterReturn
	{
	IKFR_Success = 0, ///< the ik solution is good
	IKFR_Reject = 1, ///< reject the ik solution
	IKFR_Quit = 2, ///< the ik solution is rejected and the ik call itself
	should quit with failure
	};

	/// \brief Controls what information gets validated when searching for an i nverse kinematics solution.	/// \brief Controls what information gets validated when searching for an i nverse kinematics solution.
	enum IkFilterOptions	enum IkFilterOptions
	{	{
	IKFO_CheckEnvCollisions=1, ///< will check environment collisions with the robot (not checked by default)	IKFO_CheckEnvCollisions=1, ///< will check environment collisions with the robot (not checked by default)
	IKFO_IgnoreSelfCollisions=2, ///< will not check the self-collision of the robot (checked by default)	IKFO_IgnoreSelfCollisions=2, ///< will not check the self-collision of the robot (checked by default)
	IKFO_IgnoreJointLimits=4, ///< will not check the joint limits of the r obot (checked by default). This has the side effect of only returning solut ions within 360 degrees for revolute joints, even if they have a range > 36 0.	IKFO_IgnoreJointLimits=4, ///< will not check the joint limits of the r obot (checked by default). This has the side effect of only returning solut ions within 360 degrees for revolute joints, even if they have a range > 36 0.
	IKFO_IgnoreCustomFilters=8, ///< will not use the custom filter, even i f one is set	IKFO_IgnoreCustomFilters=8, ///< will not use the custom filter, even i f one is set
	IKFO_IgnoreEndEffectorCollisions=16, ///< will not check collision with the environment and the end effector links and bodies attached to the end effector links. The end effector links are defined by \ref RobotBase::Manip ulator::GetChildLinks. Use this option when \ref RobotBase::Manipulator::Ch eckEndEffectorCollision has already been called, or it is ok for the end ef fector to collide given the IK constraints. Self-collisions between the mov ing links and end effector are still checked.	IKFO_IgnoreEndEffectorCollisions=16, ///< will not check collision with the environment and the end effector links and bodies attached to the end effector links. The end effector links are defined by \ref RobotBase::Manip ulator::GetChildLinks. Use this option when \ref RobotBase::Manipulator::Ch eckEndEffectorCollision has already been called, or it is ok for the end ef fector to collide given the IK constraints. Self-collisions between the mov ing links and end effector are still checked.
	};	};


		/// \brief Return value for the ik filter that can be optionally set on an
		ik solver.
		enum IkReturnAction
		{
		IKRA_Success = 0, ///< the ik solution is good
		IKRA_Reject = 1, ///< reject the ik solution
		IKRA_Quit = 2, ///< the ik solution is rejected and the ik call itself
		should quit with failure

		// reasons why rejected
		IKRA_QuitEndEffectorCollision = (IKRA_Quit\|0x00000080),
		IKRA_RejectKinematics = (IKRA_Reject\|0x00000010),
		IKRA_RejectSelfCollision = (IKRA_Reject\|0x00000020),
		IKRA_RejectEnvCollision = (IKRA_Reject\|0x00000040),
		IKRA_RejectJointLimits = (IKRA_Reject\|0x00000100),
		IKRA_RejectKinematicsPrecision = (IKRA_Reject\|0x00000200),
		IKRA_RejectCustomFilter = (IKRA_Reject\|0x00008000), // the reason shoul
		d be set in the upper 16 bits
		};

		/// \deprecated (12/04/29)
		static const IkReturnAction IKFR_Success RAVE_DEPRECATED = IKRA_Success;
		static const IkReturnAction IKFR_Reject RAVE_DEPRECATED = IKRA_Reject;
		static const IkReturnAction IKFR_Quit RAVE_DEPRECATED = IKRA_Quit;
		typedef IkReturnAction IkFilterReturn RAVE_DEPRECATED;

		class OPENRAVE_API IkReturn
		{
		public:
		IkReturn(IkReturnAction action) : _action(action) {
		}

		inline bool operator != (IkReturnAction action) const {
		return _action != action;
		}
		inline bool operator == (IkReturnAction action) const {
		return _action == action;
		}

		/// \brief appends the data of one IkReturn to this structure
		///
		/// _action is untouched, _vsolution is overridden if non-empty
		/// \return If data clashes, will output text and return false
		bool Append(const IkReturn& r);

		/// \brief clears the data, leaves the _action unchanged
		void Clear();

		typedef std::map<std::string, std::vector<dReal> > CustomData;
		IkReturnAction _action;
		std::vector< dReal > _vsolution; ///< the solution
		CustomData _mapdata; ///< name/value pairs for custom data computed in
		the filters. Cascading filters using the same name will overwrite this unti
		l the last executed filter (with lowest priority).
		UserDataPtr _userdata; ///< if the name/value pairs are not enough, can
		further use a pointer to custom data. Cascading filters with valid _userda
		ta pointers will overwrite this until the last executed filter (with lowest
		priority).
		};

	/** \brief <b>[interface]</b> Base class for all Inverse Kinematic solvers. <b>If not specified, method is not multi-thread safe.</b> See \ref arch_ik solver.	/** \brief <b>[interface]</b> Base class for all Inverse Kinematic solvers. <b>If not specified, method is not multi-thread safe.</b> See \ref arch_ik solver.
	\ingroup interfaces	\ingroup interfaces
	*/	*/
	class OPENRAVE_API IkSolverBase : public InterfaceBase	class OPENRAVE_API IkSolverBase : public InterfaceBase
	{	{
	public:	public:
	/** Inverse kinematics filter callback function.	/** Inverse kinematics filter callback function.

	The filter is of the form <tt>return = filterfn(solution, manipulat or, param)</tt>.	The filter is of the form <tt>return = filterfn(solution, manipulat or, param)</tt>.
	The solution is guaranteed to be set on the robot's joint values be fore this function is called. The active dof values of the robot will be se t to the manipulator's arms.	The solution is guaranteed to be set on the robot's joint values be fore this function is called. The active dof values of the robot will be se t to the manipulator's arms.

	If the filter internally modifies the robot state and it returns IK FR_Success, the filter has to restore the original robot dof values and active dofs before it returns.	If the filter internally modifies the robot state and it returns IK RA_Success, the filter has to restore the original robot dof values and active dofs before it returns.
	If the filter happens to modify solution, the the robot state has t o be set to the new solution.	If the filter happens to modify solution, the the robot state has t o be set to the new solution.

	\param solution The current solution of the manipulator. Can be mod ified by this function, but note that it will not go through previous check s again.	\param solution The current solution of the manipulator. Can be mod ified by this function, but note that it will not go through previous check s again.
	\param manipulator The current manipulator that the ik is being sol ved for.	\param manipulator The current manipulator that the ik is being sol ved for.
	\param param The paramterization that IK was called with. This is i n the manipulator base link's coordinate system (which is not necessarily t he world coordinate system).	\param param The paramterization that IK was called with. This is i n the manipulator base link's coordinate system (which is not necessarily t he world coordinate system).

	\return \ref IkFilterReturn controlling the behavior of the ik sear ch process.	\return \ref IkReturn outputs the action to take for the current ik solution and any custom parameters the filter should pass to the user.
	*/	*/

	typedef boost::function<IkFilterReturn(std::vector<dReal>&, RobotBase:: ManipulatorConstPtr, const IkParameterization&)> IkFilterCallbackFn;	typedef boost::function<IkReturn(std::vector<dReal>&, RobotBase::Manipu latorConstPtr, const IkParameterization&)> IkFilterCallbackFn;

	IkSolverBase(EnvironmentBasePtr penv) : InterfaceBase(PT_InverseKinemat icsSolver, penv) {	IkSolverBase(EnvironmentBasePtr penv) : InterfaceBase(PT_InverseKinemat icsSolver, penv) {
	}	}
	virtual ~IkSolverBase() {	virtual ~IkSolverBase() {
	}	}

	/// return the static interface type this class points to (used for saf e casting)	/// return the static interface type this class points to (used for saf e casting)
	static inline InterfaceType GetInterfaceTypeStatic() {	static inline InterfaceType GetInterfaceTypeStatic() {
	return PT_InverseKinematicsSolver;	return PT_InverseKinematicsSolver;
	}	}

	/// brief Sets the IkSolverBase attached to a specific robot and sets I kSolverBase specific options.	/// brief Sets the IkSolverBase attached to a specific robot and sets I kSolverBase specific options.
	///	///
	/// For example, some ik solvers might have different ways of computing optimal solutions.	/// For example, some ik solvers might have different ways of computing optimal solutions.
	/// \param pmanip The manipulator the IK solver is attached to	/// \param pmanip The manipulator the IK solver is attached to

	virtual bool Init(RobotBase::ManipulatorPtr pmanip) = 0;	virtual bool Init(RobotBase::ManipulatorConstPtr pmanip) = 0;

	virtual RobotBase::ManipulatorPtr GetManipulator() const = 0;	virtual RobotBase::ManipulatorPtr GetManipulator() const = 0;

	/** \brief Sets an ik solution filter that is called for every ik solut ion.	/** \brief Sets an ik solution filter that is called for every ik solut ion.

	Multiple filters can be set at once, each filter will be called acc ording to its priority; higher values get called first. The default impleme ntation of IkSolverBase manages the filters internally. Users implementing their own IkSolverBase should call \ref _CallFilters to run the internally managed filters.	Multiple filters can be set at once, each filter will be called acc ording to its priority; higher values get called first. The default impleme ntation of IkSolverBase manages the filters internally. Users implementing their own IkSolverBase should call \ref _CallFilters to run the internally managed filters.

	\param filterfn - an optional filter function to be called, see \re f IkFilterCallbackFn.
	\param priority - The priority of the filter that controls the orde r in which filters get called. Higher priority filters get called first. If not certain what to set, use 0.	\param priority - The priority of the filter that controls the orde r in which filters get called. Higher priority filters get called first. If not certain what to set, use 0.

		\param filterfn - an optional filter function to be called, see \re f IkFilterCallbackFn.
	\return a managed handle to the filter. If this handle is released, then the fitler will be removed. Release operation is <b>[multi-thread saf e]</b>.	\return a managed handle to the filter. If this handle is released, then the fitler will be removed. Release operation is <b>[multi-thread saf e]</b>.
	*/	*/
	virtual UserDataPtr RegisterCustomFilter(int priority, const IkFilterCa llbackFn& filterfn);	virtual UserDataPtr RegisterCustomFilter(int priority, const IkFilterCa llbackFn& filterfn);

	/// \deprecated (11/09/21)	/// \deprecated (11/09/21)
	virtual void SetCustomFilter(const IkFilterCallbackFn& filterfn) RAVE_D EPRECATED	virtual void SetCustomFilter(const IkFilterCallbackFn& filterfn) RAVE_D EPRECATED
	{	{
	RAVELOG_WARN("IkSolverBase::SetCustomFilter is deprecated, have to use handle=AddCustomFilter. This call will will leak memory\n");	RAVELOG_WARN("IkSolverBase::SetCustomFilter is deprecated, have to use handle=AddCustomFilter. This call will will leak memory\n");
	if( __listRegisteredFilters.size() > 0 ) {	if( __listRegisteredFilters.size() > 0 ) {
	RAVELOG_WARN("IkSolverBase::SetCustomFilter is deprecated, dele ting all current filters!\n");	RAVELOG_WARN("IkSolverBase::SetCustomFilter is deprecated, dele ting all current filters!\n");

	skipping to change at line 123	skipping to change at line 167

	/** \brief Return a joint configuration for the given end effector tran sform.	/** \brief Return a joint configuration for the given end effector tran sform.

	\param[in] param the pose the end effector has to achieve. Note tha t the end effector pose	\param[in] param the pose the end effector has to achieve. Note tha t the end effector pose
	takes into account the grasp coordinate frame for the RobotBase::Ma nipulator	takes into account the grasp coordinate frame for the RobotBase::Ma nipulator
	\param[in] q0 Return a solution nearest to the given configuration q0 in terms of the joint distance. If q0 is NULL, returns the first solutio n found	\param[in] q0 Return a solution nearest to the given configuration q0 in terms of the joint distance. If q0 is NULL, returns the first solutio n found
	\param[in] filteroptions A bitmask of \ref IkFilterOptions values c ontrolling what is checked for each ik solution.	\param[in] filteroptions A bitmask of \ref IkFilterOptions values c ontrolling what is checked for each ik solution.
	\param[out] solution [optional] Holds the IK solution	\param[out] solution [optional] Holds the IK solution
	\return true if solution is found	\return true if solution is found
	*/	*/

	virtual bool Solve(const IkParameterization& param, const std::vector<d	virtual bool Solve(const IkParameterization& param, const std::vector<d
	Real>& q0, int filteroptions, boost::shared_ptr< std::vector<dReal> > solut	Real>& q0, int filteroptions, boost::shared_ptr< std::vector<dReal> > solut
	ion) = 0;	ion = boost::shared_ptr< std::vector<dReal> >()) = 0;

		/** \brief Return a joint configuration for the given end effector tran
		sform.

		\param[in] param the pose the end effector has to achieve. Note tha
		t the end effector pose
		takes into account the grasp coordinate frame for the RobotBase::Ma
		nipulator
		\param[in] q0 Return a solution nearest to the given configuration
		q0 in terms of the joint distance. If q0 is NULL, returns the first solutio
		n found
		\param[in] filteroptions A bitmask of \ref IkFilterOptions values c
		ontrolling what is checked for each ik solution.
		\param[out] ikreturn Holds all the ik output data (including ik sol
		utions) from the many processes involved in solving ik.
		\return true if solution is found
		*/
		virtual bool Solve(const IkParameterization& param, const std::vector<d
		Real>& q0, int filteroptions, IkReturnPtr ikreturn);

	/** \brief Return all joint configurations for the given end effector t ransform.	/** \brief Return all joint configurations for the given end effector t ransform.

	\param[in] param the pose the end effector has to achieve. Note that the end effector pose	\param[in] param the pose the end effector has to achieve. Note that the end effector pose
	takes into account the grasp coordinate frame for the RobotBase::Man ipulator	takes into account the grasp coordinate frame for the RobotBase::Man ipulator
	\param[in] filteroptions A bitmask of \ref IkFilterOptions values co ntrolling what is checked for each ik solution.	\param[in] filteroptions A bitmask of \ref IkFilterOptions values co ntrolling what is checked for each ik solution.
	\param[out] solutions All solutions within a reasonable discretizati on level of the free parameters.	\param[out] solutions All solutions within a reasonable discretizati on level of the free parameters.
	\return true if at least one solution is found	\return true if at least one solution is found
	*/	*/

	virtual bool Solve(const IkParameterization& param, int filteroptions,	virtual bool SolveAll(const IkParameterization& param, int filteroption
	std::vector< std::vector<dReal> >& solutions) = 0;	s, std::vector< std::vector<dReal> >& solutions) = 0;

		/// \deprecated (12/05/01)
		virtual bool Solve(const IkParameterization& param, int filteroptions,
		std::vector< std::vector<dReal> >& solutions) RAVE_DEPRECATED {
		return SolveAll(param,filteroptions,solutions);
		}

		/** \brief Return all joint configurations for the given end effector t
		ransform.

		\param[in] param the pose the end effector has to achieve. Note that
		the end effector pose
		takes into account the grasp coordinate frame for the RobotBase::Man
		ipulator
		\param[in] filteroptions A bitmask of \ref IkFilterOptions values co
		ntrolling what is checked for each ik solution.
		\param[out] ikreturns Holds all the ik output data (including ik sol
		utions) from the many processes involved in solving ik.
		\return true if at least one solution is found
		*/
		virtual bool SolveAll(const IkParameterization& param, int filteroption
		s, std::vector<IkReturnPtr>& ikreturns);

	/** Return a joint configuration for the given end effector transform.	/** Return a joint configuration for the given end effector transform.

	Can specify the free parameters in [0,1] range. If NULL, the regula r equivalent Solve is called	Can specify the free parameters in [0,1] range. If NULL, the regula r equivalent Solve is called
	\param[in] param the pose the end effector has to achieve. Note tha t the end effector pose takes into account the grasp coordinate frame for t he RobotBase::Manipulator	\param[in] param the pose the end effector has to achieve. Note tha t the end effector pose takes into account the grasp coordinate frame for t he RobotBase::Manipulator
	\param[in] q0 Return a solution nearest to the given configuration q0 in terms of the joint distance. If q0 is empty, returns the first soluti on found	\param[in] q0 Return a solution nearest to the given configuration q0 in terms of the joint distance. If q0 is empty, returns the first soluti on found
	\param[in] vFreeParameters The free parameters of the null space of the IK solutions. Always in range of [0,1]	\param[in] vFreeParameters The free parameters of the null space of the IK solutions. Always in range of [0,1]
	\param[in] filteroptions A bitmask of \ref IkFilterOptions values c ontrolling what is checked for each ik solution.	\param[in] filteroptions A bitmask of \ref IkFilterOptions values c ontrolling what is checked for each ik solution.

	\param[out] solution Holds the IK solution, must be of size RobotBa se::Manipulator::_vecarmjoints	\param[out] solution [optional] Holds the IK solution, must be of s ize RobotBase::Manipulator::_vecarmjoints
	\return true if solution is found	\return true if solution is found
	*/	*/

	virtual bool Solve(const IkParameterization& param, const std::vector<d	virtual bool Solve(const IkParameterization& param, const std::vector<d
	Real>& q0, const std::vector<dReal>& vFreeParameters, int filteroptions, bo	Real>& q0, const std::vector<dReal>& vFreeParameters, int filteroptions, bo
	ost::shared_ptr< std::vector<dReal> > solution) = 0;	ost::shared_ptr< std::vector<dReal> > solution=boost::shared_ptr< std::vect
		or<dReal> >()) = 0;

		/** Return a joint configuration for the given end effector transform.

		Can specify the free parameters in [0,1] range. If NULL, the regula
		r equivalent Solve is called
		\param[in] param the pose the end effector has to achieve. Note tha
		t the end effector pose takes into account the grasp coordinate frame for t
		he RobotBase::Manipulator
		\param[in] q0 Return a solution nearest to the given configuration
		q0 in terms of the joint distance. If q0 is empty, returns the first soluti
		on found
		\param[in] vFreeParameters The free parameters of the null space of
		the IK solutions. Always in range of [0,1]
		\param[in] filteroptions A bitmask of \ref IkFilterOptions values c
		ontrolling what is checked for each ik solution.
		\param[out] ikreturn Holds all the ik output data (including ik sol
		utions) from the many processes involved in solving ik.
		\return true if solution is found
		*/
		virtual bool Solve(const IkParameterization& param, const std::vector<d
		Real>& q0, const std::vector<dReal>& vFreeParameters, int filteroptions, Ik
		ReturnPtr ikreturn);

	/** \brief Return all joint configurations for the given end effector t ransform.	/** \brief Return all joint configurations for the given end effector t ransform.

	Can specify the free parameters in [0,1] range. If NULL, the regula r equivalent Solve is called	Can specify the free parameters in [0,1] range. If NULL, the regula r equivalent Solve is called
	\param[in] param the pose the end effector has to achieve. Note tha t the end effector pose	\param[in] param the pose the end effector has to achieve. Note tha t the end effector pose
	takes into account the grasp coordinate frame for the RobotBase::Ma nipulator	takes into account the grasp coordinate frame for the RobotBase::Ma nipulator
	\param[in] vFreeParameters The free parameters of the null space of the IK solutions. Always in range of [0,1]	\param[in] vFreeParameters The free parameters of the null space of the IK solutions. Always in range of [0,1]
	\param[in] filteroptions A bitmask of \ref IkFilterOptions values c ontrolling what is checked for each ik solution.	\param[in] filteroptions A bitmask of \ref IkFilterOptions values c ontrolling what is checked for each ik solution.
	\param[out] solutions All solutions within a reasonable discretizat ion level of the free parameters.	\param[out] solutions All solutions within a reasonable discretizat ion level of the free parameters.
	\return true at least one solution is found	\return true at least one solution is found
	*/	*/

	virtual bool Solve(const IkParameterization& param, const std::vector<d	virtual bool SolveAll(const IkParameterization& param, const std::vecto
	Real>& vFreeParameters, int filteroptions, std::vector< std::vector<dReal>	r<dReal>& vFreeParameters, int filteroptions, std::vector< std::vector<dRea
	>& solutions) = 0;	l> >& solutions) = 0;

		/// \deprecated (12/05/01)
		virtual bool Solve(const IkParameterization& param, const std::vector<d
		Real>& vFreeParameters, int filteroptions, std::vector< std::vector<dReal>
		>& solutions) RAVE_DEPRECATED {
		return SolveAll(param,vFreeParameters,filteroptions,solutions);
		}

		/** \brief Return all joint configurations for the given end effector t
		ransform.

		Can specify the free parameters in [0,1] range. If NULL, the regula
		r equivalent Solve is called
		\param[in] param the pose the end effector has to achieve. Note tha
		t the end effector pose
		takes into account the grasp coordinate frame for the RobotBase::Ma
		nipulator
		\param[in] vFreeParameters The free parameters of the null space of
		the IK solutions. Always in range of [0,1]
		\param[in] filteroptions A bitmask of \ref IkFilterOptions values c
		ontrolling what is checked for each ik solution.
		\param[out] ikreturns Holds all the ik output data (including ik so
		lutions) from the many processes involved in solving ik.
		\return true at least one solution is found
		*/
		virtual bool SolveAll(const IkParameterization& param, const std::vecto
		r<dReal>& vFreeParameters, int filteroptions, std::vector<IkReturnPtr>& ikr
		eturns);

	/// \brief returns true if the solver supports a particular ik paramete rization as input.	/// \brief returns true if the solver supports a particular ik paramete rization as input.
	virtual bool Supports(IkParameterizationType iktype) const OPENRAVE_DUM MY_IMPLEMENTATION;	virtual bool Supports(IkParameterizationType iktype) const OPENRAVE_DUM MY_IMPLEMENTATION;

	protected:	protected:
	inline IkSolverBasePtr shared_iksolver() {	inline IkSolverBasePtr shared_iksolver() {
	return boost::static_pointer_cast<IkSolverBase>(shared_from_this()) ;	return boost::static_pointer_cast<IkSolverBase>(shared_from_this()) ;
	}	}
	inline IkSolverBaseConstPtr shared_iksolver_const() const {	inline IkSolverBaseConstPtr shared_iksolver_const() const {
	return boost::static_pointer_cast<IkSolverBase const>(shared_from_t his());	return boost::static_pointer_cast<IkSolverBase const>(shared_from_t his());
	}	}

	/// \brief calls the registered filters in their priority order and ret urns the value of the last called filter.	/// \brief calls the registered filters in their priority order and ret urns the value of the last called filter.

	virtual IkFilterReturn _CallFilters(std::vector<dReal>& solution, Robot	///
	Base::ManipulatorPtr manipulator, const IkParameterization& param);	/// The parameters are the same as \ref IkFilterCallbackFn, except the
		IkReturn is an optional input parameter and the return
		/// value is just the \ref IkReturnAction. For users that do not reques
		t the filter output, this allows the computation
		// to be optimized away.
		virtual IkReturnAction _CallFilters(std::vector<dReal>& solution, Robot
		Base::ManipulatorPtr manipulator, const IkParameterization& param, IkReturn
		Ptr ikreturn=IkReturnPtr());

	private:	private:
	virtual const char* GetHash() const {	virtual const char* GetHash() const {
	return OPENRAVE_IKSOLVER_HASH;	return OPENRAVE_IKSOLVER_HASH;
	}	}

	std::list<UserDataWeakPtr> __listRegisteredFilters; ///< internally man aged filters	std::list<UserDataWeakPtr> __listRegisteredFilters; ///< internally man aged filters

	friend class CustomIkSolverFilterData;	friend class CustomIkSolverFilterData;
	};	};

End of changes. 15 change blocks.
	30 lines changed or deleted	179 lines changed or added

	interface.h	interface.h

	skipping to change at line 75	skipping to change at line 75
	}	}

	/// \return the environment that this interface is attached to	/// \return the environment that this interface is attached to
	inline EnvironmentBasePtr GetEnv() const {	inline EnvironmentBasePtr GetEnv() const {
	return __penv;	return __penv;
	}	}

	inline const READERSMAP& GetReadableInterfaces() const {	inline const READERSMAP& GetReadableInterfaces() const {
	return __mapReadableInterfaces;	return __mapReadableInterfaces;
	}	}

	inline XMLReadablePtr GetReadableInterface(const std::string& xmltag) c
	onst	/// \brief returns the readable interface
	{	virtual XMLReadablePtr GetReadableInterface(const std::string& xmltag)
	READERSMAP::const_iterator it = __mapReadableInterfaces.find(xmltag	const;
	);
	return it != __mapReadableInterfaces.end() ? it->second : XMLReadab	/// \brief set a new readable interface and return the previously set i
	lePtr();	nterface if it exists
	}	virtual XMLReadablePtr SetReadableInterface(const std::string& xmltag,
		XMLReadablePtr readable);

	/// \brief Documentation of the interface in reStructuredText format. S ee \ref writing_plugins_doc.	/// \brief Documentation of the interface in reStructuredText format. S ee \ref writing_plugins_doc.
	virtual const std::string& GetDescription() const {	virtual const std::string& GetDescription() const {
	return __description;	return __description;
	}	}

	virtual void SetDescription(const std::string& description) {	virtual void SetDescription(const std::string& description) {
	__description = description;	__description = description;
	}	}


	skipping to change at line 133	skipping to change at line 134
	- '\b help [command name]' - get the help string of just that comma nd.	- '\b help [command name]' - get the help string of just that comma nd.
	- '\b help commands' - return the names of all the possible command s	- '\b help commands' - return the names of all the possible command s

	\param is the input stream containing the command	\param is the input stream containing the command
	\param os the output stream containing the output	\param os the output stream containing the output
	\exception openrave_exception Throw if the command is not supported .	\exception openrave_exception Throw if the command is not supported .
	\return true if the command is successfully processed, otherwise fa lse.	\return true if the command is successfully processed, otherwise fa lse.
	*/	*/
	virtual bool SendCommand(std::ostream& os, std::istream& is);	virtual bool SendCommand(std::ostream& os, std::istream& is);


	// serializes the interface, use an official serialization library?	/** \brief serializes the interface
	//virtual void Serialize(std::ostream& o, int options) const;
		The readable interfaces are also serialized within the tag, for exa
		mple:

		\code{.xml}
		<sometag> <!-- root writer -->
		<interface> <!-- first child -->
		<readableinterface/> <!-- readable interface -->
		</interface>
		</sometag>
		\endcode

		Depending on the writer format, extra tags might be created.
		*/
		virtual void Serialize(BaseXMLWriterPtr writer, int options=0) const;

	protected:	protected:
	/// \brief The function to be executed for every command.	/// \brief The function to be executed for every command.
	///	///
	/// \param sinput - input of the command	/// \param sinput - input of the command
	/// \param sout - output of the command	/// \param sout - output of the command
	/// \return If false, there was an error with the command, true if succ essful	/// \return If false, there was an error with the command, true if succ essful
	typedef boost::function<bool (std::ostream&, std::istream&)> InterfaceC ommandFn;	typedef boost::function<bool (std::ostream&, std::istream&)> InterfaceC ommandFn;
	class OPENRAVE_API InterfaceCommand	class OPENRAVE_API InterfaceCommand
	{	{

	skipping to change at line 172	skipping to change at line 186
	/// \brief Unregisters the command.	/// \brief Unregisters the command.
	virtual void UnregisterCommand(const std::string& cmdname);	virtual void UnregisterCommand(const std::string& cmdname);

	virtual const char* GetHash() const = 0;	virtual const char* GetHash() const = 0;
	std::string __description; /// \see GetDescription()	std::string __description; /// \see GetDescription()

	private:	private:
	/// Write the help commands to an output stream	/// Write the help commands to an output stream
	virtual bool _GetCommandHelp(std::ostream& sout, std::istream& sinput) const;	virtual bool _GetCommandHelp(std::ostream& sout, std::istream& sinput) const;


		inline InterfaceBase& operator=(const InterfaceBase&r) {
		throw openrave_exception("InterfaceBase copying not allowed");
		}

	mutable boost::mutex _mutexInterface; ///< internal mutex for protectin g data from methods that might be access from any thread (those methods sho uld be commented).	mutable boost::mutex _mutexInterface; ///< internal mutex for protectin g data from methods that might be access from any thread (those methods sho uld be commented).
	InterfaceType __type; ///< \see GetInterfaceType	InterfaceType __type; ///< \see GetInterfaceType

	boost::shared_ptr<void> __plugin; ///< handle to plugin that controls t he executable code. As long as this plugin pointer is present, module will not be unloaded.	UserDataPtr __plugin; ///< handle to plugin that controls the executabl e code. As long as this plugin pointer is present, module will not be unloa ded.
	std::string __struri; ///< \see GetURI	std::string __struri; ///< \see GetURI
	std::string __strpluginname; ///< the name of the plugin, necessary?	std::string __strpluginname; ///< the name of the plugin, necessary?
	std::string __strxmlid; ///< \see GetXMLId	std::string __strxmlid; ///< \see GetXMLId
	EnvironmentBasePtr __penv; ///< \see GetEnv	EnvironmentBasePtr __penv; ///< \see GetEnv
	UserDataPtr __pUserData; ///< \see GetUserData	UserDataPtr __pUserData; ///< \see GetUserData

	READERSMAP __mapReadableInterfaces; ///< pointers to extra interfaces t hat are included with this object	READERSMAP __mapReadableInterfaces; ///< pointers to extra interfaces t hat are included with this object
	typedef std::map<std::string, boost::shared_ptr<InterfaceCommand>, Case InsensitiveCompare> CMDMAP;	typedef std::map<std::string, boost::shared_ptr<InterfaceCommand>, Case InsensitiveCompare> CMDMAP;
	CMDMAP __mapCommands; ///< all registered commands	CMDMAP __mapCommands; ///< all registered commands


End of changes. 4 change blocks.
	11 lines changed or deleted	30 lines changed or added

	interfacehashes.h	interfacehashes.h

	#define OPENRAVE_COLLISIONCHECKER_HASH "690784eb9e41b068a893dbf1761c01d1"	#define OPENRAVE_COLLISIONCHECKER_HASH "80fdb22de1fda72c0d7b82e79128d161"
	#define OPENRAVE_ROBOT_HASH "176817657116a7793a3eb0c1001e7127"	#define OPENRAVE_ROBOT_HASH "4604923ba610c232f86dffb1c1d8a045"
	#define OPENRAVE_PLANNER_HASH "6fc740f9b2bbfde93d78032cbf1b9121"	#define OPENRAVE_PLANNER_HASH "d3424c2a655a3138f7f7239c78ded87e"
	#define OPENRAVE_KINBODY_HASH "b46f2dd7208a9ac04b639bc04f521c45"	#define OPENRAVE_KINBODY_HASH "6991d3349c40679133a004a4cdf54d81"
	#define OPENRAVE_SENSORSYSTEM_HASH "83208394c99a021fbdcd55ac2810e65f"	#define OPENRAVE_SENSORSYSTEM_HASH "21b5a06c9e7a138692bee96cb3eeae63"
	#define OPENRAVE_CONTROLLER_HASH "4b94816a0f156bc4a3e98f79f3898683"	#define OPENRAVE_CONTROLLER_HASH "bb0bc7917bf626949d1be91f2057dbc2"
	#define OPENRAVE_MODULE_HASH "fec1b591cb5ef03912009f9abdcd037d"	#define OPENRAVE_MODULE_HASH "7e01d4be2df6ad3d61c252565993e063"
	#define OPENRAVE_IKSOLVER_HASH "9fd7944dbecc4eed8cceb4f6d5a618ce"	#define OPENRAVE_IKSOLVER_HASH "647ba667cc638f22d8b457527076ce01"
	#define OPENRAVE_PHYSICSENGINE_HASH "257dd698a5b7a78b8bd4d05a3d322c0b"	#define OPENRAVE_PHYSICSENGINE_HASH "b14bb00a96362b57164eb7ab50050868"
	#define OPENRAVE_SENSOR_HASH "1425016f0176ff3811962fbefd63c31e"	#define OPENRAVE_SENSOR_HASH "525a812376887a27a28f5a4c688d577b"
	#define OPENRAVE_TRAJECTORY_HASH "c386a85687d6dea075d3d9ed1c68b808"	#define OPENRAVE_TRAJECTORY_HASH "a0d3418cdcc167130e579cc3a31c9b50"
	#define OPENRAVE_VIEWER_HASH "c58fb4ff10585995bc49e53f3175b473"	#define OPENRAVE_VIEWER_HASH "fdf860551b1114bbd9a165dc3266e53a"
	#define OPENRAVE_SPACESAMPLER_HASH "3b8faec230636cab285670c095c95496"	#define OPENRAVE_SPACESAMPLER_HASH "58055125519f9c64b8592c2a2f127ce0"
	#define OPENRAVE_ENVIRONMENT_HASH "d4cc98d22fe988548a1788456c51629e"	#define OPENRAVE_ENVIRONMENT_HASH "289079a18cea17206e751ade1c72a80b"
	#define OPENRAVE_PLUGININFO_HASH "8d45441947223c660c8f8ff9723bf58e"	#define OPENRAVE_PLUGININFO_HASH "b5fca1af585e6a324acd60d2829e3a98"

End of changes. 1 change blocks.
	lines changed or deleted	lines changed or added

	kinbody.h	kinbody.h

	skipping to change at line 25	skipping to change at line 25
	// You should have received a copy of the GNU Lesser General Public License	// You should have received a copy of the GNU Lesser General Public License
	// along with this program. If not, see <http://www.gnu.org/licenses/>.	// along with this program. If not, see <http://www.gnu.org/licenses/>.
	/** \file kinbody.h	/** \file kinbody.h
	\brief Kinematics body related definitions.	\brief Kinematics body related definitions.

	Automatically included with \ref openrave.h	Automatically included with \ref openrave.h
	*/	*/
	#ifndef OPENRAVE_KINBODY_H	#ifndef OPENRAVE_KINBODY_H
	#define OPENRAVE_KINBODY_H	#define OPENRAVE_KINBODY_H


	/// declare function parser class from fparser library
	template<typename Value_t> class FunctionParserBase;

	namespace OpenRAVE {	namespace OpenRAVE {


		class OpenRAVEFunctionParserReal;
		typedef boost::shared_ptr< OpenRAVEFunctionParserReal > OpenRAVEFunctionPar
		serRealPtr;

	/** \brief <b>[interface]</b> A kinematic body of links and joints. <b>If n ot specified, method is not multi-thread safe.</b> See \ref arch_kinbody.	/** \brief <b>[interface]</b> A kinematic body of links and joints. <b>If n ot specified, method is not multi-thread safe.</b> See \ref arch_kinbody.
	\ingroup interfaces	\ingroup interfaces
	*/	*/
	class OPENRAVE_API KinBody : public InterfaceBase	class OPENRAVE_API KinBody : public InterfaceBase
	{	{
	public:	public:
	/// \brief A set of properties for the kinbody. These properties are us ed to describe a set of variables used in KinBody.	/// \brief A set of properties for the kinbody. These properties are us ed to describe a set of variables used in KinBody.
	enum KinBodyProperty {	enum KinBodyProperty {

	Prop_Joints=0x1, ///< all properties of all joints	Prop_JointMimic=0x1, ///< joint mimic equations
	Prop_JointLimits=0x2\|Prop_Joints, ///< regular limits	Prop_JointLimits=0x2, ///< regular limits
	Prop_JointOffset=0x4\|Prop_Joints,	Prop_JointOffset=0x4,
	Prop_JointProperties=0x8\|Prop_Joints, ///< max velocity, max ac	Prop_JointProperties=0x8, ///< resolution, weights
	celeration, resolution, max torque	Prop_JointAccelerationVelocityTorqueLimits=0x10, ///< velocity
	Prop_Links=0x10, ///< all properties of all links	+ acceleration + torque
		Prop_Joints=Prop_JointMimic\|Prop_JointLimits\|Prop_JointOffset\|Prop_
		JointProperties\|Prop_JointAccelerationVelocityTorqueLimits, ///< all proper
		ties of all joints

	Prop_Name=0x20, ///< name changed	Prop_Name=0x20, ///< name changed
	Prop_LinkDraw=0x40, ///< toggle link geometries rendering	Prop_LinkDraw=0x40, ///< toggle link geometries rendering

	Prop_LinkGeometry=0x80\|Prop_Links, ///< the geometry of the lin	Prop_LinkGeometry=0x80, ///< the geometry of the link changed
	k changed	// 0x100
	Prop_JointMimic=0x100\|Prop_Joints, ///< joint mimic equations	// 0x200
	Prop_JointAccelerationVelocityTorqueLimits=0x200\|Prop_Joints, /	Prop_LinkStatic=0x400, ///< static property of link changed
	//< velocity + acceleration + torque	Prop_LinkEnable=0x800, ///< enable property of link changed
	Prop_LinkStatic=0x400\|Prop_Links, ///< static property of link	Prop_LinkDynamics=0x1000, ///< mass/inertia properties of link
	changed	changed
		Prop_Links=Prop_LinkDraw\|Prop_LinkGeometry\|Prop_LinkStatic\|Prop_Lin
		kEnable\|Prop_LinkDynamics, ///< all properties of all links
	// robot only	// robot only

	Prop_RobotManipulators = 0x00010000, ///< [robot only] all prop	// 0x00010000
	erties of all manipulators
	Prop_Manipulators = 0x00010000,
	Prop_RobotSensors = 0x00020000, ///< [robot only] all propertie s of all sensors	Prop_RobotSensors = 0x00020000, ///< [robot only] all propertie s of all sensors
	Prop_Sensors = 0x00020000,	Prop_Sensors = 0x00020000,
	Prop_RobotSensorPlacement = 0x00040000, ///< [robot only] relat ive sensor placement of sensors	Prop_RobotSensorPlacement = 0x00040000, ///< [robot only] relat ive sensor placement of sensors
	Prop_SensorPlacement = 0x00040000,	Prop_SensorPlacement = 0x00040000,
	Prop_RobotActiveDOFs = 0x00080000, ///< [robot only] active dof s changed	Prop_RobotActiveDOFs = 0x00080000, ///< [robot only] active dof s changed
	Prop_RobotManipulatorTool = 0x00100000, ///< [robot only] the tool coordinate system changed	Prop_RobotManipulatorTool = 0x00100000, ///< [robot only] the tool coordinate system changed
	Prop_RobotManipulatorName = 0x00200000, ///< [robot only] the manip ulator name	Prop_RobotManipulatorName = 0x00200000, ///< [robot only] the manip ulator name

		Prop_RobotManipulatorSolver = 0x00400000,
		Prop_RobotManipulators = Prop_RobotManipulatorTool \| Prop_RobotMani
		pulatorName \| Prop_RobotManipulatorSolver, ///< [robot only] all proper
		ties of all manipulators
		};

		/// \brief used for specifying the type of limit checking and the messa
		ges associated with it
		enum CheckLimitsAction {
		CLA_Nothing = 0, ///< no checking
		CLA_CheckLimits = 1, /// < checks and warns if the limits are overb
		oard (default)
		CLA_CheckLimitsSilent = 2, ///< checks the limits and silently clam
		ps the joint values (used if the code expects bad values as part of normal
		operation)
		CLA_CheckLimitsThrow = 3, ///< check the limits and throws if somet
		hing went wrong
	};	};

	/// \brief A rigid body holding all its collision and rendering data.	/// \brief A rigid body holding all its collision and rendering data.
	class OPENRAVE_API Link : public boost::enable_shared_from_this<Link>	class OPENRAVE_API Link : public boost::enable_shared_from_this<Link>
	{	{
	public:	public:
	Link(KinBodyPtr parent); ///< pass in a ODE world	Link(KinBodyPtr parent); ///< pass in a ODE world
	virtual ~Link();	virtual ~Link();

	/// \brief User data for trimesh geometries. Vertices are defined i n counter-clockwise order for outward pointing faces.	/// \brief User data for trimesh geometries. Vertices are defined i n counter-clockwise order for outward pointing faces.

	skipping to change at line 89	skipping to change at line 103
	void Append(const TRIMESH& mesh);	void Append(const TRIMESH& mesh);
	void Append(const TRIMESH& mesh, const Transform& trans);	void Append(const TRIMESH& mesh, const Transform& trans);

	AABB ComputeAABB() const;	AABB ComputeAABB() const;
	void serialize(std::ostream& o, int options=0) const;	void serialize(std::ostream& o, int options=0) const;

	friend OPENRAVE_API std::ostream& operator<<(std::ostream& O, c onst TRIMESH &trimesh);	friend OPENRAVE_API std::ostream& operator<<(std::ostream& O, c onst TRIMESH &trimesh);
	friend OPENRAVE_API std::istream& operator>>(std::istream& I, T RIMESH& trimesh);	friend OPENRAVE_API std::istream& operator>>(std::istream& I, T RIMESH& trimesh);
	};	};


	/// Describes the properties of a basic geometric primitive.	/// \brief The type of geometry primitive.
	/// Contains everything associated with a physical body along with	enum GeomType {
	a seprate (optional) render file.	GeomNone = 0,
	class OPENRAVE_API GEOMPROPERTIES	GeomBox = 1,
		GeomSphere = 2,
		GeomCylinder = 3, ///< oriented towards z-axis
		GeomTrimesh = 4,
		};

		/// \brief Describes the properties of a geometric primitive.
		///
		/// Contains everything associated with a geometry's appearance and
		shape
		class OPENRAVE_API GeometryInfo : public XMLReadable
	{	{
	public:	public:

	/// \brief The type of geometry primitive.	GeometryInfo();
	enum GeomType {	virtual ~GeometryInfo() {
	GeomNone = 0,	}
	GeomBox = 1,
	GeomSphere = 2,
	GeomCylinder = 3,
	GeomTrimesh = 4,
	};


	GEOMPROPERTIES(boost::shared_ptr<Link> parent);	/// triangulates the geometry object and initializes collisionm
	virtual ~GEOMPROPERTIES() {	esh. GeomTrimesh types must already be triangulated
		/// \param fTessellation to control how fine the triangles need
		to be. 1.0f is the default value
		bool InitCollisionMesh(float fTessellation=1);

		inline dReal GetSphereRadius() const {
		return _vGeomData.x;
	}	}

		inline dReal GetCylinderRadius() const {
		return _vGeomData.x;
		}
		inline dReal GetCylinderHeight() const {
		return _vGeomData.y;
		}
		inline const Vector& GetBoxExtents() const {
		return _vGeomData;
		}

		Transform _t; ///< Local transformation of the geom primitive w
		ith respect to the link's coordinate system.
		Vector _vGeomData; ///< for boxes, first 3 values are extents
		///< for sphere it is radius
		///< for cylinder, first 2 values are radius and height
		///< for trimesh, none
		RaveVector<float> _vDiffuseColor, _vAmbientColor; ///< hints fo
		r how to color the meshes


	/// \brief Local transformation of the geom primitive with resp	/// \brief trimesh representation of the collision data of this
	ect to the link's coordinate system.	object in this local coordinate system
		///
		/// Should be transformed by \ref _t before rendering.
		/// For spheres and cylinders, an appropriate discretization va
		lue is chosen.
		/// If empty, will be automatically computed from the geometry'
		s type and render data
		TRIMESH _meshcollision;

		GeomType _type; ///< the type of geometry primitive

		/// \brief filename for render model (optional)
		///
		/// Should be transformed by _t before rendering.
		/// If the value is "__norenderif__:x", then the viewer should
		not render the object if it supports *.x files where"x" is the file extensi
		on.
		std::string _filenamerender;

		/// \brief filename for collision data (optional)
		///
		/// If the value is "__norenderif__:x", then the viewer should
		not render the object if it supports *.x files where"x" is the file extensi
		on.
		std::string _filenamecollision;

		Vector _vRenderScale; ///< render scale of the object (x,y,z) f
		rom _filenamerender
		Vector _vCollisionScale; ///< render scale of the object (x,y,z
		) from _filenamecollision
		float _fTransparency; ///< value from 0-1 for the transparency
		of the rendered object, 0 is opaque
		bool _bVisible; ///< if true, geometry is visible as part of th
		e 3d model (default is true)
		bool _bModifiable; ///< if true, object geometry can be dynamic
		ally modified (default is true)
		};
		typedef boost::shared_ptr<GeometryInfo> GeometryInfoPtr;

		/// \brief geometry object holding a link parent and wrapping acces
		s to a protected geometry info
		class OPENRAVE_API Geometry
		{
		public:
		/// \deprecated (12/07/16)
		static const GeomType GeomNone RAVE_DEPRECATED = KinBody::Link:
		:GeomNone;
		static const GeomType GeomBox RAVE_DEPRECATED = KinBody::Link::
		GeomBox;
		static const GeomType GeomSphere RAVE_DEPRECATED = KinBody::Lin
		k::GeomSphere;
		static const GeomType GeomCylinder RAVE_DEPRECATED = KinBody::L
		ink::GeomCylinder;
		static const GeomType GeomTrimesh RAVE_DEPRECATED = KinBody::Li
		nk::GeomTrimesh;

		Geometry(boost::shared_ptr<Link> parent, const GeometryInfo& in
		fo);
		virtual ~Geometry() {
		}

		/// \brief get local geometry transform
	inline const Transform& GetTransform() const {	inline const Transform& GetTransform() const {

	return _t;	return _info._t;
	}	}
	inline GeomType GetType() const {	inline GeomType GetType() const {

	return _type;	return _info._type;
	}	}
	inline const Vector& GetRenderScale() const {	inline const Vector& GetRenderScale() const {

	return vRenderScale;	return _info._vRenderScale;
	}	}


	/// \brief render resource file, should be transformed by _t be
	fore rendering
	///
	/// If the value is "__norenderif__:x", then the viewer should
	not render the object if it supports *.x files where"x" is the file extensi
	on.
	inline const std::string& GetRenderFilename() const {	inline const std::string& GetRenderFilename() const {

	return _renderfilename;	return _info._filenamerender;
	}	}
	inline float GetTransparency() const {	inline float GetTransparency() const {

	return ftransparency;	return _info._fTransparency;
	}	}
	/// \deprecated (12/1/12)	/// \deprecated (12/1/12)
	inline bool IsDraw() const RAVE_DEPRECATED {	inline bool IsDraw() const RAVE_DEPRECATED {

	return _bVisible;	return _info._bVisible;
	}	}
	inline bool IsVisible() const {	inline bool IsVisible() const {

	return _bVisible;	return _info._bVisible;
	}	}
	inline bool IsModifiable() const {	inline bool IsModifiable() const {

	return _bModifiable;	return _info._bModifiable;
	}	}

	inline dReal GetSphereRadius() const {	inline dReal GetSphereRadius() const {

	return vGeomData.x;	return _info._vGeomData.x;
	}	}
	inline dReal GetCylinderRadius() const {	inline dReal GetCylinderRadius() const {

	return vGeomData.x;	return _info._vGeomData.x;
	}	}
	inline dReal GetCylinderHeight() const {	inline dReal GetCylinderHeight() const {

	return vGeomData.y;	return _info._vGeomData.y;
	}	}
	inline const Vector& GetBoxExtents() const {	inline const Vector& GetBoxExtents() const {

	return vGeomData;	return _info._vGeomData;
	}	}
	inline const RaveVector<float>& GetDiffuseColor() const {	inline const RaveVector<float>& GetDiffuseColor() const {

	return diffuseColor;	return _info._vDiffuseColor;
	}	}
	inline const RaveVector<float>& GetAmbientColor() const {	inline const RaveVector<float>& GetAmbientColor() const {

	return ambientColor;	return _info._vAmbientColor;
	}	}


	/// \brief collision data of the specific object in its local c	/// \brief returns the local collision mesh
	oordinate system.
	///
	/// Should be transformed by \ref GEOMPROPERTIES::GetTransform(
	) before rendering.
	/// For spheres and cylinders, an appropriate discretization va
	lue is chosen.
	inline const TRIMESH& GetCollisionMesh() const {	inline const TRIMESH& GetCollisionMesh() const {

	return collisionmesh;	return _info._meshcollision;
		}

		inline const GeometryInfo& GetInfo() const {
		return _info;
	}	}

	/// \brief returns an axis aligned bounding box given that the geometry is transformed by trans	/// \brief returns an axis aligned bounding box given that the geometry is transformed by trans
	virtual AABB ComputeAABB(const Transform& trans) const;	virtual AABB ComputeAABB(const Transform& trans) const;
	virtual void serialize(std::ostream& o, int options) const;	virtual void serialize(std::ostream& o, int options) const;

	/// \brief sets a new collision mesh and notifies every registe red callback about it	/// \brief sets a new collision mesh and notifies every registe red callback about it
	virtual void SetCollisionMesh(const TRIMESH& mesh);	virtual void SetCollisionMesh(const TRIMESH& mesh);
	/// \brief sets visible flag. if changed, notifies every regist ered callback about it.	/// \brief sets visible flag. if changed, notifies every regist ered callback about it.
	///	///

	skipping to change at line 197	skipping to change at line 277
	///	///
	/// \param position the position of the contact point specified in the link's coordinate system	/// \param position the position of the contact point specified in the link's coordinate system
	/// \param normal the unit normal of the contact point specifie d in the link's coordinate system	/// \param normal the unit normal of the contact point specifie d in the link's coordinate system
	/// \return true if the normal is changed to face outside of th e shape	/// \return true if the normal is changed to face outside of th e shape
	virtual bool ValidateContactNormal(const Vector& position, Vect or& normal) const;	virtual bool ValidateContactNormal(const Vector& position, Vect or& normal) const;

	/// \brief sets a new render filename for the geometry. This do es not change the collision	/// \brief sets a new render filename for the geometry. This do es not change the collision
	virtual void SetRenderFilename(const std::string& renderfilenam e);	virtual void SetRenderFilename(const std::string& renderfilenam e);

	protected:	protected:

	/// triangulates the geometry object and initializes collisionm
	esh. GeomTrimesh types must already be triangulated
	/// \param fTessellation to control how fine the triangles need
	to be. 1.0f is the default value
	bool InitCollisionMesh(float fTessellation=1);

	boost::weak_ptr<Link> _parent;	boost::weak_ptr<Link> _parent;

	Transform _t; ///< see \ref GetTransform	GeometryInfo _info; ///< geometry info
	Vector vGeomData; ///< for boxes, first 3 values are extents
	///< for sphere it is radius
	///< for cylinder, first 2 values are radius and height
	///< for trimesh, none
	RaveVector<float> diffuseColor, ambientColor; ///< hints for ho
	w to color the meshes
	TRIMESH collisionmesh; ///< see \ref GetCollisionMesh
	GeomType _type; ///< the type of geometry primitive
	std::string _renderfilename; ///< \see ref GetRenderFilename
	Vector vRenderScale; ///< render scale of the object (x,y,z)
	float ftransparency; ///< value from 0-1 for the transparency o
	f the rendered object, 0 is opaque

	bool _bVisible; ///< if true, geometry is visible as part of th
	e 3d model (default is true)
	bool _bModifiable; ///< if true, object geometry can be dynamic
	ally modified (default is true)

	#ifdef RAVE_PRIVATE	#ifdef RAVE_PRIVATE
	#ifdef _MSC_VER	#ifdef _MSC_VER
	friend class ColladaReader;	friend class ColladaReader;
	friend class OpenRAVEXMLParser::LinkXMLReader;	friend class OpenRAVEXMLParser::LinkXMLReader;
	friend class OpenRAVEXMLParser::KinBodyXMLReader;	friend class OpenRAVEXMLParser::KinBodyXMLReader;
	friend class XFileReader;	friend class XFileReader;
	#else	#else
	friend class ::ColladaReader;	friend class ::ColladaReader;
	friend class ::OpenRAVEXMLParser::LinkXMLReader;	friend class ::OpenRAVEXMLParser::LinkXMLReader;
	friend class ::OpenRAVEXMLParser::KinBodyXMLReader;	friend class ::OpenRAVEXMLParser::KinBodyXMLReader;
	friend class ::XFileReader;	friend class ::XFileReader;
	#endif	#endif
	#endif	#endif

		friend class RobotBase;
	friend class KinBody;	friend class KinBody;
	friend class KinBody::Link;	friend class KinBody::Link;
	};	};


		typedef boost::shared_ptr<Geometry> GeometryPtr;
		typedef boost::shared_ptr<Geometry const> GeometryConstPtr;
		typedef Geometry GEOMPROPERTIES RAVE_DEPRECATED;

	inline const std::string& GetName() const {	inline const std::string& GetName() const {
	return _name;	return _name;
	}	}

	/// \brief Indicates a static body that does not move with respect to the root link.	/// \brief Indicates a static body that does not move with respect to the root link.
	///	///
	//// Static should be used when an object has infinite mass and	//// Static should be used when an object has infinite mass and
	///< shouldn't be affected by physics (including gravity). Collisio n still works.	///< shouldn't be affected by physics (including gravity). Collisio n still works.
	inline bool IsStatic() const {	inline bool IsStatic() const {
	return _bStatic;	return _bStatic;

	skipping to change at line 308	skipping to change at line 375

	/// \brief return center of mass of the link in the global coordina te system	/// \brief return center of mass of the link in the global coordina te system
	inline Vector GetGlobalCOM() const {	inline Vector GetGlobalCOM() const {
	return _t*_tMassFrame.trans;	return _t*_tMassFrame.trans;
	}	}

	inline Vector GetCOMOffset() const {	inline Vector GetCOMOffset() const {
	return _tMassFrame.trans;	return _tMassFrame.trans;
	}	}


	/// \brief return inertia in link's local coordinate frame. The COM is GetLocalCOM()	/// \brief return inertia in link's local coordinate frame. The tra nslation component is the the COM in the link's frame.
	virtual TransformMatrix GetLocalInertia() const;	virtual TransformMatrix GetLocalInertia() const;


	// \brief return inertia in the global coordinate frame. The COM is GetCOM()	// \brief return inertia around the link's COM in the global coordi nate frame.
	virtual TransformMatrix GetGlobalInertia() const;	virtual TransformMatrix GetGlobalInertia() const;

	/// \deprecated (12/1/20)	/// \deprecated (12/1/20)
	inline TransformMatrix GetInertia() const RAVE_DEPRECATED {	inline TransformMatrix GetInertia() const RAVE_DEPRECATED {
	RAVELOG_WARN("KinBody::Link::GetInertia is deprecated, use KinB ody::Link::GetGlobalInertia\n");	RAVELOG_WARN("KinBody::Link::GetInertia is deprecated, use KinB ody::Link::GetGlobalInertia\n");
	return GetLocalInertia();	return GetLocalInertia();
	}	}


		/// \brief sets a new mass frame with respect to the link coordinat
		e system
		virtual void SetLocalMassFrame(const Transform& massframe);

		/// \brief sets new principal moments of inertia (with respect to t
		he mass frame)
		virtual void SetPrincipalMomentsOfInertia(const Vector& inertiamome
		nts);

		/// \brief set a new mass
		virtual void SetMass(dReal mass);

	/// \brief return the mass frame in the link's local coordinate sys tem that holds the center of mass and principal axes for inertia.	/// \brief return the mass frame in the link's local coordinate sys tem that holds the center of mass and principal axes for inertia.
	inline const Transform& GetLocalMassFrame() const {	inline const Transform& GetLocalMassFrame() const {
	return _tMassFrame;	return _tMassFrame;
	}	}

	/// \brief return the mass frame in the global coordinate system th at holds the center of mass and principal axes for inertia.	/// \brief return the mass frame in the global coordinate system th at holds the center of mass and principal axes for inertia.
	inline Transform GetGlobalMassFrame() const {	inline Transform GetGlobalMassFrame() const {
	return _t*_tMassFrame;	return _t*_tMassFrame;
	}	}


	skipping to change at line 363	skipping to change at line 439

	/// adds torque to a body (absolute coords)	/// adds torque to a body (absolute coords)
	/// \param add if true, torque is added to previous torques, otherw ise it is set	/// \param add if true, torque is added to previous torques, otherw ise it is set
	virtual void SetTorque(const Vector& torque, bool add);	virtual void SetTorque(const Vector& torque, bool add);

	/// forces the velocity of the link	/// forces the velocity of the link
	/// \param[in] linearvel the translational velocity	/// \param[in] linearvel the translational velocity
	/// \param[in] angularvel is the rotation axis * angular speed	/// \param[in] angularvel is the rotation axis * angular speed
	virtual void SetVelocity(const Vector& linearvel, const Vector& ang ularvel);	virtual void SetVelocity(const Vector& linearvel, const Vector& ang ularvel);


	/// get the velocity of the link	/// \brief get the velocity of the link
	/// \param[out] linearvel the translational velocity	/// \param[out] linearvel the translational velocity
	/// \param[out] angularvel is the rotation axis * angular speed	/// \param[out] angularvel is the rotation axis * angular speed
	virtual void GetVelocity(Vector& linearvel, Vector& angularvel) con st;	virtual void GetVelocity(Vector& linearvel, Vector& angularvel) con st;


	//typedef std::list<GEOMPROPERTIES>::iterator GeomPtr;	/// \brief return the linear/angular velocity of the link
	//typedef std::list<GEOMPROPERTIES>::const_iterator GeomConstPtr;	virtual std::pair<Vector,Vector> GetVelocity() const;

	/// \brief returns a list of all the geometry objects.	/// \brief returns a list of all the geometry objects.

	inline const std::list<GEOMPROPERTIES>& GetGeometries() const {	inline const std::vector<GeometryPtr>& GetGeometries() const {
	return _listGeomProperties;	return _vGeometries;
	}	}

	virtual GEOMPROPERTIES& GetGeometry(int index);	virtual GeometryPtr GetGeometry(int index);


	/// \brief swaps the current geometries with the new geometries.	/// \brief inits the current geometries with the new geometry info.
	///	///
	/// This gives a user control for dynamically changing the object g eometry. Note that the kinbody/robot hash could change.	/// This gives a user control for dynamically changing the object g eometry. Note that the kinbody/robot hash could change.

	virtual void SwapGeometries(std::list<GEOMPROPERTIES>& listNewGeome	/// \param geometries a list of geometry infos to be initialized in
	tries);	to new geometry objects, note that the geometry info data is copied
		virtual void InitGeometries(std::list<GeometryInfo>& geometries);

		/// \brief swaps the geometries with the link
		virtual void SwapGeometries(boost::shared_ptr<Link>& link);

	/// validates the contact normal on link and makes sure the normal faces "outside" of the geometry shape it lies on. An exception can be throw n if position is not on a geometry surface	/// validates the contact normal on link and makes sure the normal faces "outside" of the geometry shape it lies on. An exception can be throw n if position is not on a geometry surface
	/// \param position the position of the contact point specified in the link's coordinate system, assumes it is on a particular geometry	/// \param position the position of the contact point specified in the link's coordinate system, assumes it is on a particular geometry
	/// \param normal the unit normal of the contact point specified in the link's coordinate system	/// \param normal the unit normal of the contact point specified in the link's coordinate system
	/// \return true if the normal is changed to face outside of the sh ape	/// \return true if the normal is changed to face outside of the sh ape
	virtual bool ValidateContactNormal(const Vector& position, Vector& normal) const;	virtual bool ValidateContactNormal(const Vector& position, Vector& normal) const;

	/// \brief returns true if plink is rigidily attahced to this link.	/// \brief returns true if plink is rigidily attahced to this link.
	virtual bool IsRigidlyAttached(boost::shared_ptr<Link const> plink) const;	virtual bool IsRigidlyAttached(boost::shared_ptr<Link const> plink) const;


	skipping to change at line 410	skipping to change at line 490
	virtual void _Update();	virtual void _Update();

	Transform _t; ///< \see GetTransform	Transform _t; ///< \see GetTransform
	Transform _tMassFrame; ///< the frame for inertia and center of mas s of the link in the link's coordinate system	Transform _tMassFrame; ///< the frame for inertia and center of mas s of the link in the link's coordinate system
	dReal _mass; ///< mass of link	dReal _mass; ///< mass of link
	Vector _vinertiamoments; ///< inertia along the axes of _tMassFrame	Vector _vinertiamoments; ///< inertia along the axes of _tMassFrame
	TRIMESH collision; ///< triangles for collision checking, t riangles are always the triangulation	TRIMESH collision; ///< triangles for collision checking, t riangles are always the triangulation
	///< of the body when it is at the ident ity transformation	///< of the body when it is at the ident ity transformation

	std::string _name; ///< optional link name	std::string _name; ///< optional link name

	std::list<GEOMPROPERTIES> _listGeomProperties; ///< \see Ge tGeometries	std::vector<GeometryPtr> _vGeometries; ///< \see GetGeometr ies

	bool _bStatic; ///< \see IsStatic	bool _bStatic; ///< \see IsStatic
	bool _bIsEnabled; ///< \see IsEnabled	bool _bIsEnabled; ///< \see IsEnabled

	private:	private:
	/// Sensitive variables that should not be modified.	/// Sensitive variables that should not be modified.
	/// @name Private Link Variables	/// @name Private Link Variables
	//@{	//@{
	int _index; ///< \see GetIndex	int _index; ///< \see GetIndex
	KinBodyWeakPtr _parent; ///< \see GetParent	KinBodyWeakPtr _parent; ///< \see GetParent

	skipping to change at line 440	skipping to change at line 520
	friend class XFileReader;	friend class XFileReader;
	#else	#else
	friend class ::ColladaReader;	friend class ::ColladaReader;
	friend class ::OpenRAVEXMLParser::LinkXMLReader;	friend class ::OpenRAVEXMLParser::LinkXMLReader;
	friend class ::OpenRAVEXMLParser::KinBodyXMLReader;	friend class ::OpenRAVEXMLParser::KinBodyXMLReader;
	friend class ::OpenRAVEXMLParser::RobotXMLReader;	friend class ::OpenRAVEXMLParser::RobotXMLReader;
	friend class ::XFileReader;	friend class ::XFileReader;
	#endif	#endif
	#endif	#endif
	friend class KinBody;	friend class KinBody;

		friend class RobotBase;
	};	};
	typedef boost::shared_ptr<KinBody::Link> LinkPtr;	typedef boost::shared_ptr<KinBody::Link> LinkPtr;
	typedef boost::shared_ptr<KinBody::Link const> LinkConstPtr;	typedef boost::shared_ptr<KinBody::Link const> LinkConstPtr;
	typedef boost::weak_ptr<KinBody::Link> LinkWeakPtr;	typedef boost::weak_ptr<KinBody::Link> LinkWeakPtr;

	/// \brief Information about a joint that controls the relationship bet ween two links.	/// \brief Information about a joint that controls the relationship bet ween two links.
	class OPENRAVE_API Joint : public boost::enable_shared_from_this<Joint>	class OPENRAVE_API Joint : public boost::enable_shared_from_this<Joint>
	{	{
	public:	public:
	/** \brief The type of joint movement.	/** \brief The type of joint movement.

	skipping to change at line 471	skipping to change at line 552
	JointSlider = 0x11,	JointSlider = 0x11,
	JointPrismatic = 0x11,	JointPrismatic = 0x11,
	JointRR = 0x02,	JointRR = 0x02,
	JointRP = 0x12,	JointRP = 0x12,
	JointPR = 0x22,	JointPR = 0x22,
	JointPP = 0x32,	JointPP = 0x32,
	JointSpecialBit = 0x80000000,	JointSpecialBit = 0x80000000,
	JointUniversal = 0x80000001,	JointUniversal = 0x80000001,
	JointHinge2 = 0x80000002,	JointHinge2 = 0x80000002,
	JointSpherical = 0x80000003,	JointSpherical = 0x80000003,

		JointTrajectory = 0x80000004, ///< there is no axis defined, in
		stead the relative transformation is directly output from the trajectory af
		fine_transform structure
		};

		/// \brief Holds mimic information about position, velocity, and ac
		celeration of one axis of the joint.
		///
		/// In every array, [0] is position, [1] is velocity, [2] is accele
		ration.
		struct OPENRAVE_API MIMIC
		{
		struct DOFFormat
		{
		int16_t jointindex; ///< the index into the joints,
		if >= GetJoints.size(), then points to the passive joints
		int16_t dofindex : 14; ///< if >= 0, then points to
		a DOF of the robot that is NOT mimiced
		uint8_t axis : 2; ///< the axis of the joint index
		bool operator <(const DOFFormat& r) const;
		bool operator ==(const DOFFormat& r) const;
		boost::shared_ptr<Joint> GetJoint(KinBodyPtr parent) const;
		boost::shared_ptr<Joint const> GetJoint(KinBodyConstPtr par
		ent) const;
		};
		std::vector< DOFFormat > _vdofformat; ///< the format o
		f the values the equation takes order is important.
		struct DOFHierarchy
		{
		int16_t dofindex; ///< >=0 dof index
		uint16_t dofformatindex; ///< index into _vdofforma
		t to follow the computation
		bool operator ==(const DOFHierarchy& r) const {
		return dofindex==r.dofindex && dofformatindex == r.doff
		ormatindex;
		}
		};
		std::vector<DOFHierarchy> _vmimicdofs; ///< all dof ind
		ices that the equations depends on. DOFHierarchy::dofindex can repeat
		OpenRAVEFunctionParserRealPtr _posfn;
		std::vector<OpenRAVEFunctionParserRealPtr > _velfns, _accelfns;
		///< the velocity and acceleration partial derivatives with respec
		t to each of the values in _vdofformat
		boost::array< std::string, 3> _equations; ///< the ori
		ginal equations
	};	};


	Joint(KinBodyPtr parent);	Joint(KinBodyPtr parent, JointType type = JointNone);
	virtual ~Joint();	virtual ~Joint();

	/// \brief The unique name of the joint	/// \brief The unique name of the joint
	inline const std::string& GetName() const {	inline const std::string& GetName() const {
	return _name;	return _name;
	}	}

	inline dReal GetMaxVel(int iaxis=0) const {	inline dReal GetMaxVel(int iaxis=0) const {
	return _vmaxvel[iaxis];	return _vmaxvel[iaxis];
	}	}

	skipping to change at line 518	skipping to change at line 630
	return _attachedbodies[0];	return _attachedbodies[0];
	}	}
	inline LinkPtr GetSecondAttached() const {	inline LinkPtr GetSecondAttached() const {
	return _attachedbodies[1];	return _attachedbodies[1];
	}	}

	inline JointType GetType() const {	inline JointType GetType() const {
	return _type;	return _type;
	}	}


		/// \brief gets all resolutions for the joint axes
		///
		/// \param[in] bAppend if true will append to the end of the vector
		instead of erasing it
		virtual void GetResolutions(std::vector<dReal>& resolutions, bool b
		Append=false) const;

	/// \brief The discretization of the joint used when line-collision checking.	/// \brief The discretization of the joint used when line-collision checking.
	///	///
	/// The resolutions are set as large as possible such that the join t will not go through obstacles of determined size.	/// The resolutions are set as large as possible such that the join t will not go through obstacles of determined size.

	inline dReal GetResolution() const {	inline dReal GetResolution(int iaxis=0) const {
	return fResolution;	return fResolution;
	}	}

	virtual void SetResolution(dReal resolution);	virtual void SetResolution(dReal resolution, int iaxis=0);

	/// \brief The degrees of freedom of the joint. Each joint supports a max of 3 degrees of freedom.	/// \brief The degrees of freedom of the joint. Each joint supports a max of 3 degrees of freedom.
	virtual int GetDOF() const;	virtual int GetDOF() const;

	/// \brief Return true if joint axis has an identification at some of its lower and upper limits.	/// \brief Return true if joint axis has an identification at some of its lower and upper limits.
	///	///
	/// An identification of the lower and upper limits means that once the joint reaches its upper limits, it is also	/// An identification of the lower and upper limits means that once the joint reaches its upper limits, it is also
	/// at its lower limit. The most common identification on revolute joints at -pi and pi. 'circularity' means the	/// at its lower limit. The most common identification on revolute joints at -pi and pi. 'circularity' means the
	/// joint does not stop at limits.	/// joint does not stop at limits.
	/// Although currently not developed, it could be possible to suppo rt identification for joints that are not revolute.	/// Although currently not developed, it could be possible to suppo rt identification for joints that are not revolute.

	virtual bool IsCircular(int iaxis) const {	virtual bool IsCircular(int iaxis) const;
	return _bIsCircular.at(iaxis);
	}

	/// \brief returns true if the axis describes a rotation around an axis.	/// \brief returns true if the axis describes a rotation around an axis.
	///	///
	/// \param iaxis the axis of the joint to return the results for	/// \param iaxis the axis of the joint to return the results for
	virtual bool IsRevolute(int iaxis) const;	virtual bool IsRevolute(int iaxis) const;

	/// \brief returns true if the axis describes a translation around an axis.	/// \brief returns true if the axis describes a translation around an axis.
	///	///
	/// \param iaxis the axis of the joint to return the results for	/// \param iaxis the axis of the joint to return the results for
	virtual bool IsPrismatic(int iaxis) const;	virtual bool IsPrismatic(int iaxis) const;

	skipping to change at line 567	skipping to change at line 682

	/// \brief Return the value of the specified joint axis only.	/// \brief Return the value of the specified joint axis only.
	virtual dReal GetValue(int axis) const;	virtual dReal GetValue(int axis) const;

	/// \brief Gets the joint velocities	/// \brief Gets the joint velocities
	///	///
	/// \param bAppend if true will append to the end of the vector ins tead of erasing it	/// \param bAppend if true will append to the end of the vector ins tead of erasing it
	/// \return the degrees of freedom of the joint (even if pValues is NULL)	/// \return the degrees of freedom of the joint (even if pValues is NULL)
	virtual void GetVelocities(std::vector<dReal>& values, bool bAppend =false) const;	virtual void GetVelocities(std::vector<dReal>& values, bool bAppend =false) const;


		/// \brief Return the velocity of the specified joint axis only.
		virtual dReal GetVelocity(int axis) const;

	/// \brief Add effort (force or torque) to the joint	/// \brief Add effort (force or torque) to the joint
	virtual void AddTorque(const std::vector<dReal>& torques);	virtual void AddTorque(const std::vector<dReal>& torques);

	/// \brief The anchor of the joint in global coordinates.	/// \brief The anchor of the joint in global coordinates.
	virtual Vector GetAnchor() const;	virtual Vector GetAnchor() const;

	/// \brief The axis of the joint in global coordinates	/// \brief The axis of the joint in global coordinates
	///	///
	/// \param[in] axis the axis to get	/// \param[in] axis the axis to get
	virtual Vector GetAxis(int axis = 0) const;	virtual Vector GetAxis(int axis = 0) const;


	/** \brief Returns the limits of the joint	/** \brief Get the limits of the joint

	\param[out] vLowerLimit the lower limits	\param[out] vLowerLimit the lower limits
	\param[out] vUpperLimit the upper limits	\param[out] vUpperLimit the upper limits
	\param[in] bAppend if true will append to the end of the vector instead of erasing it	\param[in] bAppend if true will append to the end of the vector instead of erasing it
	*/	*/
	virtual void GetLimits(std::vector<dReal>& vLowerLimit, std::vector <dReal>& vUpperLimit, bool bAppend=false) const;	virtual void GetLimits(std::vector<dReal>& vLowerLimit, std::vector <dReal>& vUpperLimit, bool bAppend=false) const;


		/// \brief returns the lower and upper limit of one axis of the joi
		nt
		virtual std::pair<dReal, dReal> GetLimit(int iaxis=0) const;

	/// \brief \see GetLimits	/// \brief \see GetLimits
	virtual void SetLimits(const std::vector<dReal>& lower, const std:: vector<dReal>& upper);	virtual void SetLimits(const std::vector<dReal>& lower, const std:: vector<dReal>& upper);


	/// \deprecated (11/1/1)
	virtual void SetJointLimits(const std::vector<dReal>& lower, const
	std::vector<dReal>& upper) RAVE_DEPRECATED {
	SetLimits(lower,upper);
	}

	/** \brief Returns the max velocities of the joint	/** \brief Returns the max velocities of the joint

	\param[out] the max velocity	\param[out] the max velocity
	\param[in] bAppend if true will append to the end of the vector instead of erasing it	\param[in] bAppend if true will append to the end of the vector instead of erasing it
	*/	*/
	virtual void GetVelocityLimits(std::vector<dReal>& vmax, bool bAppe nd=false) const;	virtual void GetVelocityLimits(std::vector<dReal>& vmax, bool bAppe nd=false) const;

	virtual void GetVelocityLimits(std::vector<dReal>& vlower, std::vec tor<dReal>& vupper, bool bAppend=false) const;	virtual void GetVelocityLimits(std::vector<dReal>& vlower, std::vec tor<dReal>& vupper, bool bAppend=false) const;


		/// \brief returns the lower and upper velocity limit of one axis o
		f the joint
		virtual std::pair<dReal, dReal> GetVelocityLimit(int iaxis=0) const
		;

	/// \brief \see GetVelocityLimits	/// \brief \see GetVelocityLimits
	virtual void SetVelocityLimits(const std::vector<dReal>& vmax);	virtual void SetVelocityLimits(const std::vector<dReal>& vmax);

	/** \brief Returns the max accelerations of the joint	/** \brief Returns the max accelerations of the joint

	\param[out] the max acceleration	\param[out] the max acceleration
	\param[in] bAppend if true will append to the end of the vector instead of erasing it	\param[in] bAppend if true will append to the end of the vector instead of erasing it
	*/	*/
	virtual void GetAccelerationLimits(std::vector<dReal>& vmax, bool b Append=false) const;	virtual void GetAccelerationLimits(std::vector<dReal>& vmax, bool b Append=false) const;


		virtual dReal GetAccelerationLimit(int iaxis=0) const;

	/// \brief \see GetAccelerationLimits	/// \brief \see GetAccelerationLimits
	virtual void SetAccelerationLimits(const std::vector<dReal>& vmax);	virtual void SetAccelerationLimits(const std::vector<dReal>& vmax);

	/** \brief Returns the max torques of the joint	/** \brief Returns the max torques of the joint

	\param[out] the max torque	\param[out] the max torque
	\param[in] bAppend if true will append to the end of the vector instead of erasing it	\param[in] bAppend if true will append to the end of the vector instead of erasing it
	*/	*/
	virtual void GetTorqueLimits(std::vector<dReal>& vmax, bool bAppend =false) const;	virtual void GetTorqueLimits(std::vector<dReal>& vmax, bool bAppend =false) const;

	/// \brief \see GetTorqueLimits	/// \brief \see GetTorqueLimits
	virtual void SetTorqueLimits(const std::vector<dReal>& vmax);	virtual void SetTorqueLimits(const std::vector<dReal>& vmax);


		/// \brief gets all weights for the joint axes
		///
		/// \param[in] bAppend if true will append to the end of the vector
		instead of erasing it
		virtual void GetWeights(std::vector<dReal>& weights, bool bAppend=f
		alse) const;

	/// \brief The weight associated with a joint's axis for computing a distance in the robot configuration space.	/// \brief The weight associated with a joint's axis for computing a distance in the robot configuration space.
	virtual dReal GetWeight(int axis=0) const;	virtual dReal GetWeight(int axis=0) const;


	/// \brief \see GetWeight	/// \brief \see GetWeight
	virtual void SetWeights(const std::vector<dReal>& weights);	virtual void SetWeights(const std::vector<dReal>& weights);

	/// \brief Computes the configuration difference values1-values2 an d stores it in values1.	/// \brief Computes the configuration difference values1-values2 an d stores it in values1.
	///	///
	/// Takes into account joint limits and wrapping of circular joints .	/// Takes into account joint limits and wrapping of circular joints .
	virtual void SubtractValues(std::vector<dReal>& values1, const std: :vector<dReal>& values2) const;	virtual void SubtractValues(std::vector<dReal>& values1, const std: :vector<dReal>& values2) const;


		/// \brief Returns the configuration difference value1-value2 for a
		xis i.
		///
		/// Takes into account joint limits and wrapping of circular joints
		.
		virtual dReal SubtractValue(dReal value1, dReal value2, int iaxis)
		const;

	/// \brief Return internal offset parameter that determines the bra nch the angle centers on	/// \brief Return internal offset parameter that determines the bra nch the angle centers on
	///	///
	/// Wrap offsets are needed for rotation joints since the range is limited to 2*pi.	/// Wrap offsets are needed for rotation joints since the range is limited to 2*pi.
	/// This allows the wrap offset to be set so the joint can function in [-pi+offset,pi+offset]..	/// This allows the wrap offset to be set so the joint can function in [-pi+offset,pi+offset]..
	/// \param iaxis the axis to get the offset from	/// \param iaxis the axis to get the offset from
	inline dReal GetWrapOffset(int iaxis=0) const {	inline dReal GetWrapOffset(int iaxis=0) const {
	return _voffsets.at(iaxis);	return _voffsets.at(iaxis);
	}	}

	inline dReal GetOffset(int iaxis=0) const RAVE_DEPRECATED {	inline dReal GetOffset(int iaxis=0) const RAVE_DEPRECATED {
	return GetWrapOffset(iaxis);	return GetWrapOffset(iaxis);
	}	}

	/// \brief \see GetWrapOffset	/// \brief \see GetWrapOffset
	virtual void SetWrapOffset(dReal offset, int iaxis=0);	virtual void SetWrapOffset(dReal offset, int iaxis=0);

	/// \deprecated (11/1/16)
	virtual void SetOffset(dReal offset, int iaxis=0) RAVE_DEPRECATED {
	SetWrapOffset(offset,iaxis);
	}

	virtual void serialize(std::ostream& o, int options) const;	virtual void serialize(std::ostream& o, int options) const;

	/// @name Internal Hierarchy Methods	/// @name Internal Hierarchy Methods
	//@{	//@{
	/// \brief Return the parent link which the joint measures its angl e off from (either GetFirstAttached() or GetSecondAttached())	/// \brief Return the parent link which the joint measures its angl e off from (either GetFirstAttached() or GetSecondAttached())
	virtual LinkPtr GetHierarchyParentLink() const;	virtual LinkPtr GetHierarchyParentLink() const;
	/// \brief Return the child link whose transformation is computed b y this joint's values (either GetFirstAttached() or GetSecondAttached())	/// \brief Return the child link whose transformation is computed b y this joint's values (either GetFirstAttached() or GetSecondAttached())
	virtual LinkPtr GetHierarchyChildLink() const;	virtual LinkPtr GetHierarchyChildLink() const;

	/// \deprecated (11/1/27)
	virtual Vector GetInternalHierarchyAnchor() const RAVE_DEPRECATED;
	/// \brief The axis of the joint in local coordinates.	/// \brief The axis of the joint in local coordinates.
	virtual Vector GetInternalHierarchyAxis(int axis = 0) const;	virtual Vector GetInternalHierarchyAxis(int axis = 0) const;
	/// \brief Left multiply transform given the base body.	/// \brief Left multiply transform given the base body.
	virtual Transform GetInternalHierarchyLeftTransform() const;	virtual Transform GetInternalHierarchyLeftTransform() const;
	/// \brief Right multiply transform given the base body.	/// \brief Right multiply transform given the base body.
	virtual Transform GetInternalHierarchyRightTransform() const;	virtual Transform GetInternalHierarchyRightTransform() const;
	//@}	//@}

	/// A mimic joint's angles are automatically determined from other joints based on a general purpose formula.	/// A mimic joint's angles are automatically determined from other joints based on a general purpose formula.
	/// A user does not have control of the the mimic joint values, eve n if they appear in the DOF list.	/// A user does not have control of the the mimic joint values, eve n if they appear in the DOF list.

	skipping to change at line 742	skipping to change at line 868
	boost::array<Vector,3> _vaxes; ///< axes in body[0]' s or environment coordinate system used to define joint movement	boost::array<Vector,3> _vaxes; ///< axes in body[0]' s or environment coordinate system used to define joint movement
	Vector vanchor; ///< anchor of the joint, this is o nly used to construct the internal left/right matrices	Vector vanchor; ///< anchor of the joint, this is o nly used to construct the internal left/right matrices
	dReal fResolution; ///< interpolation resolution	dReal fResolution; ///< interpolation resolution
	boost::array<dReal,3> _vmaxvel; ///< the soft maxi mum velocity (rad/s) to move the joint when planning	boost::array<dReal,3> _vmaxvel; ///< the soft maxi mum velocity (rad/s) to move the joint when planning
	boost::array<dReal,3> fHardMaxVel; ///< the hard maxim um velocity, robot cannot exceed this velocity. used for verification check ing	boost::array<dReal,3> fHardMaxVel; ///< the hard maxim um velocity, robot cannot exceed this velocity. used for verification check ing
	boost::array<dReal,3> _vmaxaccel; ///< the maximum a cceleration (rad/s^2) of the joint	boost::array<dReal,3> _vmaxaccel; ///< the maximum a cceleration (rad/s^2) of the joint
	boost::array<dReal,3> _vmaxtorque; ///< maximum torqu e (N.m, kg m^2/s^2) that can be applied to the joint	boost::array<dReal,3> _vmaxtorque; ///< maximum torqu e (N.m, kg m^2/s^2) that can be applied to the joint
	boost::array<dReal,3> _vweights; ///< the weights of the joint for computing distance metrics.	boost::array<dReal,3> _vweights; ///< the weights of the joint for computing distance metrics.
	boost::array<dReal,3> _voffsets; ///< \see GetOff set	boost::array<dReal,3> _voffsets; ///< \see GetOff set
	boost::array<dReal,3> _vlowerlimit, _vupperlimit; ///< join t limits	boost::array<dReal,3> _vlowerlimit, _vupperlimit; ///< join t limits

	/// \brief Holds mimic information about position, velocity, and ac	boost::array<dReal,3> _vcircularlowerlimit, _vcircularupperlimit;
	celeration of one axis of the joint.	///< for circular joints, describes where the identification happens
	///	. this is set internally in _ComputeInternalInformation
	/// In every array, [0] is position, [1] is velocity, [2] is accele	TrajectoryBasePtr _trajfollow; ///< used if joint type is JointTraj
	ration.	ectory
	struct OPENRAVE_API MIMIC
	{
	struct DOFFormat
	{
	int16_t jointindex; ///< the index into the joints,
	if >= GetJoints.size(), then points to the passive joints
	int16_t dofindex : 14; ///< if >= 0, then points to
	a DOF of the robot that is NOT mimiced
	uint8_t axis : 2; ///< the axis of the joint index
	bool operator <(const DOFFormat& r) const;
	bool operator ==(const DOFFormat& r) const;
	boost::shared_ptr<Joint> GetJoint(KinBodyPtr parent) const;
	boost::shared_ptr<Joint const> GetJoint(KinBodyConstPtr par
	ent) const;
	};
	std::vector< DOFFormat > _vdofformat; ///< the format o
	f the values the equation takes order is important.
	struct DOFHierarchy
	{
	int16_t dofindex; ///< >=0 dof index
	uint16_t dofformatindex; ///< index into _vdofforma
	t to follow the computation
	bool operator ==(const DOFHierarchy& r) const {
	return dofindex==r.dofindex && dofformatindex == r.doff
	ormatindex;
	}
	};
	std::vector<DOFHierarchy> _vmimicdofs; ///< all dof ind
	ices that the equations depends on. DOFHierarchy::dofindex can repeat
	boost::shared_ptr<FunctionParserBase<dReal> > _posfn;
	std::vector<boost::shared_ptr<FunctionParserBase<dReal> > > _ve
	lfns, _accelfns; ///< the velocity and acceleration partial derivat
	ives with respect to each of the values in _vdofformat
	boost::array< std::string, 3> _equations; ///< the ori
	ginal equations
	};
	boost::array< boost::shared_ptr<MIMIC>,3> _vmimic; ///< th e mimic properties of each of the joint axes. It is theoretically possible for a multi-dof joint to have one axes mimiced and the others free. When cl oning, is it ok to copy this and assume it is constant?	boost::array< boost::shared_ptr<MIMIC>,3> _vmimic; ///< th e mimic properties of each of the joint axes. It is theoretically possible for a multi-dof joint to have one axes mimiced and the others free. When cl oning, is it ok to copy this and assume it is constant?

	/** \brief computes the partial velocities with respect to all depe ndent DOFs specified by MIMIC::_vmimicdofs.	/** \brief computes the partial velocities with respect to all depe ndent DOFs specified by MIMIC::_vmimicdofs.

	If the joint is not mimic, then just returns its own index	If the joint is not mimic, then just returns its own index
	\param[out] vpartials A list of dofindex/velocity_partial pairs . The final velocity is computed by taking the dot product. The dofindices do not repeat.	\param[out] vpartials A list of dofindex/velocity_partial pairs . The final velocity is computed by taking the dot product. The dofindices do not repeat.
	\param[in] iaxis the axis	\param[in] iaxis the axis
	\param[in,out] vcachedpartials set of cached partials for each degree of freedom	\param[in,out] vcachedpartials set of cached partials for each degree of freedom
	*/	*/
	virtual void _ComputePartialVelocities(std::vector<std::pair<int,dR eal> >& vpartials, int iaxis, std::map< std::pair<MIMIC::DOFFormat, int>, d Real >& mapcachedpartials) const;	virtual void _ComputePartialVelocities(std::vector<std::pair<int,dR eal> >& vpartials, int iaxis, std::map< std::pair<MIMIC::DOFFormat, int>, d Real >& mapcachedpartials) const;

	skipping to change at line 795	skipping to change at line 895
	Compute the left and right multiplications of the joint transfo rmation and cleans up the attached bodies.	Compute the left and right multiplications of the joint transfo rmation and cleans up the attached bodies.
	After function completes, the following parameters are initiali zed: _tRight, _tLeft, _tinvRight, _tinvLeft, _attachedbodies. _attachedbodi es does not necessarily contain the links in the same order as they were in put.	After function completes, the following parameters are initiali zed: _tRight, _tLeft, _tinvRight, _tinvLeft, _attachedbodies. _attachedbodi es does not necessarily contain the links in the same order as they were in put.
	\param plink0 the first attaching link, all axes and anchors ar e defined in its coordinate system	\param plink0 the first attaching link, all axes and anchors ar e defined in its coordinate system
	\param plink1 the second attaching link	\param plink1 the second attaching link
	\param vanchor the anchor of the rotation axes	\param vanchor the anchor of the rotation axes
	\param vaxes the axes in plink0's coordinate system of the join ts	\param vaxes the axes in plink0's coordinate system of the join ts
	\param vinitialvalues the current values of the robot used to s et the 0 offset of the robot	\param vinitialvalues the current values of the robot used to s et the 0 offset of the robot
	*/	*/
	virtual void _ComputeInternalInformation(LinkPtr plink0, LinkPtr pl ink1, const Vector& vanchor, const std::vector<Vector>& vaxes, const std::v ector<dReal>& vcurrentvalues);	virtual void _ComputeInternalInformation(LinkPtr plink0, LinkPtr pl ink1, const Vector& vanchor, const std::vector<Vector>& vaxes, const std::v ector<dReal>& vcurrentvalues);


		/// \brief evaluates the mimic joint equation using vdependentvalue
		s
		///
		/// \param[in] axis the joint axis
		/// \param[in] timederiv the time derivative to evaluate. 0 is posi
		tion, 1 is velocity, 2 is acceleration, etc
		/// \param[in] vdependentvalues input values ordered with respect t
		o _vdofformat[iaxis]
		/// \param[out] voutput the output values
		/// \return an internal error code, 0 if no error
		virtual int _Eval(int axis, uint32_t timederiv, const std::vector<d
		Real>& vdependentvalues, std::vector<dReal>& voutput);

		/// \brief compute joint velocities given the parent and child link
		transformations/velocities
		virtual void _GetVelocities(std::vector<dReal>& values, bool bAppen
		d, const std::pair<Vector,Vector>& linkparentvelocity, const std::pair<Vect
		or,Vector>& linkchildvelocity) const;

		/// \brief Return the velocity of the specified joint axis only.
		virtual dReal _GetVelocity(int axis, const std::pair<Vector,Vector>
		&linkparentvelocity, const std::pair<Vector,Vector>&linkchildvelocity) cons
		t;

	std::string _name; ///< \see GetName	std::string _name; ///< \see GetName

	boost::array<bool,3> _bIsCircular; ///< \see IsCircular	boost::array<uint8_t,3> _bIsCircular; ///< \see IsCircul ar
	boost::array<int,3> _dofbranches; ///< the branch that identified j oints are on. +1 means one loop around the identification. For revolute joi nts, the actual joint value incremented by 2pibranch. Branches are import ant for maintaining joint ranges greater than 2*pi. For circular joints, th e branches can be ignored or not.	boost::array<int,3> _dofbranches; ///< the branch that identified j oints are on. +1 means one loop around the identification. For revolute joi nts, the actual joint value incremented by 2pibranch. Branches are import ant for maintaining joint ranges greater than 2*pi. For circular joints, th e branches can be ignored or not.

	private:	private:
	/// Sensitive variables that should not be modified.	/// Sensitive variables that should not be modified.
	/// @name Private Joint Variables	/// @name Private Joint Variables
	//@{	//@{
	int dofindex; ///< the degree of freedom index in the body's DOF array, does not index in KinBody::_vecjoints!	int dofindex; ///< the degree of freedom index in the body's DOF array, does not index in KinBody::_vecjoints!
	int jointindex; ///< the joint index into KinBody:: _vecjoints	int jointindex; ///< the joint index into KinBody:: _vecjoints
	JointType _type;	JointType _type;
	bool _bActive; ///< if true, should belong to the D OF of the body, unless it is a mimic joint (_ComputeInternalInformation dec ides this)	bool _bActive; ///< if true, should belong to the D OF of the body, unless it is a mimic joint (_ComputeInternalInformation dec ides this)

	skipping to change at line 902	skipping to change at line 1017
	/// this object is managed by the sensor system and should be delet ed when it goes out of scope.	/// this object is managed by the sensor system and should be delet ed when it goes out of scope.
	SensorSystemBaseWeakPtr _psensorsystem;	SensorSystemBaseWeakPtr _psensorsystem;
	};	};

	typedef boost::shared_ptr<KinBody::ManageData> ManageDataPtr;	typedef boost::shared_ptr<KinBody::ManageData> ManageDataPtr;
	typedef boost::shared_ptr<KinBody::ManageData const> ManageDataConstPtr ;	typedef boost::shared_ptr<KinBody::ManageData const> ManageDataConstPtr ;

	/// \brief Parameters passed into the state savers to control what info rmation gets saved.	/// \brief Parameters passed into the state savers to control what info rmation gets saved.
	enum SaveParameters	enum SaveParameters
	{	{

	Save_LinkTransformation=0x00000001, ///< [default] save link tr ansformations	Save_LinkTransformation=0x00000001, ///< [default] save link tr ansformations and joint branches
	Save_LinkEnable=0x00000002, ///< [default] save link enable sta tes	Save_LinkEnable=0x00000002, ///< [default] save link enable sta tes
	Save_LinkVelocities=0x00000004, ///< save the link velocities	Save_LinkVelocities=0x00000004, ///< save the link velocities

		Save_JointMaxVelocityAndAcceleration=0x00000008, ///< save the max joint velocities and accelerations for the controller DOF
	Save_ActiveDOF=0x00010000, ///< [robot only], saves and restore s the current active degrees of freedom	Save_ActiveDOF=0x00010000, ///< [robot only], saves and restore s the current active degrees of freedom
	Save_ActiveManipulator=0x00020000, ///< [robot only], saves the active manipulator	Save_ActiveManipulator=0x00020000, ///< [robot only], saves the active manipulator
	Save_GrabbedBodies=0x00040000, ///< [robot only], saves the gra bbed state of the bodies. This does not affect the configuraiton of those b odies.	Save_GrabbedBodies=0x00040000, ///< [robot only], saves the gra bbed state of the bodies. This does not affect the configuraiton of those b odies.
	};	};

	/// \brief Helper class to save and restore the entire kinbody state.	/// \brief Helper class to save and restore the entire kinbody state.
	///	///
	/// Options can be passed to the constructor in order to choose which p arameters to save (see \ref SaveParameters)	/// Options can be passed to the constructor in order to choose which p arameters to save (see \ref SaveParameters)
	class OPENRAVE_API KinBodyStateSaver	class OPENRAVE_API KinBodyStateSaver
	{	{
	public:	public:
	KinBodyStateSaver(KinBodyPtr pbody, int options = Save_LinkTransfor mation\|Save_LinkEnable);	KinBodyStateSaver(KinBodyPtr pbody, int options = Save_LinkTransfor mation\|Save_LinkEnable);
	virtual ~KinBodyStateSaver();	virtual ~KinBodyStateSaver();
	inline KinBodyPtr GetBody() const {	inline KinBodyPtr GetBody() const {
	return _pbody;	return _pbody;
	}	}

	virtual void Restore();
		/// \brief restore the state
		///
		/// \param body if set, will attempt to restore the stored state to
		the passed in body, otherwise will restore it for the original body.
		/// \throw openrave_exception if the passed in body is not compatib
		le with the saved state, will throw
		virtual void Restore(boost::shared_ptr<KinBody> body=boost::shared_
		ptr<KinBody>());

		/// \brief release the body state. _pbody will not get restored on
		destruction
		///
		/// After this call, it will still be possible to use \ref Restore.
		virtual void Release();
	protected:	protected:
	int _options; ///< saved options	int _options; ///< saved options
	std::vector<Transform> _vLinkTransforms;	std::vector<Transform> _vLinkTransforms;
	std::vector<uint8_t> _vEnabledLinks;	std::vector<uint8_t> _vEnabledLinks;
	std::vector<std::pair<Vector,Vector> > _vLinkVelocities;	std::vector<std::pair<Vector,Vector> > _vLinkVelocities;
	std::vector<int> _vdofbranches;	std::vector<int> _vdofbranches;

		std::vector<dReal> _vMaxVelocities, _vMaxAccelerations;
	KinBodyPtr _pbody;	KinBodyPtr _pbody;
	private:	private:

	virtual void _RestoreKinBody();	virtual void _RestoreKinBody(boost::shared_ptr<KinBody> body);
	};	};

	typedef boost::shared_ptr<KinBodyStateSaver> KinBodyStateSaverPtr;	typedef boost::shared_ptr<KinBodyStateSaver> KinBodyStateSaverPtr;

	virtual ~KinBody();	virtual ~KinBody();

	/// return the static interface type this class points to (used for saf e casting)	/// return the static interface type this class points to (used for saf e casting)
	static inline InterfaceType GetInterfaceTypeStatic() {	static inline InterfaceType GetInterfaceTypeStatic() {
	return PT_KinBody;	return PT_KinBody;
	}	}

	virtual void Destroy();	virtual void Destroy();


	/// \deprecated (11/02/18) \see EnvironmentBase::ReadKinBodyXMLFile
	virtual bool InitFromFile(const std::string& filename, const Attributes
	List& atts = AttributesList()) RAVE_DEPRECATED;
	/// \deprecated (11/02/18) \see EnvironmentBase::ReadKinBodyXMLData
	virtual bool InitFromData(const std::string& data, const AttributesList
	& atts = AttributesList()) RAVE_DEPRECATED;

	/// \brief Create a kinbody with one link composed of an array of align ed bounding boxes.	/// \brief Create a kinbody with one link composed of an array of align ed bounding boxes.
	///	///
	/// \param boxes the array of aligned bounding boxes that will comprise of the body	/// \param boxes the array of aligned bounding boxes that will comprise of the body
	/// \param visible if true, the boxes will be rendered in the scene	/// \param visible if true, the boxes will be rendered in the scene
	virtual bool InitFromBoxes(const std::vector<AABB>& boxes, bool visible );	virtual bool InitFromBoxes(const std::vector<AABB>& boxes, bool visible );

	/// \brief Create a kinbody with one link composed of an array of orien ted bounding boxes.	/// \brief Create a kinbody with one link composed of an array of orien ted bounding boxes.
	///	///
	/// \param boxes the array of oriented bounding boxes that will compris e of the body	/// \param boxes the array of oriented bounding boxes that will compris e of the body
	/// \param visible if true, the boxes will be rendered in the scene	/// \param visible if true, the boxes will be rendered in the scene

	skipping to change at line 975	skipping to change at line 1097
	virtual bool InitFromSpheres(const std::vector<Vector>& spheres, bool v isible);	virtual bool InitFromSpheres(const std::vector<Vector>& spheres, bool v isible);

	/// \brief Create a kinbody with one link composed of a triangle mesh s urface	/// \brief Create a kinbody with one link composed of a triangle mesh s urface
	///	///
	/// \param trimesh the triangle mesh	/// \param trimesh the triangle mesh
	/// \param visible if true, will be rendered in the scene	/// \param visible if true, will be rendered in the scene
	virtual bool InitFromTrimesh(const Link::TRIMESH& trimesh, bool visible );	virtual bool InitFromTrimesh(const Link::TRIMESH& trimesh, bool visible );

	/// \brief Create a kinbody with one link composed of a list of geometr ies	/// \brief Create a kinbody with one link composed of a list of geometr ies
	///	///

	/// \param geometries In order to save memory, the geometries in this l ist are transferred to the link. After function completes, the size should be 0.	/// \param geometries a list of geometry infos to be initialized into n ew geometry objects, note that the geometry info data is copied
	/// \param visible if true, will be rendered in the scene	/// \param visible if true, will be rendered in the scene

	bool InitFromGeometries(std::list<KinBody::Link::GEOMPROPERTIES>& geome tries, bool visible);	bool InitFromGeometries(const std::list<KinBody::Link::GeometryInfo>& g eometries);

	/// \brief Unique name of the robot.	/// \brief Unique name of the robot.
	virtual const std::string& GetName() const {	virtual const std::string& GetName() const {
	return _name;	return _name;
	}	}

	/// \brief Set the name of the robot, notifies the environment and chec ks for uniqueness.	/// \brief Set the name of the robot, notifies the environment and chec ks for uniqueness.
	virtual void SetName(const std::string& name);	virtual void SetName(const std::string& name);

	/// Methods for accessing basic information about joints	/// Methods for accessing basic information about joints
	/// @name Basic Information	/// @name Basic Information
	//@{	//@{

	/// \brief Number controllable degrees of freedom of the body.	/// \brief Number controllable degrees of freedom of the body.
	///	///
	/// Only uses _vecjoints and last joint for computation, so can work be fore _ComputeInternalInformation is called.	/// Only uses _vecjoints and last joint for computation, so can work be fore _ComputeInternalInformation is called.
	virtual int GetDOF() const;	virtual int GetDOF() const;

	/// \brief Returns all the joint values as organized by the DOF indices .	/// \brief Returns all the joint values as organized by the DOF indices .

	virtual void GetDOFValues(std::vector<dReal>& v) const;	///
		/// \param dofindices the dof indices to return the values for. If empt
		y, will compute for all the dofs
		virtual void GetDOFValues(std::vector<dReal>& v, const std::vector<int>
		& dofindices = std::vector<int>()) const;

	/// \brief Returns all the joint velocities as organized by the DOF ind ices.	/// \brief Returns all the joint velocities as organized by the DOF ind ices.

	virtual void GetDOFVelocities(std::vector<dReal>& v) const;	///
		/// \param dofindices the dof indices to return the values for. If empt
		y, will compute for all the dofs
		virtual void GetDOFVelocities(std::vector<dReal>& v, const std::vector<
		int>& dofindices = std::vector<int>()) const;

	/// \brief Returns all the joint limits as organized by the DOF indices .	/// \brief Returns all the joint limits as organized by the DOF indices .

	virtual void GetDOFLimits(std::vector<dReal>& lowerlimit, std::vector<d	///
	Real>& upperlimit) const;	/// \param dofindices the dof indices to return the values for. If empt
		y, will compute for all the dofs
		virtual void GetDOFLimits(std::vector<dReal>& lowerlimit, std::vector<d
		Real>& upperlimit, const std::vector<int>& dofindices = std::vector<int>())
		const;

	/// \brief Returns all the joint velocity limits as organized by the DO F indices.	/// \brief Returns all the joint velocity limits as organized by the DO F indices.

	virtual void GetDOFVelocityLimits(std::vector<dReal>& lowerlimit, std::	///
	vector<dReal>& upperlimit) const;	/// \param dofindices the dof indices to return the values for. If empt
		y, will compute for all the dofs
		virtual void GetDOFVelocityLimits(std::vector<dReal>& lowerlimit, std::
		vector<dReal>& upperlimit, const std::vector<int>& dofindices = std::vector
		<int>()) const;

	/// \brief Returns the max velocity for each DOF	/// \brief Returns the max velocity for each DOF

	virtual void GetDOFVelocityLimits(std::vector<dReal>& maxvelocities) co	///
	nst;	/// \param dofindices the dof indices to return the values for. If empt
		y, will compute for all the dofs
		virtual void GetDOFVelocityLimits(std::vector<dReal>& maxvelocities, co
		nst std::vector<int>& dofindices = std::vector<int>()) const;

	/// \brief Returns the max acceleration for each DOF	/// \brief Returns the max acceleration for each DOF

	virtual void GetDOFAccelerationLimits(std::vector<dReal>& maxaccelerati	///
	ons) const;	/// \param dofindices the dof indices to return the values for. If empt
		y, will compute for all the dofs
		virtual void GetDOFAccelerationLimits(std::vector<dReal>& maxaccelerati
		ons, const std::vector<int>& dofindices = std::vector<int>()) const;

	/// \brief Returns the max torque for each DOF	/// \brief Returns the max torque for each DOF
	virtual void GetDOFTorqueLimits(std::vector<dReal>& maxaccelerations) c onst;	virtual void GetDOFTorqueLimits(std::vector<dReal>& maxaccelerations) c onst;

	/// \deprecated (11/05/26)	/// \deprecated (11/05/26)
	virtual void GetDOFMaxVel(std::vector<dReal>& v) const RAVE_DEPRECATED {	virtual void GetDOFMaxVel(std::vector<dReal>& v) const RAVE_DEPRECATED {
	GetDOFVelocityLimits(v);	GetDOFVelocityLimits(v);
	}	}
	virtual void GetDOFMaxAccel(std::vector<dReal>& v) const RAVE_DEPRECATE D {	virtual void GetDOFMaxAccel(std::vector<dReal>& v) const RAVE_DEPRECATE D {
	GetDOFAccelerationLimits(v);	GetDOFAccelerationLimits(v);
	}	}
	virtual void GetDOFMaxTorque(std::vector<dReal>& v) const;	virtual void GetDOFMaxTorque(std::vector<dReal>& v) const;

	virtual void GetDOFResolutions(std::vector<dReal>& v) const;
	virtual void GetDOFWeights(std::vector<dReal>& v) const;	/// \brief get the dof resolutions
		///
		/// \param dofindices the dof indices to return the values for. If empt
		y, will compute for all the dofs
		virtual void GetDOFResolutions(std::vector<dReal>& v, const std::vector
		<int>& dofindices = std::vector<int>()) const;

		/// \brief get dof weights
		///
		/// \param dofindices the dof indices to return the values for. If empt
		y, will compute for all the dofs
		virtual void GetDOFWeights(std::vector<dReal>& v, const std::vector<int
		>& dofindices = std::vector<int>()) const;

	/// \brief \see GetDOFVelocityLimits	/// \brief \see GetDOFVelocityLimits
	virtual void SetDOFVelocityLimits(const std::vector<dReal>& maxlimits);	virtual void SetDOFVelocityLimits(const std::vector<dReal>& maxlimits);

	/// \brief \see GetDOFAccelerationLimits	/// \brief \see GetDOFAccelerationLimits
	virtual void SetDOFAccelerationLimits(const std::vector<dReal>& maxlimi ts);	virtual void SetDOFAccelerationLimits(const std::vector<dReal>& maxlimi ts);

	/// \brief \see GetDOFTorqueLimits	/// \brief \see GetDOFTorqueLimits
	virtual void SetDOFTorqueLimits(const std::vector<dReal>& maxlimits);	virtual void SetDOFTorqueLimits(const std::vector<dReal>& maxlimits);


	/// \brief \see GetDOFWeights	/// \brief sets dof weights
	virtual void SetDOFWeights(const std::vector<dReal>& weights);	///
		/// \param dofindices the dof indices to set the values for. If empty,
		will use all the dofs
		virtual void SetDOFWeights(const std::vector<dReal>& weights, const std
		::vector<int>& dofindices = std::vector<int>());

		/// \brief \see GetDOFLimits
		virtual void SetDOFLimits(const std::vector<dReal>& lower, const std::v
		ector<dReal>& upper);

	/// \brief Returns the joints making up the controllable degrees of fre edom of the body.	/// \brief Returns the joints making up the controllable degrees of fre edom of the body.
	const std::vector<JointPtr>& GetJoints() const {	const std::vector<JointPtr>& GetJoints() const {
	return _vecjoints;	return _vecjoints;
	}	}

	/** \brief Returns the passive joints, order does not matter.	/** \brief Returns the passive joints, order does not matter.

	A passive joint is not directly controlled by the body's degrees of freedom so it has no	A passive joint is not directly controlled by the body's degrees of freedom so it has no
	joint index and no dof index. Passive joints allows mimic joints to be hidden from the users.	joint index and no dof index. Passive joints allows mimic joints to be hidden from the users.
	However, there are cases when passive joints are not mimic; for exa mple, suspension mechanism on vehicles.	However, there are cases when passive joints are not mimic; for exa mple, suspension mechanism on vehicles.
	*/	*/
	const std::vector<JointPtr>& GetPassiveJoints() const {	const std::vector<JointPtr>& GetPassiveJoints() const {
	return _vPassiveJoints;	return _vPassiveJoints;
	}	}


	/// \deprecated \see Link::GetRigidlyAttachedLinks (10/12/12)
	virtual void GetRigidlyAttachedLinks(int linkindex, std::vector<LinkPtr
	>& vattachedlinks) const RAVE_DEPRECATED;

	/// \brief Returns the joints in hierarchical order starting at the bas e link.	/// \brief Returns the joints in hierarchical order starting at the bas e link.
	///	///
	/// In the case of closed loops, the joints are returned in the order c losest to the root.	/// In the case of closed loops, the joints are returned in the order c losest to the root.
	/// All the joints affecting a particular joint's transformation will a lways come before the joint in the list.	/// All the joints affecting a particular joint's transformation will a lways come before the joint in the list.
	virtual const std::vector<JointPtr>& GetDependencyOrderedJoints() const ;	virtual const std::vector<JointPtr>& GetDependencyOrderedJoints() const ;

	/** \brief Return the set of unique closed loops of the kinematics hier archy.	/** \brief Return the set of unique closed loops of the kinematics hier archy.

	Each loop is a set of link indices and joint indices. For example, a loop of link indices:	Each loop is a set of link indices and joint indices. For example, a loop of link indices:
	[l_0,l_1,l_2] will consist of three joints connecting l_0 to l_1, l _1 to l_2, and l_2 to l_0.	[l_0,l_1,l_2] will consist of three joints connecting l_0 to l_1, l _1 to l_2, and l_2 to l_0.

	skipping to change at line 1111	skipping to change at line 1261

	/// \brief Returns the joint that covers the degree of freedom index.	/// \brief Returns the joint that covers the degree of freedom index.
	///	///
	/// Note that the mapping of joint structures is not the same as the va lues in GetJointValues since each joint can have more than one degree of fr eedom.	/// Note that the mapping of joint structures is not the same as the va lues in GetJointValues since each joint can have more than one degree of fr eedom.
	virtual JointPtr GetJointFromDOFIndex(int dofindex) const;	virtual JointPtr GetJointFromDOFIndex(int dofindex) const;
	//@}	//@}

	/// \brief Computes the configuration difference values1-values2 and st ores it in values1.	/// \brief Computes the configuration difference values1-values2 and st ores it in values1.
	///	///
	/// Takes into account joint limits and wrapping of circular joints.	/// Takes into account joint limits and wrapping of circular joints.

	virtual void SubtractDOFValues(std::vector<dReal>& values1, const std::	/// \param[inout] values1 the result is stored back in this
	vector<dReal>& values2) const;	/// \param[in] values2
		/// \param dofindices the dof indices to compute the subtraction for. I
	/// \deprecated (01/01/11)	f empty, will compute for all the dofs
	virtual void SubtractJointValues(std::vector<dReal>& q1, const std::vec	virtual void SubtractDOFValues(std::vector<dReal>& values1, const std::
	tor<dReal>& q2) const RAVE_DEPRECATED {	vector<dReal>& values2, const std::vector<int>& dofindices=std::vector<int>
	SubtractDOFValues(q1,q2);	()) const;
	}

	/// \brief Adds a torque to every joint.	/// \brief Adds a torque to every joint.
	///	///
	/// \param bAdd if true, adds to previous torques, otherwise resets the torques on all bodies and starts from 0	/// \param bAdd if true, adds to previous torques, otherwise resets the torques on all bodies and starts from 0
	virtual void SetDOFTorques(const std::vector<dReal>& torques, bool add) ;	virtual void SetDOFTorques(const std::vector<dReal>& torques, bool add) ;


	/// \deprecated (11/06/17)
	virtual void SetJointTorques(const std::vector<dReal>& torques, bool ad
	d) RAVE_DEPRECATED {
	SetDOFTorques(torques,add);
	}

	/// \brief Returns all the rigid links of the body.	/// \brief Returns all the rigid links of the body.
	virtual const std::vector<LinkPtr>& GetLinks() const {	virtual const std::vector<LinkPtr>& GetLinks() const {
	return _veclinks;	return _veclinks;
	}	}

	/// return a pointer to the link with the given name	/// return a pointer to the link with the given name
	virtual LinkPtr GetLink(const std::string& name) const;	virtual LinkPtr GetLink(const std::string& name) const;

	/// Updates the bounding box and any other parameters that could have c hanged by a simulation step	/// Updates the bounding box and any other parameters that could have c hanged by a simulation step
	virtual void SimulationStep(dReal fElapsedTime);	virtual void SimulationStep(dReal fElapsedTime);

	skipping to change at line 1167	skipping to change at line 1310
	/// \param angularvel is the rotation axis * angular speed	/// \param angularvel is the rotation axis * angular speed
	virtual bool SetVelocity(const Vector& linearvel, const Vector& angular vel);	virtual bool SetVelocity(const Vector& linearvel, const Vector& angular vel);

	/** \brief Sets the velocity of the base link and each of the joints.	/** \brief Sets the velocity of the base link and each of the joints.

	Computes internally what the correponding velocities of each of the links should be in order to	Computes internally what the correponding velocities of each of the links should be in order to
	achieve consistent results with the joint velocities. Sends the vel ocities to the physics engine.	achieve consistent results with the joint velocities. Sends the vel ocities to the physics engine.
	Velocities correspond to the link's coordinate system origin.	Velocities correspond to the link's coordinate system origin.
	\param[in] linearvel linear velocity of base link	\param[in] linearvel linear velocity of base link
	\param[in] angularvel angular velocity rotation_axis*theta_dot	\param[in] angularvel angular velocity rotation_axis*theta_dot

	\param[in] vDOFVelocities - velocities of each of the degrees of fr	\param[in] dofvelocities - velocities of each of the degrees of fre
	eeom	eom
	\param checklimits if true, will excplicitly check the joint veloci	\param[in] checklimits one of \ref CheckLimitsAction and will excpl
	ty limits before setting the values.	icitly check the joint velocity limits before setting the values and clamp
		them.
	*/	*/

	virtual void SetDOFVelocities(const std::vector<dReal>& vDOFVelocities, const Vector& linearvel, const Vector& angularvel,bool checklimits = true) ;	virtual void SetDOFVelocities(const std::vector<dReal>& dofvelocities, const Vector& linearvel, const Vector& angularvel,uint32_t checklimits = CL A_CheckLimits);

	/// \brief Sets the velocity of the joints.	/// \brief Sets the velocity of the joints.
	///	///
	/// Copies the current velocity of the base link and calls SetDOFVeloci ties(linearvel,angularvel,vDOFVelocities)	/// Copies the current velocity of the base link and calls SetDOFVeloci ties(linearvel,angularvel,vDOFVelocities)

	/// \param[in] vDOFVelocity - velocities of each of the degrees of free	/// \param[in] dofvelocities - velocities of each of the degrees of fre
	om	eom
	/// \praam checklimits if true, will excplicitly check the joint veloci	/// \param[in] checklimits if >0, will excplicitly check the joint velo
	ty limits before setting the values.	city limits before setting the values and clamp them. If == 1, then will wa
	virtual void SetDOFVelocities(const std::vector<dReal>& vDOFVelocities,	rn if the limits are overboard, if == 2, then will not warn (used for code
	bool checklimits = true);	that knows it's giving bad values)
		virtual void SetDOFVelocities(const std::vector<dReal>& dofvelocities,
		uint32_t checklimits = CLA_CheckLimits);

	/// \brief Returns the linear and angular velocities for each link	/// \brief Returns the linear and angular velocities for each link

		///
		/// \param[out] velocities The velocities of the link frames with respe
		ct to the world coordinate system are returned.
	virtual void GetLinkVelocities(std::vector<std::pair<Vector,Vector> >& velocities) const;	virtual void GetLinkVelocities(std::vector<std::pair<Vector,Vector> >& velocities) const;


		/** \brief Returns the linear and angular accelerations for each link g
		iven the dof accelerations

		Computes accelerations of the link frames with respect to the world
		coordinate system are returned.
		The base angular velocity is used when computing accelerations.
		The gravity vector from the physics engine is used as the accelera
		tions for the base link and static links.
		The derivate is taken with respect to the world origin fixed in spa
		ce (also known as spatial acceleration).
		The current angles and velocities set on the robot are used.
		Note that this function calls the internal _ComputeLinkAcceleration
		s function, so for users that are interested in overriding it, override _Co
		mputeLinkAccelerations
		\param[in] dofaccelerations the accelerations of each of the DOF
		\param[out] linkaccelerations the linear and angular accelerations
		of link (in that order)
		*/
		virtual void GetLinkAccelerations(const std::vector<dReal>& dofaccelera
		tions, std::vector<std::pair<Vector,Vector> >& linkaccelerations) const;

	/** \en \brief set the transform of the first link (the rest of the lin ks are computed based on the joint values).	/** \en \brief set the transform of the first link (the rest of the lin ks are computed based on the joint values).

	\param transform affine transformation	\param transform affine transformation

	\ja \brief èƒ´ä½“ã®çµ¶å¯¾å§¿å‹¢ã‚’è¨å®šã€æ®‹ã‚Šã®ãƒªãƒ³ã‚¯ã¯é ‹å‹•å¦ã®æ§‹é€ ã«å¾“ã£ã¦å¤‰æ›ã•ã‚Œã‚‹ï¼Ž	\ja \brief èƒ´ä½“ã®çµ¶å¯¾å§¿å‹¢ã‚’è¨å®šã€æ®‹ã‚Šã®ãƒªãƒ³ã‚¯ã¯é ‹å‹•å¦ã®æ§‹é€ ã«å¾“ã£ã¦å¤‰æ›ã•ã‚Œã‚‹ï¼Ž

	\param transform å¤‰æ›è¡Œåˆ—	\param transform å¤‰æ›è¡Œåˆ—
	*/	*/
	virtual void SetTransform(const Transform& transform);	virtual void SetTransform(const Transform& transform);

	/// \brief Return an axis-aligned bounding box of the entire object in the world coordinate system.	/// \brief Return an axis-aligned bounding box of the entire object in the world coordinate system.
	virtual AABB ComputeAABB() const;	virtual AABB ComputeAABB() const;

	/// \brief Return the center of mass of entire robot in the world coord inate system.	/// \brief Return the center of mass of entire robot in the world coord inate system.
	virtual Vector GetCenterOfMass() const;	virtual Vector GetCenterOfMass() const;

	/// \brief Enables or disables all the links.	/// \brief Enables or disables all the links.
	virtual void Enable(bool enable);	virtual void Enable(bool enable);


	/// \deprecated (10/09/08)
	virtual void EnableLink(LinkPtr plink, bool bEnable) RAVE_DEPRECATED {
	plink->Enable(bEnable);
	}

	/// \return true if any link of the KinBody is enabled	/// \return true if any link of the KinBody is enabled
	virtual bool IsEnabled() const;	virtual bool IsEnabled() const;

	/// \brief Sets all the links as visible or not visible.	/// \brief Sets all the links as visible or not visible.
	///	///
	/// \return true if changed	/// \return true if changed
	virtual bool SetVisible(bool visible);	virtual bool SetVisible(bool visible);

	/// \return true if any link of the KinBody is visible.	/// \return true if any link of the KinBody is visible.
	virtual bool IsVisible() const;	virtual bool IsVisible() const;

	/// \brief Sets the joint values of the robot.	/// \brief Sets the joint values of the robot.
	///	///
	/// \param values the values to set the joint angles (ordered by the do f indices)	/// \param values the values to set the joint angles (ordered by the do f indices)

	/// \praam checklimits if true, will excplicitly check the joint limits	/// \param[in] checklimits one of \ref CheckLimitsAction and will excpl
	before setting the values.	icitly check the joint limits before setting the values and clamp them.
	virtual void SetDOFValues(const std::vector<dReal>& values, bool checkl	/// \param dofindices the dof indices to return the values for. If empt
	imits = true);	y, will compute for all the dofs
		virtual void SetDOFValues(const std::vector<dReal>& values, uint32_t ch
		ecklimits = CLA_CheckLimits, const std::vector<int>& dofindices = std::vect
		or<int>());

	virtual void SetJointValues(const std::vector<dReal>& values, bool chec klimits = true) {	virtual void SetJointValues(const std::vector<dReal>& values, bool chec klimits = true) {

	SetDOFValues(values,checklimits);	SetDOFValues(values,static_cast<uint32_t>(checklimits));
	}	}

	/// \brief Sets the joint values and transformation of the body.	/// \brief Sets the joint values and transformation of the body.
	///	///
	/// \param values the values to set the joint angles (ordered by the do f indices)	/// \param values the values to set the joint angles (ordered by the do f indices)
	/// \param transform represents the transformation of the first body.	/// \param transform represents the transformation of the first body.

	/// \praam checklimits if true, will excplicitly check the joint limits	/// \param[in] checklimits one of \ref CheckLimitsAction and will excpl
	before setting the values.	icitly check the joint limits before setting the values and clamp them.
	virtual void SetDOFValues(const std::vector<dReal>& values, const Trans	virtual void SetDOFValues(const std::vector<dReal>& values, const Trans
	form& transform, bool checklimits = true);	form& transform, uint32_t checklimits = CLA_CheckLimits);

	virtual void SetJointValues(const std::vector<dReal>& values, const Tra nsform& transform, bool checklimits = true)	virtual void SetJointValues(const std::vector<dReal>& values, const Tra nsform& transform, bool checklimits = true)
	{	{

	SetDOFValues(values,transform,checklimits);	SetDOFValues(values,transform,static_cast<uint32_t>(checklimits));
	}	}

	/// \brief sets the transformations of all the links at once	/// \brief sets the transformations of all the links at once
	virtual void SetLinkTransformations(const std::vector<Transform>& trans forms);	virtual void SetLinkTransformations(const std::vector<Transform>& trans forms);

	/// \brief sets the transformations of all the links and dof branches a t once.	/// \brief sets the transformations of all the links and dof branches a t once.
	///	///
	/// Using dof branches allows the full joint state to be recovered	/// Using dof branches allows the full joint state to be recovered
	virtual void SetLinkTransformations(const std::vector<Transform>& trans forms, const std::vector<int>& dofbranches);	virtual void SetLinkTransformations(const std::vector<Transform>& trans forms, const std::vector<int>& dofbranches);


	skipping to change at line 1259	skipping to change at line 1413
	SetLinkTransformations(transforms);	SetLinkTransformations(transforms);
	}	}

	/// \brief sets the link velocities	/// \brief sets the link velocities
	virtual void SetLinkVelocities(const std::vector<std::pair<Vector,Vecto r> >& velocities);	virtual void SetLinkVelocities(const std::vector<std::pair<Vector,Vecto r> >& velocities);

	/// \brief Computes the translation jacobian with respect to a world po sition.	/// \brief Computes the translation jacobian with respect to a world po sition.
	///	///
	/// Gets the jacobian with respect to a link by computing the partial d ifferentials for all joints that in the path from the root node to GetLinks ()[index]	/// Gets the jacobian with respect to a link by computing the partial d ifferentials for all joints that in the path from the root node to GetLinks ()[index]
	/// (doesn't touch the rest of the values)	/// (doesn't touch the rest of the values)

	/// \param linkindex of the link that the rotation is attached to	/// \param linkindex of the link that defines the frame the position is attached to
	/// \param position position in world space where to compute derivative s from.	/// \param position position in world space where to compute derivative s from.

	/// \param vjacobian 3xDOF matrix	/// \param jacobian 3xDOF matrix
	virtual void CalculateJacobian(int linkindex, const Vector& offset, boo	/// \param dofindices the dof indices to compute the jacobian for. If e
	st::multi_array<dReal,2>& vjacobian) const;	mpty, will compute for all the dofs
	virtual void CalculateJacobian(int linkindex, const Vector& offset, std	virtual void ComputeJacobianTranslation(int linkindex, const Vector& po
	::vector<dReal>& pfJacobian) const;	sition, std::vector<dReal>& jacobian, const std::vector<int>& dofindices=st
		d::vector<int>()) const;

		/// \brief calls std::vector version of ComputeJacobian internally
		virtual void CalculateJacobian(int linkindex, const Vector& position, s
		td::vector<dReal>& jacobian) const {
		ComputeJacobianTranslation(linkindex,position,jacobian);
		}

		/// \brief calls std::vector version of ComputeJacobian internally, a l
		ittle inefficient since it copies memory
		virtual void CalculateJacobian(int linkindex, const Vector& position, b
		oost::multi_array<dReal,2>& jacobian) const;

	/// \brief Computes the rotational jacobian as a quaternion with respec t to an initial rotation.	/// \brief Computes the rotational jacobian as a quaternion with respec t to an initial rotation.
	///	///
	/// \param linkindex of the link that the rotation is attached to	/// \param linkindex of the link that the rotation is attached to
	/// \param qInitialRot the rotation in world space whose derivative to take from.	/// \param qInitialRot the rotation in world space whose derivative to take from.

	/// \param vjacobian 4xDOF matrix	/// \param jacobian 4xDOF matrix
	virtual void CalculateRotationJacobian(int linkindex, const Vector& qua	virtual void CalculateRotationJacobian(int linkindex, const Vector& qua
	t, boost::multi_array<dReal,2>& vjacobian) const;	t, std::vector<dReal>& jacobian) const;
	virtual void CalculateRotationJacobian(int linkindex, const Vector& qua
	t, std::vector<dReal>& pfJacobian) const;	/// \brief calls std::vector version of CalculateRotationJacobian inter
		nally, a little inefficient since it copies memory
		virtual void CalculateRotationJacobian(int linkindex, const Vector& qua
		t, boost::multi_array<dReal,2>& jacobian) const;

	/// \brief Computes the angular velocity jacobian of a specified link a bout the axes of world coordinates.	/// \brief Computes the angular velocity jacobian of a specified link a bout the axes of world coordinates.
	///	///
	/// \param linkindex of the link that the rotation is attached to	/// \param linkindex of the link that the rotation is attached to
	/// \param vjacobian 3xDOF matrix	/// \param vjacobian 3xDOF matrix

	virtual void CalculateAngularVelocityJacobian(int linkindex, boost::mul	virtual void ComputeJacobianAxisAngle(int linkindex, std::vector<dReal>
	ti_array<dReal,2>& vjacobian) const;	& jacobian, const std::vector<int>& dofindices=std::vector<int>()) const;
	virtual void CalculateAngularVelocityJacobian(int linkindex, std::vecto
	r<dReal>& pfJacobian) const;	/// \brief Computes the angular velocity jacobian of a specified link a
		bout the axes of world coordinates.
		virtual void CalculateAngularVelocityJacobian(int linkindex, std::vecto
		r<dReal>& jacobian) const {
		ComputeJacobianAxisAngle(linkindex,jacobian);
		}

		/// \brief calls std::vector version of CalculateAngularVelocityJacobia
		n internally, a little inefficient since it copies memory
		virtual void CalculateAngularVelocityJacobian(int linkindex, boost::mul
		ti_array<dReal,2>& jacobian) const;

		/** \brief Computes the DOFx3xDOF hessian of the linear translation

		Arjang Hourtash. "The Kinematic Hessian and Higher Derivatives", IE
		EE Symposium on Computational Intelligence in Robotics and Automation (CIRA
		), 2005.

		Can be used to find the world position acceleration
		\code
		accel = Jacobian * dofaccelerations + dofvelocities^T * Hessian * d
		ofvelocities
		\endcode

		It can also be used for a second-order approximation of the positio
		n given delta dof values
		\code
		newposition = position + Jacobian * delta + 0.5 * delta^T * Hessian
		* delta
		\endcode

		H[i,j.k] = hessian[k+DOF(j+3i)]
		delta[j] = sum_i sum_k values[i] * H[i,j,k] * values[k]

		/// \param linkindex of the link that defines the frame the positio
		n is attached to
		/// \param position position in world space where to compute deriva
		tives from.
		/// \param hessian DOFx3xDOF matrix such that numpy.dot(dq,numpy.do
		t(hessian,dq)) is the expected second-order delta translation
		/// \param dofindices the dof indices to compute the hessian for. I
		f empty, will compute for all the dofs
		*/
		virtual void ComputeHessianTranslation(int linkindex, const Vector& pos
		ition, std::vector<dReal>& hessian, const std::vector<int>& dofindices=std:
		:vector<int>()) const;

		/** \brief Computes the DOFx3xDOF hessian of the rotation represented a
		s angle-axis

		Arjang Hourtash. "The Kinematic Hessian and Higher Derivatives", IE
		EE Symposium on Computational Intelligence in Robotics and Automation (CIRA
		), 2005.

		Can be used to find the world axis-angle acceleration
		\code
		accel = Jacobian * dofaccelerations + dofvelocities^T * Hessian * d
		ofvelocities
		\endcode

		It can also be used for a second-order approximation of the axis-an
		gle given delta dof values
		\code
		newaxisangle = axisangle + Jacobian * delta + 0.5 * delta^T * Hessi
		an * delta
		\endcode

		H[i,j.k] = hessian[k+DOF(j+3i)]
		delta[j] = sum_i sum_k values[i] * H[i,j,k] * values[k]

		/// \param linkindex of the link that defines the frame the positio
		n is attached to
		/// \param hessian DOFx3xDOF matrix such that numpy.dot(dq,numpy.do
		t(hessian,dq)) is the expected second-order delta angle-axis
		/// \param dofindices the dof indices to compute the hessian for. I
		f empty, will compute for all the dofs
		*/
		virtual void ComputeHessianAxisAngle(int linkindex, std::vector<dReal>&
		hessian, const std::vector<int>& dofindices=std::vector<int>()) const;

		/// \brief link index and the linear forces and torques. Value.first is
		linear force acting on the link's COM and Value.second is torque
		typedef std::map<int, std::pair<Vector,Vector> > ForceTorqueMap;

		/** \brief Computes the inverse dynamics (torques) from the current rob
		ot position, velocity, and acceleration.

		The dof values are ready from GetDOFValues() and GetDOFVelocities()
		. Because openrave does not have a state for robot acceleration,
		it has to be inserted as a parameter to this function. Acceleration
		due to gravitation is extracted from GetEnv()->GetPhysicsEngine()->GetGrav
		ity().
		The method uses Recursive Newton Euler algorithm from Walker Orin
		and Corke.
		\param[out] doftorques The output torques.
		\param[in] dofaccelerations The dof accelerations of the current ro
		bot state. If the size is 0, assumes all accelerations are 0 (this should b
		e faster)
		\param[in] externalforcetorque [optional] Specifies all the externa
		l forces/torques acting on the links at their center of mass.
		*/
		virtual void ComputeInverseDynamics(std::vector<dReal>& doftorques, con
		st std::vector<dReal>& dofaccelerations, const ForceTorqueMap& externalforc
		etorque=ForceTorqueMap()) const;

		/** \brief Computes the separated inverse dynamics torque terms from th
		e current robot position, velocity, and acceleration.

		torques = M(dofvalues) * dofaccel + C(dofvalues,dofvel) * dofvel +
		G(dofvalues)

		Where
		torques - generalized forces associated with dofvalues
		M - manipulator inertia tensor (symmetric joint-space inertia)
		C - coriolis and centripetal effects
		G - gravity loading + external forces due to externalforcetorque +
		base link angular acceleration contribution

		The dof values are ready from GetDOFValues() and GetDOFVelocities()
		. Because openrave does not have a state for robot acceleration,
		it has to be inserted as a parameter to this function. Acceleration
		due to gravitation is extracted from GetEnv()->GetPhysicsEngine()->GetGrav
		ity().
		The method uses Recursive Newton Euler algorithm from Walker Orin
		and Corke.
		\param[out] doftorquecomponents A set of 3 torques [M(dofvalues) *
		dofaccel, C(dofvalues,dofvel) * dofvel, G(dofvalues)]
		\param[in] dofaccelerations The dof accelerations of the current ro
		bot state. If the size is 0, assumes all accelerations are 0 (this should b
		e faster)
		\param[in] externalforcetorque [optional] Specifies all the externa
		l forces/torques acting on the links at their center of mass.
		*/
		virtual void ComputeInverseDynamics(boost::array< std::vector<dReal>, 3
		>& doftorquecomponents, const std::vector<dReal>& dofaccelerations, const F
		orceTorqueMap& externalforcetorque=ForceTorqueMap()) const;

	/// \brief Check if body is self colliding. Links that are joined toget her are ignored.	/// \brief Check if body is self colliding. Links that are joined toget her are ignored.
	virtual bool CheckSelfCollision(CollisionReportPtr report = CollisionRe portPtr()) const;	virtual bool CheckSelfCollision(CollisionReportPtr report = CollisionRe portPtr()) const;

	/// \return true if two bodies should be considered as one during colli sion (ie one is grabbing the other)	/// \return true if two bodies should be considered as one during colli sion (ie one is grabbing the other)
	virtual bool IsAttached(KinBodyConstPtr body) const;	virtual bool IsAttached(KinBodyConstPtr body) const;

	/// \brief Recursively get all attached bodies of this body, including this body.	/// \brief Recursively get all attached bodies of this body, including this body.
	///	///
	/// \param setAttached fills with the attached bodies. If any bodies ar e already in setAttached, then ignores recursing on their attached bodies.	/// \param setAttached fills with the attached bodies. If any bodies ar e already in setAttached, then ignores recursing on their attached bodies.

	skipping to change at line 1367	skipping to change at line 1618

	/// \brief Register a callback with the interface.	/// \brief Register a callback with the interface.
	///	///
	/// Everytime a static property of the interface changes, all	/// Everytime a static property of the interface changes, all
	/// registered callbacks are called to update the users of the changes. Note that the callbacks will	/// registered callbacks are called to update the users of the changes. Note that the callbacks will
	/// block the thread that made the parameter change.	/// block the thread that made the parameter change.
	/// \param callback	/// \param callback
	/// \param properties a mask of the \ref KinBodyProperty values that th e callback should be called for when they change	/// \param properties a mask of the \ref KinBodyProperty values that th e callback should be called for when they change
	virtual UserDataPtr RegisterChangeCallback(int properties, const boost: :function<void()>& callback);	virtual UserDataPtr RegisterChangeCallback(int properties, const boost: :function<void()>& callback);


	virtual void serialize(std::ostream& o, int options) const;	void Serialize(BaseXMLWriterPtr writer, int options=0) const;

	/// \brief A md5 hash unique to the particular kinematic and geometric structure of a KinBody.	/// \brief A md5 hash unique to the particular kinematic and geometric structure of a KinBody.
	///	///
	/// This 32 byte string can be used to check if two bodies have the sam e kinematic structure and can be used	/// This 32 byte string can be used to check if two bodies have the sam e kinematic structure and can be used
	/// to index into tables when looking for body-specific models. OpenRAV E stores all	/// to index into tables when looking for body-specific models. OpenRAV E stores all
	/// such models in the OPENRAVE_HOME directory (usually ~/.openrave), i ndexed by the particular robot/body hashes.	/// such models in the OPENRAVE_HOME directory (usually ~/.openrave), i ndexed by the particular robot/body hashes.
	/// \return md5 hash string of kinematics/geometry	/// \return md5 hash string of kinematics/geometry
	virtual const std::string& GetKinematicsGeometryHash() const;	virtual const std::string& GetKinematicsGeometryHash() const;


	/// \deprecated (10/11/18)
	virtual void SetJointVelocities(const std::vector<dReal>& pJointVelocit
	ies) RAVE_DEPRECATED {
	SetDOFVelocities(pJointVelocities);
	}

	/// \deprecated (10/11/18)
	virtual void GetVelocity(Vector& linearvel, Vector& angularvel) const R
	AVE_DEPRECATED;

	/// \brief Sets the joint offsets so that the current configuration bec omes the new zero state of the robot.	/// \brief Sets the joint offsets so that the current configuration bec omes the new zero state of the robot.
	///	///
	/// When this function returns, the returned DOF values should be all z ero for controllable joints.	/// When this function returns, the returned DOF values should be all z ero for controllable joints.
	/// Mimic equations will use the new offsetted values when computing th eir joints.	/// Mimic equations will use the new offsetted values when computing th eir joints.
	/// This is primarily used for calibrating a robot's zero position	/// This is primarily used for calibrating a robot's zero position
	virtual void SetZeroConfiguration();	virtual void SetZeroConfiguration();


		/// \brief Treats the current pose as a pose not in collision, which se
		ts the adjacent pairs of links
		virtual void SetNonCollidingConfiguration();

	/// Functions dealing with configuration specifications	/// Functions dealing with configuration specifications
	/// @name Configuration Specification API	/// @name Configuration Specification API
	//@{	//@{

	/// \brief return the configuration specification of the joint values a nd transform	/// \brief return the configuration specification of the joint values a nd transform

	virtual const ConfigurationSpecification& GetConfigurationSpecification	///
	() const;	/// Note that the return type is by-value, so should not be used in ite
		ration
		virtual ConfigurationSpecification GetConfigurationSpecification(const
		std::string& interpolation="") const;

	/// \brief return the configuration specification of the specified join t indices.	/// \brief return the configuration specification of the specified join t indices.
	///	///
	/// Note that the return type is by-value, so should not be used in ite ration	/// Note that the return type is by-value, so should not be used in ite ration

	virtual ConfigurationSpecification GetConfigurationSpecificationIndices (const std::vector<int>& indices) const;	virtual ConfigurationSpecification GetConfigurationSpecificationIndices (const std::vector<int>& indices, const std::string& interpolation="") cons t;

	/// \brief sets joint values and transform of the body using configurat ion values as specified by \ref GetConfigurationSpecification()	/// \brief sets joint values and transform of the body using configurat ion values as specified by \ref GetConfigurationSpecification()
	///	///
	/// \param itvalues the iterator to the vector containing the dof value s. Must have GetConfigurationSpecification().GetDOF() values!	/// \param itvalues the iterator to the vector containing the dof value s. Must have GetConfigurationSpecification().GetDOF() values!

	virtual void SetConfigurationValues(std::vector<dReal>::const_iterator	/// \param[in] checklimits one of \ref CheckLimitsAction and will excpl
	itvalues, bool checklimits = true);	icitly check the joint limits before setting the values and clamp them.
		virtual void SetConfigurationValues(std::vector<dReal>::const_iterator
		itvalues, uint32_t checklimits = CLA_CheckLimits);

	/// \brief returns the configuration values as specified by \ref GetCon figurationSpecification()	/// \brief returns the configuration values as specified by \ref GetCon figurationSpecification()
	virtual void GetConfigurationValues(std::vector<dReal>& v) const;	virtual void GetConfigurationValues(std::vector<dReal>& v) const;

	//@}	//@}


		/// only used for hashes...
		virtual void serialize(std::ostream& o, int options) const;

	protected:	protected:
	/// \brief constructors declared protected so that user always goes thr ough environment to create bodies	/// \brief constructors declared protected so that user always goes thr ough environment to create bodies
	KinBody(InterfaceType type, EnvironmentBasePtr penv);	KinBody(InterfaceType type, EnvironmentBasePtr penv);
	inline KinBodyPtr shared_kinbody() {	inline KinBodyPtr shared_kinbody() {
	return boost::static_pointer_cast<KinBody>(shared_from_this());	return boost::static_pointer_cast<KinBody>(shared_from_this());
	}	}
	inline KinBodyConstPtr shared_kinbody_const() const {	inline KinBodyConstPtr shared_kinbody_const() const {
	return boost::static_pointer_cast<KinBody const>(shared_from_this() );	return boost::static_pointer_cast<KinBody const>(shared_from_this() );
	}	}


	skipping to change at line 1448	skipping to change at line 1700
	}	}

	/** \brief Final post-processing stage before a kinematics body can be used.	/** \brief Final post-processing stage before a kinematics body can be used.

	This method is called after the body is finished being initialized with data and before being added to the environment. Also builds the hashes . Builds the internal hierarchy and kinematic body hash.	This method is called after the body is finished being initialized with data and before being added to the environment. Also builds the hashes . Builds the internal hierarchy and kinematic body hash.

	Avoids making specific calls on the collision checker (like CheckCo llision) or physics engine (like simulating velocities/torques) since this information can change depending on the attached plugin.	Avoids making specific calls on the collision checker (like CheckCo llision) or physics engine (like simulating velocities/torques) since this information can change depending on the attached plugin.
	*/	*/
	virtual void _ComputeInternalInformation();	virtual void _ComputeInternalInformation();


		/// \brief returns the dof velocities and link velocities
		///
		/// \param[in] usebaselinkvelocity if true, will compute all velocities
		using the base link velocity. otherwise will assume it is 0
		virtual void _ComputeDOFLinkVelocities(std::vector<dReal>& dofvelocitie
		s, std::vector<std::pair<Vector,Vector> >& linkvelocities, bool usebaselink
		velocity=true) const;

		/// \brief Computes accelerations of the links given all the necessary
		data of the robot. \see GetLinkAccelerations
		///
		/// for passive joints that are not mimic and are not static, will call
		Joint::GetVelocities to get their initial velocities (this is state depend
		ent!)
		/// \param dofvelocities if size is 0, will assume all velocities are 0
		/// \param dofaccelerations if size is 0, will assume all accelerations
		are 0
		virtual void _ComputeLinkAccelerations(const std::vector<dReal>& dofvel
		ocities, const std::vector<dReal>& dofaccelerations, const std::vector< std
		::pair<Vector, Vector> >& linkvelocities, std::vector<std::pair<Vector,Vect
		or> >& linkaccelerations, const Vector& gravity) const;

	/// \brief Called to notify the body that certain groups of parameters have been changed.	/// \brief Called to notify the body that certain groups of parameters have been changed.
	///	///
	/// This function in calls every registers calledback that is tracking the changes. It also	/// This function in calls every registers calledback that is tracking the changes. It also
	/// recomputes the hashes if geometry changed.	/// recomputes the hashes if geometry changed.
	virtual void _ParametersChanged(int parameters);	virtual void _ParametersChanged(int parameters);

	/// \brief Return true if two bodies should be considered as one during collision (ie one is grabbing the other)	/// \brief Return true if two bodies should be considered as one during collision (ie one is grabbing the other)
	virtual bool _IsAttached(KinBodyConstPtr body, std::set<KinBodyConstPtr >& setChecked) const;	virtual bool _IsAttached(KinBodyConstPtr body, std::set<KinBodyConstPtr >& setChecked) const;

	/// \brief adds an attached body	/// \brief adds an attached body
	virtual void _AttachBody(KinBodyPtr body);	virtual void _AttachBody(KinBodyPtr body);

	/// \brief removes an attached body	/// \brief removes an attached body
	///	///
	/// \return true if body was successfully found and removed	/// \return true if body was successfully found and removed
	virtual bool _RemoveAttachedBody(KinBodyPtr body);	virtual bool _RemoveAttachedBody(KinBodyPtr body);

	/// \brief resets cached information dependent on the collision checker (usually called when the collision checker is switched or some big mode is set.	/// \brief resets cached information dependent on the collision checker (usually called when the collision checker is switched or some big mode is set.
	virtual void _ResetInternalCollisionCache();	virtual void _ResetInternalCollisionCache();


	/// creates the function parser connected to this body's joint values
	virtual boost::shared_ptr<FunctionParserBase<dReal> > _CreateFunctionPa
	rser();

	std::string _name; ///< name of body	std::string _name; ///< name of body
	std::vector<JointPtr> _vecjoints; ///< \see GetJoints	std::vector<JointPtr> _vecjoints; ///< \see GetJoints
	std::vector<JointPtr> _vTopologicallySortedJoints; ///< \see GetDepende ncyOrderedJoints	std::vector<JointPtr> _vTopologicallySortedJoints; ///< \see GetDepende ncyOrderedJoints
	std::vector<JointPtr> _vTopologicallySortedJointsAll; ///< Similar to _ vDependencyOrderedJoints except includes _vecjoints and _vPassiveJoints	std::vector<JointPtr> _vTopologicallySortedJointsAll; ///< Similar to _ vDependencyOrderedJoints except includes _vecjoints and _vPassiveJoints
	std::vector<int> _vTopologicallySortedJointIndicesAll; ///< the joint i ndices of the joints in _vTopologicallySortedJointsAll. Passive joint indic es have _vecjoints.size() added to them.	std::vector<int> _vTopologicallySortedJointIndicesAll; ///< the joint i ndices of the joints in _vTopologicallySortedJointsAll. Passive joint indic es have _vecjoints.size() added to them.
	std::vector<JointPtr> _vDOFOrderedJoints; ///< all joints of the body o rdered on how they are arranged within the degrees of freedom	std::vector<JointPtr> _vDOFOrderedJoints; ///< all joints of the body o rdered on how they are arranged within the degrees of freedom
	std::vector<LinkPtr> _veclinks; ///< \see GetLinks	std::vector<LinkPtr> _veclinks; ///< \see GetLinks
	std::vector<int> _vDOFIndices; ///< cached start joint indices, indexed by dof indices	std::vector<int> _vDOFIndices; ///< cached start joint indices, indexed by dof indices
	std::vector<std::pair<int16_t,int16_t> > _vAllPairsShortestPaths; ///< all-pairs shortest paths through the link hierarchy. The first value descri bes the parent link index, and the second value is an index into _vecjoints or _vPassiveJoints. If the second value is greater or equal to _vecjoints .size() then it indexes into _vPassiveJoints.	std::vector<std::pair<int16_t,int16_t> > _vAllPairsShortestPaths; ///< all-pairs shortest paths through the link hierarchy. The first value descri bes the parent link index, and the second value is an index into _vecjoints or _vPassiveJoints. If the second value is greater or equal to _vecjoints .size() then it indexes into _vPassiveJoints.
	std::vector<int8_t> _vJointsAffectingLinks; ///< joint x link: (jointin dex*_veclinks.size()+linkindex). entry is non-zero if the joint affects the link in the forward kinematics. If negative, the partial derivative of ds/ dtheta should be negated.	std::vector<int8_t> _vJointsAffectingLinks; ///< joint x link: (jointin dex*_veclinks.size()+linkindex). entry is non-zero if the joint affects the link in the forward kinematics. If negative, the partial derivative of ds/ dtheta should be negated.

End of changes. 105 change blocks.
	266 lines changed or deleted	666 lines changed or added

	openrave.h	openrave.h
	// -- coding: utf-8 --	// -- coding: utf-8 --

	// Copyright (C) 2006-2011 Rosen Diankov <rosen.diankov@gmail.com>	// Copyright (C) 2006-2012 Rosen Diankov <rosen.diankov@gmail.com>
	//	//
	// This file is part of OpenRAVE.	// This file is part of OpenRAVE.
	// OpenRAVE is free software: you can redistribute it and/or modify	// OpenRAVE is free software: you can redistribute it and/or modify
	// it under the terms of the GNU Lesser General Public License as published by	// it under the terms of the GNU Lesser General Public License as published by
	// the Free Software Foundation, either version 3 of the License, or	// the Free Software Foundation, either version 3 of the License, or
	// at your option) any later version.	// at your option) any later version.
	//	//
	// This program is distributed in the hope that it will be useful,	// This program is distributed in the hope that it will be useful,
	// but WITHOUT ANY WARRANTY; without even the implied warranty of	// but WITHOUT ANY WARRANTY; without even the implied warranty of
	// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the	// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the

	skipping to change at line 150	skipping to change at line 150
	ORE_InvalidArguments=1, ///< passed in input arguments are not valid	ORE_InvalidArguments=1, ///< passed in input arguments are not valid
	ORE_EnvironmentNotLocked=2,	ORE_EnvironmentNotLocked=2,
	ORE_CommandNotSupported=3, ///< string command could not be parsed or i s not supported	ORE_CommandNotSupported=3, ///< string command could not be parsed or i s not supported
	ORE_Assert=4,	ORE_Assert=4,
	ORE_InvalidPlugin=5, ///< shared object is not a valid plugin	ORE_InvalidPlugin=5, ///< shared object is not a valid plugin
	ORE_InvalidInterfaceHash=6, ///< interface hashes do not match between plugins	ORE_InvalidInterfaceHash=6, ///< interface hashes do not match between plugins
	ORE_NotImplemented=7, ///< function is not implemented by the interface .	ORE_NotImplemented=7, ///< function is not implemented by the interface .
	ORE_InconsistentConstraints=8, ///< returned solutions or trajectories do not follow the constraints of the planner/module. The constraints invali dated here are planning constraints, not programming constraints.	ORE_InconsistentConstraints=8, ///< returned solutions or trajectories do not follow the constraints of the planner/module. The constraints invali dated here are planning constraints, not programming constraints.
	ORE_NotInitialized=9, ///< when object is used without it getting fully initialized	ORE_NotInitialized=9, ///< when object is used without it getting fully initialized
	ORE_InvalidState=10, ///< the state of the object is not consistent wit h its parameters, or cannot be used. This is usually due to a programming e rror where a vector is not the correct length, etc.	ORE_InvalidState=10, ///< the state of the object is not consistent wit h its parameters, or cannot be used. This is usually due to a programming e rror where a vector is not the correct length, etc.

		ORE_Timeout=11, ///< process timed out
	};	};

	inline const char* GetErrorCodeString(OpenRAVEErrorCode error)	inline const char* GetErrorCodeString(OpenRAVEErrorCode error)
	{	{
	switch(error) {	switch(error) {
	case ORE_Failed: return "Failed";	case ORE_Failed: return "Failed";
	case ORE_InvalidArguments: return "InvalidArguments";	case ORE_InvalidArguments: return "InvalidArguments";
	case ORE_EnvironmentNotLocked: return "EnvironmentNotLocked";	case ORE_EnvironmentNotLocked: return "EnvironmentNotLocked";
	case ORE_CommandNotSupported: return "CommandNotSupported";	case ORE_CommandNotSupported: return "CommandNotSupported";
	case ORE_Assert: return "Assert";	case ORE_Assert: return "Assert";
	case ORE_InvalidPlugin: return "InvalidPlugin";	case ORE_InvalidPlugin: return "InvalidPlugin";
	case ORE_InvalidInterfaceHash: return "InvalidInterfaceHash";	case ORE_InvalidInterfaceHash: return "InvalidInterfaceHash";
	case ORE_NotImplemented: return "NotImplemented";	case ORE_NotImplemented: return "NotImplemented";
	case ORE_InconsistentConstraints: return "InconsistentConstraints";	case ORE_InconsistentConstraints: return "InconsistentConstraints";
	case ORE_NotInitialized: return "NotInitialized";	case ORE_NotInitialized: return "NotInitialized";
	case ORE_InvalidState: return "InvalidState";	case ORE_InvalidState: return "InvalidState";

		case ORE_Timeout: return "Timeout";
	}	}
	// should throw an exception?	// should throw an exception?
	return "";	return "";
	}	}

	/// \brief Exception that all OpenRAVE internal methods throw; the error co des are held in \ref OpenRAVEErrorCode.	/// \brief Exception that all OpenRAVE internal methods throw; the error co des are held in \ref OpenRAVEErrorCode.
	class OPENRAVE_API openrave_exception : public std::exception	class OPENRAVE_API openrave_exception : public std::exception
	{	{
	public:	public:
	openrave_exception() : std::exception(), _s("unknown exception"), _erro r(ORE_Failed) {	openrave_exception() : std::exception(), _s("unknown exception"), _erro r(ORE_Failed) {

	skipping to change at line 575	skipping to change at line 577

	#define OPENRAVE_ASSERT_FORMAT(testexpr, s, args, errorcode) { if( !(testex pr) ) { throw OpenRAVE::openrave_exception(boost::str(boost::format("[%s:%d ] (%s) failed " s)%(__PRETTY_FUNCTION__)%(__LINE__)%(# testexpr)%args),erro rcode); } }	#define OPENRAVE_ASSERT_FORMAT(testexpr, s, args, errorcode) { if( !(testex pr) ) { throw OpenRAVE::openrave_exception(boost::str(boost::format("[%s:%d ] (%s) failed " s)%(__PRETTY_FUNCTION__)%(__LINE__)%(# testexpr)%args),erro rcode); } }

	#define OPENRAVE_ASSERT_FORMAT0(testexpr, s, errorcode) { if( !(testexpr) ) { throw OpenRAVE::openrave_exception(boost::str(boost::format("[%s:%d] (%s ) failed " s)%(__PRETTY_FUNCTION__)%(__LINE__)%(# testexpr)),errorcode); } }	#define OPENRAVE_ASSERT_FORMAT0(testexpr, s, errorcode) { if( !(testexpr) ) { throw OpenRAVE::openrave_exception(boost::str(boost::format("[%s:%d] (%s ) failed " s)%(__PRETTY_FUNCTION__)%(__LINE__)%(# testexpr)),errorcode); } }

	// note that expr1 and expr2 will be evaluated twice if not equal	// note that expr1 and expr2 will be evaluated twice if not equal
	#define OPENRAVE_ASSERT_OP_FORMAT(expr1,op,expr2,s, args, errorcode) { if( !((expr1) op (expr2)) ) { throw OpenRAVE::openrave_exception(boost::str(boo st::format("[%s:%d] %s %s %s, (eval %s %s %s) " s)%(__PRETTY_FUNCTION__)%(_ _LINE__)%(# expr1)%(# op)%(# expr2)%(expr1)%(# op)%(expr2)%args),errorcode) ; } }	#define OPENRAVE_ASSERT_OP_FORMAT(expr1,op,expr2,s, args, errorcode) { if( !((expr1) op (expr2)) ) { throw OpenRAVE::openrave_exception(boost::str(boo st::format("[%s:%d] %s %s %s, (eval %s %s %s) " s)%(__PRETTY_FUNCTION__)%(_ _LINE__)%(# expr1)%(# op)%(# expr2)%(expr1)%(# op)%(expr2)%args),errorcode) ; } }

	#define OPENRAVE_ASSERT_OP_FORMAT0(expr1,op,expr2,s, errorcode) { if( !((ex pr1) op (expr2)) ) { throw OpenRAVE::openrave_exception(boost::str(boost::f ormat("[%s:%d] %s %s %s, (eval %s %s %s) " s)%(__PRETTY_FUNCTION__)%(__LINE __)%(# expr1)%(# op)%(# expr2)%(expr1)%(# op)%(expr2)),errorcode); } }	#define OPENRAVE_ASSERT_OP_FORMAT0(expr1,op,expr2,s, errorcode) { if( !((ex pr1) op (expr2)) ) { throw OpenRAVE::openrave_exception(boost::str(boost::f ormat("[%s:%d] %s %s %s, (eval %s %s %s) " s)%(__PRETTY_FUNCTION__)%(__LINE __)%(# expr1)%(# op)%(# expr2)%(expr1)%(# op)%(expr2)),errorcode); } }


	#define OPENRAVE_ASSERT_OP(expr1,op,expr2) { if( !((expr1) op (expr2)) ) { throw OpenRAVE::openrave_exception(boost::str(boost::format("[%s:%d] %s!=%s , (eval %s!=%s) ")%(__PRETTY_FUNCTION__)%(__LINE__)%(# expr1)%(# op)%(# exp r2)%(expr1)%(# op)%(expr2)),ORE_Assert); } }	#define OPENRAVE_ASSERT_OP(expr1,op,expr2) { if( !((expr1) op (expr2)) ) { throw OpenRAVE::openrave_exception(boost::str(boost::format("[%s:%d] %s %s %s, (eval %s %s %s) ")%(__PRETTY_FUNCTION__)%(__LINE__)%(# expr1)%(# op)%(# expr2)%(expr1)%(# op)%(expr2)),ORE_Assert); } }

	#define OPENRAVE_DUMMY_IMPLEMENTATION { throw OPENRAVE_EXCEPTION_FORMAT0("n ot implemented",ORE_NotImplemented); }	#define OPENRAVE_DUMMY_IMPLEMENTATION { throw OPENRAVE_EXCEPTION_FORMAT0("n ot implemented",ORE_NotImplemented); }

	/// \brief Enumeration of all the interfaces.	/// \brief Enumeration of all the interfaces.
	enum InterfaceType	enum InterfaceType
	{	{
	PT_Planner=1, ///< describes \ref PlannerBase interface	PT_Planner=1, ///< describes \ref PlannerBase interface
	PT_Robot=2, ///< describes \ref RobotBase interface	PT_Robot=2, ///< describes \ref RobotBase interface
	PT_SensorSystem=3, ///< describes \ref SensorSystemBase interface	PT_SensorSystem=3, ///< describes \ref SensorSystemBase interface
	PT_Controller=4, ///< describes \ref ControllerBase interface	PT_Controller=4, ///< describes \ref ControllerBase interface

	skipping to change at line 618	skipping to change at line 620
	class EnvironmentBase;	class EnvironmentBase;
	class KinBody;	class KinBody;
	class SensorSystemBase;	class SensorSystemBase;
	class PhysicsEngineBase;	class PhysicsEngineBase;
	class SensorBase;	class SensorBase;
	class CollisionCheckerBase;	class CollisionCheckerBase;
	class ViewerBase;	class ViewerBase;
	class SpaceSamplerBase;	class SpaceSamplerBase;
	class IkParameterization;	class IkParameterization;
	class ConfigurationSpecification;	class ConfigurationSpecification;

		class IkReturn;

	typedef boost::shared_ptr<CollisionReport> CollisionReportPtr;	typedef boost::shared_ptr<CollisionReport> CollisionReportPtr;
	typedef boost::shared_ptr<CollisionReport const> CollisionReportConstPtr;	typedef boost::shared_ptr<CollisionReport const> CollisionReportConstPtr;
	typedef boost::shared_ptr<InterfaceBase> InterfaceBasePtr;	typedef boost::shared_ptr<InterfaceBase> InterfaceBasePtr;
	typedef boost::shared_ptr<InterfaceBase const> InterfaceBaseConstPtr;	typedef boost::shared_ptr<InterfaceBase const> InterfaceBaseConstPtr;
	typedef boost::weak_ptr<InterfaceBase> InterfaceBaseWeakPtr;	typedef boost::weak_ptr<InterfaceBase> InterfaceBaseWeakPtr;
	typedef boost::shared_ptr<KinBody> KinBodyPtr;	typedef boost::shared_ptr<KinBody> KinBodyPtr;
	typedef boost::shared_ptr<KinBody const> KinBodyConstPtr;	typedef boost::shared_ptr<KinBody const> KinBodyConstPtr;
	typedef boost::weak_ptr<KinBody> KinBodyWeakPtr;	typedef boost::weak_ptr<KinBody> KinBodyWeakPtr;
	typedef boost::shared_ptr<RobotBase> RobotBasePtr;	typedef boost::shared_ptr<RobotBase> RobotBasePtr;

	skipping to change at line 667	skipping to change at line 670
	typedef boost::shared_ptr<ViewerBase> ViewerBasePtr;	typedef boost::shared_ptr<ViewerBase> ViewerBasePtr;
	typedef boost::shared_ptr<ViewerBase const> ViewerBaseConstPtr;	typedef boost::shared_ptr<ViewerBase const> ViewerBaseConstPtr;
	typedef boost::weak_ptr<ViewerBase> ViewerBaseWeakPtr;	typedef boost::weak_ptr<ViewerBase> ViewerBaseWeakPtr;
	typedef boost::shared_ptr<SpaceSamplerBase> SpaceSamplerBasePtr;	typedef boost::shared_ptr<SpaceSamplerBase> SpaceSamplerBasePtr;
	typedef boost::shared_ptr<SpaceSamplerBase const> SpaceSamplerBaseConstPtr;	typedef boost::shared_ptr<SpaceSamplerBase const> SpaceSamplerBaseConstPtr;
	typedef boost::weak_ptr<SpaceSamplerBase> SpaceSamplerBaseWeakPtr;	typedef boost::weak_ptr<SpaceSamplerBase> SpaceSamplerBaseWeakPtr;
	typedef boost::shared_ptr<EnvironmentBase> EnvironmentBasePtr;	typedef boost::shared_ptr<EnvironmentBase> EnvironmentBasePtr;
	typedef boost::shared_ptr<EnvironmentBase const> EnvironmentBaseConstPtr;	typedef boost::shared_ptr<EnvironmentBase const> EnvironmentBaseConstPtr;
	typedef boost::weak_ptr<EnvironmentBase> EnvironmentBaseWeakPtr;	typedef boost::weak_ptr<EnvironmentBase> EnvironmentBaseWeakPtr;


		typedef boost::shared_ptr<IkReturn> IkReturnPtr;
		typedef boost::weak_ptr<IkReturn> IkReturnWeakPtr;

		class BaseXMLReader;
		typedef boost::shared_ptr<BaseXMLReader> BaseXMLReaderPtr;
		typedef boost::shared_ptr<BaseXMLReader const> BaseXMLReaderConstPtr;
		class BaseXMLWriter;
		typedef boost::shared_ptr<BaseXMLWriter> BaseXMLWriterPtr;
		typedef boost::shared_ptr<BaseXMLWriter const> BaseXMLWriterConstPtr;

	///< Cloning Options for interfaces and environments	///< Cloning Options for interfaces and environments
	enum CloningOptions {	enum CloningOptions {
	Clone_Bodies = 1, ///< clone all the bodies/robots of the environment, exclude attached interfaces like sensors/controllers	Clone_Bodies = 1, ///< clone all the bodies/robots of the environment, exclude attached interfaces like sensors/controllers
	Clone_Viewer = 2, ///< clone the viewer type, although figures won't be copied, new viewer does try to match views	Clone_Viewer = 2, ///< clone the viewer type, although figures won't be copied, new viewer does try to match views
	Clone_Simulation = 4, ///< clone the physics engine and simulation stat e (ie, timesteps, gravity)	Clone_Simulation = 4, ///< clone the physics engine and simulation stat e (ie, timesteps, gravity)
	Clone_RealControllers = 8, ///< if specified, will clone the real contr ollers of all the robots, otherwise each robot gets ideal controller	Clone_RealControllers = 8, ///< if specified, will clone the real contr ollers of all the robots, otherwise each robot gets ideal controller
	Clone_Sensors = 16, ///< if specified, will clone the sensors attached to the robot and added to the environment	Clone_Sensors = 16, ///< if specified, will clone the sensors attached to the robot and added to the environment
	};	};

	/// base class for readable interfaces	/// base class for readable interfaces

	class OPENRAVE_API XMLReadable	class OPENRAVE_API XMLReadable : public UserData
	{	{
	public:	public:
	XMLReadable(const std::string& xmlid) : __xmlid(xmlid) {	XMLReadable(const std::string& xmlid) : __xmlid(xmlid) {
	}	}
	virtual ~XMLReadable() {	virtual ~XMLReadable() {
	}	}
	virtual const std::string& GetXMLId() const {	virtual const std::string& GetXMLId() const {
	return __xmlid;	return __xmlid;
	}	}

		/// \brief serializes the interface
		virtual void Serialize(BaseXMLWriterPtr writer, int options=0) const {
		}
	private:	private:
	std::string __xmlid;	std::string __xmlid;
	};	};

	typedef boost::shared_ptr<XMLReadable> XMLReadablePtr;	typedef boost::shared_ptr<XMLReadable> XMLReadablePtr;
	typedef boost::shared_ptr<XMLReadable const> XMLReadableConstPtr;	typedef boost::shared_ptr<XMLReadable const> XMLReadableConstPtr;


		/// \brief a list of key-value pairs. It is possible for keys to repeat.
	typedef std::list<std::pair<std::string,std::string> > AttributesList;	typedef std::list<std::pair<std::string,std::string> > AttributesList;

	/// \deprecated (11/02/18)
	typedef AttributesList XMLAttributesList RAVE_DEPRECATED;


	/// base class for all xml readers. XMLReaders are used to process data fro	/// \brief base class for all xml readers. XMLReaders are used to process d
	m	ata from xml files.
	/// xml files. Custom readers can be registered through EnvironmentBase.	///
	/// By default it can record all data that is encountered inside the xml re	/// Custom readers can be registered through \ref RaveRegisterXMLReader.
	ader
	class OPENRAVE_API BaseXMLReader : public boost::enable_shared_from_this<Ba seXMLReader>	class OPENRAVE_API BaseXMLReader : public boost::enable_shared_from_this<Ba seXMLReader>
	{	{
	public:	public:
	enum ProcessElement	enum ProcessElement
	{	{
	PE_Pass=0, ///< current tag was not supported, so pass onto ano ther class	PE_Pass=0, ///< current tag was not supported, so pass onto ano ther class
	PE_Support=1, ///< current tag will be processed by this class	PE_Support=1, ///< current tag will be processed by this class
	PE_Ignore=2, ///< current tag and all its children should be ig nored	PE_Ignore=2, ///< current tag and all its children should be ig nored
	};	};
	BaseXMLReader() {	BaseXMLReader() {

	skipping to change at line 739	skipping to change at line 755
	virtual bool endElement(const std::string& name) = 0;	virtual bool endElement(const std::string& name) = 0;

	/// gets called for all data in between tags.	/// gets called for all data in between tags.
	/// \param ch a string to the data	/// \param ch a string to the data
	virtual void characters(const std::string& ch) = 0;	virtual void characters(const std::string& ch) = 0;

	/// XML filename/resource used for this class (can be empty)	/// XML filename/resource used for this class (can be empty)
	std::string _filename;	std::string _filename;
	};	};


	typedef boost::shared_ptr<BaseXMLReader> BaseXMLReaderPtr;
	typedef boost::shared_ptr<BaseXMLReader const> BaseXMLReaderConstPtr;

	typedef boost::function<BaseXMLReaderPtr(InterfaceBasePtr, const Attributes List&)> CreateXMLReaderFn;	typedef boost::function<BaseXMLReaderPtr(InterfaceBasePtr, const Attributes List&)> CreateXMLReaderFn;

	/// reads until the tag ends	/// reads until the tag ends
	class OPENRAVE_API DummyXMLReader : public BaseXMLReader	class OPENRAVE_API DummyXMLReader : public BaseXMLReader
	{	{
	public:	public:
	DummyXMLReader(const std::string& pfieldname, const std::string& pparen tname, boost::shared_ptr<std::ostream> osrecord = boost::shared_ptr<std::os tream>());	DummyXMLReader(const std::string& pfieldname, const std::string& pparen tname, boost::shared_ptr<std::ostream> osrecord = boost::shared_ptr<std::os tream>());
	virtual ProcessElement startElement(const std::string& name, const Attr ibutesList& atts);	virtual ProcessElement startElement(const std::string& name, const Attr ibutesList& atts);
	virtual bool endElement(const std::string& name);	virtual bool endElement(const std::string& name);
	virtual void characters(const std::string& ch);	virtual void characters(const std::string& ch);
	const std::string& GetFieldName() const {	const std::string& GetFieldName() const {
	return _fieldname;	return _fieldname;
	}	}
	private:	private:
	std::string _parentname; /// XML filename	std::string _parentname; /// XML filename
	std::string _fieldname;	std::string _fieldname;
	boost::shared_ptr<std::ostream> _osrecord; ///< used to store the x ml data	boost::shared_ptr<std::ostream> _osrecord; ///< used to store the x ml data
	boost::shared_ptr<BaseXMLReader> _pcurreader;	boost::shared_ptr<BaseXMLReader> _pcurreader;
	};	};


		/// \brief base class for writing to XML files.
		///
		/// OpenRAVE Interfaces accept a BaseXMLWriter instance and call its write
		methods to write the data.
		class OPENRAVE_API BaseXMLWriter : public boost::enable_shared_from_this<Ba
		seXMLWriter>
		{
		public:
		virtual ~BaseXMLWriter() {
		}
		/// \brief return the format for the data writing, should be all lower
		capitals.
		///
		/// Samples formats are 'openrave', 'collada'
		virtual const std::string& GetFormat() const = 0;

		/// \brief saves character data to the child. Special characters like '
		<' are automatically converted to fit inside XML.
		///
		/// \throw openrave_exception throws if this element cannot have charac
		ter data or the character data was not written
		virtual void SetCharData(const std::string& data) = 0;

		/// \brief returns a writer for child elements
		virtual BaseXMLWriterPtr AddChild(const std::string& xmltag, const Attr
		ibutesList& atts=AttributesList()) = 0;
		};

	} // end namespace OpenRAVE	} // end namespace OpenRAVE

	// define the math functions	// define the math functions

	#define MATH_EXP RaveExp	#if OPENRAVE_PRECISION // 1 if double precision
	#define MATH_LOG RaveLog	#define OPENRAVE_MATH_EXP_DOUBLE RaveExp
	#define MATH_COS RaveCos	#define OPENRAVE_MATH_LOG_DOUBLE RaveLog
	#define MATH_SIN RaveSin	#define OPENRAVE_MATH_COS_DOUBLE RaveCos
	#define MATH_TAN RaveTan	#define OPENRAVE_MATH_SIN_DOUBLE RaveSin
	#define MATH_LOG2 RaveLog2	#define OPENRAVE_MATH_TAN_DOUBLE RaveTan
	#define MATH_LOG10 RaveLog10	#define OPENRAVE_MATH_LOG2_DOUBLE RaveLog2
	#define MATH_ACOS RaveAcos	#define OPENRAVE_MATH_LOG10_DOUBLE RaveLog10
	#define MATH_ASIN RaveAsin	#define OPENRAVE_MATH_ACOS_DOUBLE RaveAcos
	#define MATH_ATAN2 RaveAtan2	#define OPENRAVE_MATH_ASIN_DOUBLE RaveAsin
	#define MATH_POW RavePow	#define OPENRAVE_MATH_ATAN2_DOUBLE RaveAtan2
	#define MATH_SQRT RaveSqrt	#define OPENRAVE_MATH_POW_DOUBLE RavePow
	#define MATH_FABS RaveFabs	#define OPENRAVE_MATH_SQRT_DOUBLE RaveSqrt
		#define OPENRAVE_MATH_FABS_DOUBLE RaveFabs
		#else // 32bit float
		#define OPENRAVE_MATH_EXP_FLOAT RaveExp
		#define OPENRAVE_MATH_LOG_FLOAT RaveLog
		#define OPENRAVE_MATH_COS_FLOAT RaveCos
		#define OPENRAVE_MATH_SIN_FLOAT RaveSin
		#define OPENRAVE_MATH_TAN_FLOAT RaveTan
		#define OPENRAVE_MATH_LOG2_FLOAT RaveLog2
		#define OPENRAVE_MATH_LOG10_FLOAT RaveLog10
		#define OPENRAVE_MATH_ACOS_FLOAT RaveAcos
		#define OPENRAVE_MATH_ASIN_FLOAT RaveAsin
		#define OPENRAVE_MATH_ATAN2_FLOAT RaveAtan2
		#define OPENRAVE_MATH_POW_FLOAT RavePow
		#define OPENRAVE_MATH_SQRT_FLOAT RaveSqrt
		#define OPENRAVE_MATH_FABS_FLOAT RaveFabs
		#endif

	#include <openrave/geometry.h>	#include <openrave/geometry.h>
	#include <openrave/mathextra.h>	#include <openrave/mathextra.h>

	namespace OpenRAVE {	namespace OpenRAVE {
	using geometry::RaveVector;	using geometry::RaveVector;
	using geometry::RaveTransform;	using geometry::RaveTransform;
	using geometry::RaveTransformMatrix;	using geometry::RaveTransformMatrix;
	typedef RaveVector<dReal> Vector;	typedef RaveVector<dReal> Vector;
	typedef RaveTransform<dReal> Transform;	typedef RaveTransform<dReal> Transform;

	skipping to change at line 841	skipping to change at line 892

	IKP_TranslationXAxisAngle4D=0x4400000b, ///< end effector origin reache s desired 3D translation, manipulator direction makes a specific angle with x-axis like a cone, angle is from 0-pi. Axes defined in the manipulator b ase link's coordinate system)	IKP_TranslationXAxisAngle4D=0x4400000b, ///< end effector origin reache s desired 3D translation, manipulator direction makes a specific angle with x-axis like a cone, angle is from 0-pi. Axes defined in the manipulator b ase link's coordinate system)
	IKP_TranslationYAxisAngle4D=0x4400000c, ///< end effector origin reache s desired 3D translation, manipulator direction makes a specific angle with y-axis like a cone, angle is from 0-pi. Axes defined in the manipulator b ase link's coordinate system)	IKP_TranslationYAxisAngle4D=0x4400000c, ///< end effector origin reache s desired 3D translation, manipulator direction makes a specific angle with y-axis like a cone, angle is from 0-pi. Axes defined in the manipulator b ase link's coordinate system)
	IKP_TranslationZAxisAngle4D=0x4400000d, ///< end effector origin reache s desired 3D translation, manipulator direction makes a specific angle with z-axis like a cone, angle is from 0-pi. Axes are defined in the manipulato r base link's coordinate system.	IKP_TranslationZAxisAngle4D=0x4400000d, ///< end effector origin reache s desired 3D translation, manipulator direction makes a specific angle with z-axis like a cone, angle is from 0-pi. Axes are defined in the manipulato r base link's coordinate system.

	IKP_TranslationXAxisAngleZNorm4D=0x4400000e, ///< end effector origin r eaches desired 3D translation, manipulator direction needs to be orthogonal to z-axis and be rotated at a certain angle starting from the x-axis (defi ned in the manipulator base link's coordinate system)	IKP_TranslationXAxisAngleZNorm4D=0x4400000e, ///< end effector origin r eaches desired 3D translation, manipulator direction needs to be orthogonal to z-axis and be rotated at a certain angle starting from the x-axis (defi ned in the manipulator base link's coordinate system)
	IKP_TranslationYAxisAngleXNorm4D=0x4400000f, ///< end effector origin r eaches desired 3D translation, manipulator direction needs to be orthogonal to x-axis and be rotated at a certain angle starting from the y-axis (defi ned in the manipulator base link's coordinate system)	IKP_TranslationYAxisAngleXNorm4D=0x4400000f, ///< end effector origin r eaches desired 3D translation, manipulator direction needs to be orthogonal to x-axis and be rotated at a certain angle starting from the y-axis (defi ned in the manipulator base link's coordinate system)
	IKP_TranslationZAxisAngleYNorm4D=0x44000010, ///< end effector origin r eaches desired 3D translation, manipulator direction needs to be orthogonal to y-axis and be rotated at a certain angle starting from the z-axis (defi ned in the manipulator base link's coordinate system)	IKP_TranslationZAxisAngleYNorm4D=0x44000010, ///< end effector origin r eaches desired 3D translation, manipulator direction needs to be orthogonal to y-axis and be rotated at a certain angle starting from the z-axis (defi ned in the manipulator base link's coordinate system)

	IKP_NumberOfParameterizations=16, ///< number of parameterizations (does not count IKP_None)	IKP_NumberOfParameterizations=16, ///< number of parameterizations (does not count IKP_None)


		IKP_VelocityDataBit = 0x00008000, ///< bit is set if the data represent
		s the time-derivate velocity of an IkParameterization
		IKP_Transform6DVelocity = IKP_Transform6D\|IKP_VelocityDataBit,
		IKP_Rotation3DVelocity = IKP_Rotation3D\|IKP_VelocityDataBit,
		IKP_Translation3DVelocity = IKP_Translation3D\|IKP_VelocityDataBit,
		IKP_Direction3DVelocity = IKP_Direction3D\|IKP_VelocityDataBit,
		IKP_Ray4DVelocity = IKP_Ray4D\|IKP_VelocityDataBit,
		IKP_Lookat3DVelocity = IKP_Lookat3D\|IKP_VelocityDataBit,
		IKP_TranslationDirection5DVelocity = IKP_TranslationDirection5D\|IKP_Vel
		ocityDataBit,
		IKP_TranslationXY2DVelocity = IKP_TranslationXY2D\|IKP_VelocityDataBit,
		IKP_TranslationXYOrientation3DVelocity = IKP_TranslationXYOrientation3D
		\|IKP_VelocityDataBit,
		IKP_TranslationLocalGlobal6DVelocity = IKP_TranslationLocalGlobal6D\|IKP
		_VelocityDataBit,
		IKP_TranslationXAxisAngle4DVelocity = IKP_TranslationXAxisAngle4D\|IKP_V
		elocityDataBit,
		IKP_TranslationYAxisAngle4DVelocity = IKP_TranslationYAxisAngle4D\|IKP_V
		elocityDataBit,
		IKP_TranslationZAxisAngle4DVelocity = IKP_TranslationZAxisAngle4D\|IKP_V
		elocityDataBit,
		IKP_TranslationXAxisAngleZNorm4DVelocity = IKP_TranslationXAxisAngleZNo
		rm4D\|IKP_VelocityDataBit,
		IKP_TranslationYAxisAngleXNorm4DVelocity = IKP_TranslationYAxisAngleXNo
		rm4D\|IKP_VelocityDataBit,
		IKP_TranslationZAxisAngleYNorm4DVelocity = IKP_TranslationZAxisAngleYNo
		rm4D\|IKP_VelocityDataBit,

		IKP_UniqueIdMask = 0x0000ffff, ///< the mask for the unique ids
		IKP_CustomDataBit = 0x00010000, ///< bit is set if the ikparameterizati
		on contains custom data, this is only used when serializing the ik paramete
		rizations
	};	};


		/// \brief returns a string of the ik parameterization type names
		///
		/// \param[in] alllowercase If 1, sets all characters to lower case. Otherw
		ise can include upper case in order to match \ref IkParameterizationType de
		finition.
		OPENRAVE_API const std::map<IkParameterizationType,std::string>& RaveGetIkP
		arameterizationMap(int alllowercase=0);

		/// \brief returns the IkParameterizationType given the unique id detmerine
		d b IKP_UniqueIdMask
		OPENRAVE_API IkParameterizationType RaveGetIkTypeFromUniqueId(int uniqueid)
		;

	/** \brief A configuration specification references values in the environme nt that then define a configuration-space which can be searched for.	/** \brief A configuration specification references values in the environme nt that then define a configuration-space which can be searched for.

	It is composed of several groups targetting values for individual bodie s. It is serialized into XML. The XML syntax is as follows:	It is composed of several groups targetting values for individual bodie s. It is serialized into XML. The XML syntax is as follows:

	\code	\code
	<configuration>	<configuration>
	<group name="string" offset="#OFF1" dof="#D1" interpolation="string"/>	<group name="string" offset="#OFF1" dof="#D1" interpolation="string"/>
	<group name="string" offset="#OFF2" dof="#D2" interpolation="string"/>	<group name="string" offset="#OFF2" dof="#D2" interpolation="string"/>
	</configuration>	</configuration>
	\endcode	\endcode

	skipping to change at line 932	skipping to change at line 1012

	virtual ~ConfigurationSpecification() {	virtual ~ConfigurationSpecification() {
	}	}

	/// \brief return the dimension of the configuraiton space (degrees of freedom)	/// \brief return the dimension of the configuraiton space (degrees of freedom)
	virtual int GetDOF() const;	virtual int GetDOF() const;

	/// \brief check if the groups form a continguous space	/// \brief check if the groups form a continguous space
	///	///
	/// If there are two or more groups with the same semantic names, will fail. Theese groups should be merged into one.	/// If there are two or more groups with the same semantic names, will fail. Theese groups should be merged into one.

		/// \return true if valid, otherwise false
	virtual bool IsValid() const;	virtual bool IsValid() const;


		/// \brief check if the groups form a continguous space
		///
		/// If there are two or more groups with the same semantic names, will
		fail. Theese groups should be merged into one.
		/// \throw openrave_exception if not valid
		virtual void Validate() const;

	virtual bool operator==(const ConfigurationSpecification& r) const;	virtual bool operator==(const ConfigurationSpecification& r) const;
	virtual bool operator!=(const ConfigurationSpecification& r) const;	virtual bool operator!=(const ConfigurationSpecification& r) const;

	/// \brief return the group whose name begins with a particular string.	/// \brief return the group whose name begins with a particular string.
	///	///
	/// If multiple groups exist that begin with the same string, then the shortest one is returned.	/// If multiple groups exist that begin with the same string, then the shortest one is returned.
	/// \throw openrave_exception if a group is not found	/// \throw openrave_exception if a group is not found
	virtual const Group& GetGroupFromName(const std::string& name) const;	virtual const Group& GetGroupFromName(const std::string& name) const;

	/// \brief return the group whose name begins with a particular string.	/// \brief return the group whose name begins with a particular string.

	skipping to change at line 981	skipping to change at line 1068

	/** \brief Return the most compatible group that represents the time-de rivative data of the group.	/** \brief Return the most compatible group that represents the time-de rivative data of the group.

	For example given a 'joint_values' group, this will return the clos est 'joint_velocities' group.	For example given a 'joint_values' group, this will return the clos est 'joint_velocities' group.
	\param name the name of the group to query	\param name the name of the group to query
	\param exactmatch if true, will only return groups whose name exact ly matches with g.name	\param exactmatch if true, will only return groups whose name exact ly matches with g.name
	\return an iterator part of _vgroups that represents the most compa tible group. If no group is found, will return _vgroups.end()	\return an iterator part of _vgroups that represents the most compa tible group. If no group is found, will return _vgroups.end()
	*/	*/
	virtual std::vector<Group>::const_iterator FindTimeDerivativeGroup(cons t std::string& name, bool exactmatch=false) const;	virtual std::vector<Group>::const_iterator FindTimeDerivativeGroup(cons t std::string& name, bool exactmatch=false) const;


	/** \brief adds a velocity group for every position group.	/** \brief adds velocity, acceleration, etc groups for every position g roup.


	If velocities groups already exist, they are checked for and/or mod ified. Note that the configuration space	If the derivative groups already exist, they are checked for and/or modified. Note that the configuration space
	might be re-ordered as a result of this function call. If a new gro up is added, its interpolation will be	might be re-ordered as a result of this function call. If a new gro up is added, its interpolation will be
	the derivative of the position group as returned by \ref GetInterpo lationDerivative.	the derivative of the position group as returned by \ref GetInterpo lationDerivative.

		\param deriv The position derivative to add, this must be greater t han 0. If 2 is specified, only the acceleration groups of the alread presen t position groups will be added.
	\param adddeltatime If true will add the 'deltatime' tag, which is necessary for trajectory sampling	\param adddeltatime If true will add the 'deltatime' tag, which is necessary for trajectory sampling
	*/	*/

	virtual void AddVelocityGroups(bool adddeltatime);	virtual void AddDerivativeGroups(int deriv, bool adddeltatime=false);

		/// \deprecated (12/07/30)
		inline void AddVelocityGroups(bool adddeltatime) RAVE_DEPRECATED {
		AddDerivativeGroups(1,adddeltatime);
		}

	/** \brief converts all the groups to the corresponding velocity groups and returns the specification	/** \brief converts all the groups to the corresponding velocity groups and returns the specification

	The velocity configuration space will have a one-to-one corresponde nce with the original configuration.	The velocity configuration space will have a one-to-one corresponde nce with the original configuration.
	The interpolation of each of the groups will correspondingly repres ent the derivative as returned by \ref GetInterpolationDerivative.	The interpolation of each of the groups will correspondingly repres ent the derivative as returned by \ref GetInterpolationDerivative.

		Only position specifications will be converted, any other groups wi ll be left untouched.
	*/	*/
	virtual ConfigurationSpecification ConvertToVelocitySpecification() con st;	virtual ConfigurationSpecification ConvertToVelocitySpecification() con st;

	/// \brief returns a new specification of just particular time-derivati ve groups.	/// \brief returns a new specification of just particular time-derivati ve groups.
	///	///
	/// \param timederivative the time derivative to query groups from. 0 i s positions/joint values, 1 is velocities, 2 is accelerations, etc	/// \param timederivative the time derivative to query groups from. 0 i s positions/joint values, 1 is velocities, 2 is accelerations, etc
	virtual ConfigurationSpecification GetTimeDerivativeSpecification(int t imederivative) const;	virtual ConfigurationSpecification GetTimeDerivativeSpecification(int t imederivative) const;

	/** \brief set the offsets of each group in order to get a contiguous c onfiguration space	/** \brief set the offsets of each group in order to get a contiguous c onfiguration space
	*/	*/

	skipping to change at line 1036	skipping to change at line 1130
	For groups that are merged, the interpolation either has to match f or both groups, or one of the groups needs an empty interpolation.	For groups that are merged, the interpolation either has to match f or both groups, or one of the groups needs an empty interpolation.
	\throw openrave_exception throws if groups do not contain enough in formation to be merged or interpolations do not match.	\throw openrave_exception throws if groups do not contain enough in formation to be merged or interpolations do not match.
	*/	*/
	virtual ConfigurationSpecification operator+ (const ConfigurationSpecif ication& r) const;	virtual ConfigurationSpecification operator+ (const ConfigurationSpecif ication& r) const;

	/** \brief extracts an affine transform given the start of a configurat ion space point	/** \brief extracts an affine transform given the start of a configurat ion space point

	Looks for 'affine_transform' groups. If pbody is not initialized, w ill choose the first affine_transform found.	Looks for 'affine_transform' groups. If pbody is not initialized, w ill choose the first affine_transform found.
	\param[inout] t the transform holding the default values, which wil l be overwritten with the new values.	\param[inout] t the transform holding the default values, which wil l be overwritten with the new values.
	\param[in] itdata data in the format of this configuration specific ation.	\param[in] itdata data in the format of this configuration specific ation.

		\param[in] timederivative the time derivative of the data to extrac t
	\return true if at least one group was found for extracting	\return true if at least one group was found for extracting
	*/	*/

	virtual bool ExtractTransform(Transform& t, std::vector<dReal>::const_i terator itdata, KinBodyConstPtr pbody) const;	virtual bool ExtractTransform(Transform& t, std::vector<dReal>::const_i terator itdata, KinBodyConstPtr pbody, int timederivative=0) const;

	/** \brief extracts an ikparameterization given the start of a configur ation space point	/** \brief extracts an ikparameterization given the start of a configur ation space point

	Looks for 'ikparam' groups.	Looks for 'ikparam' groups.
	\param[inout] ikparam filled with ikparameterization (if found)	\param[inout] ikparam filled with ikparameterization (if found)
	\param[in] itdata data in the format of this configuration specific ation	\param[in] itdata data in the format of this configuration specific ation

		\param[in] timederivative the time derivative of the data to extrac t
	\return true if at least one group was found for extracting	\return true if at least one group was found for extracting
	*/	*/
	virtual bool ExtractIkParameterization(IkParameterization& ikparam, std ::vector<dReal>::const_iterator itdata, int timederivative=0) const;	virtual bool ExtractIkParameterization(IkParameterization& ikparam, std ::vector<dReal>::const_iterator itdata, int timederivative=0) const;

	/** \brief extracts the affine values	/** \brief extracts the affine values

	Looks for 'affine_X' groups. If pbody is not initialized, will choo se the first affine_X found.	Looks for 'affine_X' groups. If pbody is not initialized, will choo se the first affine_X found.
	\param[inout] itvalues iterator to vector that holds the default va lues and will be overwritten with the new values. must be initialized	\param[inout] itvalues iterator to vector that holds the default va lues and will be overwritten with the new values. must be initialized
	\param[in] itdata data in the format of this configuration specific ation.	\param[in] itdata data in the format of this configuration specific ation.
	\param[in] affinedofs the format of the affine dofs requested	\param[in] affinedofs the format of the affine dofs requested

	skipping to change at line 1134	skipping to change at line 1230
	\param sourcespec the source configuration specification	\param sourcespec the source configuration specification
	\param numpoints the number of points to convert. The target and so urce strides are gtarget.dof and gsource.dof	\param numpoints the number of points to convert. The target and so urce strides are gtarget.dof and gsource.dof
	\param penv [optional] The environment which might be needed to fil l in unknown data. Assumes environment is locked.	\param penv [optional] The environment which might be needed to fil l in unknown data. Assumes environment is locked.
	\param filluninitialized If there exists target groups that cannot be initialized, then will set default values using the current environment. For example, the current joint values of the body will be used.	\param filluninitialized If there exists target groups that cannot be initialized, then will set default values using the current environment. For example, the current joint values of the body will be used.
	*/	*/
	static void ConvertData(std::vector<dReal>::iterator ittargetdata, cons t ConfigurationSpecification& targetspec, std::vector<dReal>::const_iterato r itsourcedata, const ConfigurationSpecification& sourcespec, size_t numpoi nts, EnvironmentBaseConstPtr penv, bool filluninitialized = true);	static void ConvertData(std::vector<dReal>::iterator ittargetdata, cons t ConfigurationSpecification& targetspec, std::vector<dReal>::const_iterato r itsourcedata, const ConfigurationSpecification& sourcespec, size_t numpoi nts, EnvironmentBaseConstPtr penv, bool filluninitialized = true);

	/// \brief gets the name of the interpolation that represents the deriv ative of the passed in interpolation.	/// \brief gets the name of the interpolation that represents the deriv ative of the passed in interpolation.
	///	///
	/// For example GetInterpolationDerivative("quadratic") -> "linear"	/// For example GetInterpolationDerivative("quadratic") -> "linear"

	static std::string GetInterpolationDerivative(const std::string& interp	/// \param interpolation the interpolation to start at
	olation);	/// \param deriv the number of derivatives to take, should be > 0
		static std::string GetInterpolationDerivative(const std::string& interp
		olation, int deriv=1);

	std::vector<Group> _vgroups;	std::vector<Group> _vgroups;
	};	};

	OPENRAVE_API std::ostream& operator<<(std::ostream& O, const ConfigurationS pecification &spec);	OPENRAVE_API std::ostream& operator<<(std::ostream& O, const ConfigurationS pecification &spec);
	OPENRAVE_API std::istream& operator>>(std::istream& I, ConfigurationSpecifi cation& spec);	OPENRAVE_API std::istream& operator>>(std::istream& I, ConfigurationSpecifi cation& spec);

	typedef boost::shared_ptr<ConfigurationSpecification> ConfigurationSpecific ationPtr;	typedef boost::shared_ptr<ConfigurationSpecification> ConfigurationSpecific ationPtr;
	typedef boost::shared_ptr<ConfigurationSpecification const> ConfigurationSp ecificationConstPtr;	typedef boost::shared_ptr<ConfigurationSpecification const> ConfigurationSp ecificationConstPtr;


	skipping to change at line 1214	skipping to change at line 1312
	case IKP_Translation3D: SetTranslation3D(t.trans); break;	case IKP_Translation3D: SetTranslation3D(t.trans); break;
	case IKP_Lookat3D: SetLookat3D(t.trans); break;	case IKP_Lookat3D: SetLookat3D(t.trans); break;
	default:	default:
	throw openrave_exception(str(boost::format("IkParameterization constructor does not support type 0x%x")%_type));	throw openrave_exception(str(boost::format("IkParameterization constructor does not support type 0x%x")%_type));
	}	}
	}	}

	inline IkParameterizationType GetType() const {	inline IkParameterizationType GetType() const {
	return _type;	return _type;
	}	}


		/// \brief returns a string version of \ref GetType
	inline const std::string& GetName() const;	inline const std::string& GetName() const;


	/// \brief Returns the minimum degree of freedoms required for the IK t ype.	/// \brief Returns the minimum degree of freedoms required for the IK t ype. Does \b not count custom data.
	static int GetDOF(IkParameterizationType type) {	static int GetDOF(IkParameterizationType type) {
	return (type>>28)&0xf;	return (type>>28)&0xf;
	}	}

	/// \brief Returns the minimum degree of freedoms required for the IK t ype.	/// \brief Returns the minimum degree of freedoms required for the IK t ype. Does \b not count custom data.
	inline int GetDOF() const {	inline int GetDOF() const {
	return (_type>>28)&0xf;	return (_type>>28)&0xf;
	}	}


	/// \brief Returns the number of values used to represent the parameter ization ( >= dof ). The number of values serialized is this number plus 1 f or the iktype.	/// \brief Returns the number of values used to represent the parameter ization ( >= dof ). Does \b not count custom data.
	static int GetNumberOfValues(IkParameterizationType type) {	static int GetNumberOfValues(IkParameterizationType type) {
	return (type>>24)&0xf;	return (type>>24)&0xf;
	}	}

	/// \brief Returns the number of values used to represent the parameter ization ( >= dof ). The number of values serialized is this number plus 1 f or the iktype.	/// \brief Returns the number of values used to represent the parameter ization ( >= dof ). Does \b not count custom data.
	inline int GetNumberOfValues() const {	inline int GetNumberOfValues() const {
	return (_type>>24)&0xf;	return (_type>>24)&0xf;
	}	}

	inline void SetTransform6D(const Transform& t) {	inline void SetTransform6D(const Transform& t) {
	_type = IKP_Transform6D; _transform = t;	_type = IKP_Transform6D; _transform = t;
	}	}
	inline void SetRotation3D(const Vector& quaternion) {	inline void SetRotation3D(const Vector& quaternion) {
	_type = IKP_Rotation3D; _transform.rot = quaternion;	_type = IKP_Rotation3D; _transform.rot = quaternion;
	}	}

	skipping to change at line 1352	skipping to change at line 1452
	inline std::pair<Vector,dReal> GetTranslationXAxisAngleZNorm4D() const {	inline std::pair<Vector,dReal> GetTranslationXAxisAngleZNorm4D() const {
	return std::make_pair(_transform.trans,_transform.rot.x);	return std::make_pair(_transform.trans,_transform.rot.x);
	}	}
	inline std::pair<Vector,dReal> GetTranslationYAxisAngleXNorm4D() const {	inline std::pair<Vector,dReal> GetTranslationYAxisAngleXNorm4D() const {
	return std::make_pair(_transform.trans,_transform.rot.x);	return std::make_pair(_transform.trans,_transform.rot.x);
	}	}
	inline std::pair<Vector,dReal> GetTranslationZAxisAngleYNorm4D() const {	inline std::pair<Vector,dReal> GetTranslationZAxisAngleYNorm4D() const {
	return std::make_pair(_transform.trans,_transform.rot.x);	return std::make_pair(_transform.trans,_transform.rot.x);
	}	}


	/// \deprecated (11/02/15)
	//@{
	inline void SetTransform(const Transform& t) RAVE_DEPRECATED {
	SetTransform6D(t);
	}
	inline void SetRotation(const Vector& quaternion) RAVE_DEPRECATED {
	SetRotation3D(quaternion);
	}
	inline void SetTranslation(const Vector& trans) RAVE_DEPRECATED {
	SetTranslation3D(trans);
	}
	inline void SetDirection(const Vector& dir) RAVE_DEPRECATED {
	SetDirection3D(dir);
	}
	inline void SetRay(const RAY& ray) RAVE_DEPRECATED {
	SetRay4D(ray);
	}
	inline void SetLookat(const Vector& trans) RAVE_DEPRECATED {
	SetLookat3D(trans);
	}
	inline void SetTranslationDirection(const RAY& ray) RAVE_DEPRECATED {
	SetTranslationDirection5D(ray);
	}
	inline const Transform& GetTransform() const RAVE_DEPRECATED {
	return _transform;
	}
	inline const Vector& GetRotation() const RAVE_DEPRECATED {
	return _transform.rot;
	}
	inline const Vector& GetTranslation() const RAVE_DEPRECATED {
	return _transform.trans;
	}
	inline const Vector& GetDirection() const RAVE_DEPRECATED {
	return _transform.rot;
	}
	inline const Vector& GetLookat() const RAVE_DEPRECATED {
	return _transform.trans;
	}
	inline const RAY GetRay() const RAVE_DEPRECATED {
	return RAY(_transform.trans,_transform.rot);
	}
	inline const RAY GetTranslationDirection() const RAVE_DEPRECATED {
	return RAY(_transform.trans,_transform.rot);
	}
	//@}

	/// \brief Computes the distance squared between two IK parmaeterizatio ns.	/// \brief Computes the distance squared between two IK parmaeterizatio ns.
	inline dReal ComputeDistanceSqr(const IkParameterization& ikparam) cons t	inline dReal ComputeDistanceSqr(const IkParameterization& ikparam) cons t
	{	{
	const dReal anglemult = 0.4; // this is a hack that should be r emoved....	const dReal anglemult = 0.4; // this is a hack that should be r emoved....
	BOOST_ASSERT(_type==ikparam.GetType());	BOOST_ASSERT(_type==ikparam.GetType());
	switch(_type) {	switch(_type) {
	case IKP_Transform6D: {	case IKP_Transform6D: {
	Transform t0 = GetTransform6D(), t1 = ikparam.GetTransform6D();	Transform t0 = GetTransform6D(), t1 = ikparam.GetTransform6D();
	dReal fcos = RaveFabs(t0.rot.dot(t1.rot));	dReal fcos = RaveFabs(t0.rot.dot(t1.rot));
	dReal facos = fcos >= 1 ? 0 : RaveAcos(fcos);	dReal facos = fcos >= 1 ? 0 : RaveAcos(fcos);

	skipping to change at line 1509	skipping to change at line 1563
	return (p0.first-p1.first).lengthsqr3() + anglediff*anglediff;	return (p0.first-p1.first).lengthsqr3() + anglediff*anglediff;
	}	}
	default:	default:
	BOOST_ASSERT(0);	BOOST_ASSERT(0);
	}	}
	return 1e30;	return 1e30;
	}	}

	/// \brief fills the iterator with the serialized values of the ikparam eterization.	/// \brief fills the iterator with the serialized values of the ikparam eterization.
	///	///

	/// the container the iterator points to needs to have \ref GetNumberOf	/// The container the iterator points to needs to have \ref GetNumberOf
	Values() available.	Values() available.
		/// Does not support custom data
		/// Don't normalize quaternions since it could hold velocity data.
	inline void GetValues(std::vector<dReal>::iterator itvalues) const	inline void GetValues(std::vector<dReal>::iterator itvalues) const
	{	{

	switch(_type) {	switch(_type & ~IKP_VelocityDataBit) {
	case IKP_Transform6D:	case IKP_Transform6D:
	*itvalues++ = _transform.rot.x;	*itvalues++ = _transform.rot.x;
	*itvalues++ = _transform.rot.y;	*itvalues++ = _transform.rot.y;
	*itvalues++ = _transform.rot.z;	*itvalues++ = _transform.rot.z;
	*itvalues++ = _transform.rot.w;	*itvalues++ = _transform.rot.w;
	*itvalues++ = _transform.trans.x;	*itvalues++ = _transform.trans.x;
	*itvalues++ = _transform.trans.y;	*itvalues++ = _transform.trans.y;
	*itvalues++ = _transform.trans.z;	*itvalues++ = _transform.trans.z;
	break;	break;
	case IKP_Rotation3D:	case IKP_Rotation3D:

	skipping to change at line 1592	skipping to change at line 1648
	*itvalues++ = _transform.rot.x;	*itvalues++ = _transform.rot.x;
	*itvalues++ = _transform.trans.x;	*itvalues++ = _transform.trans.x;
	*itvalues++ = _transform.trans.y;	*itvalues++ = _transform.trans.y;
	*itvalues++ = _transform.trans.z;	*itvalues++ = _transform.trans.z;
	break;	break;
	default:	default:
	throw OPENRAVE_EXCEPTION_FORMAT("does not support parameterizat ion 0x%x", _type,ORE_InvalidArguments);	throw OPENRAVE_EXCEPTION_FORMAT("does not support parameterizat ion 0x%x", _type,ORE_InvalidArguments);
	}	}
	}	}


	inline void Set(std::vector<dReal>::const_iterator itvalues, IkParamete	/// \brief sets a serialized set of values for the IkParameterization
	rizationType iktype)	///
		/// Function does not handle custom data. Don't normalize quaternions s
		ince it could hold velocity data.
		inline void SetValues(std::vector<dReal>::const_iterator itvalues, IkPa
		rameterizationType iktype)
	{	{
	_type = iktype;	_type = iktype;

	switch(_type) {	switch(_type & ~IKP_VelocityDataBit) {
	case IKP_Transform6D:	case IKP_Transform6D:
	_transform.rot.x = *itvalues++;	_transform.rot.x = *itvalues++;
	_transform.rot.y = *itvalues++;	_transform.rot.y = *itvalues++;
	_transform.rot.z = *itvalues++;	_transform.rot.z = *itvalues++;
	_transform.rot.w = *itvalues++;	_transform.rot.w = *itvalues++;
	_transform.trans.x = *itvalues++;	_transform.trans.x = *itvalues++;
	_transform.trans.y = *itvalues++;	_transform.trans.y = *itvalues++;
	_transform.trans.z = *itvalues++;	_transform.trans.z = *itvalues++;
	break;	break;
	case IKP_Rotation3D:	case IKP_Rotation3D:

	skipping to change at line 1675	skipping to change at line 1734
	_transform.rot.x = *itvalues++;	_transform.rot.x = *itvalues++;
	_transform.trans.x = *itvalues++;	_transform.trans.x = *itvalues++;
	_transform.trans.y = *itvalues++;	_transform.trans.y = *itvalues++;
	_transform.trans.z = *itvalues++;	_transform.trans.z = *itvalues++;
	break;	break;
	default:	default:
	throw OPENRAVE_EXCEPTION_FORMAT("does not support parameterizat ion 0x%x", _type,ORE_InvalidArguments);	throw OPENRAVE_EXCEPTION_FORMAT("does not support parameterizat ion 0x%x", _type,ORE_InvalidArguments);
	}	}
	}	}


	static ConfigurationSpecification GetConfigurationSpecification(IkParam	inline void Set(std::vector<dReal>::const_iterator itvalues, IkParamete
	eterizationType iktype);	rizationType iktype) {
	inline ConfigurationSpecification GetConfigurationSpecification() const	SetValues(itvalues,iktype);
		}

		/** \brief sets named custom data in the ik parameterization

		The custom data is serialized along with the rest of the parameters
		and can also be part of a configuration specification under the "ikparam_v
		alues" anotation.
		The custom data name can have meta-tags for the type of transformat
		ion the data undergos when \ref MultiplyTransform is called. For example, i
		f the user wants to have an extra 3 values that represent "direction", then
		the direction has to be rotated along with all the data or coordinate syst
		ems can get lost. The anotations are specified by putting:

		\b _transform=%s_

		somewhere in the string. The %s can be: \b direction, \b point, \b
		quat, \b ikparam

		If \b ikparam, the first value is expected to be the unique id of t
		he ik type (GetType()&IKP_UniqueIdMask). The other values can be computed f
		rom \ref IkParameterization::GetValues

		\param name Describes the type of data, cannot contain spaces or ne
		w lines.
		\param values the values representing the data
		\throw openrave_exception throws if the name is invalid
		*/
		inline void SetCustomValues(const std::string& name, const std::vector<
		dReal>& values)
		{
		OPENRAVE_ASSERT_OP_FORMAT0( name.size(), >, 0, "name is empty", ORE
		_InvalidArguments );
		OPENRAVE_ASSERT_OP_FORMAT0(std::count_if(name.begin(), name.end(),
		_IsValidCharInName), ==, (int)name.size(), "name has invalid characters",OR
		E_InvalidArguments);
		_mapCustomData[name] = values;
		}

		/// \brief sets named custom data in the ik parameterization (\see SetC
		ustomValues)
		inline void SetCustomValue(const std::string& name, dReal value)
	{	{

	return GetConfigurationSpecification(GetType());	OPENRAVE_ASSERT_OP_FORMAT0( name.size(), >, 0, "name is empty", ORE
		_InvalidArguments );
		OPENRAVE_ASSERT_OP_FORMAT0(std::count_if(name.begin(), name.end(),
		_IsValidCharInName), ==, (int)name.size(), "name has invalid characters",OR
		E_InvalidArguments);
		_mapCustomData[name].resize(1);
		_mapCustomData[name][0] = value;
		}

		/// \brief gets custom data if it exists, returns false if it doesn't
		inline bool GetCustomValues(const std::string& name, std::vector<dReal>
		& values) const
		{
		std::map<std::string, std::vector<dReal> >::const_iterator it = _ma
		pCustomData.find(name);
		if( it == _mapCustomData.end() ) {
		return false;
		}
		values = it->second;
		return true;
		}

		/// \brief returns a const reference of the custom data key/value pairs
		inline const std::map<std::string, std::vector<dReal> >& GetCustomDataM
		ap() const
		{
		return _mapCustomData;
		}

		/// \brief clears custom data
		///
		/// \param name if name is empty, will clear all the data, otherwise wi
		ll clear only the custom data with that name
		/// \return number of elements erased
		inline size_t ClearCustomValues(const std::string& name=std::string())
		{
		if( name.size() > 0 ) {
		return _mapCustomData.erase(name) > 0;
		}
		else {
		size_t num = _mapCustomData.size();
		_mapCustomData.clear();
		return num;
		}
		}

		static ConfigurationSpecification GetConfigurationSpecification(IkParam
		eterizationType iktype, const std::string& interpolation="");
		inline ConfigurationSpecification GetConfigurationSpecification(const s
		td::string& interpolation="") const
		{
		return GetConfigurationSpecification(GetType(),interpolation);
		}

		/// \brief in-place left-transform into a new coordinate system. Equiva
		lent to t * ikparam
		inline IkParameterization& MultiplyTransform(const Transform& t) {
		switch(GetType()) {
		case IKP_Transform6D:
		_transform = t * _transform;
		break;
		case IKP_Transform6DVelocity:
		_transform.trans = t.rotate(_transform.trans);
		_transform.rot = quatMultiply(t.rot,_transform.rot);
		break;
		case IKP_Rotation3D:
		case IKP_Rotation3DVelocity:
		_transform.rot = quatMultiply(t.rot,_transform.rot);
		break;
		case IKP_Translation3D:
		_transform.trans = t * _transform.trans;
		break;
		case IKP_Translation3DVelocity:
		_transform.trans = t.rotate(_transform.trans);
		break;
		case IKP_Direction3D:
		case IKP_Direction3DVelocity:
		_transform.rot = t.rotate(_transform.rot);
		break;
		case IKP_Ray4D:
		_transform.trans = t * _transform.trans;
		_transform.rot = t.rotate(_transform.rot);
		break;
		case IKP_Ray4DVelocity:
		_transform.trans = t.rotate(_transform.trans);
		_transform.rot = t.rotate(_transform.rot);
		break;
		case IKP_Lookat3D:
		SetLookat3D(RAY(t*GetLookat3D(),t.rotate(GetLookat3DDirection()
		)));
		break;
		case IKP_TranslationDirection5D:
		_transform.trans = t * _transform.trans;
		_transform.rot = t.rotate(_transform.rot);
		break;
		case IKP_TranslationDirection5DVelocity:
		_transform.trans = t.rotate(_transform.trans);
		_transform.rot = t.rotate(_transform.rot);
		break;
		case IKP_TranslationXY2D:
		SetTranslationXY2D(t*GetTranslationXY2D());
		break;
		case IKP_TranslationXY2DVelocity:
		_transform.trans = t.rotate(_transform.trans);
		break;
		case IKP_TranslationXYOrientation3D: {
		Vector v = GetTranslationXYOrientation3D();
		Vector voldtrans(v.x,v.y,0);
		Vector vnewtrans = t*voldtrans;
		dReal zangle = -normalizeAxisRotation(Vector(0,0,1),t.rot).firs
		t;
		SetTranslationXYOrientation3D(Vector(vnewtrans.y,vnewtrans.y,v.
		z+zangle));
		break;
		}
		case IKP_TranslationXYOrientation3DVelocity: {
		Vector v = GetTranslationXYOrientation3D();
		Vector voldtrans(v.x,v.y,0);
		_transform.trans = t.rotate(voldtrans);
		_transform.trans.z = quatRotate(t.rot,Vector(0,0,v.z)).z;
		break;
		}
		case IKP_TranslationLocalGlobal6D:
		_transform.trans = t*_transform.trans;
		break;
		case IKP_TranslationLocalGlobal6DVelocity:
		_transform.trans = t.rotate(_transform.trans);
		break;
		case IKP_TranslationXAxisAngle4D: {
		_transform.trans = t*_transform.trans;
		// do not support rotations
		break;
		}
		case IKP_TranslationXAxisAngle4DVelocity: {
		_transform.trans = t.rotate(_transform.trans);
		// do not support rotations
		break;
		}
		case IKP_TranslationYAxisAngle4D: {
		_transform.trans = t*_transform.trans;
		// do not support rotations
		break;
		}
		case IKP_TranslationYAxisAngle4DVelocity: {
		_transform.trans = t.rotate(_transform.trans);
		// do not support rotations
		break;
		}
		case IKP_TranslationZAxisAngle4D: {
		_transform.trans = t*_transform.trans;
		// do not support rotations
		break;
		}
		case IKP_TranslationZAxisAngle4DVelocity: {
		_transform.trans = t.rotate(_transform.trans);
		// do not support rotations
		break;
		}

		case IKP_TranslationXAxisAngleZNorm4D: {
		_transform.trans = t*_transform.trans;
		// only support rotation along z-axis
		_transform.rot.x -= normalizeAxisRotation(Vector(0,0,1),t.rot).
		first;
		break;
		}
		case IKP_TranslationXAxisAngleZNorm4DVelocity: {
		_transform.trans = t.rotate(_transform.trans);
		// only support rotation along z-axis
		_transform.rot.x = quatRotate(t.rot,Vector(0,0,_transform.rot.x
		)).z;
		break;
		}
		case IKP_TranslationYAxisAngleXNorm4D: {
		_transform.trans = t*_transform.trans;
		// only support rotation along x-axis
		_transform.rot.x -= normalizeAxisRotation(Vector(1,0,0),t.rot).
		first;
		break;
		}
		case IKP_TranslationYAxisAngleXNorm4DVelocity: {
		_transform.trans = t.rotate(_transform.trans);
		// only support rotation along x-axis
		_transform.rot.x = quatRotate(t.rot,Vector(_transform.rot.x,0,0
		)).x;
		break;
		}
		case IKP_TranslationZAxisAngleYNorm4D: {
		_transform.trans = t*_transform.trans;
		// only support rotation along y-axis
		_transform.rot.x -= normalizeAxisRotation(Vector(0,1,0),t.rot).
		first;
		break;
		}
		case IKP_TranslationZAxisAngleYNorm4DVelocity: {
		_transform.trans = t.rotate(_transform.trans);
		// only support rotation along y-axis
		_transform.rot.x = quatRotate(t.rot,Vector(0,_transform.rot.x,0
		)).y;
		break;
		}
		default:
		throw openrave_exception(str(boost::format("parameterization 0x
		%x does not support left-transform")%GetType()));
		}
		for(std::map<std::string, std::vector<dReal> >::iterator it = _mapC
		ustomData.begin(); it != _mapCustomData.end(); ++it) {
		_MultiplyTransform(t, it->first, it->second);
		}
		return *this;
		}

		/** \brief in-place right-transform into a new coordinate system. Equiv
		alent to ikparam*t

		Note that depending on the ikparam type, some information from the
		passed in transform can get lost or misinterpreted. For example
		Translation3D types do not have a rotation, so assume identity.

		For ik types that have 3D directions stored, the transformation is
		the following:
		\code
		quatRotate(quatMultiply(quatRotateDirection(Vector(0,0,1),direction
		), t.rot), Vector(0,0,1))
		\endcode
		Basically it is how the local z axis gets transformed and convertin
		g that back to world coordinates.
		*/
		inline IkParameterization& MultiplyTransformRight(const Transform& t) {
		switch(GetType()) {
		case IKP_Transform6D:
		_transform *= t;
		break;
		// case IKP_Transform6DVelocity:
		// _transform.trans = t.rotate(_transform.trans);
		// _transform.rot = quatMultiply(t.rot,_transform.rot);
		// break;
		case IKP_Rotation3D:
		// case IKP_Rotation3DVelocity:
		_transform.rot = quatMultiply(_transform.rot,t.rot);
		break;
		case IKP_Translation3D:
		_transform.trans = _transform.trans + t.trans;
		break;
		// case IKP_Translation3DVelocity:
		// _transform.trans = t.rotate(_transform.trans);
		// break;
		case IKP_Direction3D:
		// case IKP_Direction3DVelocity:
		_transform.rot = quatRotate(quatMultiply(quatRotateDirection(Ve
		ctor(0,0,1),_transform.rot), t.rot), Vector(0,0,1));
		break;
		// case IKP_Ray4D:
		// _transform.trans = t * _transform.trans;
		// _transform.rot = t.rotate(_transform.rot);
		// break;
		// case IKP_Ray4DVelocity:
		// _transform.trans = t.rotate(_transform.trans);
		// _transform.rot = t.rotate(_transform.rot);
		// break;
		case IKP_Lookat3D:
		SetLookat3D(GetLookat3D() + t.trans);
		break;
		case IKP_TranslationDirection5D: {
		Vector qorig = quatRotateDirection(Vector(0,0,1),_transform.rot
		);
		Vector q = quatMultiply(qorig, t.rot);
		_transform.trans += quatRotate(qorig,t.trans);
		_transform.rot = quatRotate(q, Vector(0,0,1));
		break;
		}
		// case IKP_TranslationDirection5DVelocity:
		// _transform.trans = t.rotate(_transform.trans);
		// _transform.rot = t.rotate(_transform.rot);
		// break;
		case IKP_TranslationXY2D:
		SetTranslationXY2D(GetTranslationXY2D() + t.trans);
		break;
		// case IKP_TranslationXY2DVelocity:
		// _transform.trans = t.rotate(_transform.trans);
		// break;
		case IKP_TranslationXYOrientation3D: {
		Vector v = GetTranslationXYOrientation3D();
		Vector voldtrans(v.x,v.y,0);
		Vector q = quatFromAxisAngle(Vector(0,0,1),v.z);
		Vector vnewtrans = voldtrans + quatRotate(q,t.trans);
		dReal zangle = -normalizeAxisRotation(Vector(0,0,1),t.rot).firs
		t;
		SetTranslationXYOrientation3D(Vector(vnewtrans.y,vnewtrans.y,v.
		z+zangle));
		break;
		}
		// case IKP_TranslationXYOrientation3DVelocity: {
		// Vector v = GetTranslationXYOrientation3D();
		// Vector voldtrans(v.x,v.y,0);
		// _transform.trans = t.rotate(voldtrans);
		// _transform.trans.z = quatRotate(t.rot,Vector(0,0,v.z)).z;
		// break;
		// }
		case IKP_TranslationLocalGlobal6D:
		_transform.trans = _transform.trans + t.trans;
		break;
		// case IKP_TranslationLocalGlobal6DVelocity:
		// _transform.trans = t.rotate(_transform.trans);
		// break;
		// case IKP_TranslationXAxisAngle4D: {
		// _transform.trans = t*_transform.trans;
		// // do not support rotations
		// break;
		// }
		// case IKP_TranslationXAxisAngle4DVelocity: {
		// _transform.trans = t.rotate(_transform.trans);
		// // do not support rotations
		// break;
		// }
		// case IKP_TranslationYAxisAngle4D: {
		// _transform.trans = t*_transform.trans;
		// // do not support rotations
		// break;
		// }
		// case IKP_TranslationYAxisAngle4DVelocity: {
		// _transform.trans = t.rotate(_transform.trans);
		// // do not support rotations
		// break;
		// }
		// case IKP_TranslationZAxisAngle4D: {
		// _transform.trans = t*_transform.trans;
		// // do not support rotations
		// break;
		// }
		// case IKP_TranslationZAxisAngle4DVelocity: {
		// _transform.trans = t.rotate(_transform.trans);
		// // do not support rotations
		// break;
		// }
		case IKP_TranslationXAxisAngleZNorm4D: {
		Vector q = quatFromAxisAngle(Vector(0,0,1),_transform.rot.x);
		_transform.trans += quatRotate(q,t.trans);
		// only support rotation along z-axis
		_transform.rot.x -= normalizeAxisRotation(Vector(0,0,1),t.rot).
		first;
		break;
		}
		// case IKP_TranslationXAxisAngleZNorm4DVelocity: {
		// _transform.trans = t.rotate(_transform.trans);
		// // only support rotation along z-axis
		// _transform.rot.x = quatRotate(t.rot,Vector(0,0,_transform.rot
		.x)).z;
		// break;
		// }
		case IKP_TranslationYAxisAngleXNorm4D: {
		Vector q = quatFromAxisAngle(Vector(1,0,0),_transform.rot.x);
		_transform.trans += quatRotate(q,t.trans);
		// only support rotation along x-axis
		_transform.rot.x -= normalizeAxisRotation(Vector(1,0,0),t.rot).
		first;
		break;
		}
		// case IKP_TranslationYAxisAngleXNorm4DVelocity: {
		// _transform.trans = t.rotate(_transform.trans);
		// // only support rotation along x-axis
		// _transform.rot.x = quatRotate(t.rot,Vector(_transform.rot.x,0
		,0)).x;
		// break;
		// }
		case IKP_TranslationZAxisAngleYNorm4D: {
		Vector q = quatFromAxisAngle(Vector(0,1,0),_transform.rot.x);
		_transform.trans += quatRotate(q,t.trans);
		// only support rotation along y-axis
		_transform.rot.x -= normalizeAxisRotation(Vector(0,1,0),t.rot).
		first;
		break;
		}
		// case IKP_TranslationZAxisAngleYNorm4DVelocity: {
		// _transform.trans = t.rotate(_transform.trans);
		// // only support rotation along y-axis
		// _transform.rot.x = quatRotate(t.rot,Vector(0,_transform.rot.x
		,0)).y;
		// break;
		// }
		default:
		throw openrave_exception(str(boost::format("parameterization 0x
		%x does not support right-transforms")%GetType()));
		}
		for(std::map<std::string, std::vector<dReal> >::iterator it = _mapC
		ustomData.begin(); it != _mapCustomData.end(); ++it) {
		_MultiplyTransformRight(t, it->first, it->second);
		}
		return *this;
		}

		inline IkParameterization operator*(const Transform& t) const {
		IkParameterization iknew(*this);
		iknew.MultiplyTransformRight(t);
		return iknew;
	}	}

	protected:	protected:

		inline static bool _IsValidCharInName(char c) {
		return c < 0 \|\| c >= 33;
		}
		inline static void _MultiplyTransform(const Transform& t, const std::st
		ring& name, std::vector<dReal>& values)
		{
		size_t startoffset = name.find("_transform=");
		if( startoffset != std::string::npos ) {
		size_t endoffset = name.find("_", startoffset+11);
		std::string transformtype;
		if( endoffset == std::string::npos ) {
		transformtype = name.substr(startoffset+11);
		}
		else {
		transformtype = name.substr(startoffset+11,endoffset-starto
		ffset-11);
		}
		if( transformtype == "direction" ) {
		Vector v(values.at(0),values.at(1), values.at(2));
		v = t.rotate(v);
		values.at(0) = v[0]; values.at(1) = v[1]; values.at(2) = v[
		2];
		}
		else if( transformtype == "point" ) {
		Vector v(values.at(0),values.at(1), values.at(2));
		v = t*v;
		values.at(0) = v[0]; values.at(1) = v[1]; values.at(2) = v[
		2];
		}
		else if( transformtype == "quat" ) {
		Vector v(values.at(0),values.at(1), values.at(2),values.at(
		3));
		v = quatMultiply(t.rot,v);
		values.at(0) = v[0]; values.at(1) = v[1]; values.at(2) = v[
		2]; values.at(3) = v[3];
		}
		else if( transformtype == "ikparam" ) {
		IkParameterizationType newiktype = RaveGetIkTypeFromUniqueI
		d(static_cast<int>(values.at(0)+0.5));
		IkParameterization newikparam;
		OPENRAVE_ASSERT_OP_FORMAT0(IkParameterization::GetNumberOfV
		alues(newiktype)+1, ==, (int)values.size(),"expected values not equal",ORE_
		InvalidState);
		newikparam.SetValues(values.begin()+1,newiktype);
		newikparam.MultiplyTransform(t);
		newikparam.GetValues(values.begin()+1);
		}
		else {
		throw OPENRAVE_EXCEPTION_FORMAT("IkParameterization custom
		data '%s' does not have a valid transform",name,ORE_InvalidState);
		}
		}
		}

		inline static void _MultiplyTransformRight(const Transform& t, const st
		d::string& name, std::vector<dReal>& values)
		{
		size_t startoffset = name.find("_transform=");
		if( startoffset != std::string::npos ) {
		size_t endoffset = name.find("_", startoffset+11);
		std::string transformtype;
		if( endoffset == std::string::npos ) {
		transformtype = name.substr(startoffset+11);
		}
		else {
		transformtype = name.substr(startoffset+11,endoffset-starto
		ffset-11);
		}
		if( transformtype == "direction" ) {
		Vector v(values.at(0),values.at(1), values.at(2));
		v = quatRotate(quatMultiply(quatRotateDirection(Vector(0,0,
		1),v), t.rot), Vector(0,0,1));
		values.at(0) = v[0]; values.at(1) = v[1]; values.at(2) = v[
		2];
		}
		else if( transformtype == "point" ) {
		Vector v(values.at(0),values.at(1), values.at(2));
		v += t.trans;
		values.at(0) = v[0]; values.at(1) = v[1]; values.at(2) = v[
		2];
		}
		else if( transformtype == "quat" ) {
		Vector v(values.at(0),values.at(1), values.at(2),values.at(
		3));
		v = quatMultiply(v,t.rot);
		values.at(0) = v[0]; values.at(1) = v[1]; values.at(2) = v[
		2]; values.at(3) = v[3];
		}
		else if( transformtype == "ikparam" ) {
		IkParameterizationType newiktype = RaveGetIkTypeFromUniqueI
		d(static_cast<int>(values.at(0)+0.5));
		IkParameterization newikparam;
		OPENRAVE_ASSERT_OP_FORMAT0(IkParameterization::GetNumberOfV
		alues(newiktype)+1, ==, (int)values.size(),"expected values not equal",ORE_
		InvalidState);
		newikparam.SetValues(values.begin()+1,newiktype);
		newikparam.MultiplyTransformRight(t);
		newikparam.GetValues(values.begin()+1);
		}
		else {
		throw OPENRAVE_EXCEPTION_FORMAT("IkParameterization custom
		data '%s' does not have a valid transform",name,ORE_InvalidState);
		}
		}
		}

	Transform _transform;	Transform _transform;
	IkParameterizationType _type;	IkParameterizationType _type;

		std::map<std::string, std::vector<dReal> > _mapCustomData;

	friend IkParameterization operator* (const Transform &t, const IkParame terization &ikparam);	friend IkParameterization operator* (const Transform &t, const IkParame terization &ikparam);
	friend OPENRAVE_API std::ostream& operator<<(std::ostream& O, const IkP arameterization &ikparam);	friend OPENRAVE_API std::ostream& operator<<(std::ostream& O, const IkP arameterization &ikparam);
	friend OPENRAVE_API std::istream& operator>>(std::istream& I, IkParamet erization& ikparam);	friend OPENRAVE_API std::istream& operator>>(std::istream& I, IkParamet erization& ikparam);
	};	};

	inline IkParameterization operator* (const Transform &t, const IkParameteri zation &ikparam)	inline IkParameterization operator* (const Transform &t, const IkParameteri zation &ikparam)
	{	{
	IkParameterization local;	IkParameterization local;
	switch(ikparam.GetType()) {	switch(ikparam.GetType()) {
	case IKP_Transform6D:	case IKP_Transform6D:
	local.SetTransform6D(t * ikparam.GetTransform6D());	local.SetTransform6D(t * ikparam.GetTransform6D());
	break;	break;
	case IKP_Rotation3D:	case IKP_Rotation3D:

	local.SetRotation3D(quatMultiply(quatInverse(t.rot),ikparam.GetRota tion3D()));	local.SetRotation3D(quatMultiply(t.rot,ikparam.GetRotation3D()));
	break;	break;
	case IKP_Translation3D:	case IKP_Translation3D:
	local.SetTranslation3D(t*ikparam.GetTranslation3D());	local.SetTranslation3D(t*ikparam.GetTranslation3D());
	break;	break;
	case IKP_Direction3D:	case IKP_Direction3D:
	local.SetDirection3D(t.rotate(ikparam.GetDirection3D()));	local.SetDirection3D(t.rotate(ikparam.GetDirection3D()));
	break;	break;
	case IKP_Ray4D:	case IKP_Ray4D:
	local.SetRay4D(RAY(t*ikparam.GetRay4D().pos,t.rotate(ikparam.GetRay 4D().dir)));	local.SetRay4D(RAY(t*ikparam.GetRay4D().pos,t.rotate(ikparam.GetRay 4D().dir)));
	break;	break;

	skipping to change at line 1766	skipping to change at line 2293
	local.SetTranslationYAxisAngleXNorm4D(t*p.first,p.second);	local.SetTranslationYAxisAngleXNorm4D(t*p.first,p.second);
	break;	break;
	}	}
	case IKP_TranslationZAxisAngleYNorm4D: {	case IKP_TranslationZAxisAngleYNorm4D: {
	std::pair<Vector,dReal> p = ikparam.GetTranslationZAxisAngleYNorm4D ();	std::pair<Vector,dReal> p = ikparam.GetTranslationZAxisAngleYNorm4D ();
	// don't change the angle since don't know the exact direction it i s pointing at	// don't change the angle since don't know the exact direction it i s pointing at
	local.SetTranslationZAxisAngleYNorm4D(t*p.first,p.second);	local.SetTranslationZAxisAngleYNorm4D(t*p.first,p.second);
	break;	break;
	}	}
	default:	default:

	throw openrave_exception(str(boost::format("does not support parame	// internal MultiplyTransform supports more types
	terization %d")%ikparam.GetType()));	return IkParameterization(ikparam).MultiplyTransform(t);
		}
		local._mapCustomData = ikparam._mapCustomData;
		for(std::map<std::string, std::vector<dReal> >::iterator it = local._ma
		pCustomData.begin(); it != local._mapCustomData.end(); ++it) {
		IkParameterization::_MultiplyTransform(t, it->first,it->second);
	}	}
	return local;	return local;
	}	}

	OPENRAVE_API std::ostream& operator<<(std::ostream& O, const IkParameteriza tion &ikparam);	OPENRAVE_API std::ostream& operator<<(std::ostream& O, const IkParameteriza tion &ikparam);
	OPENRAVE_API std::istream& operator>>(std::istream& I, IkParameterization& ikparam);	OPENRAVE_API std::istream& operator>>(std::istream& I, IkParameterization& ikparam);

	/// \brief Selects which DOFs of the affine transformation to include in th e active configuration.	/// \brief Selects which DOFs of the affine transformation to include in th e active configuration.
	enum DOFAffine	enum DOFAffine
	{	{

	skipping to change at line 1829	skipping to change at line 2361
	*/	*/
	OPENRAVE_API void RaveGetAffineDOFValuesFromTransform(std::vector<dReal>::i terator itvalues, const Transform& t, int affinedofs, const Vector& vActvAf fineRotationAxis=Vector(0,0,1));	OPENRAVE_API void RaveGetAffineDOFValuesFromTransform(std::vector<dReal>::i terator itvalues, const Transform& t, int affinedofs, const Vector& vActvAf fineRotationAxis=Vector(0,0,1));

	/** \brief Converts affine dof values into a transform.	/** \brief Converts affine dof values into a transform.

	Note that depending on what the dof values holds, only a part of the tr ansform will be updated.	Note that depending on what the dof values holds, only a part of the tr ansform will be updated.
	\param[out] t the output transform	\param[out] t the output transform
	\param[in] itvalues the start iterator of the affine dof values	\param[in] itvalues the start iterator of the affine dof values
	\param[in] affinedofs the affine dof mask	\param[in] affinedofs the affine dof mask
	\param[in] vActvAffineRotationAxis optional rotation axis if affinedofs specified \ref DOF_RotationAxis	\param[in] vActvAffineRotationAxis optional rotation axis if affinedofs specified \ref DOF_RotationAxis

		\param[in] if true will normalize rotations, should set to false if ext racting velocity data
	*/	*/

	OPENRAVE_API void RaveGetTransformFromAffineDOFValues(Transform& t, std::ve ctor<dReal>::const_iterator itvalues, int affinedofs, const Vector& vActvAf fineRotationAxis=Vector(0,0,1));	OPENRAVE_API void RaveGetTransformFromAffineDOFValues(Transform& t, std::ve ctor<dReal>::const_iterator itvalues, int affinedofs, const Vector& vActvAf fineRotationAxis=Vector(0,0,1), bool normalize=true);


	OPENRAVE_API ConfigurationSpecification RaveGetAffineConfigurationSpecifica tion(int affinedofs,KinBodyConstPtr pbody=KinBodyConstPtr());	OPENRAVE_API ConfigurationSpecification RaveGetAffineConfigurationSpecifica tion(int affinedofs,KinBodyConstPtr pbody=KinBodyConstPtr(),const std::stri ng& interpolation="");

	}	}

	#include <openrave/plugininfo.h>	#include <openrave/plugininfo.h>
	#include <openrave/interface.h>	#include <openrave/interface.h>
	#include <openrave/spacesampler.h>	#include <openrave/spacesampler.h>
	#include <openrave/kinbody.h>	#include <openrave/kinbody.h>
	#include <openrave/trajectory.h>	#include <openrave/trajectory.h>
	#include <openrave/module.h>	#include <openrave/module.h>
	#include <openrave/collisionchecker.h>	#include <openrave/collisionchecker.h>

	skipping to change at line 1922	skipping to change at line 2455
	return boost::static_pointer_cast<T const>(pinterface);	return boost::static_pointer_cast<T const>(pinterface);
	}	}
	}	}
	return boost::shared_ptr<T>();	return boost::shared_ptr<T>();
	}	}

	/// \brief returns a lower case string of the interface type	/// \brief returns a lower case string of the interface type
	OPENRAVE_API const std::map<InterfaceType,std::string>& RaveGetInterfaceNam esMap();	OPENRAVE_API const std::map<InterfaceType,std::string>& RaveGetInterfaceNam esMap();
	OPENRAVE_API const std::string& RaveGetInterfaceName(InterfaceType type);	OPENRAVE_API const std::string& RaveGetInterfaceName(InterfaceType type);


	/// \brief returns a string of the ik parameterization type names (can incl
	ude upper case in order to match \ref IkParameterizationType)
	OPENRAVE_API const std::map<IkParameterizationType,std::string>& RaveGetIkP
	arameterizationMap();

	/// \brief Returns the openrave home directory where settings, cache, and o ther files are stored.	/// \brief Returns the openrave home directory where settings, cache, and o ther files are stored.
	///	///
	/// On Linux/Unix systems, this is usually $HOME/.openrave, on Windows this is $HOMEPATH/.openrave	/// On Linux/Unix systems, this is usually $HOME/.openrave, on Windows this is $HOMEPATH/.openrave
	OPENRAVE_API std::string RaveGetHomeDirectory();	OPENRAVE_API std::string RaveGetHomeDirectory();

	/// \brief Searches for a filename in the database and returns a full path/ URL to it	/// \brief Searches for a filename in the database and returns a full path/ URL to it
	///	///
	/// \param filename the relative filename in the database	/// \param filename the relative filename in the database
	/// \param bRead if true will only return a file if it exists. If false, wi ll return the filename of the first valid database directory.	/// \param bRead if true will only return a file if it exists. If false, wi ll return the filename of the first valid database directory.
	/// \return a non-empty string if a file could be found.	/// \return a non-empty string if a file could be found.
	OPENRAVE_API std::string RaveFindDatabaseFile(const std::string& filename, bool bRead=true);	OPENRAVE_API std::string RaveFindDatabaseFile(const std::string& filename, bool bRead=true);

	/// \brief Explicitly initializes the global OpenRAVE state (optional).	/// \brief Explicitly initializes the global OpenRAVE state (optional).
	///	///

	/// Optional function to initialize openrave plugins and logging.	/// Optional function to initialize openrave plugins, logging, and read OPE NRAVE_* environment variables.
	/// Although environment creation will automatically make sure this functio n is called, users might want	/// Although environment creation will automatically make sure this functio n is called, users might want
	/// explicit control of when this happens.	/// explicit control of when this happens.

		/// Will not do anything if OpenRAVE runtime is already initialized. If OPE NRAVE_* environment variables must be re-read, first call \ref RaveDestroy.
	/// \param bLoadAllPlugins If true will load all the openrave plugins autom atically that can be found in the OPENRAVE_PLUGINS environment path	/// \param bLoadAllPlugins If true will load all the openrave plugins autom atically that can be found in the OPENRAVE_PLUGINS environment path
	/// \return 0 if successful, otherwise an error code	/// \return 0 if successful, otherwise an error code
	OPENRAVE_API int RaveInitialize(bool bLoadAllPlugins=true, int level = Leve l_Info);	OPENRAVE_API int RaveInitialize(bool bLoadAllPlugins=true, int level = Leve l_Info);

	/// \brief Initializes the global state from an already loaded OpenRAVE env ironment.	/// \brief Initializes the global state from an already loaded OpenRAVE env ironment.
	///	///
	/// Because of shared object boundaries, it is necessary to pass the global state pointer	/// Because of shared object boundaries, it is necessary to pass the global state pointer
	/// around. If using plugin.h, this function is automatically called by \re f CreateInterfaceValidated.	/// around. If using plugin.h, this function is automatically called by \re f CreateInterfaceValidated.
	/// It is also called by and every InterfaceBase constructor.	/// It is also called by and every InterfaceBase constructor.
	/// \param[in] globalstate	/// \param[in] globalstate

	skipping to change at line 2056	skipping to change at line 2587
	OPENRAVE_API EnvironmentBasePtr RaveGetEnvironment(int id);	OPENRAVE_API EnvironmentBasePtr RaveGetEnvironment(int id);

	/// \brief Return all the created OpenRAVE environments.	/// \brief Return all the created OpenRAVE environments.
	OPENRAVE_API void RaveGetEnvironments(std::list<EnvironmentBasePtr>& listen vironments);	OPENRAVE_API void RaveGetEnvironments(std::list<EnvironmentBasePtr>& listen vironments);

	/// \brief Returns the current registered reader for the interface type/xml id	/// \brief Returns the current registered reader for the interface type/xml id
	///	///
	/// \throw openrave_exception Will throw with ORE_InvalidArguments if regis tered function could not be found.	/// \throw openrave_exception Will throw with ORE_InvalidArguments if regis tered function could not be found.
	OPENRAVE_API BaseXMLReaderPtr RaveCallXMLReader(InterfaceType type, const s td::string& xmltag, InterfaceBasePtr pinterface, const AttributesList& atts );	OPENRAVE_API BaseXMLReaderPtr RaveCallXMLReader(InterfaceType type, const s td::string& xmltag, InterfaceBasePtr pinterface, const AttributesList& atts );


		/** \brief Returns the absolute path of the filename on the local filesyste
		m resolving relative paths from OpenRAVE paths.

		The OpenRAVE paths consist of a list of directories specified by $OPENR
		AVE_DATA environment variable and custom added user paths.
		Requires boost::filesystem to be installed
		\param filename the filename to look for
		\param curdir the current directory in case the filename is relative
		\return an empty string if file isn't found, otherwise path to full fil
		ename on local filesystem
		*/
		OPENRAVE_API std::string RaveFindLocalFile(const std::string& filename, con
		st std::string& curdir="");

		/// \brief Sets the default data access options for cad resources/robot fil
		es
		///
		/// Controls how files are processed in functions like \ref RaveFindLocalFi
		le
		/// \param accessoptions - if 1 will only allow resources inside directorie
		s specified from OPERNAVE_DATA environment variable. This allows reject of
		full paths from unsecure/unauthenticated resources.
		OPENRAVE_API void RaveSetDataAccess(int accessoptions);

		/// \brief Returns the acess options set
		///
		/// \see RaveSetDataAccess
		OPENRAVE_API int RaveGetDataAccess();

	//@}	//@}

	/// \deprecated (11/06/03), use \ref SpaceSamplerBase	/// \deprecated (11/06/03), use \ref SpaceSamplerBase
	OPENRAVE_API void RaveInitRandomGeneration(uint32_t seed);	OPENRAVE_API void RaveInitRandomGeneration(uint32_t seed);
	/// \deprecated (11/06/03), use \ref SpaceSamplerBase	/// \deprecated (11/06/03), use \ref SpaceSamplerBase
	OPENRAVE_API uint32_t RaveRandomInt();	OPENRAVE_API uint32_t RaveRandomInt();
	/// \deprecated (11/06/03), use \ref SpaceSamplerBase	/// \deprecated (11/06/03), use \ref SpaceSamplerBase
	OPENRAVE_API float RaveRandomFloat(IntervalType interval=IT_Closed);	OPENRAVE_API float RaveRandomFloat(IntervalType interval=IT_Closed);
	/// \deprecated (11/06/03), use \ref SpaceSamplerBase	/// \deprecated (11/06/03), use \ref SpaceSamplerBase
	OPENRAVE_API double RaveRandomDouble(IntervalType interval=IT_Closed);	OPENRAVE_API double RaveRandomDouble(IntervalType interval=IT_Closed);

	skipping to change at line 2107	skipping to change at line 2659
	typedef InterfaceBasePtr (PluginExportFn_OpenRAVECreateInterface)(Interfac eType type, const std::string& name, const char pluginhash, const char* en vhash, EnvironmentBasePtr penv);	typedef InterfaceBasePtr (PluginExportFn_OpenRAVECreateInterface)(Interfac eType type, const std::string& name, const char pluginhash, const char* en vhash, EnvironmentBasePtr penv);

	/// \brief Called to fill information about the plugin, see \ref GetPluginA ttributesValidated.	/// \brief Called to fill information about the plugin, see \ref GetPluginA ttributesValidated.
	/// \ingroup plugin_exports	/// \ingroup plugin_exports
	typedef bool (PluginExportFn_OpenRAVEGetPluginAttributes)(PLUGININFO pinf o, int size, const char* infohash);	typedef bool (PluginExportFn_OpenRAVEGetPluginAttributes)(PLUGININFO pinf o, int size, const char* infohash);

	/// \brief Called before plugin is unloaded from openrave. See \ref Destroy Plugin.	/// \brief Called before plugin is unloaded from openrave. See \ref Destroy Plugin.
	/// \ingroup plugin_exports	/// \ingroup plugin_exports
	typedef void (*PluginExportFn_DestroyPlugin)();	typedef void (*PluginExportFn_DestroyPlugin)();


	/// \deprecated	/// \deprecated (12/01/01)
	typedef InterfaceBasePtr (*PluginExportFn_CreateInterface)(InterfaceType ty	typedef InterfaceBasePtr (*PluginExportFn_CreateInterface)(InterfaceType ty
	pe, const std::string& name, const char* pluginhash, EnvironmentBasePtr pen	pe, const std::string& name, const char* pluginhash, EnvironmentBasePtr pen
	v);	v) RAVE_DEPRECATED;


	/// \deprecated	/// \deprecated (12/01/01)
	typedef bool (PluginExportFn_GetPluginAttributes)(PLUGININFO pinfo, int s	typedef bool (PluginExportFn_GetPluginAttributes)(PLUGININFO pinfo, int s
	ize);	ize) RAVE_DEPRECATED;

	// define inline functions	// define inline functions
	const std::string& IkParameterization::GetName() const	const std::string& IkParameterization::GetName() const
	{	{
	std::map<IkParameterizationType,std::string>::const_iterator it = RaveG etIkParameterizationMap().find(_type);	std::map<IkParameterizationType,std::string>::const_iterator it = RaveG etIkParameterizationMap().find(_type);
	if( it != RaveGetIkParameterizationMap().end() ) {	if( it != RaveGetIkParameterizationMap().end() ) {
	return it->second;	return it->second;
	}	}
	throw openrave_exception(str(boost::format("IkParameterization iktype 0 x%x not supported")));	throw openrave_exception(str(boost::format("IkParameterization iktype 0 x%x not supported")));
	}	}

	skipping to change at line 2167	skipping to change at line 2719
	BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::ViewerBase)	BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::ViewerBase)
	BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::SpaceSamplerBase)	BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::SpaceSamplerBase)
	BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::GraphHandle)	BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::GraphHandle)
	BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::IkParameterization)	BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::IkParameterization)
	BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::ConfigurationSpecification)	BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::ConfigurationSpecification)
	BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::ConfigurationSpecification::Group)	BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::ConfigurationSpecification::Group)
	BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::ConfigurationSpecification::Reader)	BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::ConfigurationSpecification::Reader)
	BOOST_TYPEOF_REGISTER_TEMPLATE(OpenRAVE::RaveVector, 1)	BOOST_TYPEOF_REGISTER_TEMPLATE(OpenRAVE::RaveVector, 1)
	BOOST_TYPEOF_REGISTER_TEMPLATE(OpenRAVE::RaveTransform, 1)	BOOST_TYPEOF_REGISTER_TEMPLATE(OpenRAVE::RaveTransform, 1)
	BOOST_TYPEOF_REGISTER_TEMPLATE(OpenRAVE::RaveTransformMatrix, 1)	BOOST_TYPEOF_REGISTER_TEMPLATE(OpenRAVE::RaveTransformMatrix, 1)

		BOOST_TYPEOF_REGISTER_TEMPLATE(OpenRAVE::OrientedBox, 1)

	BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::KinBody)	BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::KinBody)
	BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::KinBody::Joint)	BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::KinBody::Joint)
	BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::KinBody::Joint::MIMIC)	BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::KinBody::Joint::MIMIC)
	BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::KinBody::Link)	BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::KinBody::Link)
	BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::KinBody::Link::GEOMPROPERTIES)	BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::KinBody::Link::GEOMPROPERTIES)
	BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::KinBody::Link::TRIMESH)	BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::KinBody::Link::TRIMESH)
	BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::KinBody::KinBodyStateSaver)	BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::KinBody::KinBodyStateSaver)
	BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::KinBody::BodyState)	BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::KinBody::BodyState)
	BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::KinBody::ManageData)	BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::KinBody::ManageData)

	skipping to change at line 2192	skipping to change at line 2745
	BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::RobotBase::RobotStateSaver)	BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::RobotBase::RobotStateSaver)

	BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::TrajectoryBase)	BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::TrajectoryBase)
	BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::TrajectoryBase::TPOINT)	BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::TrajectoryBase::TPOINT)
	BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::TrajectoryBase::TSEGMENT)	BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::TrajectoryBase::TSEGMENT)

	BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::PLUGININFO)	BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::PLUGININFO)
	BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::XMLReadable)	BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::XMLReadable)
	BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::InterfaceBase)	BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::InterfaceBase)
	BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::BaseXMLReader)	BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::BaseXMLReader)

		BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::BaseXMLWriter)

	BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::EnvironmentBase)	BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::EnvironmentBase)
	BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::EnvironmentBase::BODYSTATE)	BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::EnvironmentBase::BODYSTATE)

	BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::RAY)	BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::RAY)
	BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::AABB)	BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::AABB)
	BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::OBB)	BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::OBB)
	BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::TRIANGLE)	BOOST_TYPEOF_REGISTER_TYPE(OpenRAVE::TRIANGLE)
	#endif	#endif


End of changes. 54 change blocks.
	110 lines changed or deleted	768 lines changed or added

	planner.h	planner.h

	skipping to change at line 70	skipping to change at line 70

	Also allows the parameters and descriptions to be serialized to reS tructuredText for documentation purposes.	Also allows the parameters and descriptions to be serialized to reS tructuredText for documentation purposes.
	*/	*/
	class OPENRAVE_API PlannerParameters : public BaseXMLReader, public XML Readable	class OPENRAVE_API PlannerParameters : public BaseXMLReader, public XML Readable
	{	{
	public:	public:
	PlannerParameters();	PlannerParameters();
	virtual ~PlannerParameters() {	virtual ~PlannerParameters() {
	}	}


		/// \brief saves and restores the state using PlannerParameters::_s
		etstatefn and PlannerParameters::_getstatefn
		class OPENRAVE_API StateSaver
		{
		public:
		StateSaver(boost::shared_ptr<PlannerParameters> params);
		virtual ~StateSaver();
		inline boost::shared_ptr<PlannerParameters> GetParameters() con
		st {
		return _params;
		}
		virtual void Restore();
		protected:
		boost::shared_ptr<PlannerParameters> _params;
		std::vector<dReal> _values;
		private:
		virtual void _Restore();
		};

		typedef boost::shared_ptr<StateSaver> StateSaverPtr;

	/** \brief Attemps to copy data from one set of parameters to anoth er in the safest manner.	/** \brief Attemps to copy data from one set of parameters to anoth er in the safest manner.

	First serializes the data of the right hand into a string, then initializes the current parameters via >>	First serializes the data of the right hand into a string, then initializes the current parameters via >>
	pointers to functions are copied directly	pointers to functions are copied directly
	*/	*/
	virtual PlannerParameters& operator=(const PlannerParameters& r);	virtual PlannerParameters& operator=(const PlannerParameters& r);
	virtual void copy(boost::shared_ptr<PlannerParameters const> r);	virtual void copy(boost::shared_ptr<PlannerParameters const> r);


	/// sets up the planner parameters to use the active joints of the robot	/// \brief sets up the planner parameters to use the active joints of the robot
	virtual void SetRobotActiveJoints(RobotBasePtr robot);	virtual void SetRobotActiveJoints(RobotBasePtr robot);


		/** \brief sets up the planner parameters to use the configuration
		specification space

		The configuraiton groups should point to controllable target ob
		jects. By default, this includes:
		- joint_values
		- joint_velocities
		- affine_transform
		- affine_velocities
		- grab
		The following internal parameters will be set:
		- _diffstatefn
		- _distmetricfn - weights used for distance metric are retrieve
		d at this time and stored
		- _samplefn
		- _sampleneighfn
		- _setstatefn
		- _getstatefn
		- _neighstatefn
		- _checkpathconstraintsfn
		- _vConfigLowerLimit
		- _vConfigUpperLimit
		- _vConfigVelocityLimit
		- _vConfigAccelerationLimit
		- _vConfigResolution
		- vinitialconfig
		- _configurationspecification
		\throw openrave_exception If the configuration specification is
		invalid or points to targets that are not present in the environment.
		*/
		virtual void SetConfigurationSpecification(EnvironmentBasePtr env,
		const ConfigurationSpecification& spec);

		/// \brief veriries that the configuration space and all parameters
		are consistent
		///
		/// Assumes at minimum that _setstatefn and _getstatefn are set. C
		orrect environment should be
		/// locked when this function is called since _getstatefn will be c
		alled.
		/// \throw openrave_exception If not consistent, will throw an exce
		ption
		virtual void Validate() const;

	/// \brief the configuration specification in which the planner wor ks in. This specification is passed to the trajecotry creation modules.	/// \brief the configuration specification in which the planner wor ks in. This specification is passed to the trajecotry creation modules.
	ConfigurationSpecification _configurationspecification;	ConfigurationSpecification _configurationspecification;

	/// \brief Cost function on the state pace (optional).	/// \brief Cost function on the state pace (optional).
	///	///
	/// cost = _costfn(config)	/// cost = _costfn(config)
	/// \param cost the cost of being in the current state	/// \param cost the cost of being in the current state
	typedef boost::function<dReal(const std::vector<dReal>&)> CostFn;	typedef boost::function<dReal(const std::vector<dReal>&)> CostFn;
	CostFn _costfn;	CostFn _costfn;


	skipping to change at line 199	skipping to change at line 253
	\param qdelta the delta to add	\param qdelta the delta to add
	\param fromgoal 1 if q is coming from a goal state, 0 if it is coming from an initial state	\param fromgoal 1 if q is coming from a goal state, 0 if it is coming from an initial state

	In RRTs this is used for the extension operation. The new state is stored in the first parameter q.	In RRTs this is used for the extension operation. The new state is stored in the first parameter q.
	Note that the function can also add a filter to the final desti nation (like projecting onto a constraint manifold).	Note that the function can also add a filter to the final desti nation (like projecting onto a constraint manifold).
	*/	*/
	typedef boost::function<bool (std::vector<dReal>&,const std::vector <dReal>&, int)> NeighStateFn;	typedef boost::function<bool (std::vector<dReal>&,const std::vector <dReal>&, int)> NeighStateFn;
	NeighStateFn _neighstatefn;	NeighStateFn _neighstatefn;

	/// to specify multiple initial or goal configurations, put them in to the vector in series	/// to specify multiple initial or goal configurations, put them in to the vector in series

	/// (note: not all planners support multiple goals)	/// size always has to be a multiple of GetDOF()
		/// note: not all planners support multiple goals
	std::vector<dReal> vinitialconfig, vgoalconfig;	std::vector<dReal> vinitialconfig, vgoalconfig;

	/// \brief the absolute limits of the configuration space.	/// \brief the absolute limits of the configuration space.
	std::vector<dReal> _vConfigLowerLimit, _vConfigUpperLimit;	std::vector<dReal> _vConfigLowerLimit, _vConfigUpperLimit;

	/// \brief the absolute velocity limits of each DOF of the configur ation space.	/// \brief the absolute velocity limits of each DOF of the configur ation space.
	std::vector<dReal> _vConfigVelocityLimit;	std::vector<dReal> _vConfigVelocityLimit;

	/// \brief the absolute acceleration limits of each DOF of the conf iguration space.	/// \brief the absolute acceleration limits of each DOF of the conf iguration space.
	std::vector<dReal> _vConfigAccelerationLimit;	std::vector<dReal> _vConfigAccelerationLimit;

	skipping to change at line 240	skipping to change at line 295
	/// \brief The serialized planner parameters to pass to the path op timizer.	/// \brief The serialized planner parameters to pass to the path op timizer.
	///	///
	/// For example: std::stringstream(_sPostProcessingParameters) >> _ parameters;	/// For example: std::stringstream(_sPostProcessingParameters) >> _ parameters;
	std::string _sPostProcessingParameters;	std::string _sPostProcessingParameters;

	/// \brief Extra parameters data that does not fit within this plan ner parameters structure, but is still important not to lose all the inform ation.	/// \brief Extra parameters data that does not fit within this plan ner parameters structure, but is still important not to lose all the inform ation.
	std::string _sExtraParameters;	std::string _sExtraParameters;

	/// \brief Return the degrees of freedom of the planning configurat ion space	/// \brief Return the degrees of freedom of the planning configurat ion space
	virtual int GetDOF() const {	virtual int GetDOF() const {

	return (int)_vConfigLowerLimit.size();	return _configurationspecification.GetDOF();
	}	}

	protected:	protected:
	inline boost::shared_ptr<PlannerBase::PlannerParameters> shared_par ameters() {	inline boost::shared_ptr<PlannerBase::PlannerParameters> shared_par ameters() {
	return boost::static_pointer_cast<PlannerBase::PlannerParameter s>(shared_from_this());	return boost::static_pointer_cast<PlannerBase::PlannerParameter s>(shared_from_this());
	}	}
	inline boost::shared_ptr<PlannerBase::PlannerParameters const > sha red_parameters_const() const {	inline boost::shared_ptr<PlannerBase::PlannerParameters const > sha red_parameters_const() const {
	return boost::static_pointer_cast<PlannerBase::PlannerParameter s const>(shared_from_this());	return boost::static_pointer_cast<PlannerBase::PlannerParameter s const>(shared_from_this());
	}	}


End of changes. 5 change blocks.
	3 lines changed or deleted	69 lines changed or added

	planningutils.h	planningutils.h

	skipping to change at line 43	skipping to change at line 43
	///	///
	/// Return 0 if jitter failed and robot is in collision, -1 if robot origin ally not in collision, 1 if jitter succeeded and position is different.	/// Return 0 if jitter failed and robot is in collision, -1 if robot origin ally not in collision, 1 if jitter succeeded and position is different.
	OPENRAVE_API int JitterActiveDOF(RobotBasePtr robot,int nMaxIterations=5000 ,dReal fRand=0.03f,const PlannerBase::PlannerParameters::NeighStateFn& neig hstatefn = PlannerBase::PlannerParameters::NeighStateFn());	OPENRAVE_API int JitterActiveDOF(RobotBasePtr robot,int nMaxIterations=5000 ,dReal fRand=0.03f,const PlannerBase::PlannerParameters::NeighStateFn& neig hstatefn = PlannerBase::PlannerParameters::NeighStateFn());

	/// \brief Jitters the transform of a body until it escapes collision.	/// \brief Jitters the transform of a body until it escapes collision.
	OPENRAVE_API bool JitterTransform(KinBodyPtr pbody, float fJitter, int nMax Iterations=1000);	OPENRAVE_API bool JitterTransform(KinBodyPtr pbody, float fJitter, int nMax Iterations=1000);

	/** \brief validates a trajectory with respect to the planning constraints. <b>[multi-thread safe]</b>	/** \brief validates a trajectory with respect to the planning constraints. <b>[multi-thread safe]</b>

	checks internal data structures and verifies that all trajectory via po ints do not violate joint position, velocity, and acceleration limits.	checks internal data structures and verifies that all trajectory via po ints do not violate joint position, velocity, and acceleration limits.

		\param parameters the planner parameters passed to the planner that ret urned the trajectory. If not initialized, will attempt to create a new Plan nerParameters structure from trajectory->GetConfigurationSpecification()
	\param trajectory trajectory of points to be checked	\param trajectory trajectory of points to be checked

	\param parameters the planner parameters passed to the planner that ret urned the trajectory
	\param samplingstep If == 0, then will only test the supports points in trajectory->GetPoints(). If > 0, then will sample the trajectory at this t ime interval and check that smoothness is satisfied along with segment cons traints.	\param samplingstep If == 0, then will only test the supports points in trajectory->GetPoints(). If > 0, then will sample the trajectory at this t ime interval and check that smoothness is satisfied along with segment cons traints.
	\throw openrave_exception If the trajectory is invalid, will throw ORE_ InconsistentConstraints.	\throw openrave_exception If the trajectory is invalid, will throw ORE_ InconsistentConstraints.
	*/	*/
	OPENRAVE_API void VerifyTrajectory(PlannerBase::PlannerParametersConstPtr p arameters, TrajectoryBaseConstPtr trajectory, dReal samplingstep=0.002);	OPENRAVE_API void VerifyTrajectory(PlannerBase::PlannerParametersConstPtr p arameters, TrajectoryBaseConstPtr trajectory, dReal samplingstep=0.002);


	/** \brief Smooth the trajectory points consisting of active dofs of the ro bot while avoiding collisions. <b>[multi-thread safe]</b>	/** \brief Smooth the trajectory points to avoiding collisions by extractin g and using the currently set active dofs of the robot. <b>[multi-thread sa fe]</b>

	Only initial and goal configurations are preserved.	Only initial and goal configurations are preserved.

		The velocities for the current trajectory are overwritten.
		The returned trajectory will contain data only for the currenstly set a
		ctive dofs of the robot.
	\param traj the trajectory that initially contains the input points, it is modified to contain the new re-timed data.	\param traj the trajectory that initially contains the input points, it is modified to contain the new re-timed data.
	\param robot use the robot's active dofs to initialize the trajectory s pace	\param robot use the robot's active dofs to initialize the trajectory s pace
	\param plannername the name of the planner to use to smooth. If empty, will use the default trajectory re-timer.	\param plannername the name of the planner to use to smooth. If empty, will use the default trajectory re-timer.
	\param hastimestamps if true, use the already initialized timestamps of the trajectory	\param hastimestamps if true, use the already initialized timestamps of the trajectory
	\param plannerparameters XML string to be appended to PlannerBase::Plan nerParameters::_sExtraParameters passed in to the planner.	\param plannerparameters XML string to be appended to PlannerBase::Plan nerParameters::_sExtraParameters passed in to the planner.
	*/	*/

	OPENRAVE_API void SmoothActiveDOFTrajectory(TrajectoryBasePtr traj, RobotBa sePtr robot, bool hastimestamps=false, dReal fmaxvelmult=1, const std::stri ng& plannername="", const std::string& plannerparameters="");	OPENRAVE_API void SmoothActiveDOFTrajectory(TrajectoryBasePtr traj, RobotBa sePtr robot, dReal fmaxvelmult=1, dReal fmaxaccelmult=1, const std::string& plannername="", const std::string& plannerparameters="");


	/** \brief Smooth the trajectory points consisting of affine dofs while avo iding collisions. <b>[multi-thread safe]</b>	/** \brief Smooth the trajectory points consisting of affine transformation values while avoiding collisions. <b>[multi-thread safe]</b>

	Only initial and goal configurations are preserved.	Only initial and goal configurations are preserved.
	\param traj the trajectory that initially contains the input points, it is modified to contain the new re-timed data.	\param traj the trajectory that initially contains the input points, it is modified to contain the new re-timed data.
	\param maxvelocities the max velocities of each dof	\param maxvelocities the max velocities of each dof
	\param maxaccelerations the max acceleration of each dof	\param maxaccelerations the max acceleration of each dof
	\param plannername the name of the planner to use to smooth. If empty, will use the default trajectory re-timer.	\param plannername the name of the planner to use to smooth. If empty, will use the default trajectory re-timer.
	\param hastimestamps if true, use the already initialized timestamps of the trajectory	\param hastimestamps if true, use the already initialized timestamps of the trajectory
	\param plannerparameters XML string to be appended to PlannerBase::Plan nerParameters::_sExtraParameters passed in to the planner.	\param plannerparameters XML string to be appended to PlannerBase::Plan nerParameters::_sExtraParameters passed in to the planner.
	*/	*/

	OPENRAVE_API void SmoothAffineTrajectory(TrajectoryBasePtr traj, const std: :vector<dReal>& maxvelocities, const std::vector<dReal>& maxaccelerations, bool hastimestamps=false, const std::string& plannername="", const std::str ing& plannerparameters="");	OPENRAVE_API void SmoothAffineTrajectory(TrajectoryBasePtr traj, const std: :vector<dReal>& maxvelocities, const std::vector<dReal>& maxaccelerations, const std::string& plannername="", const std::string& plannerparameters="") ;


	/** \brief Retime the trajectory points consisting of active dofs. <b>[mult	/** \brief Smooth the trajectory points to avoiding collisions by extractin
	i-thread safe]</b>	g and using the positional data from the trajectory. <b>[multi-thread safe]
		</b>

		Only initial and goal configurations are preserved.
		The velocities for the current trajectory are overwritten.
		The returned trajectory will contain data only for the currenstly set a
		ctive dofs of the robot.
		\param traj the trajectory that initially contains the input points, it
		is modified to contain the new re-timed data.
		\param plannername the name of the planner to use to smooth. If empty,
		will use the default trajectory re-timer.
		\param hastimestamps if true, use the already initialized timestamps of
		the trajectory
		\param plannerparameters XML string to be appended to PlannerBase::Plan
		nerParameters::_sExtraParameters passed in to the planner.
		*/
		OPENRAVE_API void SmoothTrajectory(TrajectoryBasePtr traj, dReal fmaxvelmul
		t=1, dReal fmaxaccelmult=1, const std::string& plannername="", const std::s
		tring& plannerparameters="");

		/** \brief Retime the trajectory points by extracting and using the current
		ly set active dofs of the robot. <b>[multi-thread safe]</b>

	Collision is not checked. Every waypoint in the trajectory is guarantee d to be hit.	Collision is not checked. Every waypoint in the trajectory is guarantee d to be hit.

		The velocities for the current trajectory are overwritten.
		The returned trajectory will contain data only for the currenstly set a
		ctive dofs of the robot.
	\param traj the trajectory that initially contains the input points, it is modified to contain the new re-timed data.	\param traj the trajectory that initially contains the input points, it is modified to contain the new re-timed data.
	\param robot use the robot's active dofs to initialize the trajectory s pace	\param robot use the robot's active dofs to initialize the trajectory s pace
	\param plannername the name of the planner to use to retime. If empty, will use the default trajectory re-timer.	\param plannername the name of the planner to use to retime. If empty, will use the default trajectory re-timer.
	\param hastimestamps if true, use the already initialized timestamps of the trajectory	\param hastimestamps if true, use the already initialized timestamps of the trajectory
	\param plannerparameters XML string to be appended to PlannerBase::Plan nerParameters::_sExtraParameters passed in to the planner.	\param plannerparameters XML string to be appended to PlannerBase::Plan nerParameters::_sExtraParameters passed in to the planner.
	*/	*/

	OPENRAVE_API void RetimeActiveDOFTrajectory(TrajectoryBasePtr traj, RobotBa sePtr robot, bool hastimestamps=false, dReal fmaxvelmult=1, const std::stri ng& plannername="", const std::string& plannerparameters="");	OPENRAVE_API void RetimeActiveDOFTrajectory(TrajectoryBasePtr traj, RobotBa sePtr robot, bool hastimestamps=false, dReal fmaxvelmult=1, dReal fmaxaccel mult=1, const std::string& plannername="", const std::string& plannerparame ters="");


	/** \brief Retime the trajectory points consisting of affine dofs while avo iding collisions. <b>[multi-thread safe]</b>	/** \brief Retime the trajectory points consisting of affine transformation values while avoiding collisions. <b>[multi-thread safe]</b>

	Collision is not checked. Every waypoint in the trajectory is guarantee d to be hit.	Collision is not checked. Every waypoint in the trajectory is guarantee d to be hit.

		The velocities for the current trajectory are overwritten.
	\param traj the trajectory that initially contains the input points, it is modified to contain the new re-timed data.	\param traj the trajectory that initially contains the input points, it is modified to contain the new re-timed data.
	\param maxvelocities the max velocities of each dof	\param maxvelocities the max velocities of each dof
	\param maxaccelerations the max acceleration of each dof	\param maxaccelerations the max acceleration of each dof
	\param plannername the name of the planner to use to retime. If empty, will use the default trajectory re-timer.	\param plannername the name of the planner to use to retime. If empty, will use the default trajectory re-timer.
	\param hastimestamps if true, use the already initialized timestamps of the trajectory	\param hastimestamps if true, use the already initialized timestamps of the trajectory
	\param plannerparameters XML string to be appended to PlannerBase::Plan nerParameters::_sExtraParameters passed in to the planner.	\param plannerparameters XML string to be appended to PlannerBase::Plan nerParameters::_sExtraParameters passed in to the planner.
	*/	*/
	OPENRAVE_API void RetimeAffineTrajectory(TrajectoryBasePtr traj, const std: :vector<dReal>& maxvelocities, const std::vector<dReal>& maxaccelerations, bool hastimestamps=false, const std::string& plannername="", const std::str ing& plannerparameters="");	OPENRAVE_API void RetimeAffineTrajectory(TrajectoryBasePtr traj, const std: :vector<dReal>& maxvelocities, const std::vector<dReal>& maxaccelerations, bool hastimestamps=false, const std::string& plannername="", const std::str ing& plannerparameters="");


		/** \brief Retime the trajectory points using all the positional data from
		the trajectory. <b>[multi-thread safe]</b>

		Collision is not checked. Every waypoint in the trajectory is guarantee
		d to be hit.
		The velocities for the current trajectory are overwritten.
		The returned trajectory will contain data only for the currenstly set a
		ctive dofs of the robot.
		\param traj the trajectory that initially contains the input points, it
		is modified to contain the new re-timed data.
		\param plannername the name of the planner to use to retime. If empty,
		will use the default trajectory re-timer.
		\param hastimestamps if true, use the already initialized timestamps of
		the trajectory
		\param plannerparameters XML string to be appended to PlannerBase::Plan
		nerParameters::_sExtraParameters passed in to the planner.
		*/
		OPENRAVE_API void RetimeTrajectory(TrajectoryBasePtr traj, bool hastimestam
		ps=false, dReal fmaxvelmult=1, dReal fmaxaccelmult=1, const std::string& pl
		annername="", const std::string& plannerparameters="");

	/** \brief Inserts a waypoint into a trajectory at the index specified, and retimes the segment before and after the trajectory. <b>[multi-thread safe ]</b>	/** \brief Inserts a waypoint into a trajectory at the index specified, and retimes the segment before and after the trajectory. <b>[multi-thread safe ]</b>

	Collision is not checked on the modified segments of the trajectory.	Collision is not checked on the modified segments of the trajectory.
	\param index The index where to start modifying the trajectory.	\param index The index where to start modifying the trajectory.
	\param dofvalues the configuration to insert into the trajectcory (acti ve dof values of the robot)	\param dofvalues the configuration to insert into the trajectcory (acti ve dof values of the robot)
	\param dofvelocities the velocities that the inserted point should star t with	\param dofvelocities the velocities that the inserted point should star t with
	\param traj the trajectory that initially contains the input points, it is modified to contain the new re-timed data.	\param traj the trajectory that initially contains the input points, it is modified to contain the new re-timed data.
	\param robot use the robot's active dofs to initialize the trajectory s pace	\param robot use the robot's active dofs to initialize the trajectory s pace
	\param plannername the name of the planner to use to retime. If empty, will use the default trajectory re-timer.	\param plannername the name of the planner to use to retime. If empty, will use the default trajectory re-timer.
	*/	*/

	OPENRAVE_API void InsertActiveDOFWaypointWithRetiming(int index, const std:	OPENRAVE_API void InsertActiveDOFWaypointWithRetiming(int index, const std:
	:vector<dReal>& dofvalues, const std::vector<dReal>& dofvelocities, Traject	:vector<dReal>& dofvalues, const std::vector<dReal>& dofvelocities, Traject
	oryBasePtr traj, RobotBasePtr robot, dReal fmaxvelmult=1, const std::string	oryBasePtr traj, RobotBasePtr robot, dReal fmaxvelmult=1, dReal fmaxaccelmu
	& plannername="");	lt=1, const std::string& plannername="");

		/** \brief insert a waypoint in a timed trajectory and smooth so that the t
		rajectory always goes through the waypoint at the specified velocity.

		The PlannerParameters is automatically determined from the trajectory's
		configuration space
		\param[in] index The index where to insert the new waypoint. A negative
		value starts from the end.
		\param dofvalues the configuration to insert into the trajectcory (acti
		ve dof values of the robot)
		\param dofvelocities the velocities that the inserted point should star
		t with
		\param traj the trajectory that initially contains the input points, it
		is modified to contain the new re-timed data.
		\param plannername the name of the planner to use to smooth. If empty,
		will use the default trajectory smoother.
		*/
		OPENRAVE_API void InsertWaypointWithSmoothing(int index, const std::vector<
		dReal>& dofvalues, const std::vector<dReal>& dofvelocities, TrajectoryBaseP
		tr traj, dReal fmaxvelmult=1, dReal fmaxaccelmult=1, const std::string& pla
		nnername="");

	/// \brief convert the trajectory and all its points to a new specification	/// \brief convert the trajectory and all its points to a new specification
	OPENRAVE_API void ConvertTrajectorySpecification(TrajectoryBasePtr traj, co nst ConfigurationSpecification& spec);	OPENRAVE_API void ConvertTrajectorySpecification(TrajectoryBasePtr traj, co nst ConfigurationSpecification& spec);


	/// \brief reverses the order of the trajectory waypoints and times.	/** \brief computes the trajectory derivatives and modifies the trajetory c
		onfiguration to store them.

		If necessary will change the configuration specification of the traject
		ory.
		If more derivatives are requested than the trajectory supports, will ig
		nore them. For example, acceleration of a linear trajectory.
		\param traj the re-timed trajectory
		\param maxderiv the maximum derivative to assure. If 1, will assure vel
		ocities, if 2 will assure accelerations, etc.
		*/
		OPENRAVE_API void ComputeTrajectoryDerivatives(TrajectoryBasePtr traj, int
		maxderiv);

		/// \brief returns a new trajectory with the order of the waypoints and tim
		es reversed.
	///	///
	/// Velocities are just negated and the new trajectory is not guaranteed to be executable or valid	/// Velocities are just negated and the new trajectory is not guaranteed to be executable or valid
	OPENRAVE_API TrajectoryBasePtr ReverseTrajectory(TrajectoryBaseConstPtr tra j);	OPENRAVE_API TrajectoryBasePtr ReverseTrajectory(TrajectoryBaseConstPtr tra j);

	/// \brief merges the contents of multiple trajectories into one so that ev erything can be played simultaneously.	/// \brief merges the contents of multiple trajectories into one so that ev erything can be played simultaneously.
	///	///
	/// Each trajectory needs to have a 'deltatime' group for timestamps. The t rajectories cannot share common configuration data because only one	/// Each trajectory needs to have a 'deltatime' group for timestamps. The t rajectories cannot share common configuration data because only one
	/// trajectories's data can be set at a time.	/// trajectories's data can be set at a time.
	/// \throw openrave_exception throws an exception if the trajectory data is incompatible and cannot be merged.	/// \throw openrave_exception throws an exception if the trajectory data is incompatible and cannot be merged.
	OPENRAVE_API TrajectoryBasePtr MergeTrajectories(const std::list<Trajectory BaseConstPtr>& listtrajectories);	OPENRAVE_API TrajectoryBasePtr MergeTrajectories(const std::list<Trajectory BaseConstPtr>& listtrajectories);


	/** \brief sets the planner parameters structure from a configuration speci
	fication

	Attempt to set default values for all parameters
	*/
	OPENRAVE_API void SetPlannerParametersFromSpecification(PlannerBase::Planne
	rParametersPtr parameters, const ConfigurationSpecification& spec);

	/** \brief represents the DH parameters for one joint	/** \brief represents the DH parameters for one joint

	T = Z_1 X_1 Z_2 X_2 ... X_n Z_n	T = Z_1 X_1 Z_2 X_2 ... X_n Z_n

	where	where
	Z_i = [cos(theta) -sin(theta) 0 0; sin(theta) cos(theta) 0 0; 0 0 1 d]	Z_i = [cos(theta) -sin(theta) 0 0; sin(theta) cos(theta) 0 0; 0 0 1 d]
	X_i = [1 0 0 a; 0 cos(alpha) -sin(alpha) 0; 0 sin(alpha) cos(alpha) 0]	X_i = [1 0 0 a; 0 cos(alpha) -sin(alpha) 0; 0 sin(alpha) cos(alpha) 0]

	http://en.wikipedia.org/wiki/Denavit%E2%80%93Hartenberg_parameters	http://en.wikipedia.org/wiki/Denavit%E2%80%93Hartenberg_parameters
	*/	*/

	skipping to change at line 166	skipping to change at line 209
	\note The coordinate systems computed from the DH parameters do not mat ch the OpenRAVE link coordinate systems.	\note The coordinate systems computed from the DH parameters do not mat ch the OpenRAVE link coordinate systems.
	\param vparameters One set of parameters are returned for each joint. \ see DHParameter.	\param vparameters One set of parameters are returned for each joint. \ see DHParameter.
	\param tstart the initial transform in the body coordinate system to th e first joint	\param tstart the initial transform in the body coordinate system to th e first joint
	*/	*/
	OPENRAVE_API void GetDHParameters(std::vector<DHParameter>& vparameters, Ki nBodyConstPtr pbody);	OPENRAVE_API void GetDHParameters(std::vector<DHParameter>& vparameters, Ki nBodyConstPtr pbody);

	/// \brief Line collision	/// \brief Line collision
	class OPENRAVE_API LineCollisionConstraint	class OPENRAVE_API LineCollisionConstraint
	{	{
	public:	public:

	LineCollisionConstraint();	LineCollisionConstraint() RAVE_DEPRECATED;
	bool Check(PlannerBase::PlannerParametersWeakPtr _params, KinBodyPtr ro	/// \param listCheckCollisions initialize with these bodies to check en
	bot, const std::vector<dReal>& pQ0, const std::vector<dReal>& pQ1, Interval	vironment and self-collision with
	Type interval, PlannerBase::ConfigurationListPtr pvCheckedConfigurations);	LineCollisionConstraint(const std::list<KinBodyPtr>& listCheckCollision
		s, bool bCheckEnv=true);
		virtual ~LineCollisionConstraint() {
		}

		/// \brief set user check fucntions
		///
		/// Two functions can be set, one to be called before check collision a
		nd one after.
		/// \param bCallAfterCheckCollision if set, function will be called aft
		er check collision functions.
		virtual void SetUserCheckFunction(const boost::function<bool() >& userc
		heckfn, bool bCallAfterCheckCollision=false);

		/// \deprecated (12/04/23)
		virtual bool Check(PlannerBase::PlannerParametersWeakPtr _params, KinBo
		dyPtr body, const std::vector<dReal>& pQ0, const std::vector<dReal>& pQ1, I
		ntervalType interval, PlannerBase::ConfigurationListPtr pvCheckedConfigurat
		ions) RAVE_DEPRECATED;

		/// \brief checks line collision. Uses the constructor's self-collision
		s
		virtual bool Check(PlannerBase::PlannerParametersWeakPtr _params, const
		std::vector<dReal>& pQ0, const std::vector<dReal>& pQ1, IntervalType inter
		val, PlannerBase::ConfigurationListPtr pvCheckedConfigurations);

	protected:	protected:

		virtual bool _CheckState();

	std::vector<dReal> _vtempconfig, dQ;	std::vector<dReal> _vtempconfig, dQ;
	CollisionReportPtr _report;	CollisionReportPtr _report;

		std::list<KinBodyPtr> _listCheckSelfCollisions;
		bool _bCheckEnv;
		boost::array< boost::function<bool() >, 2> _usercheckfns;
	};	};

	/// \brief simple distance metric based on joint weights	/// \brief simple distance metric based on joint weights
	class OPENRAVE_API SimpleDistanceMetric	class OPENRAVE_API SimpleDistanceMetric
	{	{
	public:	public:
	SimpleDistanceMetric(RobotBasePtr robot);	SimpleDistanceMetric(RobotBasePtr robot);
	dReal Eval(const std::vector<dReal>& c0, const std::vector<dReal>& c1);	dReal Eval(const std::vector<dReal>& c0, const std::vector<dReal>& c1);
	protected:	protected:
	RobotBasePtr _robot;	RobotBasePtr _robot;

	skipping to change at line 209	skipping to change at line 272
	///	///
	/// \param nummaxsamples the max samples to query from a particular workspa ce goal. This does not necessarily mean every goal will have this many samp les.	/// \param nummaxsamples the max samples to query from a particular workspa ce goal. This does not necessarily mean every goal will have this many samp les.
	/// \param nummaxtries number of attemps to return a goal per Sample call.	/// \param nummaxtries number of attemps to return a goal per Sample call.
	/// \param fsampleprob The probability to attempt to sample a goal	/// \param fsampleprob The probability to attempt to sample a goal
	class OPENRAVE_API ManipulatorIKGoalSampler	class OPENRAVE_API ManipulatorIKGoalSampler
	{	{
	public:	public:
	ManipulatorIKGoalSampler(RobotBase::ManipulatorConstPtr pmanip, const s td::list<IkParameterization>&listparameterizations, int nummaxsamples=20, i nt nummaxtries=10, dReal fsampleprob=1);	ManipulatorIKGoalSampler(RobotBase::ManipulatorConstPtr pmanip, const s td::list<IkParameterization>&listparameterizations, int nummaxsamples=20, i nt nummaxtries=10, dReal fsampleprob=1);
	virtual ~ManipulatorIKGoalSampler() {	virtual ~ManipulatorIKGoalSampler() {
	}	}

	//void SetCheckPathConstraintsFn(const PlannerBase::PlannerParameters::
	CheckPathConstraintFn& checkfn)	/// \brief if can sample, returns IkReturn pointer
		virtual IkReturnPtr Sample();
	virtual bool Sample(std::vector<dReal>& vgoal);	virtual bool Sample(std::vector<dReal>& vgoal);


		/// \brief samples the rests of the samples until cannot be sampled any
		more.
		///
		/// \param vsamples vector is rest with samples
		/// \return true if a sample was inserted into vsamples
		bool SampleAll(std::list<IkReturnPtr>& samples);

		//void SetCheckPathConstraintsFn(const PlannerBase::PlannerParameters::
		CheckPathConstraintFn& checkfn)

	virtual int GetIkParameterizationIndex(int index);	virtual int GetIkParameterizationIndex(int index);
	virtual void SetSamplingProb(dReal fsampleprob);	virtual void SetSamplingProb(dReal fsampleprob);

	/// \brief set a jitter distance for the goal	/// \brief set a jitter distance for the goal
	///	///
	/// \param maxdist If > 0, allows jittering of the goal IK if they caus e the robot to be in collision and no IK solutions to be found	/// \param maxdist If > 0, allows jittering of the goal IK if they caus e the robot to be in collision and no IK solutions to be found
	virtual void SetJitter(dReal maxdist);	virtual void SetJitter(dReal maxdist);

	protected:	protected:
	struct SampleInfo	struct SampleInfo

	skipping to change at line 234	skipping to change at line 308
	SpaceSamplerBasePtr _psampler;	SpaceSamplerBasePtr _psampler;
	int _orgindex;	int _orgindex;
	};	};
	RobotBasePtr _probot;	RobotBasePtr _probot;
	RobotBase::ManipulatorConstPtr _pmanip;	RobotBase::ManipulatorConstPtr _pmanip;
	int _nummaxsamples, _nummaxtries;	int _nummaxsamples, _nummaxtries;
	std::list<SampleInfo> _listsamples;	std::list<SampleInfo> _listsamples;
	SpaceSamplerBasePtr _pindexsampler;	SpaceSamplerBasePtr _pindexsampler;
	dReal _fsampleprob, _fjittermaxdist;	dReal _fsampleprob, _fjittermaxdist;
	CollisionReportPtr _report;	CollisionReportPtr _report;

	std::vector< std::vector<dReal> > _viksolutions;	std::vector< IkReturnPtr > _vikreturns;
	std::list<int> _listreturnedsamples;	std::list<int> _listreturnedsamples;
	std::vector<dReal> _vfreestart;	std::vector<dReal> _vfreestart;

	int _tempikindex; ///< if _viksolutions.size() > 0, points to the origi nal ik index of those solutions	int _tempikindex; ///< if _vikreturns.size() > 0, points to the origina l ik index of those solutions
	};	};

	typedef boost::shared_ptr<ManipulatorIKGoalSampler> ManipulatorIKGoalSample rPtr;	typedef boost::shared_ptr<ManipulatorIKGoalSampler> ManipulatorIKGoalSample rPtr;

	} // planningutils	} // planningutils
	} // OpenRAVE	} // OpenRAVE

	#endif	#endif

End of changes. 23 change blocks.
	30 lines changed or deleted	148 lines changed or added

	robot.h	robot.h
	// -- coding: utf-8 --	// -- coding: utf-8 --

	// Copyright (C) 2006-2011 Rosen Diankov <rosen.diankov@gmail.com>	// Copyright (C) 2006-2012 Rosen Diankov <rosen.diankov@gmail.com>
	//	//
	// This file is part of OpenRAVE.	// This file is part of OpenRAVE.
	// OpenRAVE is free software: you can redistribute it and/or modify	// OpenRAVE is free software: you can redistribute it and/or modify
	// it under the terms of the GNU Lesser General Public License as published by	// it under the terms of the GNU Lesser General Public License as published by
	// the Free Software Foundation, either version 3 of the License, or	// the Free Software Foundation, either version 3 of the License, or
	// at your option) any later version.	// at your option) any later version.
	//	//
	// This program is distributed in the hope that it will be useful,	// This program is distributed in the hope that it will be useful,
	// but WITHOUT ANY WARRANTY; without even the implied warranty of	// but WITHOUT ANY WARRANTY; without even the implied warranty of
	// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the	// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the

	skipping to change at line 33	skipping to change at line 33
	#define OPENRAVE_ROBOT_H	#define OPENRAVE_ROBOT_H

	namespace OpenRAVE {	namespace OpenRAVE {

	/** \brief <b>[interface]</b> A robot is a kinematic body that has attached manipulators, sensors, and controllers. <b>If not specified, method is not multi-thread safe.</b> See \ref arch_robot.	/** \brief <b>[interface]</b> A robot is a kinematic body that has attached manipulators, sensors, and controllers. <b>If not specified, method is not multi-thread safe.</b> See \ref arch_robot.
	\ingroup interfaces	\ingroup interfaces
	*/	*/
	class OPENRAVE_API RobotBase : public KinBody	class OPENRAVE_API RobotBase : public KinBody
	{	{
	public:	public:

		/// \brief holds all user-set manipulator information used to initializ
		e the Manipulator class
		class OPENRAVE_API ManipulatorInfo
		{
		public:
		ManipulatorInfo() : _vdirection(0,0,1) {
		}
		virtual ~ManipulatorInfo() {
		}

		std::string _name;
		std::string _sBaseLinkName, _sEffectorLinkName; ///< name of the ba
		se and effector links of the robot used to determine the chain
		Transform _tLocalTool;
		std::vector<dReal> _vClosingDirection; ///< the normal direction to
		move joints to 'close' the hand
		Vector _vdirection;
		std::string _sIkSolverXMLId; ///< xml id of the IkSolver interface
		to attach
		std::vector<std::string> _vGripperJointNames; ///< names of
		the gripper joints
		};
		typedef boost::shared_ptr<ManipulatorInfo> ManipulatorInfoPtr;

	/// \brief Defines a chain of joints for an arm and set of joints for a gripper. Simplifies operating with them.	/// \brief Defines a chain of joints for an arm and set of joints for a gripper. Simplifies operating with them.
	class OPENRAVE_API Manipulator : public boost::enable_shared_from_this< Manipulator>	class OPENRAVE_API Manipulator : public boost::enable_shared_from_this< Manipulator>
	{	{

	Manipulator(RobotBasePtr probot);	Manipulator(RobotBasePtr probot, const ManipulatorInfo& info);
	Manipulator(const Manipulator &r);	Manipulator(const Manipulator &r);

	Manipulator(RobotBasePtr probot, const Manipulator &r);
		/// \brief can switch the underyling robot
		Manipulator(RobotBasePtr probot, boost::shared_ptr<Manipulator cons
		t> r);

	public:	public:
	virtual ~Manipulator();	virtual ~Manipulator();


	/// \brief Return the transformation of the end effector (manipulat	/// \brief return a serializable info holding everything to initial
	or frame).	ize a manipulator
		inline const ManipulatorInfo& GetInfo() const {
		return _info;
		}

		/// \brief Return the transformation of the manipulator frame
	///	///

	/// All inverse kinematics and grasping queries are specifying this	/// The manipulator frame is defined by the the end effector link p
	frame.	osition * GetLocalToolTransform()
		/// All inverse kinematics and jacobian queries are specifying this
		frame.
	virtual Transform GetTransform() const;	virtual Transform GetTransform() const;


		/// \brief return the linear/angular velocity of the manipulator co
		ordinate system
		virtual std::pair<Vector,Vector> GetVelocity() const;

	virtual Transform GetEndEffectorTransform() const {	virtual Transform GetEndEffectorTransform() const {
	return GetTransform();	return GetTransform();
	}	}

	virtual const std::string& GetName() const {	virtual const std::string& GetName() const {

	return _name;	return _info._name;
	}	}
	virtual RobotBasePtr GetRobot() const {	virtual RobotBasePtr GetRobot() const {

	return RobotBasePtr(_probot);	return RobotBasePtr(__probot);
	}	}

	/// \brief Sets the ik solver and initializes it with the current m anipulator.	/// \brief Sets the ik solver and initializes it with the current m anipulator.
	///	///
	/// Due to complications with translation,rotation,direction,and ra y ik,	/// Due to complications with translation,rotation,direction,and ra y ik,

	/// the ik solver should take into account the grasp transform (_tL ocalTool) internally.	/// the ik solver should take into account the grasp transform (_in fo._tLocalTool) internally.
	/// The actual ik primitives are transformed into the base frame on ly.	/// The actual ik primitives are transformed into the base frame on ly.
	virtual bool SetIkSolver(IkSolverBasePtr iksolver);	virtual bool SetIkSolver(IkSolverBasePtr iksolver);

	/// \brief Returns the currently set ik solver	/// \brief Returns the currently set ik solver

	virtual IkSolverBasePtr GetIkSolver() const {	virtual IkSolverBasePtr GetIkSolver() const;
	return _pIkSolver;
	}

	/// \deprecated (11/02/08) use GetIkSolver()->GetNumFreeParameters(
	)
	virtual int GetNumFreeParameters() const RAVE_DEPRECATED;

	/// \deprecated (11/02/08) use GetIkSolver()->GetFreeParameters()
	virtual bool GetFreeParameters(std::vector<dReal>& vFreeParameters)
	const RAVE_DEPRECATED;

	/// \brief the base used for the iksolver	/// \brief the base used for the iksolver
	virtual LinkPtr GetBase() const {	virtual LinkPtr GetBase() const {

	return _pBase;	return __pBase;
	}	}

	/// \brief the end effector link (used to define workspace distance )	/// \brief the end effector link (used to define workspace distance )
	virtual LinkPtr GetEndEffector() const {	virtual LinkPtr GetEndEffector() const {

	return _pEndEffector;	return __pEffector;
	}	}

	/// \brief Return transform with respect to end effector defining t he grasp coordinate system	/// \brief Return transform with respect to end effector defining t he grasp coordinate system
	virtual Transform GetLocalToolTransform() const {	virtual Transform GetLocalToolTransform() const {

	return _tLocalTool;	return _info._tLocalTool;
	}	}

	/// \brief Sets the local tool transform with respect to the end ef fector.	/// \brief Sets the local tool transform with respect to the end ef fector.
	///	///
	/// Because this call will change manipulator hash, it resets the l oaded IK and sets the Prop_RobotManipulatorTool message.	/// Because this call will change manipulator hash, it resets the l oaded IK and sets the Prop_RobotManipulatorTool message.
	virtual void SetLocalToolTransform(const Transform& t);	virtual void SetLocalToolTransform(const Transform& t);

	/// \brief new name for manipulator	/// \brief new name for manipulator
	///	///
	/// \throw openrave_exception if name is already used in another ma nipulator	/// \throw openrave_exception if name is already used in another ma nipulator

	skipping to change at line 121	skipping to change at line 143

	/// \brief Return the indices of the DOFs of the arm (used for IK, etc).	/// \brief Return the indices of the DOFs of the arm (used for IK, etc).
	///	///
	/// Usually the DOF indices from pBase to pEndEffector	/// Usually the DOF indices from pBase to pEndEffector
	virtual const std::vector<int>& GetArmIndices() const {	virtual const std::vector<int>& GetArmIndices() const {
	return __varmdofindices;	return __varmdofindices;
	}	}

	/// \brief return the normal direction to move joints to 'close' th e hand	/// \brief return the normal direction to move joints to 'close' th e hand
	virtual const std::vector<dReal>& GetClosingDirection() const {	virtual const std::vector<dReal>& GetClosingDirection() const {

	return _vClosingDirection;	return _info._vClosingDirection;
	}	}

	/// \brief direction of palm/head/manipulator used for approaching. defined inside the manipulator/grasp coordinate system	/// \brief direction of palm/head/manipulator used for approaching. defined inside the manipulator/grasp coordinate system
	virtual Vector GetLocalToolDirection() const {	virtual Vector GetLocalToolDirection() const {

	return _vdirection;	return _info._vdirection;
	}	}

	/// \deprecated (11/10/15) use GetLocalToolDirection	/// \deprecated (11/10/15) use GetLocalToolDirection
	virtual Vector GetDirection() const {	virtual Vector GetDirection() const {
	return GetLocalToolDirection();	return GetLocalToolDirection();
	}	}

	/// \brief Find a close solution to the current robot's joint value s.	/// \brief Find a close solution to the current robot's joint value s.
	///	///
	/// The function is a wrapper around the IkSolver interface.	/// The function is a wrapper around the IkSolver interface.
	/// Note that the solution returned is not guaranteed to be the clo sest solution. In order to compute that, will have to	/// Note that the solution returned is not guaranteed to be the clo sest solution. In order to compute that, will have to
	/// compute all the ik solutions using FindIKSolutions.	/// compute all the ik solutions using FindIKSolutions.
	/// \param param The transformation of the end-effector in the glob al coord system	/// \param param The transformation of the end-effector in the glob al coord system
	/// \param solution Will be of size GetArmIndices().size() and cont ain the best solution	/// \param solution Will be of size GetArmIndices().size() and cont ain the best solution
	/// \param[in] filteroptions A bitmask of \ref IkFilterOptions valu es controlling what is checked for each ik solution.	/// \param[in] filteroptions A bitmask of \ref IkFilterOptions valu es controlling what is checked for each ik solution.
	virtual bool FindIKSolution(const IkParameterization& param, std::v ector<dReal>& solution, int filteroptions) const;	virtual bool FindIKSolution(const IkParameterization& param, std::v ector<dReal>& solution, int filteroptions) const;
	virtual bool FindIKSolution(const IkParameterization& param, const std::vector<dReal>& vFreeParameters, std::vector<dReal>& solution, int filt eroptions) const;	virtual bool FindIKSolution(const IkParameterization& param, const std::vector<dReal>& vFreeParameters, std::vector<dReal>& solution, int filt eroptions) const;

		virtual bool FindIKSolution(const IkParameterization& param, int fi
		lteroptions, IkReturnPtr ikreturn) const;
		virtual bool FindIKSolution(const IkParameterization& param, const
		std::vector<dReal>& vFreeParameters, int filteroptions, IkReturnPtr ikretur
		n) const;

	/// \brief Find all the IK solutions for the given end effector tra nsform	/// \brief Find all the IK solutions for the given end effector tra nsform
	///	///
	/// \param param The transformation of the end-effector in the glob al coord system	/// \param param The transformation of the end-effector in the glob al coord system
	/// \param solutions An array of all solutions, each element in sol utions is of size GetArmIndices().size()	/// \param solutions An array of all solutions, each element in sol utions is of size GetArmIndices().size()
	/// \param[in] filteroptions A bitmask of \ref IkFilterOptions valu es controlling what is checked for each ik solution.	/// \param[in] filteroptions A bitmask of \ref IkFilterOptions valu es controlling what is checked for each ik solution.
	virtual bool FindIKSolutions(const IkParameterization& param, std:: vector<std::vector<dReal> >& solutions, int filteroptions) const;	virtual bool FindIKSolutions(const IkParameterization& param, std:: vector<std::vector<dReal> >& solutions, int filteroptions) const;
	virtual bool FindIKSolutions(const IkParameterization& param, const std::vector<dReal>& vFreeParameters, std::vector<std::vector<dReal> >& sol utions, int filteroptions) const;	virtual bool FindIKSolutions(const IkParameterization& param, const std::vector<dReal>& vFreeParameters, std::vector<std::vector<dReal> >& sol utions, int filteroptions) const;

		virtual bool FindIKSolutions(const IkParameterization& param, int f
		ilteroptions, std::vector<IkReturnPtr>& vikreturns) const;
		virtual bool FindIKSolutions(const IkParameterization& param, const
		std::vector<dReal>& vFreeParameters, int filteroptions, std::vector<IkRetu
		rnPtr>& vikreturns) const;

	/** \brief returns the parameterization of a given IK type for the current manipulator position.	/** \brief returns the parameterization of a given IK type for the current manipulator position.

	Ideally pluging the returned ik parameterization into FindIkSol ution should return the a manipulator configuration	Ideally pluging the returned ik parameterization into FindIkSol ution should return the a manipulator configuration
	such that a new call to GetIkParameterization returns the same values. In other words:	such that a new call to GetIkParameterization returns the same values. In other words:
	\code	\code
	ikparam = manip->GetIkParameterization(iktype);	ikparam = manip->GetIkParameterization(iktype);
	... move robot	... move robot
	std::vector<dReal> sol;	std::vector<dReal> sol;
	if( FindIKSolution(ikparam,sol, filteroptions) ) {	if( FindIKSolution(ikparam,sol, filteroptions) ) {
	manip->GetRobot()->SetActiveDOFs(manip->GetArmIndices());	manip->GetRobot()->SetActiveDOFs(manip->GetArmIndices());
	manip->GetRobot()->SetActiveDOFValues(sol);	manip->GetRobot()->SetActiveDOFValues(sol);
	BOOST_ASSERT( dist(manip->GetIkParameterization(iktype), ik param) <= epsilon );	BOOST_ASSERT( dist(manip->GetIkParameterization(iktype), ik param) <= epsilon );
	}	}
	\endcode	\endcode
	\param iktype the type of parameterization to request	\param iktype the type of parameterization to request

		\param inworld if true will return the parameterization in the world coordinate system, otherwise in the base link (\ref GetBase()) coordi nate system
	*/	*/

	virtual IkParameterization GetIkParameterization(IkParameterization Type iktype) const;	virtual IkParameterization GetIkParameterization(IkParameterization Type iktype, bool inworld=true) const;

	/** \brief returns a full parameterization of a given IK type for t he current manipulator position using an existing IkParameterization as the seed.	/** \brief returns a full parameterization of a given IK type for t he current manipulator position using an existing IkParameterization as the seed.


		Custom data is copied to the new parameterization. Furthermore, some IK types like Lookat3D and TranslationLocalGlobal6D set constraints i n the global coordinate system of the manipulator. Because these values are not stored in manipulator itself, they have to be passed in through an exi sting IkParameterization.
	Ideally pluging the returned ik parameterization into FindIkSol ution should return the a manipulator configuration	Ideally pluging the returned ik parameterization into FindIkSol ution should return the a manipulator configuration
	such that a new call to GetIkParameterization returns the same values.	such that a new call to GetIkParameterization returns the same values.

	\param ikparam Some IK types like Lookat3D and TranslationLocal	\param ikparam The parameterization to use as seed.
	Global6D set constraints in the global coordinate system of the manipulator	\param inworld if true will return the parameterization in the
	. Because these values are not stored in manipulator itself, they have to b	world coordinate system, otherwise in the base link (\ref GetBase()) coordi
	e passed in through an existing IkParameterization.	nate system
	*/	*/

	virtual IkParameterization GetIkParameterization(const IkParameteri zation& ikparam) const;	virtual IkParameterization GetIkParameterization(const IkParameteri zation& ikparam, bool inworld=true) const;

	/// \brief Get all child joints of the manipulator starting at the pEndEffector link	/// \brief Get all child joints of the manipulator starting at the pEndEffector link
	virtual void GetChildJoints(std::vector<JointPtr>& vjoints) const;	virtual void GetChildJoints(std::vector<JointPtr>& vjoints) const;

	/// \brief Get all child DOF indices of the manipulator starting at the pEndEffector link	/// \brief Get all child DOF indices of the manipulator starting at the pEndEffector link
	virtual void GetChildDOFIndices(std::vector<int>& vdofndices) const ;	virtual void GetChildDOFIndices(std::vector<int>& vdofndices) const ;

	/// \brief returns true if a link is part of the child links of the manipulator.	/// \brief returns true if a link is part of the child links of the manipulator.
	///	///
	/// The child links do not include the arm links.	/// The child links do not include the arm links.

	skipping to change at line 209	skipping to change at line 238
	virtual void GetIndependentLinks(std::vector<LinkPtr>& vlinks) cons t;	virtual void GetIndependentLinks(std::vector<LinkPtr>& vlinks) cons t;

	/** \brief Checks collision with only the gripper given its end-eff ector transform. Ignores disabled links.	/** \brief Checks collision with only the gripper given its end-eff ector transform. Ignores disabled links.

	\param tEE the end effector transform	\param tEE the end effector transform
	\param[out] report [optional] collision report	\param[out] report [optional] collision report
	\return true if a collision occurred	\return true if a collision occurred
	*/	*/
	virtual bool CheckEndEffectorCollision(const Transform& tEE, Collis ionReportPtr report = CollisionReportPtr()) const;	virtual bool CheckEndEffectorCollision(const Transform& tEE, Collis ionReportPtr report = CollisionReportPtr()) const;


		/** \brief Checks collision with only the gripper given an IK param
		eterization of the gripper.

		Some IkParameterizations can fully determine the gripper 6DOF l
		ocation. If the type is Transform6D or the manipulator arm DOF <= IkParamet
		erization DOF, then this would be possible. In the latter case, an ik solve
		r is required to support the ik parameterization.
		\param ikparam the ik parameterization determining the gripper
		transform
		\param[out] report [optional] collision report
		\return true if a collision occurred
		/// \throw openrave_exception if the gripper location cannot be
		fully determined from the passed in ik parameterization.
		*/
		virtual bool CheckEndEffectorCollision(const IkParameterization& ik
		param, CollisionReportPtr report = CollisionReportPtr()) const;

	/** \brief Checks collision with the environment with all the indep endent links of the robot. Ignores disabled links.	/** \brief Checks collision with the environment with all the indep endent links of the robot. Ignores disabled links.

	\param[out] report [optional] collision report	\param[out] report [optional] collision report
	\return true if a collision occurred	\return true if a collision occurred
	*/	*/
	virtual bool CheckIndependentCollision(CollisionReportPtr report = CollisionReportPtr()) const;	virtual bool CheckIndependentCollision(CollisionReportPtr report = CollisionReportPtr()) const;

	/** \brief Checks collision with a target body and all the independ ent links of the robot. Ignores disabled links.	/** \brief Checks collision with a target body and all the independ ent links of the robot. Ignores disabled links.

	\param[in] the body to check the independent links with	\param[in] the body to check the independent links with
	\param[out] report [optional] collision report	\param[out] report [optional] collision report
	\return true if a collision occurred	\return true if a collision occurred
	*/	*/
	//virtual bool CheckIndependentCollision(KinBodyConstPtr body, Coll isionReportPtr report = CollisionReportPtr()) const;	//virtual bool CheckIndependentCollision(KinBodyConstPtr body, Coll isionReportPtr report = CollisionReportPtr()) const;

	/// \brief return true if the body is being grabbed by any link on this manipulator	/// \brief return true if the body is being grabbed by any link on this manipulator
	virtual bool IsGrabbing(KinBodyConstPtr body) const;	virtual bool IsGrabbing(KinBodyConstPtr body) const;


	/// \brief computes the jacobian of the manipulator arm indices fro	/// \brief computes the jacobian of the manipulator arm indices of
	m the current manipulator frame origin.	the current manipulator frame world position.
	virtual void CalculateJacobian(boost::multi_array<dReal,2>& mjacobi	///
	an) const;	/// The manipulator frame is computed from Manipulator::GetTransfor
		m()
		virtual void CalculateJacobian(std::vector<dReal>& jacobian) const;

		/// \brief calls std::vector version of CalculateJacobian internall
		y, a little inefficient since it copies memory
		virtual void CalculateJacobian(boost::multi_array<dReal,2>& jacobia
		n) const;

	/// \brief computes the quaternion jacobian of the manipulator arm indices from the current manipulator frame rotation.	/// \brief computes the quaternion jacobian of the manipulator arm indices from the current manipulator frame rotation.

	virtual void CalculateRotationJacobian(boost::multi_array<dReal,2>&	virtual void CalculateRotationJacobian(std::vector<dReal>& jacobian
	mjacobian) const;	) const;

		/// \brief calls std::vector version of CalculateRotationJacobian i
		nternally, a little inefficient since it copies memory
		virtual void CalculateRotationJacobian(boost::multi_array<dReal,2>&
		jacobian) const;

	/// \brief computes the angule axis jacobian of the manipulator arm indices.	/// \brief computes the angule axis jacobian of the manipulator arm indices.

	virtual void CalculateAngularVelocityJacobian(boost::multi_array<dR	virtual void CalculateAngularVelocityJacobian(std::vector<dReal>& j
	eal,2>& mjacobian) const;	acobian) const;

		/// \brief calls std::vector version of CalculateAngularVelocityJac
		obian internally, a little inefficient since it copies memory
		virtual void CalculateAngularVelocityJacobian(boost::multi_array<dR
		eal,2>& jacobian) const;

		/// \brief return a copy of the configuration specification of the
		arm indices
		///
		/// Note that the return type is by-value, so should not be used in
		iteration
		virtual ConfigurationSpecification GetArmConfigurationSpecification
		(const std::string& interpolation="") const;

	virtual void serialize(std::ostream& o, int options) const;	virtual void serialize(std::ostream& o, int options) const;

	/// \brief Return hash of just the manipulator definition.	/// \brief Return hash of just the manipulator definition.
	virtual const std::string& GetStructureHash() const;	virtual const std::string& GetStructureHash() const;

	/// \brief Return hash of all kinematics information that involves solving the inverse kinematics equations.	/// \brief Return hash of all kinematics information that involves solving the inverse kinematics equations.
	///	///
	/// This includes joint axes, joint positions, and final grasp tran sform. Hash is used to cache the solvers.	/// This includes joint axes, joint positions, and final grasp tran sform. Hash is used to cache the solvers.
	virtual const std::string& GetKinematicsStructureHash() const;	virtual const std::string& GetKinematicsStructureHash() const;
	protected:	protected:

	std::string _name;	/// \brief compute internal information from user-set info
	LinkPtr _pBase, _pEndEffector;	virtual void _ComputeInternalInformation();
	Transform _tLocalTool;
	std::vector<dReal> _vClosingDirection;
	Vector _vdirection;
	IkSolverBasePtr _pIkSolver;
	std::string _strIkSolver;
	std::vector<std::string> _vgripperjointnames; ///< names of
	the gripper joints


		ManipulatorInfo _info; ///< user-set information
	private:	private:

	RobotBaseWeakPtr _probot;	RobotBaseWeakPtr __probot;
		LinkPtr __pBase, __pEffector; ///< contains weak links to robot
	std::vector<int> __vgripperdofindices, __varmdofindices;	std::vector<int> __vgripperdofindices, __varmdofindices;

		ConfigurationSpecification __armspec; ///< reflects __varmdofindice
		s
		mutable IkSolverBasePtr __pIkSolver;
	mutable std::string __hashstructure, __hashkinematicsstructure;	mutable std::string __hashstructure, __hashkinematicsstructure;

	#ifdef RAVE_PRIVATE	#ifdef RAVE_PRIVATE
	#ifdef _MSC_VER	#ifdef _MSC_VER
	friend class ColladaReader;	friend class ColladaReader;
	friend class OpenRAVEXMLParser::ManipulatorXMLReader;	friend class OpenRAVEXMLParser::ManipulatorXMLReader;
	friend class OpenRAVEXMLParser::RobotXMLReader;	friend class OpenRAVEXMLParser::RobotXMLReader;
	friend class XFileReader;	friend class XFileReader;
	#else	#else
	friend class ::ColladaReader;	friend class ::ColladaReader;

	skipping to change at line 341	skipping to change at line 394
	friend class ::OpenRAVEXMLParser::AttachedSensorXMLReader;	friend class ::OpenRAVEXMLParser::AttachedSensorXMLReader;
	friend class ::OpenRAVEXMLParser::RobotXMLReader;	friend class ::OpenRAVEXMLParser::RobotXMLReader;
	friend class ::XFileReader;	friend class ::XFileReader;
	#endif	#endif
	#endif	#endif
	friend class RobotBase;	friend class RobotBase;
	};	};
	typedef boost::shared_ptr<RobotBase::AttachedSensor> AttachedSensorPtr;	typedef boost::shared_ptr<RobotBase::AttachedSensor> AttachedSensorPtr;
	typedef boost::shared_ptr<RobotBase::AttachedSensor const> AttachedSens orConstPtr;	typedef boost::shared_ptr<RobotBase::AttachedSensor const> AttachedSens orConstPtr;


	/// \brief The information of a currently grabbed body.
	class OPENRAVE_API Grabbed
	{
	public:
	KinBodyWeakPtr pbody; ///< the grabbed body
	LinkPtr plinkrobot; ///< robot link that is grabbing the bo
	dy
	std::vector<LinkConstPtr> vCollidingLinks, vNonCollidingLinks;
	///< vCollidingLinks: robot links that already collide with the body. T
	his will always include plinkrobot and any other body's first link attached
	to plinkrobot (or static versions)
	Transform troot; ///< root transform (of first link of body
	) relative to plinkrobot's transform. In other words, pbody->GetTransform()
	== plinkrobot->GetTransform()*troot
	};

	/// \brief Helper class derived from KinBodyStateSaver to additionaly s ave robot information.	/// \brief Helper class derived from KinBodyStateSaver to additionaly s ave robot information.
	class OPENRAVE_API RobotStateSaver : public KinBodyStateSaver	class OPENRAVE_API RobotStateSaver : public KinBodyStateSaver
	{	{
	public:	public:
	RobotStateSaver(RobotBasePtr probot, int options = Save_LinkTransfo rmation\|Save_LinkEnable\|Save_ActiveDOF\|Save_ActiveManipulator);	RobotStateSaver(RobotBasePtr probot, int options = Save_LinkTransfo rmation\|Save_LinkEnable\|Save_ActiveDOF\|Save_ActiveManipulator);
	virtual ~RobotStateSaver();	virtual ~RobotStateSaver();

	virtual void Restore();
		/// \brief restore the state
		///
		/// \param robot if set, will attempt to restore the stored state t
		o the passed in body, otherwise will restore it for the original body.
		/// \throw openrave_exception if the passed in body is not compatib
		le with the saved state, will throw
		virtual void Restore(boost::shared_ptr<RobotBase> robot=boost::shar
		ed_ptr<RobotBase>());

		/// \brief release the body state. _pbody will not get restored on
		destruction
		///
		/// After this call, it will still be possible to use \ref Restore.
		virtual void Release();

	protected:	protected:
	RobotBasePtr _probot;	RobotBasePtr _probot;
	std::vector<int> vactivedofs;	std::vector<int> vactivedofs;
	int affinedofs;	int affinedofs;
	Vector rotationaxis;	Vector rotationaxis;

	int nActiveManip;	ManipulatorPtr _pManipActive;
	std::vector<Grabbed> _vGrabbedBodies;	std::vector<UserDataPtr> _vGrabbedBodies;
	private:	private:

	virtual void _RestoreRobot();	virtual void _RestoreRobot(boost::shared_ptr<RobotBase> robot);
	};	};

	typedef boost::shared_ptr<RobotStateSaver> RobotStateSaverPtr;	typedef boost::shared_ptr<RobotStateSaver> RobotStateSaverPtr;

	virtual ~RobotBase();	virtual ~RobotBase();

	/// \brief Return the static interface type this class points to (used for safe casting).	/// \brief Return the static interface type this class points to (used for safe casting).
	static inline InterfaceType GetInterfaceTypeStatic() {	static inline InterfaceType GetInterfaceTypeStatic() {
	return PT_Robot;	return PT_Robot;
	}	}

	virtual void Destroy();	virtual void Destroy();


	/// \deprecated (11/02/18) \see EnvironmentBase::ReadRobotXMLFile
	virtual bool InitFromFile(const std::string& filename, const Attributes
	List& atts = AttributesList()) RAVE_DEPRECATED;
	/// \deprecated (11/02/18) \see EnvironmentBase::ReadRobotXMLData
	virtual bool InitFromData(const std::string& data, const AttributesList
	& atts = AttributesList()) RAVE_DEPRECATED;

	/// \brief Returns the manipulators of the robot	/// \brief Returns the manipulators of the robot
	virtual std::vector<ManipulatorPtr>& GetManipulators() {	virtual std::vector<ManipulatorPtr>& GetManipulators() {
	return _vecManipulators;	return _vecManipulators;
	}	}

	virtual std::vector<AttachedSensorPtr>& GetAttachedSensors() {	virtual std::vector<AttachedSensorPtr>& GetAttachedSensors() {
	return _vecSensors;	return _vecSensors;
	}	}


	virtual void SetDOFValues(const std::vector<dReal>& vJointValues, bool	virtual void SetDOFValues(const std::vector<dReal>& vJointValues, uint3
	checklimits = true);	2_t checklimits = 1, const std::vector<int>& dofindices = std::vector<int>(
	virtual void SetDOFValues(const std::vector<dReal>& vJointValues, const	));
	Transform& transbase, bool checklimits = true);	virtual void SetDOFValues(const std::vector<dReal>& vJointValues, const
		Transform& transbase, uint32_t checklimits = 1);

	virtual void SetLinkTransformations(const std::vector<Transform>& trans forms);	virtual void SetLinkTransformations(const std::vector<Transform>& trans forms);
	virtual void SetLinkTransformations(const std::vector<Transform>& trans forms, const std::vector<int>& dofbranches);	virtual void SetLinkTransformations(const std::vector<Transform>& trans forms, const std::vector<int>& dofbranches);


		virtual bool SetVelocity(const Vector& linearvel, const Vector& angular
		vel);
		virtual void SetDOFVelocities(const std::vector<dReal>& dofvelocities,
		const Vector& linearvel, const Vector& angularvel,uint32_t checklimits = 1)
		;
		virtual void SetDOFVelocities(const std::vector<dReal>& dofvelocities,
		uint32_t checklimits = 1);

	/// Transforms the robot and updates the attached sensors and grabbed b odies.	/// Transforms the robot and updates the attached sensors and grabbed b odies.
	virtual void SetTransform(const Transform& trans);	virtual void SetTransform(const Transform& trans);

	/** Methods using the active degrees of freedoms of the robot. Active D OFs are a way for the	/** Methods using the active degrees of freedoms of the robot. Active D OFs are a way for the
	user to specify degrees of freedom of interest for a current execut ion block. All planners	user to specify degrees of freedom of interest for a current execut ion block. All planners
	by default use the robot's active DOF and active manipultor. For ev ery Get* method, there is	by default use the robot's active DOF and active manipultor. For ev ery Get* method, there is
	a corresponding GetActive* method rather than the methods when sett ing joints. The active	a corresponding GetActive* method rather than the methods when sett ing joints. The active
	DOFs also include affine transfomrations of the robot's base. Affin e transformation DOFs can	DOFs also include affine transfomrations of the robot's base. Affin e transformation DOFs can
	be found after the joint DOFs in this order: X, Y, Z, Rotation wher e rotation can be around	be found after the joint DOFs in this order: X, Y, Z, Rotation wher e rotation can be around
	a specified axis a full 3D rotation. Usually the affine transforam tion is with respect to	a specified axis a full 3D rotation. Usually the affine transforam tion is with respect to

	skipping to change at line 450	skipping to change at line 503
	}	}
	virtual int GetAffineDOF() const {	virtual int GetAffineDOF() const {
	return _nAffineDOFs;	return _nAffineDOFs;
	}	}

	/// \deprecated (11/10/07)	/// \deprecated (11/10/07)
	virtual int GetAffineDOFIndex(DOFAffine dof) const {	virtual int GetAffineDOFIndex(DOFAffine dof) const {
	return GetActiveDOFIndices().size()+RaveGetIndexFromAffineDOF(GetAf fineDOF(),dof);	return GetActiveDOFIndices().size()+RaveGetIndexFromAffineDOF(GetAf fineDOF(),dof);
	}	}


	/// \brief return the configuration specification of the active dofs	/// \brief return a copy of the configuration specification of the acti
	virtual const ConfigurationSpecification& GetActiveConfigurationSpecifi	ve dofs
	cation() const;	///
		/// Note that the return type is by-value, so should not be used in ite
		ration
		virtual ConfigurationSpecification GetActiveConfigurationSpecification(
		const std::string& interpolation="") const;

	/// \brief Return the set of active dof indices of the joints.	/// \brief Return the set of active dof indices of the joints.
	virtual const std::vector<int>& GetActiveDOFIndices() const;	virtual const std::vector<int>& GetActiveDOFIndices() const;

	virtual Vector GetAffineRotationAxis() const {	virtual Vector GetAffineRotationAxis() const {
	return vActvAffineRotationAxis;	return vActvAffineRotationAxis;
	}	}
	virtual void SetAffineTranslationLimits(const Vector& lower, const Vect or& upper);	virtual void SetAffineTranslationLimits(const Vector& lower, const Vect or& upper);
	virtual void SetAffineRotationAxisLimits(const Vector& lower, const Vec tor& upper);	virtual void SetAffineRotationAxisLimits(const Vector& lower, const Vec tor& upper);
	virtual void SetAffineRotation3DLimits(const Vector& lower, const Vecto r& upper);	virtual void SetAffineRotation3DLimits(const Vector& lower, const Vecto r& upper);

	skipping to change at line 528	skipping to change at line 583
	virtual Vector GetAffineRotationAxisWeights() const {	virtual Vector GetAffineRotationAxisWeights() const {
	return _vRotationAxisWeights;	return _vRotationAxisWeights;
	}	}
	virtual Vector GetAffineRotation3DWeights() const {	virtual Vector GetAffineRotation3DWeights() const {
	return _vRotation3DWeights;	return _vRotation3DWeights;
	}	}
	virtual dReal GetAffineRotationQuatWeights() const {	virtual dReal GetAffineRotationQuatWeights() const {
	return _fQuatAngleResolution;	return _fQuatAngleResolution;
	}	}


	virtual void SetActiveDOFValues(const std::vector<dReal>& values, bool checklimits=true);	virtual void SetActiveDOFValues(const std::vector<dReal>& values, uint3 2_t checklimits=1);
	virtual void GetActiveDOFValues(std::vector<dReal>& v) const;	virtual void GetActiveDOFValues(std::vector<dReal>& v) const;

	virtual void SetActiveDOFVelocities(const std::vector<dReal>& velocitie s, bool checklimits=true);	virtual void SetActiveDOFVelocities(const std::vector<dReal>& velocitie s, uint32_t checklimits=1);
	virtual void GetActiveDOFVelocities(std::vector<dReal>& velocities) con st;	virtual void GetActiveDOFVelocities(std::vector<dReal>& velocities) con st;
	virtual void GetActiveDOFLimits(std::vector<dReal>& lower, std::vector< dReal>& upper) const;	virtual void GetActiveDOFLimits(std::vector<dReal>& lower, std::vector< dReal>& upper) const;
	virtual void GetActiveDOFResolutions(std::vector<dReal>& v) const;	virtual void GetActiveDOFResolutions(std::vector<dReal>& v) const;
	virtual void GetActiveDOFWeights(std::vector<dReal>& v) const;	virtual void GetActiveDOFWeights(std::vector<dReal>& v) const;
	virtual void GetActiveDOFVelocityLimits(std::vector<dReal>& v) const;	virtual void GetActiveDOFVelocityLimits(std::vector<dReal>& v) const;
	virtual void GetActiveDOFAccelerationLimits(std::vector<dReal>& v) cons t;	virtual void GetActiveDOFAccelerationLimits(std::vector<dReal>& v) cons t;
	virtual void GetActiveDOFMaxVel(std::vector<dReal>& v) const {	virtual void GetActiveDOFMaxVel(std::vector<dReal>& v) const {
	return GetActiveDOFVelocityLimits(v);	return GetActiveDOFVelocityLimits(v);
	}	}
	virtual void GetActiveDOFMaxAccel(std::vector<dReal>& v) const {	virtual void GetActiveDOFMaxAccel(std::vector<dReal>& v) const {
	return GetActiveDOFAccelerationLimits(v);	return GetActiveDOFAccelerationLimits(v);
	}	}

	/// computes the configuration difference q1-q2 and stores it in q1. Ta kes into account joint limits and circular joints	/// computes the configuration difference q1-q2 and stores it in q1. Ta kes into account joint limits and circular joints
	virtual void SubtractActiveDOFValues(std::vector<dReal>& q1, const std: :vector<dReal>& q2) const;	virtual void SubtractActiveDOFValues(std::vector<dReal>& q1, const std: :vector<dReal>& q2) const;


	/// sets the active manipulator of the robot	/// \deprecated (12/07/23)
	/// \param index manipulator index	virtual void SetActiveManipulator(int index) RAVE_DEPRECATED;
	virtual void SetActiveManipulator(int index);
	/// sets the active manipulator of the robot	/// \brief sets the active manipulator of the robot
		///
	/// \param manipname manipulator name	/// \param manipname manipulator name

	virtual void SetActiveManipulator(const std::string& manipname);	/// \throw openrave_exception if manipulator not present, will throw an
		exception
		virtual ManipulatorPtr SetActiveManipulator(const std::string& manipnam
		e);
		virtual void SetActiveManipulator(ManipulatorConstPtr pmanip);
	virtual ManipulatorPtr GetActiveManipulator();	virtual ManipulatorPtr GetActiveManipulator();
	virtual ManipulatorConstPtr GetActiveManipulator() const;	virtual ManipulatorConstPtr GetActiveManipulator() const;

	/// \return index of the current active manipulator
	virtual int GetActiveManipulatorIndex() const {	/// \brief adds a manipulator the list
	return _nActiveManip;	///
	}	/// Will copy the information of this manipulator object into a new man
		ipulator and initialize it with the robot.
		/// Will change the robot structure hash..
		/// \return the new manipulator attached to the robot
		/// \throw openrave_exception If there exists a manipulator with the sa
		me name, will throw an exception
		virtual ManipulatorPtr AddManipulator(const ManipulatorInfo& manipinfo)
		;

		/// \brief removes a manipulator from the robot list.
		///
		/// Will change the robot structure hash..
		/// if the active manipulator is set to this manipulator, it will be se
		t to None afterwards
		virtual void RemoveManipulator(ManipulatorPtr manip);

		/// \deprecated (12/07/23)
		virtual int GetActiveManipulatorIndex() const RAVE_DEPRECATED;

	/// \deprecated (11/10/04) send directly through controller	/// \deprecated (11/10/04) send directly through controller
	virtual bool SetMotion(TrajectoryBaseConstPtr ptraj) RAVE_DEPRECATED;	virtual bool SetMotion(TrajectoryBaseConstPtr ptraj) RAVE_DEPRECATED;

	/// \deprecated (11/10/04)	/// \deprecated (11/10/04)
	virtual bool SetActiveMotion(TrajectoryBaseConstPtr ptraj) RAVE_DEPRECA TED;	virtual bool SetActiveMotion(TrajectoryBaseConstPtr ptraj) RAVE_DEPRECA TED;

	/// \deprecated (11/10/04)	/// \deprecated (11/10/04)
	virtual bool SetActiveMotion(TrajectoryBaseConstPtr ptraj, dReal fSpeed ) RAVE_DEPRECATED;	virtual bool SetActiveMotion(TrajectoryBaseConstPtr ptraj, dReal fSpeed ) RAVE_DEPRECATED;

	/** \brief Calculates the translation jacobian with respect to a link.	/** \brief Calculates the translation jacobian with respect to a link.

	Calculates the partial differentials for the active degrees of free dom that in the path from the root node to _veclinks[index]	Calculates the partial differentials for the active degrees of free dom that in the path from the root node to _veclinks[index]
	(doesn't touch the rest of the values).	(doesn't touch the rest of the values).
	\param mjacobian a 3 x ActiveDOF matrix	\param mjacobian a 3 x ActiveDOF matrix
	*/	*/

	virtual void CalculateActiveJacobian(int index, const Vector& offset, b	virtual void CalculateActiveJacobian(int index, const Vector& offset, s
	oost::multi_array<dReal,2>& mjacobian) const;	td::vector<dReal>& jacobian) const;
	virtual void CalculateActiveJacobian(int index, const Vector& offset, s	virtual void CalculateActiveJacobian(int index, const Vector& offset, b
	td::vector<dReal>& pfJacobian) const;	oost::multi_array<dReal,2>& jacobian) const;


	virtual void CalculateActiveRotationJacobian(int index, const Vector& q	virtual void CalculateActiveRotationJacobian(int index, const Vector& q
	InitialRot, boost::multi_array<dReal,2>& vjacobian) const;	InitialRot, std::vector<dReal>& jacobian) const;
	virtual void CalculateActiveRotationJacobian(int index, const Vector& q	virtual void CalculateActiveRotationJacobian(int index, const Vector& q
	InitialRot, std::vector<dReal>& pfJacobian) const;	InitialRot, boost::multi_array<dReal,2>& jacobian) const;

	/** Calculates the angular velocity jacobian of a specified link about the axes of world coordinates.	/** Calculates the angular velocity jacobian of a specified link about the axes of world coordinates.

	\param index of the link that the rotation is attached to	\param index of the link that the rotation is attached to
	\param mjacobian 3x(num ACTIVE DOF) matrix	\param mjacobian 3x(num ACTIVE DOF) matrix
	*/	*/

	virtual void CalculateActiveAngularVelocityJacobian(int index, boost::m	virtual void CalculateActiveAngularVelocityJacobian(int index, std::vec
	ulti_array<dReal,2>& mjacobian) const;	tor<dReal>& jacobian) const;
	virtual void CalculateActiveAngularVelocityJacobian(int index, std::vec	virtual void CalculateActiveAngularVelocityJacobian(int index, boost::m
	tor<dReal>& pfJacobian) const;	ulti_array<dReal,2>& jacobian) const;

	virtual const std::set<int>& GetNonAdjacentLinks(int adjacentoptions=0) const;	virtual const std::set<int>& GetNonAdjacentLinks(int adjacentoptions=0) const;

	//@}	//@}

	/** A grabbed body becomes part of the robot and its relative pose with respect to a robot's	/** A grabbed body becomes part of the robot and its relative pose with respect to a robot's
	link will be fixed. KinBody::_AttachBody is called for every grabbe d body in order to make	link will be fixed. KinBody::_AttachBody is called for every grabbe d body in order to make
	the grabbed body a part of the robot. Once grabbed, the inter-colli sions between the robot	the grabbed body a part of the robot. Once grabbed, the inter-colli sions between the robot
	and the body are regarded as self-collisions; any outside collision s of the body and the	and the body are regarded as self-collisions; any outside collision s of the body and the
	environment are regarded as environment collisions with the robot.	environment are regarded as environment collisions with the robot.

	skipping to change at line 645	skipping to change at line 716

	/** \brief Release the body if grabbed.	/** \brief Release the body if grabbed.

	\param body body to release	\param body body to release
	*/	*/
	virtual void Release(KinBodyPtr body);	virtual void Release(KinBodyPtr body);

	/// Release all grabbed bodies.	/// Release all grabbed bodies.
	virtual void ReleaseAllGrabbed(); ///< release all bodies	virtual void ReleaseAllGrabbed(); ///< release all bodies


	/** Releases and grabs all bodies, has the effect of recalculating all the initial collision with the bodies.	/** \brief Releases and grabs all bodies, has the effect of recalculati ng all the initial collision with the bodies.

	This has the effect of resetting the current collisions any grabbed body makes with the robot into an ignore list.	This has the effect of resetting the current collisions any grabbed body makes with the robot into an ignore list.
	*/	*/
	virtual void RegrabAll();	virtual void RegrabAll();

	/** \brief return the robot link that is currently grabbing the body. I f the body is not grabbed, will return an empty pointer.	/** \brief return the robot link that is currently grabbing the body. I f the body is not grabbed, will return an empty pointer.

	\param[in] body the body to check	\param[in] body the body to check
	*/	*/
	virtual LinkPtr IsGrabbing(KinBodyConstPtr body) const;	virtual LinkPtr IsGrabbing(KinBodyConstPtr body) const;

	skipping to change at line 708	skipping to change at line 779
	/// \brief gets the robot controller	/// \brief gets the robot controller
	virtual ControllerBasePtr GetController() const {	virtual ControllerBasePtr GetController() const {
	return ControllerBasePtr();	return ControllerBasePtr();
	}	}

	/// \brief set a controller for a robot	/// \brief set a controller for a robot
	/// \param pController - if NULL, sets the controller of this robot to NULL. otherwise attemps to set the controller to this robot.	/// \param pController - if NULL, sets the controller of this robot to NULL. otherwise attemps to set the controller to this robot.
	/// \param args - the argument list to pass when initializing the contr oller	/// \param args - the argument list to pass when initializing the contr oller
	virtual bool SetController(ControllerBasePtr controller, const std::vec tor<int>& dofindices, int nControlTransformation);	virtual bool SetController(ControllerBasePtr controller, const std::vec tor<int>& dofindices, int nControlTransformation);


	/// \deprecated (10/11/16)
	virtual bool SetController(ControllerBasePtr controller, const std::str
	ing& args) RAVE_DEPRECATED {
	std::vector<int> dofindices;
	for(int i = 0; i < GetDOF(); ++i) {
	dofindices.push_back(i);
	}
	return SetController(controller,dofindices,1);
	}

	/// \deprecated (11/10/04)	/// \deprecated (11/10/04)
	void GetFullTrajectoryFromActive(TrajectoryBasePtr pfulltraj, Trajector yBaseConstPtr pActiveTraj, bool bOverwriteTransforms=true) RAVE_DEPRECATED;	void GetFullTrajectoryFromActive(TrajectoryBasePtr pfulltraj, Trajector yBaseConstPtr pActiveTraj, bool bOverwriteTransforms=true) RAVE_DEPRECATED;

	protected:	protected:
	RobotBase(EnvironmentBasePtr penv);	RobotBase(EnvironmentBasePtr penv);

	inline RobotBasePtr shared_robot() {	inline RobotBasePtr shared_robot() {
	return boost::static_pointer_cast<RobotBase>(shared_from_this());	return boost::static_pointer_cast<RobotBase>(shared_from_this());
	}	}
	inline RobotBaseConstPtr shared_robot_const() const {	inline RobotBaseConstPtr shared_robot_const() const {
	return boost::static_pointer_cast<RobotBase const>(shared_from_this ());	return boost::static_pointer_cast<RobotBase const>(shared_from_this ());
	}	}


		/// \brief internal use only Releases and grabs the body inside the
		grabbed structure from _vGrabbedBodies.
		virtual void _Regrab(UserDataPtr pgrabbed);

	/// \brief Proprocess the manipulators and sensors and build the specif ic robot hashes.	/// \brief Proprocess the manipulators and sensors and build the specif ic robot hashes.
	virtual void _ComputeInternalInformation();	virtual void _ComputeInternalInformation();

	/// \brief Called to notify the body that certain groups of parameters have been changed.	/// \brief Called to notify the body that certain groups of parameters have been changed.
	///	///
	/// This function in calls every registers calledback that is tracking the changes.	/// This function in calls every registers calledback that is tracking the changes.
	virtual void _ParametersChanged(int parameters);	virtual void _ParametersChanged(int parameters);


	std::vector<Grabbed> _vGrabbedBodies; ///< vector of grabbed bodies	std::vector<UserDataPtr> _vGrabbedBodies; ///< vector of grabbed bodies
	virtual void _UpdateGrabbedBodies();	virtual void _UpdateGrabbedBodies();
	virtual void _UpdateAttachedSensors();	virtual void _UpdateAttachedSensors();
	std::vector<ManipulatorPtr> _vecManipulators; ///< \see GetManipulators	std::vector<ManipulatorPtr> _vecManipulators; ///< \see GetManipulators

	int _nActiveManip; ///< \see GetActiveManipulatorIndex	ManipulatorPtr _pManipActive;

	std::vector<AttachedSensorPtr> _vecSensors; ///< \see GetAttachedSensor s	std::vector<AttachedSensorPtr> _vecSensors; ///< \see GetAttachedSensor s

	std::vector<int> _vActiveDOFIndices, _vAllDOFIndices;	std::vector<int> _vActiveDOFIndices, _vAllDOFIndices;
	Vector vActvAffineRotationAxis;	Vector vActvAffineRotationAxis;
	int _nActiveDOF; ///< Active degrees of freedom; if -1, use robot dofs	int _nActiveDOF; ///< Active degrees of freedom; if -1, use robot dofs
	int _nAffineDOFs; ///< dofs describe what affine transformations are al lowed	int _nAffineDOFs; ///< dofs describe what affine transformations are al lowed

	Vector _vTranslationLowerLimits, _vTranslationUpperLimits, _vTranslatio nMaxVels, _vTranslationResolutions, _vTranslationWeights;	Vector _vTranslationLowerLimits, _vTranslationUpperLimits, _vTranslatio nMaxVels, _vTranslationResolutions, _vTranslationWeights;
	/// the xyz components are used if the rotation axis is solely about X, Y,or Z; otherwise the W component is used.	/// the xyz components are used if the rotation axis is solely about X, Y,or Z; otherwise the W component is used.

	skipping to change at line 789	skipping to change at line 854
	friend class ::Environment;	friend class ::Environment;
	friend class ::ColladaReader;	friend class ::ColladaReader;
	friend class ::ColladaWriter;	friend class ::ColladaWriter;
	friend class ::OpenRAVEXMLParser::RobotXMLReader;	friend class ::OpenRAVEXMLParser::RobotXMLReader;
	friend class ::OpenRAVEXMLParser::ManipulatorXMLReader;	friend class ::OpenRAVEXMLParser::ManipulatorXMLReader;
	friend class ::OpenRAVEXMLParser::AttachedSensorXMLReader;	friend class ::OpenRAVEXMLParser::AttachedSensorXMLReader;
	friend class ::XFileReader;	friend class ::XFileReader;
	#endif	#endif
	#endif	#endif
	friend class RaveDatabase;	friend class RaveDatabase;

		friend class Grabbed;
	};	};

	} // end namespace OpenRAVE	} // end namespace OpenRAVE

	#endif // ROBOT_H	#endif // ROBOT_H

End of changes. 53 change blocks.
	120 lines changed or deleted	222 lines changed or added

	sensor.h	sensor.h

	skipping to change at line 370	skipping to change at line 370
	virtual UserDataPtr RegisterDataCallback(SensorType type, const boost:: function<void(SensorDataConstPtr)>& callback) OPENRAVE_DUMMY_IMPLEMENTATION ;	virtual UserDataPtr RegisterDataCallback(SensorType type, const boost:: function<void(SensorDataConstPtr)>& callback) OPENRAVE_DUMMY_IMPLEMENTATION ;

	/// \return the name of the sensor	/// \return the name of the sensor
	virtual const std::string& GetName() const {	virtual const std::string& GetName() const {
	return _name;	return _name;
	}	}
	virtual void SetName(const std::string& newname) {	virtual void SetName(const std::string& newname) {
	_name = newname;	_name = newname;
	}	}


	/// \deprecated (11/03/28)
	virtual bool Init(const std::string&) RAVE_DEPRECATED {
	RAVELOG_WARN("SensorBase::Init has been deprecated\n"); return Conf
	igure(CC_PowerOn)>0;
	}
	virtual void Reset(int) RAVE_DEPRECATED {
	RAVELOG_WARN("SensorBase::Reset has been deprecated\n"); Configure(
	CC_PowerOff);
	}

	protected:	protected:
	std::string _name; ///< name of the sensor	std::string _name; ///< name of the sensor

	private:	private:
	virtual const char* GetHash() const {	virtual const char* GetHash() const {
	return OPENRAVE_SENSOR_HASH;	return OPENRAVE_SENSOR_HASH;
	}	}
	};	};

	} // end namespace OpenRAVE	} // end namespace OpenRAVE

End of changes. 1 change blocks.
	10 lines changed or deleted	0 lines changed or added

	utils.h	utils.h

	skipping to change at line 35	skipping to change at line 35

	#include <openrave/config.h>	#include <openrave/config.h>
	#include <stdint.h>	#include <stdint.h>
	#include <string>	#include <string>
	#include <istream>	#include <istream>
	#include <vector>	#include <vector>

	#include <boost/shared_ptr.hpp>	#include <boost/shared_ptr.hpp>
	#include <boost/weak_ptr.hpp>	#include <boost/weak_ptr.hpp>
	#include <boost/function.hpp>	#include <boost/function.hpp>

		#include <boost/assert.hpp>

	#include <time.h>	#include <time.h>

	#ifndef _WIN32	#ifndef _WIN32
	#if !(defined(CLOCK_GETTIME_FOUND) && (POSIX_TIMERS > 0 \|\| _POSIX_TIMERS > 0))	#if !(defined(CLOCK_GETTIME_FOUND) && (POSIX_TIMERS > 0 \|\| _POSIX_TIMERS > 0))
	#include <sys/time.h>	#include <sys/time.h>
	#endif	#endif
	#else	#else
	#define WIN32_LEAN_AND_MEAN	#define WIN32_LEAN_AND_MEAN
	#include <windows.h>	#include <windows.h>

	skipping to change at line 270	skipping to change at line 271
	return max;	return max;
	}	}
	return value;	return value;
	}	}

	template <typename T>	template <typename T>
	inline T NormalizeCircularAngle(T theta, T min, T max)	inline T NormalizeCircularAngle(T theta, T min, T max)
	{	{
	if (theta < min) {	if (theta < min) {
	T range = max-min;	T range = max-min;

		BOOST_ASSERT(range>0);
	theta += range;	theta += range;
	while (theta < min) {	while (theta < min) {
	theta += range;	theta += range;
	}	}
	}	}
	else if (theta > max) {	else if (theta > max) {
	T range = max-min;	T range = max-min;

		BOOST_ASSERT(range>0);
	theta -= range;	theta -= range;
	while (theta > max) {	while (theta > max) {
	theta -= range;	theta -= range;
	}	}
	}	}
	return theta;	return theta;
	}	}

	template <typename T>	template <typename T>
	inline T SubtractCircularAngle(T f0, T f1)	inline T SubtractCircularAngle(T f0, T f1)

End of changes. 3 change blocks.
	0 lines changed or deleted	3 lines changed or added

	viewer.h	viewer.h

	skipping to change at line 119	skipping to change at line 119

	/// \brief callback function for item selection	/// \brief callback function for item selection
	///	///
	/// If the function returns true, then the object will be selected. Oth erwise, the object remains unselected.	/// If the function returns true, then the object will be selected. Oth erwise, the object remains unselected.
	/// callback(target link,offset,direction)	/// callback(target link,offset,direction)
	typedef boost::function<bool (KinBody::LinkPtr plink,RaveVector<float>, RaveVector<float>)> ItemSelectionCallbackFn;	typedef boost::function<bool (KinBody::LinkPtr plink,RaveVector<float>, RaveVector<float>)> ItemSelectionCallbackFn;

	/// \brief registers a function with the viewer that gets called everyt ime mouse button is clicked	/// \brief registers a function with the viewer that gets called everyt ime mouse button is clicked
	///	///
	/// \return a handle to the callback. If this handle is deleted, the ca llback will be unregistered.	/// \return a handle to the callback. If this handle is deleted, the ca llback will be unregistered.

	virtual boost::shared_ptr<void> RegisterItemSelectionCallback(const Ite mSelectionCallbackFn& fncallback) OPENRAVE_DUMMY_IMPLEMENTATION;	virtual UserDataPtr RegisterItemSelectionCallback(const ItemSelectionCa llbackFn& fncallback) OPENRAVE_DUMMY_IMPLEMENTATION;

	/// \brief callback function for item selection	/// \brief callback function for item selection
	/// callback(imagememory,width,height,pixeldepth)	/// callback(imagememory,width,height,pixeldepth)
	///	///
	/// \param imagememory width x height x pixeldepth RGB image	/// \param imagememory width x height x pixeldepth RGB image
	typedef boost::function<void (const uint8_t*,int,int,int)> ViewerImageC allbackFn;	typedef boost::function<void (const uint8_t*,int,int,int)> ViewerImageC allbackFn;

	/// \brief registers a function with the viewer that gets called for ev ery new image rendered.	/// \brief registers a function with the viewer that gets called for ev ery new image rendered.
	///	///
	/// \return a handle to the callback. If this handle is deleted, the ca llback will be unregistered.	/// \return a handle to the callback. If this handle is deleted, the ca llback will be unregistered.

	virtual boost::shared_ptr<void> RegisterViewerImageCallback(const Viewe	virtual UserDataPtr RegisterViewerImageCallback(const ViewerImageCallba
	rImageCallbackFn& fncallback) OPENRAVE_DUMMY_IMPLEMENTATION;	ckFn& fncallback) OPENRAVE_DUMMY_IMPLEMENTATION;

		/// \brief callback function for viewer thread
		typedef boost::function<void ()> ViewerThreadCallbackFn;

		/// \brief registers a function with the viewer that gets called in the
		viewer's GUI thread for every cycle the viewer refreshes at
		///
		/// The environment will not be locked when the thread is called
		/// \return a handle to the callback. If this handle is deleted, the ca
		llback will be unregistered.
		virtual UserDataPtr RegisterViewerThreadCallback(const ViewerThreadCall
		backFn& fncallback) OPENRAVE_DUMMY_IMPLEMENTATION;

	/// \brief controls whether the viewer synchronizes with the newest env ironment automatically	/// \brief controls whether the viewer synchronizes with the newest env ironment automatically
	virtual void SetEnvironmentSync(bool bUpdate) OPENRAVE_DUMMY_IMPLEMENTA TION;	virtual void SetEnvironmentSync(bool bUpdate) OPENRAVE_DUMMY_IMPLEMENTA TION;

	/// \brief forces synchronization with the environment, returns when th e environment is fully synchronized.	/// \brief forces synchronization with the environment, returns when th e environment is fully synchronized.
	///	///
	/// Note that this method might not work if environment is locked in cu rrent thread	/// Note that this method might not work if environment is locked in cu rrent thread
	virtual void EnvironmentSync() OPENRAVE_DUMMY_IMPLEMENTATION;	virtual void EnvironmentSync() OPENRAVE_DUMMY_IMPLEMENTATION;

	virtual void SetSize(int w, int h) OPENRAVE_DUMMY_IMPLEMENTATION;	virtual void SetSize(int w, int h) OPENRAVE_DUMMY_IMPLEMENTATION;

	skipping to change at line 168	skipping to change at line 177
	SetName(ptitle);	SetName(ptitle);
	}	}

	virtual const std::string& GetName() const OPENRAVE_DUMMY_IMPLEMENTATIO N;	virtual const std::string& GetName() const OPENRAVE_DUMMY_IMPLEMENTATIO N;

	/// \deprecated (11/06/10)	/// \deprecated (11/06/10)
	virtual void UpdateCameraTransform() RAVE_DEPRECATED OPENRAVE_DUMMY_IMP LEMENTATION;	virtual void UpdateCameraTransform() RAVE_DEPRECATED OPENRAVE_DUMMY_IMP LEMENTATION;
	/// \deprecated (11/06/10)	/// \deprecated (11/06/10)
	typedef ItemSelectionCallbackFn ViewerCallbackFn RAVE_DEPRECATED;	typedef ItemSelectionCallbackFn ViewerCallbackFn RAVE_DEPRECATED;
	/// \deprecated (11/06/10)	/// \deprecated (11/06/10)

	virtual boost::shared_ptr<void> RegisterCallback(int properties, const ItemSelectionCallbackFn& fncallback) RAVE_DEPRECATED	virtual UserDataPtr RegisterCallback(int properties, const ItemSelectio nCallbackFn& fncallback) RAVE_DEPRECATED
	{	{
	return RegisterItemSelectionCallback(fncallback);	return RegisterItemSelectionCallback(fncallback);
	}	}


	/// \deprecated (11/03/02) Any type of model should be added through th
	e openrave environment instead of viewer directly.
	virtual bool LoadModel(const std::string& pfilename) RAVE_DEPRECATED OP
	ENRAVE_DUMMY_IMPLEMENTATION;

	protected:	protected:
	virtual void SetViewerData(KinBodyPtr body, UserDataPtr data) {	virtual void SetViewerData(KinBodyPtr body, UserDataPtr data) {
	body->SetViewerData(data);	body->SetViewerData(data);
	}	}

	virtual GraphHandlePtr plot3(const float* ppoints, int numPoints, int s tride, float fPointSize, const RaveVector<float>& color, int drawstyle = 0) OPENRAVE_DUMMY_IMPLEMENTATION;	virtual GraphHandlePtr plot3(const float* ppoints, int numPoints, int s tride, float fPointSize, const RaveVector<float>& color, int drawstyle = 0) OPENRAVE_DUMMY_IMPLEMENTATION;
	virtual GraphHandlePtr plot3(const float* ppoints, int numPoints, int s tride, float fPointSize, const float* colors, int drawstyle = 0, bool bhasa lpha=false) OPENRAVE_DUMMY_IMPLEMENTATION;	virtual GraphHandlePtr plot3(const float* ppoints, int numPoints, int s tride, float fPointSize, const float* colors, int drawstyle = 0, bool bhasa lpha=false) OPENRAVE_DUMMY_IMPLEMENTATION;

	virtual GraphHandlePtr drawlinestrip(const float* ppoints, int numPoint s, int stride, float fwidth, const RaveVector<float>& color) OPENRAVE_DUMMY _IMPLEMENTATION;	virtual GraphHandlePtr drawlinestrip(const float* ppoints, int numPoint s, int stride, float fwidth, const RaveVector<float>& color) OPENRAVE_DUMMY _IMPLEMENTATION;
	virtual GraphHandlePtr drawlinestrip(const float* ppoints, int numPoint s, int stride, float fwidth, const float* colors) OPENRAVE_DUMMY_IMPLEMENTA TION;	virtual GraphHandlePtr drawlinestrip(const float* ppoints, int numPoint s, int stride, float fwidth, const float* colors) OPENRAVE_DUMMY_IMPLEMENTA TION;

End of changes. 4 change blocks.
	9 lines changed or deleted	16 lines changed or added

This html diff was produced by rfcdiff 1.41. The latest version is available from http://tools.ietf.org/tools/rfcdiff/

	collisionchecker.h	collisionchecker.h

	skipping to change at line 119	skipping to change at line 119
	virtual bool SetCollisionOptions(int collisionoptions) = 0;	virtual bool SetCollisionOptions(int collisionoptions) = 0;

	/// \brief get the current collision options	/// \brief get the current collision options
	virtual int GetCollisionOptions() const = 0;	virtual int GetCollisionOptions() const = 0;

	virtual void SetTolerance(dReal tolerance) = 0;	virtual void SetTolerance(dReal tolerance) = 0;

	/// notified when a new body has been initialized in the environment	/// notified when a new body has been initialized in the environment
	virtual bool InitKinBody(KinBodyPtr pbody) = 0;	virtual bool InitKinBody(KinBodyPtr pbody) = 0;


	/// \deprecated (10/12/03) use \see EnableLink
	virtual bool Enable(KinBodyConstPtr pbody, bool bEnable) RAVE_DEPRECATE
	D = 0;

	/// enables or disables a link from being considered in collisions
	/// \return true if operation succeeded
	virtual bool EnableLink(KinBody::LinkConstPtr pbody, bool bEnable) = 0;

	/// Each function takes an optional pointer to a CollisionReport struct ure and returns true if collision occurs.	/// Each function takes an optional pointer to a CollisionReport struct ure and returns true if collision occurs.
	/// \name Collision specific functions.	/// \name Collision specific functions.
	/// \anchor collision_checking	/// \anchor collision_checking
	//@{	//@{

	/// \brief checks collision of a body and a scene. Attached bodies are respected. If CO_ActiveDOFs is set, will only check affected links of the b ody.	/// \brief checks collision of a body and a scene. Attached bodies are respected. If CO_ActiveDOFs is set, will only check affected links of the b ody.
	virtual bool CheckCollision(KinBodyConstPtr pbody1, CollisionReportPtr report = CollisionReportPtr())=0;	virtual bool CheckCollision(KinBodyConstPtr pbody1, CollisionReportPtr report = CollisionReportPtr())=0;

	/// \brief checks collision between two bodies. Attached bodies are res pected. If CO_ActiveDOFs is set, will only check affected links of the pbod y1.	/// \brief checks collision between two bodies. Attached bodies are res pected. If CO_ActiveDOFs is set, will only check affected links of the pbod y1.
	virtual bool CheckCollision(KinBodyConstPtr pbody1, KinBodyConstPtr pbo dy2, CollisionReportPtr report = CollisionReportPtr())=0;	virtual bool CheckCollision(KinBodyConstPtr pbody1, KinBodyConstPtr pbo dy2, CollisionReportPtr report = CollisionReportPtr())=0;

End of changes. 1 change blocks.
	8 lines changed or deleted	0 lines changed or added

	controller.h	controller.h

	skipping to change at line 43	skipping to change at line 43
	ControllerBase(EnvironmentBasePtr penv) : InterfaceBase(PT_Controller, penv) {	ControllerBase(EnvironmentBasePtr penv) : InterfaceBase(PT_Controller, penv) {
	}	}
	virtual ~ControllerBase() {	virtual ~ControllerBase() {
	}	}

	/// \brief return the static interface type this class points to (used for safe casting)	/// \brief return the static interface type this class points to (used for safe casting)
	static inline InterfaceType GetInterfaceTypeStatic() {	static inline InterfaceType GetInterfaceTypeStatic() {
	return PT_Controller;	return PT_Controller;
	}	}


	/// \brief Initializes the controller
	/// \param robot the robot that uses the controller
	/// \param args extra arguments that the controller takes.
	/// \return true on successful initialization
	virtual bool Init(RobotBasePtr robot, const std::string& args) RAVE_DEP
	RECATED {
	if( !robot ) {
	return false;
	}
	std::vector<int> dofindices;
	for(int i = 0; i < robot->GetDOF(); ++i) {
	dofindices.push_back(i);
	}
	return Init(robot,dofindices,true);
	}

	/// \brief initializes the controller and specifies the controlled dof	/// \brief initializes the controller and specifies the controlled dof
	/// \param robot the robot that uses the controller	/// \param robot the robot that uses the controller
	/// \param dofindices the indices that controller will have exclusive a ccess to	/// \param dofindices the indices that controller will have exclusive a ccess to
	/// \param nControlTransformation \see IsControlTransformation	/// \param nControlTransformation \see IsControlTransformation
	/// \return true on successful initialization	/// \return true on successful initialization
	virtual bool Init(RobotBasePtr robot, const std::vector<int>& dofindice s, int nControlTransformation) = 0;	virtual bool Init(RobotBasePtr robot, const std::vector<int>& dofindice s, int nControlTransformation) = 0;

	/// \brief returns the dof indices controlled	/// \brief returns the dof indices controlled
	virtual const std::vector<int>& GetControlDOFIndices() const = 0;	virtual const std::vector<int>& GetControlDOFIndices() const = 0;

	/// \brief returns non-zero value if base affine transformation is cont rolled.	/// \brief returns non-zero value if base affine transformation is cont rolled.
	///	///
	/// Only one controller can modify translation and orientation per robo t. For now, the two cannot be divided.	/// Only one controller can modify translation and orientation per robo t. For now, the two cannot be divided.
	virtual int IsControlTransformation() const = 0;	virtual int IsControlTransformation() const = 0;

	virtual RobotBasePtr GetRobot() const = 0;	virtual RobotBasePtr GetRobot() const = 0;

	/// \brief Resets the current controller trajectories and any other sta te associated with the robot	/// \brief Resets the current controller trajectories and any other sta te associated with the robot
	/// \param options - specific options that can be used to control what to reset	/// \param options - specific options that can be used to control what to reset

	virtual void Reset(int options) = 0;	virtual void Reset(int options=0) = 0;

	/// \brief go to a specific position in configuration space. <b>[multi- thread safe]</b>	/// \brief go to a specific position in configuration space. <b>[multi- thread safe]</b>
	/// \param values the final configuration in the control dofs	/// \param values the final configuration in the control dofs
	/// \param trans the transformation of the base. If not specified will use the current robot transformation. Ignored if controller does not use it	/// \param trans the transformation of the base. If not specified will use the current robot transformation. Ignored if controller does not use it
	/// \return true if position operation successful.	/// \return true if position operation successful.
	virtual bool SetDesired(const std::vector<dReal>& values, TransformCons tPtr trans=TransformConstPtr()) = 0;	virtual bool SetDesired(const std::vector<dReal>& values, TransformCons tPtr trans=TransformConstPtr()) = 0;

	/// \brief Follow a path in configuration space, adds to the queue of t rajectories already in execution. <b>[multi-thread safe]</b>	/// \brief Follow a path in configuration space, adds to the queue of t rajectories already in execution. <b>[multi-thread safe]</b>
	/// \param ptraj - the trajectory	/// \param ptraj - the trajectory
	/// \return true if trajectory operation successful	/// \return true if trajectory operation successful

	skipping to change at line 161	skipping to change at line 146
	/// \param dofindices robot dof indices to control	/// \param dofindices robot dof indices to control
	/// \throw openrave_exception if the controller dofs interfere with cur rent set dofs, will throw an exception	/// \throw openrave_exception if the controller dofs interfere with cur rent set dofs, will throw an exception
	virtual bool AttachController(ControllerBasePtr controller, const std:: vector<int>& dofindices, int nControlTransformation);	virtual bool AttachController(ControllerBasePtr controller, const std:: vector<int>& dofindices, int nControlTransformation);

	/// \brief removes a controller from being managed. <b>[multi-thread sa fe]</b>	/// \brief removes a controller from being managed. <b>[multi-thread sa fe]</b>
	virtual void RemoveController(ControllerBasePtr controller);	virtual void RemoveController(ControllerBasePtr controller);

	/// \brief gets the controller responsible for dof (in the robot). If d of < 0, returns the transform controller. <b>[multi-thread safe]</b>	/// \brief gets the controller responsible for dof (in the robot). If d of < 0, returns the transform controller. <b>[multi-thread safe]</b>
	virtual ControllerBasePtr GetController(int dof) const;	virtual ControllerBasePtr GetController(int dof) const;


	virtual void Reset(int options);	virtual void Reset(int options=0);
	virtual bool SetDesired(const std::vector<dReal>& values, TransformCons tPtr trans=TransformConstPtr());	virtual bool SetDesired(const std::vector<dReal>& values, TransformCons tPtr trans=TransformConstPtr());
	virtual bool SetPath(TrajectoryBaseConstPtr ptraj);	virtual bool SetPath(TrajectoryBaseConstPtr ptraj);
	virtual void SimulationStep(dReal fTimeElapsed);	virtual void SimulationStep(dReal fTimeElapsed);

	/// \brief returns true only if all controllers return true	/// \brief returns true only if all controllers return true
	virtual bool IsDone();	virtual bool IsDone();

	/// \brief return the maximum time	/// \brief return the maximum time
	virtual dReal GetTime() const;	virtual dReal GetTime() const;


End of changes. 3 change blocks.
	18 lines changed or deleted	2 lines changed or added