Fixes to kinematic chain implementation

git-svn-id: http://svn.code.sf.net/p/simox/code/trunk@666 042f3d55-54a8-47e9-b7fb-15903f145c44

VirtualRobot/KinematicChain.cpp

@@ -9,8 +9,8 @@
 namespace VirtualRobot
 {
 
-KinematicChain::KinematicChain(const std::string& name, const std::vector< RobotNodePtr >& robotNodes, RobotNodePtr tcp, RobotNodePtr kinematicRoot /*= RobotNodePtr()*/ )
-	:RobotNodeSet(name, RobotPtr(), robotNodes, kinematicRoot)
+KinematicChain::KinematicChain(const std::string& name, RobotPtr robot, const std::vector< RobotNodePtr >& robotNodes, RobotNodePtr tcp, RobotNodePtr kinematicRoot /*= RobotNodePtr()*/ )
+    :RobotNodeSet(name, robot, robotNodes, kinematicRoot)
 {
 	if (robotNodes.size()==0)
 		THROW_VR_EXCEPTION("Zero sized Kinematic chain is not allowed");
@@ -30,7 +30,7 @@ KinematicChain::KinematicChain(const std::string& name, const std::vector< Robot
 
 	this->robot = robotNodes[0]->getRobot();
 	if (!tcp)
-		this->tcp = robotNodes[0];
+        this->tcp = robotNodes[this->getSize()-1];
 	else
 		this->tcp = tcp;
 }

VirtualRobot/KinematicChain.h

@@ -35,7 +35,7 @@ namespace VirtualRobot
 			  \param kinematicRoot	This specifies the first node of the robot's kinematic tree to be used for updating all members of this set. 
 									kinematicRoot does not have to be a node of this set. If not given, the first entry of robotNodes is used.
 			  */
-			KinematicChain(const std::string& name, const std::vector< RobotNodePtr >& robotNodes, RobotNodePtr tcp = RobotNodePtr(), RobotNodePtr kinematicRoot = RobotNodePtr() );
+            KinematicChain(const std::string& name, RobotPtr robot, const std::vector< RobotNodePtr >& robotNodes, RobotNodePtr tcp = RobotNodePtr(), RobotNodePtr kinematicRoot = RobotNodePtr() );
 
 			/*!
 			*/