Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[20519] pkg/trunk
|
From: <mee...@us...> - 2009-08-03 20:13:47
|
Revision: 20519
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=20519&view=rev
Author: meeussen
Date: 2009-08-03 20:13:35 +0000 (Mon, 03 Aug 2009)
Log Message:
-----------
kdl segment/joint naming patch was applied upstream, so remove it locally. Fix two controllers that were missing includes and wrong jacobian construction
Modified Paths:
--------------
pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_constraint_controller.h
pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_constraint_controller.cpp
pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/plug_controller.h
pkg/trunk/sandbox/experimental_controllers/src/plug_controller.cpp
pkg/trunk/stacks/geometry/kdl/Makefile
pkg/trunk/stacks/geometry/kdl/total.patch
Modified: pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_constraint_controller.h
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_constraint_controller.h 2009-08-03 20:07:05 UTC (rev 20518)
+++ pkg/trunk/controllers/robot_mechanism_controllers/include/robot_mechanism_controllers/endeffector_constraint_controller.h 2009-08-03 20:13:35 UTC (rev 20519)
@@ -40,6 +40,7 @@
#include "kdl/chain.hpp"
#include "kdl/frames.hpp"
#include "kdl/chainfksolver.hpp"
+#include "kdl/chainjnttojacsolver.hpp"
#include "ros/node.h"
#include "robot_msgs/Wrench.h"
#include "misc_utils/subscription_guard.h"
Modified: pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_constraint_controller.cpp
===================================================================
--- pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_constraint_controller.cpp 2009-08-03 20:07:05 UTC (rev 20518)
+++ pkg/trunk/controllers/robot_mechanism_controllers/src/endeffector_constraint_controller.cpp 2009-08-03 20:13:35 UTC (rev 20519)
@@ -150,7 +150,7 @@
initialized_ = true;
}
- Jacobian jacobian(kdl_chain_.getNrOfJoints(), kdl_chain_.getNrOfSegments());
+ Jacobian jacobian(kdl_chain_.getNrOfJoints());
jnt_to_jac_solver_->JntToJac(jnt_pos, jacobian);
Modified: pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/plug_controller.h
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/plug_controller.h 2009-08-03 20:07:05 UTC (rev 20518)
+++ pkg/trunk/sandbox/experimental_controllers/include/experimental_controllers/plug_controller.h 2009-08-03 20:13:35 UTC (rev 20519)
@@ -40,6 +40,7 @@
#include "kdl/chain.hpp"
#include "kdl/frames.hpp"
#include "kdl/chainfksolver.hpp"
+#include "kdl/chainjnttojacsolver.hpp"
#include "ros/node.h"
#include "robot_msgs/Wrench.h"
#include "robot_msgs/PoseStamped.h"
Modified: pkg/trunk/sandbox/experimental_controllers/src/plug_controller.cpp
===================================================================
--- pkg/trunk/sandbox/experimental_controllers/src/plug_controller.cpp 2009-08-03 20:07:05 UTC (rev 20518)
+++ pkg/trunk/sandbox/experimental_controllers/src/plug_controller.cpp 2009-08-03 20:13:35 UTC (rev 20519)
@@ -160,7 +160,7 @@
initialized_ = true;
}
- Jacobian jacobian(kdl_chain_.getNrOfJoints(), kdl_chain_.getNrOfSegments());
+ Jacobian jacobian(kdl_chain_.getNrOfJoints());
jnt_to_jac_solver_->JntToJac(jnt_pos, jacobian);
// TODO: Write a function for doing this conversion
Modified: pkg/trunk/stacks/geometry/kdl/Makefile
===================================================================
--- pkg/trunk/stacks/geometry/kdl/Makefile 2009-08-03 20:07:05 UTC (rev 20518)
+++ pkg/trunk/stacks/geometry/kdl/Makefile 2009-08-03 20:13:35 UTC (rev 20519)
@@ -1,6 +1,6 @@
all: installed
-SVN_REVISION=-r 30247
+SVN_REVISION=-r 30400
SVN_DIR = kdl-svn-trunk
OLD_SVN_DIR = kdl-svn
SVN_URL = http://svn.mech.kuleuven.be/repos/orocos/trunk/kdl
Modified: pkg/trunk/stacks/geometry/kdl/total.patch
===================================================================
--- pkg/trunk/stacks/geometry/kdl/total.patch 2009-08-03 20:07:05 UTC (rev 20518)
+++ pkg/trunk/stacks/geometry/kdl/total.patch 2009-08-03 20:13:35 UTC (rev 20519)
@@ -1,327 +1,6 @@
-Index: tests/kinfamtest.cpp
-===================================================================
---- tests/kinfamtest.cpp (revision 30247)
-+++ tests/kinfamtest.cpp (working copy)
-@@ -24,44 +24,44 @@
- {
- double q;
- Joint j;
-- j=Joint(Joint::None);
-+ j=Joint("Joint 1", Joint::None);
- CPPUNIT_ASSERT_EQUAL(Joint::None,j.getType());
- random(q);
- CPPUNIT_ASSERT_EQUAL(j.pose(q),Frame::Identity());
- random(q);
- CPPUNIT_ASSERT_EQUAL(j.twist(q),Twist::Zero());
- random(q);
-- j=Joint(Joint::RotX);
-+ j=Joint("Joint 2", Joint::RotX);
- CPPUNIT_ASSERT_EQUAL(Joint::RotX,j.getType());
- CPPUNIT_ASSERT_EQUAL(j.pose(q),Frame(Rotation::RotX(q)));
- random(q);
- CPPUNIT_ASSERT_EQUAL(j.twist(q),Twist(Vector::Zero(),Vector(q,0,0)));
- random(q);
-- j=Joint(Joint::RotY);
-+ j=Joint("Joint 3", Joint::RotY);
- CPPUNIT_ASSERT_EQUAL(Joint::RotY,j.getType());
- CPPUNIT_ASSERT_EQUAL(j.pose(q),Frame(Rotation::RotY(q)));
- random(q);
- CPPUNIT_ASSERT_EQUAL(j.twist(q),Twist(Vector::Zero(),Vector(0,q,0)));
- random(q);
-- j=Joint(Joint::RotZ);
-+ j=Joint("Joint 4", Joint::RotZ);
- CPPUNIT_ASSERT_EQUAL(Joint::RotZ,j.getType());
- CPPUNIT_ASSERT_EQUAL(j.pose(q),Frame(Rotation::RotZ(q)));
- random(q);
- CPPUNIT_ASSERT_EQUAL(j.twist(q),Twist(Vector::Zero(),Vector(0,0,q)));
- random(q);
-- j=Joint(Joint::TransX);
-+ j=Joint("Joint 5", Joint::TransX);
- CPPUNIT_ASSERT_EQUAL(Joint::TransX,j.getType());
- CPPUNIT_ASSERT_EQUAL(j.pose(q),Frame(Vector(q,0,0)));
- random(q);
- CPPUNIT_ASSERT_EQUAL(j.twist(q),Twist(Vector(q,0,0),Vector::Zero()));
- random(q);
-- j=Joint(Joint::TransY);
-+ j=Joint("Joint 6", Joint::TransY);
- CPPUNIT_ASSERT_EQUAL(Joint::TransY,j.getType());
- CPPUNIT_ASSERT_EQUAL(j.pose(q),Frame(Vector(0,q,0)));
- random(q);
- CPPUNIT_ASSERT_EQUAL(j.twist(q),Twist(Vector(0,q,0),Vector::Zero()));
- random(q);
-- j=Joint(Joint::TransZ);
-+ j=Joint("Joint 7", Joint::TransZ);
- CPPUNIT_ASSERT_EQUAL(Joint::TransZ,j.getType());
- CPPUNIT_ASSERT_EQUAL(j.pose(q),Frame(Vector(0,0,q)));
- random(q);
-@@ -75,49 +75,49 @@
- double q,qdot;
- Frame f,f1;
- random(f);
-- s = Segment(Joint(Joint::None),f);
-+ s = Segment("Segment 0", Joint("Joint 0", Joint::None),f);
- random(q);
- random(qdot);
- f1=s.getJoint().pose(q)*f;
- CPPUNIT_ASSERT_EQUAL(f1,s.pose(q));
- CPPUNIT_ASSERT_EQUAL(s.getJoint().twist(qdot).RefPoint(f1.p),s.twist(q,qdot));
- random(f);
-- s = Segment(Joint(Joint::RotX),f);
-+ s = Segment("Segment 1", Joint("Joint 1", Joint::RotX),f);
- random(q);
- random(qdot);
- f1=s.getJoint().pose(q)*f;
- CPPUNIT_ASSERT_EQUAL(f1,s.pose(q));
- CPPUNIT_ASSERT_EQUAL(s.getJoint().twist(qdot).RefPoint(f1.p),s.twist(q,qdot));
- random(f);
-- s = Segment(Joint(Joint::RotY),f);
-+ s = Segment("Segment 3", Joint("Joint 3", Joint::RotY),f);
- random(q);
- random(qdot);
- f1=s.getJoint().pose(q)*f;
- CPPUNIT_ASSERT_EQUAL(f1,s.pose(q));
- CPPUNIT_ASSERT_EQUAL(s.getJoint().twist(qdot).RefPoint(f1.p),s.twist(q,qdot));
- random(f);
-- s = Segment(Joint(Joint::RotZ),f);
-+ s = Segment("Segment 4", Joint("Joint 4", Joint::RotZ),f);
- random(q);
- random(qdot);
- f1=s.getJoint().pose(q)*f;
- CPPUNIT_ASSERT_EQUAL(f1,s.pose(q));
- CPPUNIT_ASSERT_EQUAL(s.getJoint().twist(qdot).RefPoint(f1.p),s.twist(q,qdot));
- random(f);
-- s = Segment(Joint(Joint::TransX),f);
-+ s = Segment("Segment 5", Joint("Joint 5", Joint::TransX),f);
- random(q);
- random(qdot);
- f1=s.getJoint().pose(q)*f;
- CPPUNIT_ASSERT_EQUAL(f1,s.pose(q));
- CPPUNIT_ASSERT_EQUAL(s.getJoint().twist(qdot).RefPoint(f1.p),s.twist(q,qdot));
- random(f);
-- s = Segment(Joint(Joint::TransY),f);
-+ s = Segment("Segment 6", Joint("Joint 6", Joint::TransY),f);
- random(q);
- random(qdot);
- f1=s.getJoint().pose(q)*f;
- CPPUNIT_ASSERT_EQUAL(f1,s.pose(q));
- CPPUNIT_ASSERT_EQUAL(s.getJoint().twist(qdot).RefPoint(f1.p),s.twist(q,qdot));
- random(f);
-- s = Segment(Joint(Joint::TransZ),f);
-+ s = Segment("Segment 7", Joint("Joint 7", Joint::TransZ),f);
- random(q);
- random(qdot);
- f1=s.getJoint().pose(q)*f;
-@@ -129,21 +129,21 @@
- {
- Chain chain1;
-
-- chain1.addSegment(Segment(Joint(Joint::RotZ),
-+ chain1.addSegment(Segment("Segment 0", Joint("Joint 0", Joint::RotZ),
- Frame(Vector(0.0,0.0,0.0))));
-- chain1.addSegment(Segment(Joint(Joint::RotX),
-+ chain1.addSegment(Segment("Segment 1", Joint("Joint 1", Joint::RotX),
- Frame(Vector(0.0,0.0,0.9))));
-- chain1.addSegment(Segment(Joint(Joint::RotX),
-+ chain1.addSegment(Segment("Segment 2", Joint("Joint 2", Joint::RotX),
- Frame(Vector(0.0,0.0,1.2))));
-- chain1.addSegment(Segment(Joint(Joint::RotZ),
-+ chain1.addSegment(Segment("Segment 3", Joint("Joint 3", Joint::RotZ),
- Frame(Vector(0.0,0.0,1.5))));
-- chain1.addSegment(Segment(Joint(Joint::RotX),
-+ chain1.addSegment(Segment("Segment 4", Joint("Joint 4", Joint::RotX),
- Frame(Vector(0.0,0.0,0.0))));
-- chain1.addSegment(Segment(Joint(Joint::RotZ),
-+ chain1.addSegment(Segment("Segment 5", Joint("Joint 5", Joint::RotZ),
- Frame(Vector(0.0,0.0,0.4))));
- CPPUNIT_ASSERT_EQUAL(chain1.getNrOfJoints(),(uint)6);
- CPPUNIT_ASSERT_EQUAL(chain1.getNrOfSegments(),(uint)6);
-- chain1.addSegment(Segment(Joint(Joint::None),Frame(Vector(0.0,0.1,0.0))));
-+ chain1.addSegment(Segment("Segment 6", Joint("Joint 6", Joint::None),Frame(Vector(0.0,0.1,0.0))));
- CPPUNIT_ASSERT_EQUAL(chain1.getNrOfJoints(),(uint)6);
- CPPUNIT_ASSERT_EQUAL(chain1.getNrOfSegments(),(uint)7);
-
-@@ -158,50 +158,57 @@
- void KinFamTest::TreeTest()
- {
- Tree tree1;
-- Segment segment1(Joint(Joint::None));
-- Segment segment2(Joint(Joint::RotX),Frame(Vector(0.1,0.2,0.3)));
-- Segment segment3(Joint(Joint::TransZ),Frame(Rotation::RotX(1.57)));
-+ Segment segment1("Segment 1", Joint("Joint 1", Joint::None));
-+ Segment segment2("Segment 2", Joint("Joint 2", Joint::RotX),Frame(Vector(0.1,0.2,0.3)));
-+ Segment segment3("Segment 3", Joint("Joint 3", Joint::TransZ),Frame(Rotation::RotX(1.57)));
-+ Segment segment4("Segment 4", Joint("Joint 4", Joint::RotX),Frame(Vector(0.1,0.2,0.3)));
-+ Segment segment5("Segment 5", Joint("Joint 5", Joint::RotX),Frame(Vector(0.1,0.2,0.3)));
-+ Segment segment6("Segment 6", Joint("Joint 6", Joint::RotX),Frame(Vector(0.1,0.2,0.3)));
-+ Segment segment7("Segment 7", Joint("Joint 7", Joint::RotX),Frame(Vector(0.1,0.2,0.3)));
-
- cout<<tree1<<endl;
-
-- CPPUNIT_ASSERT(tree1.addSegment(segment1,"Segment1","root"));
-- CPPUNIT_ASSERT(tree1.addSegment(segment2,"Segment2","root"));
-- CPPUNIT_ASSERT(tree1.addSegment(segment3,"Segment3","Segment1"));
-- CPPUNIT_ASSERT(tree1.addSegment(segment2,"Segment4","Segment3"));
-- CPPUNIT_ASSERT(!tree1.addSegment(segment1,"Segment5","Segment6"));
-- CPPUNIT_ASSERT(!tree1.addSegment(segment1,"Segment1","Segment4"));
-+ CPPUNIT_ASSERT(tree1.addSegment(segment1,"root"));
-+ CPPUNIT_ASSERT(tree1.addSegment(segment2,"root"));
-+ CPPUNIT_ASSERT(tree1.addSegment(segment3,"Segment 1"));
-+ CPPUNIT_ASSERT(tree1.addSegment(segment4,"Segment 3"));
-+ CPPUNIT_ASSERT(!tree1.addSegment(segment1,"Segment 6"));
-+ CPPUNIT_ASSERT(!tree1.addSegment(segment1,"Segment 4"));
-
- cout<<tree1<<endl;
-
- Tree tree2;
-- CPPUNIT_ASSERT(tree2.addSegment(segment1,"Segment1","root"));
-- CPPUNIT_ASSERT(tree2.addSegment(segment2,"Segment2","root"));
-- CPPUNIT_ASSERT(tree2.addSegment(segment3,"Segment3","Segment1"));
-+ CPPUNIT_ASSERT(tree2.addSegment(segment5,"root"));
-+ CPPUNIT_ASSERT(tree2.addSegment(segment6,"root"));
-+ CPPUNIT_ASSERT(tree2.addSegment(segment7,"Segment 6"));
-
- cout<<tree2<<endl;
-
- Chain chain1;
-- chain1.addSegment(Segment(Joint(Joint::RotZ),
-+ chain1.addSegment(Segment("Segment 8", Joint("Joint 8", Joint::RotZ),
- Frame(Vector(0.0,0.0,0.0))));
-- chain1.addSegment(Segment(Joint(Joint::RotX),
-+ chain1.addSegment(Segment("Segment 9", Joint("Joint 9", Joint::RotX),
- Frame(Vector(0.0,0.0,0.9))));
-- chain1.addSegment(Segment(Joint(Joint::RotX),
-+ chain1.addSegment(Segment("Segment 10", Joint("Joint 10", Joint::RotX),
- Frame(Vector(0.0,0.0,1.2))));
-- chain1.addSegment(Segment(Joint(Joint::RotZ),
-+ chain1.addSegment(Segment("Segment 11", Joint("Joint 11", Joint::RotZ),
- Frame(Vector(0.0,0.0,1.5))));
-- chain1.addSegment(Segment(Joint(Joint::RotX),
-+ chain1.addSegment(Segment("Segment 12", Joint("Joint 12", Joint::RotX),
- Frame(Vector(0.0,0.0,0.0))));
-- chain1.addSegment(Segment(Joint(Joint::RotZ),
-+ chain1.addSegment(Segment("Segment 13", Joint("Joint 13", Joint::RotZ),
- Frame(Vector(0.0,0.0,0.4))));
-
-
-- CPPUNIT_ASSERT(tree2.addChain(chain1,"Chain1","Segment2"));
-+ CPPUNIT_ASSERT(tree2.addChain(chain1, "Segment 6"));
- cout<<tree2<<endl;
-- CPPUNIT_ASSERT(tree1.addTree(tree2,"Tree2","Segment2"));
-+ CPPUNIT_ASSERT(tree1.addTree(tree2, "Segment 2"));
- cout<<tree1<<endl;
-
-- Chain extract_chain1 = tree1.getChain("Segment2", "Segment4");
-- Chain extract_chain2 = tree1.getChain("Segment4", "Segment2");
-+ Chain extract_chain1;
-+ CPPUNIT_ASSERT(tree1.getChain("Segment 2", "Segment 4", extract_chain1));
-+ CPPUNIT_ASSERT(tree1.getChain("Segment 2", "Segment 4", extract_chain1)); // to make sure chain gets cleared
-+ Chain extract_chain2;
-+ CPPUNIT_ASSERT(tree1.getChain("Segment 4", "Segment 2", extract_chain2));
- CPPUNIT_ASSERT(extract_chain1.getNrOfJoints()==extract_chain2.getNrOfJoints());
- CPPUNIT_ASSERT(extract_chain1.getNrOfSegments()==extract_chain2.getNrOfSegments());
- ChainFkSolverPos_recursive solver1(extract_chain1);
-Index: tests/solvertest.cpp
-===================================================================
---- tests/solvertest.cpp (revision 30247)
-+++ tests/solvertest.cpp (working copy)
-@@ -13,66 +13,66 @@
- {
- srand( (unsigned)time( NULL ));
-
-- chain1.addSegment(Segment(Joint(Joint::RotZ),
-+ chain1.addSegment(Segment("Segment 1", Joint("Joint 1", Joint::RotZ),
- Frame(Vector(0.0,0.0,0.0))));
-- chain1.addSegment(Segment(Joint(Joint::RotX),
-+ chain1.addSegment(Segment("Segment 2", Joint("Joint 2", Joint::RotX),
- Frame(Vector(0.0,0.0,0.9))));
-- chain1.addSegment(Segment(Joint(Joint::None),
-+ chain1.addSegment(Segment("Segment 3", Joint("Joint 3", Joint::None),
- Frame(Vector(-0.4,0.0,0.0))));
-- chain1.addSegment(Segment(Joint(Joint::RotX),
-+ chain1.addSegment(Segment("Segment 4", Joint("Joint 4", Joint::RotX),
- Frame(Vector(0.0,0.0,1.2))));
-- chain1.addSegment(Segment(Joint(Joint::None),
-+ chain1.addSegment(Segment("Segment 5", Joint("Joint 5", Joint::None),
- Frame(Vector(0.4,0.0,0.0))));
-- chain1.addSegment(Segment(Joint(Joint::RotZ),
-+ chain1.addSegment(Segment("Segment 6", Joint("Joint 6", Joint::RotZ),
- Frame(Vector(0.0,0.0,1.4))));
-- chain1.addSegment(Segment(Joint(Joint::RotX),
-+ chain1.addSegment(Segment("Segment 7", Joint("Joint 7", Joint::RotX),
- Frame(Vector(0.0,0.0,0.0))));
-- chain1.addSegment(Segment(Joint(Joint::RotZ),
-+ chain1.addSegment(Segment("Segment 8", Joint("Joint 8", Joint::RotZ),
- Frame(Vector(0.0,0.0,0.4))));
-- chain1.addSegment(Segment(Joint(Joint::None),
-+ chain1.addSegment(Segment("Segment 9", Joint("Joint 9", Joint::None),
- Frame(Vector(0.0,0.0,0.0))));
-
-- chain2.addSegment(Segment(Joint(Joint::RotZ),
-+ chain2.addSegment(Segment("Segment 1", Joint("Joint 1", Joint::RotZ),
- Frame(Vector(0.0,0.0,0.5))));
-- chain2.addSegment(Segment(Joint(Joint::RotX),
-+ chain2.addSegment(Segment("Segment 2", Joint("Joint 2", Joint::RotX),
- Frame(Vector(0.0,0.0,0.4))));
-- chain2.addSegment(Segment(Joint(Joint::RotX),
-+ chain2.addSegment(Segment("Segment 3", Joint("Joint 3", Joint::RotX),
- Frame(Vector(0.0,0.0,0.3))));
-- chain2.addSegment(Segment(Joint(Joint::RotX),
-+ chain2.addSegment(Segment("Segment 4", Joint("Joint 4", Joint::RotX),
- Frame(Vector(0.0,0.0,0.2))));
-- chain2.addSegment(Segment(Joint(Joint::RotZ),
-+ chain2.addSegment(Segment("Segment 5", Joint("Joint 5", Joint::RotZ),
- Frame(Vector(0.0,0.0,0.1))));
-
-
-- chain3.addSegment(Segment(Joint(Joint::RotZ),
-+ chain3.addSegment(Segment("Segment 1", Joint("Joint 1", Joint::RotZ),
- Frame(Vector(0.0,0.0,0.0))));
-- chain3.addSegment(Segment(Joint(Joint::RotX),
-+ chain3.addSegment(Segment("Segment 2", Joint("Joint 2", Joint::RotX),
- Frame(Vector(0.0,0.0,0.9))));
-- chain3.addSegment(Segment(Joint(Joint::RotZ),
-+ chain3.addSegment(Segment("Segment 3", Joint("Joint 3", Joint::RotZ),
- Frame(Vector(-0.4,0.0,0.0))));
-- chain3.addSegment(Segment(Joint(Joint::RotX),
-+ chain3.addSegment(Segment("Segment 4", Joint("Joint 4", Joint::RotX),
- Frame(Vector(0.0,0.0,1.2))));
-- chain3.addSegment(Segment(Joint(Joint::None),
-+ chain3.addSegment(Segment("Segment 5", Joint("Joint 5", Joint::None),
- Frame(Vector(0.4,0.0,0.0))));
-- chain3.addSegment(Segment(Joint(Joint::RotZ),
-+ chain3.addSegment(Segment("Segment 6", Joint("Joint 6", Joint::RotZ),
- Frame(Vector(0.0,0.0,1.4))));
-- chain3.addSegment(Segment(Joint(Joint::RotX),
-+ chain3.addSegment(Segment("Segment 7", Joint("Joint 7", Joint::RotX),
- Frame(Vector(0.0,0.0,0.0))));
-- chain3.addSegment(Segment(Joint(Joint::RotZ),
-+ chain3.addSegment(Segment("Segment 8", Joint("Joint 8", Joint::RotZ),
- Frame(Vector(0.0,0.0,0.4))));
-- chain3.addSegment(Segment(Joint(Joint::RotY),
-+ chain3.addSegment(Segment("Segment 9", Joint("Joint 9", Joint::RotY),
- Frame(Vector(0.0,0.0,0.0))));
-
-
-- chain4.addSegment(Segment(Joint(Vector(10,0,0), Vector(1,0,1),Joint::RotAxis),
-+ chain4.addSegment(Segment("Segment 1", Joint("Joint 1", Vector(10,0,0), Vector(1,0,1),Joint::RotAxis),
- Frame(Vector(0.0,0.0,0.5))));
-- chain4.addSegment(Segment(Joint(Vector(0,5,0), Vector(1,0,0),Joint::RotAxis),
-+ chain4.addSegment(Segment("Segment 2", Joint("Joint 2", Vector(0,5,0), Vector(1,0,0),Joint::RotAxis),
- Frame(Vector(0.0,0.0,0.4))));
-- chain4.addSegment(Segment(Joint(Vector(0,0,5), Vector(1,0,4),Joint::RotAxis),
-+ chain4.addSegment(Segment("Segment 3", Joint("Joint 3", Vector(0,0,5), Vector(1,0,4),Joint::RotAxis),
- Frame(Vector(0.0,0.0,0.3))));
-- chain4.addSegment(Segment(Joint(Vector(0,0,0), Vector(1,0,0),Joint::RotAxis),
-+ chain4.addSegment(Segment("Segment 4", Joint("Joint 4", Vector(0,0,0), Vector(1,0,0),Joint::RotAxis),
- Frame(Vector(0.0,0.0,0.2))));
-- chain4.addSegment(Segment(Joint(Vector(0,0,0), Vector(0,0,1),Joint::RotAxis),
-+ chain4.addSegment(Segment("Segment 5", Joint("Joint 5", Vector(0,0,0), Vector(0,0,1),Joint::RotAxis),
- Frame(Vector(0.0,0.0,0.1))));
-
- }
Index: src/tree.hpp
===================================================================
---- src/tree.hpp (revision 30247)
+--- src/tree.hpp (revision 30400)
+++ src/tree.hpp (working copy)
@@ -37,7 +37,7 @@
class TreeElement
@@ -344,71 +23,8 @@
};
};
-@@ -69,13 +69,15 @@
- int nrOfJoints;
- int nrOfSegments;
-
-- bool addTreeRecursive(SegmentMap::const_iterator root, const std::string& tree_name, const std::string& hook_name);
-+ std::string root_name;
-
-+ bool addTreeRecursive(SegmentMap::const_iterator root, const std::string& hook_name);
-+
- public:
- /**
- * The constructor of a tree, a new tree is always empty
+@@ -142,7 +142,9 @@
*/
-- Tree();
-+ Tree(const std::string& root_name="root");
- Tree(const Tree& in);
- Tree& operator= (const Tree& arg);
-
-@@ -84,39 +86,33 @@
- * hook_name as segment_name
- *
- * @param segment new segment to add
-- * @param segment_name name of the new segment
- * @param hook_name name of the segment to connect this
- * segment with.
- *
- * @return false if hook_name could not be found.
- */
-- bool addSegment(const Segment& segment, const std::string& segment_name, const std::string& hook_name);
-+ bool addSegment(const Segment& segment, const std::string& hook_name);
-
- /**
- * Adds a complete chain to the end of the segment with
-- * hook_name as segment_name. Segment i of
-- * the chain will get chain_name+".Segment"+i as segment_name.
-+ * hook_name as segment_name.
- *
-- * @param chain Chain to add
-- * @param chain_name name of the chain
- * @param hook_name name of the segment to connect the chain with.
- *
- * @return false if hook_name could not be found.
- */
-- bool addChain(const Chain& chain, const std::string& chain_name, const std::string& hook_name);
-+ bool addChain(const Chain& chain, const std::string& hook_name);
-
- /**
- * Adds a complete tree to the end of the segment with
-- * hookname as segment_name. The segments of the tree will get
-- * tree_name+segment_name as segment_name.
-+ * hookname as segment_name.
- *
- * @param tree Tree to add
-- * @param tree_name name of the tree
- * @param hook_name name of the segment to connect the tree with
- *
- * @return false if hook_name could not be found
- */
-- bool addTree(const Tree& tree, const std::string& tree_name,const std::string& hook_name);
-+ bool addTree(const Tree& tree, const std::string& hook_name);
-
- /**
- * Request the total number of joints in the tree.\n
-@@ -146,18 +142,30 @@
- */
SegmentMap::const_iterator getSegment(const std::string& segment_name)const
{
- return segments.find(segment_name);
@@ -416,36 +32,12 @@
+ if (it == segments.end()) throw seg_name_ex;
+ else return it;
};
-+ /**
-+ * Request the root segment of the tree
-+ *
-+ * @return constant iterator pointing to the root segment
-+ */
-+ SegmentMap::const_iterator getRootSegment()const
-+ {
-+ return segments.find(root_name);
-+ };
-
/**
- * Request the chain of the tree between chain_root and chain_tip. The chain_root must be an ancester from chain_tip
- *
- * @param chain_root the name of the root segment of the chain
- * @param chain_tip the name of the tip segment of the chain
-+ * @param chain the resulting chain
- *
-- * @return the chain form chain_root to chain_tip, copied from the tree
-+ * @return success or failure
- */
-- Chain getChain(const std::string& chain_root, const std::string& chain_tip)const;
-+ bool getChain(const std::string& chain_root, const std::string& chain_tip, Chain& chain)const;
+ * Request the root segment of the tree
+@@ -173,6 +175,11 @@
-
- const SegmentMap& getSegments()const
-@@ -166,6 +174,12 @@
- }
-
virtual ~Tree(){};
-+
+
+ class segment_name_exception: public std::exception{
+ virtual const char* what() const throw(){
+ return "Segment Name excption";}
@@ -454,601 +46,46 @@
};
}
#endif
-Index: src/segment.cpp
-===================================================================
---- src/segment.cpp (revision 30247)
-+++ src/segment.cpp (working copy)
-@@ -21,20 +21,22 @@
-
- namespace KDL {
-
-- Segment::Segment(const Joint& _joint, const Frame& _f_tip, const RigidBodyInertia& _I):
-+ Segment::Segment(const std::string& _name, const Joint& _joint, const Frame& _f_tip, const RigidBodyInertia& _I):
-+ name(_name),
- joint(_joint),I(_I),
- f_tip(_joint.pose(0).Inverse() * _f_tip)
- {
- }
-
- Segment::Segment(const Segment& in):
-- joint(in.joint),I(in.I),
-+ name(in.name),joint(in.joint),I(in.I),
- f_tip(in.f_tip)
- {
- }
-
- Segment& Segment::operator=(const Segment& arg)
- {
-+ name=arg.name;
- joint=arg.joint;
- I=arg.I;
- f_tip=arg.f_tip;
-Index: src/joint.cpp
-===================================================================
---- src/joint.cpp (revision 30247)
-+++ src/joint.cpp (working copy)
-@@ -24,17 +24,17 @@
- namespace KDL {
-
- // constructor for joint along x,y or z axis, at origin of reference frame
-- Joint::Joint(const JointType& _type, const double& _scale, const double& _offset,
-+ Joint::Joint(const std::string& _name, const JointType& _type, const double& _scale, const double& _offset,
- const double& _inertia, const double& _damping, const double& _stiffness):
-- type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness)
-+ name(_name),type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness)
- {
- if (type == RotAxis || type == TransAxis) throw joint_type_ex;
- }
-
- // constructor for joint along arbitrary axis, at arbitrary origin
-- Joint::Joint(const Vector& _origin, const Vector& _axis, const JointType& _type, const double& _scale, const double& _offset,
-- const double& _inertia, const double& _damping, const double& _stiffness):
-- origin(_origin), axis(_axis / _axis.Norm()), type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness)
-+ Joint::Joint(const std::string& _name, const Vector& _origin, const Vector& _axis, const JointType& _type, const double& _scale,
-+ const double& _offset, const double& _inertia, const double& _damping, const double& _stiffness):
-+ name(_name), origin(_origin), axis(_axis / _axis.Norm()), type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness)
- {
- if (type != RotAxis && type != TransAxis) throw joint_type_ex;
-
Index: src/tree.cpp
===================================================================
---- src/tree.cpp (revision 30247)
+--- src/tree.cpp (revision 30400)
+++ src/tree.cpp (working copy)
-@@ -25,18 +25,19 @@
- namespace KDL {
- using namespace std;
+@@ -27,7 +27,7 @@
--Tree::Tree() :
-- nrOfSegments(0), nrOfJoints(0) {
-- segments.insert(make_pair("root", TreeElement::Root()));
-+Tree::Tree(const std::string& _root_name) :
-+ nrOfSegments(0), nrOfJoints(0),root_name(_root_name) {
+ Tree::Tree(const std::string& _root_name) :
+ nrOfSegments(0), nrOfJoints(0),root_name(_root_name) {
+- segments.insert(make_pair(root_name, TreeElement::Root()));
+ segments.insert(make_pair(root_name, TreeElement::Root(root_name)));
}
Tree::Tree(const Tree& in) {
- segments.clear();
- nrOfSegments = 0;
+@@ -36,9 +36,8 @@
nrOfJoints = 0;
-+ root_name = in.root_name;
+ root_name = in.root_name;
-- segments.insert(make_pair("root", TreeElement::Root()));
-- this->addTree(in, "", "root");
+- segments.insert(make_pair(root_name, TreeElement::Root()));
+ segments.insert(make_pair(root_name, TreeElement::Root(root_name)));
-+ this->addTree(in, root_name);
-
+ this->addTree(in, root_name);
+-
}
-@@ -44,21 +45,21 @@
- segments.clear();
- nrOfSegments = 0;
+ Tree& Tree::operator=(const Tree& in) {
+@@ -47,7 +46,7 @@
nrOfJoints = 0;
-+ root_name = in.root_name;
+ root_name = in.root_name;
-- segments.insert(make_pair("root", TreeElement::Root()));
-- this->addTree(in, "", "root");
+- segments.insert(make_pair(in.root_name, TreeElement::Root()));
+ segments.insert(make_pair(in.root_name, TreeElement::Root(root_name)));
-+ this->addTree(in, root_name);
+ this->addTree(in, root_name);
return *this;
}
+@@ -110,6 +109,9 @@
--bool Tree::addSegment(const Segment& segment, const std::string& segment_name,
-- const std::string& hook_name) {
-+bool Tree::addSegment(const Segment& segment, const std::string& hook_name) {
- SegmentMap::iterator parent = segments.find(hook_name);
- //check if parent exists
- if (parent == segments.end())
- return false;
- pair<SegmentMap::iterator, bool> retval;
- //insert new element
-- retval = segments.insert(make_pair(segment_name, TreeElement(segment,
-+ retval = segments.insert(make_pair(segment.getName(), TreeElement(segment,
- parent, nrOfJoints)));
- //check if insertion succeeded
- if (!retval.second)
-@@ -73,39 +74,31 @@
- return true;
- }
-
--bool Tree::addChain(const Chain& chain, const std::string& chain_name,
-- const std::string& hook_name) {
-+bool Tree::addChain(const Chain& chain, const std::string& hook_name) {
- string parent_name = hook_name;
- for (unsigned int i = 0; i < chain.getNrOfSegments(); i++) {
-- ostringstream segment_name;
-- segment_name << chain_name << "Segment" << i;
-- if (this->addSegment(chain.getSegment(i), segment_name.str(),
-- parent_name))
-- parent_name = segment_name.str();
-+ if (this->addSegment(chain.getSegment(i), parent_name))
-+ parent_name = chain.getSegment(i).getName();
- else
- return false;
- }
- return true;
- }
-
--bool Tree::addTree(const Tree& tree, const std::string& tree_name,
-- const std::string& hook_name) {
-- return this->addTreeRecursive(tree.getSegment("root"), tree_name, hook_name);
-+bool Tree::addTree(const Tree& tree, const std::string& hook_name) {
-+ return this->addTreeRecursive(tree.getRootSegment(), hook_name);
- }
-
--bool Tree::addTreeRecursive(SegmentMap::const_iterator root,
-- const std::string& tree_name, const std::string& hook_name) {
-+bool Tree::addTreeRecursive(SegmentMap::const_iterator root, const std::string& hook_name) {
- //get iterator for root-segment
- SegmentMap::const_iterator child;
- //try to add all of root's children
- for (unsigned int i = 0; i < root->second.children.size(); i++) {
- child = root->second.children[i];
- //Try to add the child
-- if (this->addSegment(child->second.segment, tree_name + child->first,
-- hook_name)) {
-+ if (this->addSegment(child->second.segment, hook_name)) {
- //if child is added, add all the child's children
-- if (!(this->addTreeRecursive(child, tree_name, tree_name
-- + child->first)))
-+ if (!(this->addTreeRecursive(child, child->first)))
- //if it didn't work, return false
- return false;
- } else
-@@ -115,25 +108,26 @@
- return true;
- }
-
--Chain Tree::getChain(const std::string& chain_root, const std::string& chain_tip)const
-+bool Tree::getChain(const std::string& chain_root, const std::string& chain_tip, Chain& chain)const
+ bool Tree::getChain(const std::string& chain_root, const std::string& chain_tip, Chain& chain)const
{
-- Chain chain;
+ // clear chain
+ chain = Chain();
-
++
// walk down from chain_root and chain_tip to the root of the tree
vector<SegmentMap::key_type> parents_chain_root, parents_chain_tip;
for (SegmentMap::const_iterator s=getSegment(chain_root); s!=segments.end(); s=s->second.parent){
- parents_chain_root.push_back(s->first);
-- if (s->first == "root") break;
-+ if (s->first == root_name) break;
- }
-- if (parents_chain_root.empty() || parents_chain_root.back() != "root") return chain;
-+ if (parents_chain_root.empty() || parents_chain_root.back() != root_name) return false;
- for (SegmentMap::const_iterator s=getSegment(chain_tip); s!=segments.end(); s=s->second.parent){
- parents_chain_tip.push_back(s->first);
-- if (s->first == "root") break;
-+ if (s->first == root_name) break;
- }
-- if (parents_chain_tip.empty() || parents_chain_tip.back() != "root") return chain;
-+ if (parents_chain_tip.empty() || parents_chain_tip.back() != root_name) return false;
-
- // remove common part of segment lists
-- SegmentMap::key_type last_segment = "root";
-+ SegmentMap::key_type last_segment = root_name;
- while (!parents_chain_root.empty() && !parents_chain_tip.empty() &&
- parents_chain_root.back() == parents_chain_tip.back()){
- last_segment = parents_chain_root.back();
-@@ -149,17 +143,18 @@
- Frame f_tip = seg.pose(0.0).Inverse();
- Joint jnt = seg.getJoint();
- if (jnt.getType() == Joint::RotX || jnt.getType() == Joint::RotY || jnt.getType() == Joint::RotZ || jnt.getType() == Joint::RotAxis)
-- jnt = Joint(f_tip*jnt.JointOrigin(), f_tip.M*jnt.JointAxis(), Joint::RotAxis);
-+ jnt = Joint(jnt.getName(), f_tip*jnt.JointOrigin(), f_tip.M*jnt.JointAxis(), Joint::RotAxis);
- else if (jnt.getType() == Joint::TransX || jnt.getType() == Joint::TransY || jnt.getType() == Joint::TransZ || jnt.getType() == Joint::TransAxis)
-- jnt = Joint(f_tip*jnt.JointOrigin(), f_tip.M*jnt.JointAxis(), Joint::TransAxis);
-- chain.addSegment(Segment(jnt, f_tip, getSegment(parents_chain_root[s+1])->second.segment.getInertia()));
-+ jnt = Joint(jnt.getName(),f_tip*jnt.JointOrigin(), f_tip.M*jnt.JointAxis(), Joint::TransAxis);
-+ chain.addSegment(Segment(getSegment(parents_chain_root[s+1])->second.segment.getName(),
-+ jnt, f_tip, getSegment(parents_chain_root[s+1])->second.segment.getInertia()));
- }
-
- // add the segments from the common frame to the tip frame
-- for (int s=parents_chain_tip.size()-1; s>-1; s--)
-+ for (int s=parents_chain_tip.size()-1; s>-1; s--){
- chain.addSegment(getSegment(parents_chain_tip[s])->second.segment);
--
-- return chain;
-+ }
-+ return true;
- }
-
- }
-Index: src/segment.hpp
-===================================================================
---- src/segment.hpp (revision 30247)
-+++ src/segment.hpp (working copy)
-@@ -46,6 +46,7 @@
- class Segment {
- friend class Chain;
- private:
-+ std::string name;
- Joint joint;
- RigidBodyInertia I;
- Frame f_tip;
-@@ -60,7 +61,7 @@
- * the segment, default: Frame::Identity()
- * @param M rigid body inertia of the segment, default: Inertia::Zero()
- */
-- Segment(const Joint& joint=Joint(Joint::None), const Frame& f_tip=Frame::Identity(),const RigidBodyInertia& I = RigidBodyInertia::Zero());
-+ Segment(const std::string& name="NoName", const Joint& joint=Joint("NoName", Joint::None), const Frame& f_tip=Frame::Identity(),const RigidBodyInertia& I = RigidBodyInertia::Zero());
- Segment(const Segment& in);
- Segment& operator=(const Segment& arg);
-
-@@ -88,6 +89,16 @@
- Twist twist(const double& q,const double& qdot)const;
-
- /**
-+ * Request the name of the segment
-+ *
-+ *
-+ * @return const reference to the name of the segment
-+ */
-+ const std::string& getName()const
-+ {
-+ return name;
-+ }
-+ /**
- * Request the joint of the segment
- *
- *
-Index: src/joint.hpp
-===================================================================
---- src/joint.hpp (revision 30247)
-+++ src/joint.hpp (working copy)
-@@ -48,6 +48,7 @@
- /**
- * Constructor of a joint.
- *
-+ * @param name of the joint
- * @param type type of the joint, default: Joint::None
- * @param scale scale between joint input and actual geometric
- * movement, default: 1
-@@ -58,7 +59,7 @@
- * @param stiffness 1D stiffness along the joint axis,
- * default: 0
- */
-- Joint(const JointType& type=None,const double& scale=1,const double& offset=0,
-+ Joint(const std::string& name="NoName", const JointType& type=None,const double& scale=1,const double& offset=0,
- const double& inertia=0,const double& damping=0,const double& stiffness=0);
- /**
- * Constructor of a joint.
-@@ -74,7 +75,7 @@
- * @param stiffness 1D stiffness along the joint axis,
- * default: 0
- */
-- Joint(const Vector& _origin, const Vector& _axis, const JointType& type, const double& _scale=1, const double& _offset=0,
-+ Joint(const std::string& name, const Vector& _origin, const Vector& _axis, const JointType& type, const double& _scale=1, const double& _offset=0,
- const double& _inertia=0, const double& _damping=0, const double& _stiffness=0);
-
- /**
-@@ -108,8 +109,17 @@
- * @return Vector
- */
- Vector JointOrigin() const;
--
- /**
-+ * Request the name of the joint
-+ *
-+ *
-+ * @return const reference to the name of the joint
-+ */
-+ const std::string& getName()const
-+ {
-+ return name;
-+ }
-+ /**
- * Request the type of the joint.
- *
- * @return const reference to the type
-@@ -153,6 +163,7 @@
- virtual ~Joint();
-
- private:
-+ std::string name;
- Joint::JointType type;
- double scale;
- double offset;
-Index: src/bindings/python/frames.sip
-===================================================================
---- src/bindings/python/frames.sip (revision 30248)
-+++ src/bindings/python/frames.sip (working copy)
-@@ -137,9 +137,11 @@
- static Rotation RotY(double angle)/Factory/;
- static Rotation RotZ(double angle)/Factory/;
- static Rotation Rot(const Vector& vec,double angle)/Factory/;
-+ static Rotation Rot2(const Vector& vec,double angle)/Factory/;
- static Rotation EulerZYZ(double Alfa,double Beta,double Gamma)/Factory/;
- static Rotation RPY(double roll,double pitch,double yaw)/Factory/;
- static Rotation EulerZYX(double Alfa,double Beta,double Gamma)/Factory/;
-+ static Rotation Quaternion(double x, double y, double z, double w)/Factory/;
-
-
- void DoRotX(double angle);
-@@ -151,6 +153,8 @@
- void GetEulerZYZ(double& alfa /Out/,double& beta /Out/,double& gamma /Out/) const;
- void GetRPY(double& roll /Out/,double& pitch /Out/,double& yaw /Out/) const;
- void GetEulerZYX(double& Alfa /Out/,double& Beta /Out/,double& Gamma /Out/) const;
-+ void GetQuaternion(double& x /Out/,double& y /Out/,double& z /Out/, double& w) const;
-+
-
- Vector operator*(const Vector& v) const /Numeric,Factory/;
- Twist operator*(const Twist& arg) const /Numeric,Factory/;
-@@ -159,6 +163,11 @@
- Vector UnitX() const /Factory/;
- Vector UnitY() const /Factory/;
- Vector UnitZ() const /Factory/;
-+
-+ void UnitX(const Vector& X);
-+ void UnitY(const Vector& X);
-+ void UnitZ(const Vector& X);
-+
-
- };
- bool Equal(const Rotation& a,const Rotation& b,double eps=epsilon);
-Index: src/bindings/python/kinfam.sip
-===================================================================
---- src/bindings/python/kinfam.sip (revision 30247)
-+++ src/bindings/python/kinfam.sip (working copy)
-@@ -329,39 +329,74 @@
- ChainIkSolverVel_wdls(const Chain& chain,double eps=0.00001,int maxiter=150);
-
- virtual int CartToJnt(const JntArray& q_in, const Twist& v_in, JntArray& qdot_out);
-- void setWeightTS(SIP_PYTUPLE);
-+ void setWeightTS(SIP_PYLIST);
- %MethodCode
-- //void setWeightTS(const ublas::symmetric_matrix<double> Mx);
-+ //void setWeightTS(const Eigen::MatrixXd& Mx);
- //Mx has to be a 6x6 Matrix
-- double arr[6];
-- PyArg_ParseTuple(a0, "dddddd", &arr[0], &arr[1], &arr[2], &arr[3], &arr[4], &arr[5]);
-- ublas::symmetric_matrix<double> Wts(ublas::identity_matrix<double>(6));
-- Wts(0,0) = arr[0];
-- Wts(1,1) = arr[1];
-- Wts(2,2) = arr[2];
-- Wts(3,3) = arr[3];
-- Wts(4,5) = arr[4];
-- Wts(5,5) = arr[5];
-
-- sipCpp->setWeightTS(Wts);
--
-+ Py_ssize_t numRows,numCols;
-+ double c_item;
-+ PyObject *list=a0;
-+ numRows=PyList_Size(list);
-+ PyObject *temp1;
-+ temp1=PyList_GetItem(list,0);
-+ numCols=PyList_Size(temp1);
-+ if (numRows!=numCols) {
-+ sipIsErr=1; //todo: raise exception
-+ }
-+ if (numRows!=6) {
-+ sipIsErr=1; //todo: raise exception
-+ }
-+ Eigen::MatrixXd Mx;
-+ Mx=Eigen::MatrixXd::Identity(numRows,numCols);
-+
-+ for (Py_ssize_t r=0;r<numRows;r++) {
-+ PyObject *row;
-+ row=PyList_GetItem(list,r);
-+ if (numCols!=PyList_Size(row)) {
-+ sipIsErr=1; //todo: raise exception
-+ }
-+ for (Py_ssize_t c=0;c<numCols;c++) {
-+ PyObject *item;
-+ item=PyList_GetItem(row,c);
-+ c_item=PyFloat_AsDouble(item);
-+ Mx(r,c)= c_item;
-+ }
-+ }
-+ sipCpp->setWeightTS(Mx);
- %End
-
-- void setWeightJS(SIP_PYTUPLE);
-+ void setWeightJS(SIP_PYLIST);
- %MethodCode
-- //void setWeightJS(const ublas::symmetric_matrix<double> Mx);
-- //Mx has to be a 6x6 Matrix
-- double arr[6];
-- PyArg_ParseTuple(a0, "dddddd", &arr[0], &arr[1], &arr[2], &arr[3], &arr[4], &arr[5]);
-- ublas::symmetric_matrix<double> Wts(ublas::identity_matrix<double>(6));
-- Wts(0,0) = arr[0];
-- Wts(1,1) = arr[1];
-- Wts(2,2) = arr[2];
-- Wts(3,3) = arr[3];
-- Wts(4,5) = arr[4];
-- Wts(5,5) = arr[5];
--
-- sipCpp->setWeightJS(Wts);
-+ //void setWeightJS(const Eigen::MatrixXd& Mx);
-+ //Mx has to be a simetric positive definite Matrix
-+ //unsigned int nOfJoints=sipCpp->chain.getNrOfJoints(); //To check that we are receiving valid data dimensions. This doesn't work, chain is a private member. todo: How can we check for this?
-+ Py_ssize_t numRows,numCols;
-+ double c_item;
-+ PyObject *list=a0;
-+ numRows=PyList_Size(list);
-+ PyObject *temp1;
-+ temp1=PyList_GetItem(list,0);
-+ numCols=PyList_Size(temp1);
-+ if (numRows!=numCols) {
-+ sipIsErr=1; //todo: raise exception
-+ }
-+ Eigen::MatrixXd Mq;
-+ Mq=Eigen::MatrixXd::Identity(numRows,numCols);
-+ for (Py_ssize_t r=0;r<numRows;r++) {
-+ PyObject *row;
-+ row=PyList_GetItem(list,r);
-+ if (numCols!=PyList_Size(row)) {
-+ sipIsErr=1; //todo: raise exception
-+ }
-+ for (Py_ssize_t c=0;c<numCols;c++) {
-+ PyObject *item;
-+ item=PyList_GetItem(row,c);
-+ c_item=PyFloat_AsDouble(item);
-+ Mq(r,c)= c_item;
-+ }
-+ }
-+ sipCpp->setWeightJS(Mq);
- %End
-
- void setLambda(const double& lambda);
-Index: config/FindEigen2.cmake
-===================================================================
---- config/FindEigen2.cmake 2009-07-13 21:23:42.827620408 +0200
-+++ config/FindEigen2.cmake 2009-07-13 21:21:43.260158060 +0200
-@@ -1,7 +1,7 @@
--FIND_PATH(EIGEN2_INCLUDE_DIR Eigen/Core )
-+FIND_PATH(EIGEN2_INCLUDE_DIR Eigen/Core /usr/include /usr/include/eigen2)
- IF ( EIGEN2_INCLUDE_DIR )
- MESSAGE(STATUS "-- Looking for Eigen2 - found")
- SET(KDL_CFLAGS "${KDL_CFLAGS} -I${EIGEN2_INCLUDE_DIR}" CACHE INTERNAL "")
- ELSE ( EIGEN2_INCLUDE_DIR )
- MESSAGE(FATAL_ERROR "-- Looking for Eigen2 - not found")
--ENDIF ( EIGEN2_INCLUDE_DIR )
-\ No newline at end of file
-+ENDIF ( EIGEN2_INCLUDE_DIR )
-Index: src/bindings/python/kinfam.sip
-===================================================================
---- src/bindings/python/kinfam.sip 2009-07-13 21:23:48.117644375 +0200
-+++ src/bindings/python/kinfam.sip 2009-07-13 21:21:44.124157407 +0200
-@@ -30,7 +87,7 @@
-
- public:
- enum JointType {RotX,RotY,RotZ,TransX,TransY,TransZ,None};
-- Joint(JointType type=None,double scale=1,double offset=0,
-+ Joint(std::string name=std::string("NoName"), JointType type=None,double scale=1,double offset=0,
- double inertia=0,const double damping=0,double stiffness=0);
- Joint(const Joint& in);
-
-@@ -52,10 +109,11 @@
-
- %TypeHeaderCode
- #include <segment.hpp>
-+#include <string>
- using namespace KDL;
- %End
- public:
-- Segment(const Joint& joint=Joint::None, const Frame& f_from_parent=Frame::Identity());
-+ Segment( const std::string name = std::string("NoName"), const Joint& joint=Joint("NoName",Joint::None), const Frame& f_from_parent=Frame::Identity());
- Segment(const Segment& in);
-
- const char* __repr__();
-
-Index: src/bindings/python/configure.py.in
-===================================================================
---- src/bindings/python/configure.py.in 2009-07-13 22:29:03.949893050 +0200
-+++ src/bindings/python/configure.py.in 2009-07-13 22:27:42.019080711 +0200
-@@ -7,6 +7,8 @@
- build_path = '@PROJ_BINARY_DIR@/src'
- python_path = src_path + '/bindings/python'
- kdl_cflags ='@KDL_CFLAGS@'
-+install_path = '@CMAKE_INSTALL_PREFIX@'
-+
- # create paths for temporary files if don't exist
- if not os.path.isdir("./sip"):
- os.mkdir("./sip")
-@@ -62,6 +64,8 @@
- makefile.extra_lib_dirs = [build_path]
- makefile.extra_include_dirs = [src_path,build_path]
- makefile.extra_cxxflags=[kdl_cflags]
-+#Give the linker a hint about where to look for the libraries
-+makefile.LFLAGS.append("-Wl,-rpath," + install_path + "/lib")
- # Generate the Makefile itself.
- makefile.generate()
-
-Index: src/bindings/python/kinfam.sip
-===================================================================
---- src/bindings/python/kinfam.sip 2009-07-13 22:49:45.136464058 +0200
-+++ src/bindings/python/kinfam.sip 2009-07-13 22:44:16.159832886 +0200
-@@ -19,6 +19,7 @@
- //License along with this library; if not, write to the Free Software
- //Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
-
-+%Include std_string.sip
-
- class Joint{
-
-Index: src/bindings/python/std_string.sip
-===================================================================
---- src/bindings/python/std_string.sip 1970-01-01 01:00:00.000000000 +0100
-+++ src/bindings/python/std_string.sip 2009-07-13 22:43:11.610296410 +0200
-@@ -0,0 +1,60 @@
-+//Copyright (C) 2005 Torsten Marek <shlomme at gmx.net>
-+//
-+//This library is free software; you can redistribute it and/or
-+//modify it under the terms of the GNU Lesser General Public
-+//License as published by the Free Software Foundation; either
-+//version 2.1 of the License, or (at your option) any later version.
-+//
-+//This library is distributed in the hope that it will be useful,
-+//but WITHOUT ANY WARRANTY; without even the implied warranty of
-+//MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
-+//Lesser General Public License for more details.
-+//
-+//You should have received a copy of the GNU Lesser General Public
-+//License along with this library; if not, write to the Free Software
-+//Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
-+
-+
-+%MappedType std::string
-+{
-+%TypeHeaderCode
-+#include <string>
-+%End
-+
-+%ConvertFromTypeCode
-+ // convert an std::string to a Python (unicode) string
-+ PyObject* newstring;
-+ newstring = PyUnicode_DecodeUTF8(sipCpp->c_str(), sipCpp->length(), NULL);
-+ if(newstring == NULL) {
-+ PyErr_Clear();
-+ newstring = PyString_FromString(sipCpp->c_str());
-+ }
-+ return newstring;
-+%End
-+
-+%ConvertToTypeCode
-+ // Allow a Python string (or a unicode string) whenever a string is
-+ // expected.
-+ // If argument is a Unicode string, just decode it to UTF-8
-+ // If argument is a Python string, assume it's UTF-8
-+ if (sipIsErr == NULL)
-+ return (PyString_Check(sipPy) || PyUnicode_Check(sipPy));
-+ if (sipPy == Py_None) {
-+ *sipCppPtr = new std::string;
-+ return 1;
-+ }
-+ if (PyUnicode_Check(sipPy)) {
-+ PyObject* s = PyUnicode_AsEncodedString(sipPy, "UTF-8", "");
-+ *sipCppPtr = new std::string(PyString_AS_STRING(s));
-+ Py_DECREF(s);
-+ return 1;
-+ }
-+ if (PyString_Check(sipPy)) {
-+ *sipCppPtr = new std::string(PyString_AS_STRING(sipPy));
-+ return 1;
-+ }
-+
-+ return 0;
-+%End
-+};
-+
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
