Added Juergen's original changes to KDL

FreeCAD · Oct 14, 2015 · 9304f7a · 9304f7a
1 parent 1908e85
commit 9304f7a
Show file tree

Hide file tree

Showing 6 changed files with 54 additions and 3 deletions.
diff --git a/src/Mod/Robot/App/kdl_cp/chainiksolverpos_nr_jl.cpp b/src/Mod/Robot/App/kdl_cp/chainiksolverpos_nr_jl.cpp
@@ -23,6 +23,15 @@
 
 #include "chainiksolverpos_nr_jl.hpp"
 
+// FreeCAD change
+#ifndef M_PI
+    #define M_PI    3.14159265358979323846 /* pi */
+#endif
+
+#ifndef M_PI_2
+    #define M_PI_2  1.57079632679489661923 /* pi/2 */
+#endif
+
 namespace KDL
 {
     ChainIkSolverPos_NR_JL::ChainIkSolverPos_NR_JL(const Chain& _chain, const JntArray& _q_min, const JntArray& _q_max, ChainFkSolverPos& _fksolver,ChainIkSolverVel& _iksolver,
@@ -50,13 +59,15 @@ namespace KDL
 
                 for(unsigned int j=0; j<q_min.rows(); j++) {
                   if(q_out(j) < q_min(j))
-                    q_out(j) = q_min(j);
+                    //q_out(j) = q_min(j); // FreeCAD change
+                    q_out(j) = q_out(j) + M_PI *2;
                 }
 
 
                 for(unsigned int j=0; j<q_max.rows(); j++) {
                     if(q_out(j) > q_max(j))
-                      q_out(j) = q_max(j);
+                      //q_out(j) = q_max(j); // FreeCAD change
+                      q_out(j) = q_out(j) - M_PI *2;
                 }
             }
 

diff --git a/src/Mod/Robot/App/kdl_cp/trajectory.hpp b/src/Mod/Robot/App/kdl_cp/trajectory.hpp
@@ -78,6 +78,12 @@ namespace KDL {
 	class Trajectory
 	{
 	public:
+        virtual Path*      GetPath() = 0;
+        // The underlying Path - FreeCAD change
+
+        virtual VelocityProfile* GetProfile() = 0;
+        // The velocity profile - FreeCAD change
+
 		virtual double Duration() const = 0;
 		// The duration of the trajectory
 

diff --git a/src/Mod/Robot/App/kdl_cp/trajectory_composite.cpp b/src/Mod/Robot/App/kdl_cp/trajectory_composite.cpp
@@ -23,6 +23,7 @@ namespace KDL {
 
     Trajectory_Composite::Trajectory_Composite():duration(0.0)
     {
+        path = new Path_Composite(); // FreeCAD change
     }
 
     double Trajectory_Composite::Duration() const{
@@ -92,6 +93,7 @@ namespace KDL {
         vt.insert(vt.end(),elem);
         duration += elem->Duration();
         vd.insert(vd.end(),duration);
+        path->Add(elem->GetPath(),false); // FreeCAD change
     }
 
     void Trajectory_Composite::Destroy() {
@@ -101,6 +103,8 @@ namespace KDL {
         }
         vt.erase(vt.begin(),vt.end());
         vd.erase(vd.begin(),vd.end());
+
+        delete path; // FreeCAD change
     }
 
     Trajectory_Composite::~Trajectory_Composite() {
@@ -124,7 +128,17 @@ namespace KDL {
         }
         return comp;
     }
+
+    // FreeCAD change
+    Path* Trajectory_Composite::GetPath()
+    {
+        return path;
+    }
 
+    VelocityProfile* Trajectory_Composite::GetProfile()
+    {
+        return 0;
+    }
 }
 
 
diff --git a/src/Mod/Robot/App/kdl_cp/trajectory_composite.hpp b/src/Mod/Robot/App/kdl_cp/trajectory_composite.hpp
@@ -37,6 +37,7 @@ class Trajectory_Composite: public Trajectory
 		VectorDouble  vd;      // contains end time for each Trajectory
 		double duration;    // total duration of the composed
 				    // Trajectory
+        Path_Composite* path; // FreeCAD change
 
 	public:
 		Trajectory_Composite();
@@ -53,6 +54,11 @@ class Trajectory_Composite: public Trajectory
 		virtual void Destroy();
 		virtual void Write(std::ostream& os) const;
 		virtual Trajectory* Clone() const;
+        virtual Path*      GetPath(); // FreeCAD change
+		virtual VelocityProfile* GetProfile(); // FreeCAD change
+
+        // access the single members
+        Trajectory *Get(unsigned int n){return vt[n];} // FreeCAD change
 
 		virtual ~Trajectory_Composite();
 	};

diff --git a/src/Mod/Robot/App/kdl_cp/trajectory_stationary.hpp b/src/Mod/Robot/App/kdl_cp/trajectory_stationary.hpp
@@ -29,9 +29,22 @@ namespace KDL {
 	  {
 		double duration;
 		Frame pos;
+		VelocityProfile* prof; // FreeCAD change
+		Path*      path; // FreeCAD change
 	public:
 		Trajectory_Stationary(double _duration,const Frame& _pos):
 		  duration(_duration),pos(_pos) {}
+
+        // FreeCAD change
+	    virtual Path* GetPath() {
+            return path; 
+        }
+
+        // FreeCAD change
+	    virtual VelocityProfile* GetProfile() {
+            return prof;
+        }
+
 		virtual double Duration() const {
 			return duration;
 		}
@@ -49,6 +62,7 @@ namespace KDL {
 		virtual Trajectory* Clone() const {
 			return new Trajectory_Stationary(duration,pos);
 		}
+
 		virtual ~Trajectory_Stationary() {}
 	};
 

diff --git a/src/Mod/Robot/App/kdl_cp/utilities/rallNd.h b/src/Mod/Robot/App/kdl_cp/utilities/rallNd.h
@@ -76,7 +76,7 @@ class RallNd :
 template <>
 class RallNd<2> : public Rall2d<double>  {
 public:
-	RallNd() {}* (dwz. met evenveel numerieke operaties als een 
+	RallNd() {} /* (dwz. met evenveel numerieke operaties als een  */
 	RallNd(const Rall2d<double>& arg) :
 		Rall2d<double>(arg) {}
 	RallNd(double value,double d[]) {