Move joint_positions forwardKinematics to motion planner.

fizyr · Jul 29, 2016 · d82d90a · d82d90a
1 parent 9282083
commit d82d90a
Show file tree

Hide file tree

Showing 2 changed files with 2 additions and 45 deletions.
diff --git a/include/dr_kdl/dr_kdl.hpp b/include/dr_kdl/dr_kdl.hpp
@@ -2,10 +2,9 @@
 #include <map>
 #include <string>
 
-#include <sensor_msgs/JointState.h>
-#include <kdl/tree.hpp>
 #include <Eigen/Geometry>
-#include "urpp/kinematics.hpp"
+#include <kdl/tree.hpp>
+#include <sensor_msgs/JointState.h>
 
 namespace dr {
 
@@ -44,12 +43,6 @@ Eigen::Isometry3d getTransform(
 	std::vector<double> const & joint_positions   ///< The positions of the joints in the same order as the joint name vector.
 );
 
-Eigen::Isometry3d getTransform(
-	KDL::Chain const & chain, 					  ///< The chain
-	urpp::joint_position const & joint_positions, ///< The joint positions
-	const std::string & joint_name_prefix = "ur_" ///< Prefix for ur arm joint names.
-);
-
 
 /// KDL tree wrapper.
 class KdlTree : public KDL::Tree {
@@ -116,16 +109,6 @@ class KdlTree : public KDL::Tree {
 		return getTransform(getChain(source, target), joints.name, joints.position);
 	}
 
-	Eigen::Isometry3d transform(
-		std::string const & source,                   ///< The source frame.
-		std::string const & target,                   ///< The target frame.
-		urpp::joint_position const & joint_positions, ///< The joint positions.
-		std::string const & joint_name_prefix = "ur_" ///< joint name prefix for ur arm.
-	) const {
-		return getTransform(getChain(source, target), joint_positions, joint_name_prefix);
-	}
-
-
 };
 
 }
diff --git a/src/dr_kdl.cpp b/src/dr_kdl.cpp
@@ -87,32 +87,6 @@ Eigen::Isometry3d getTransform(KDL::Chain const & chain, std::vector<std::string
 	return toEigen(transform);
 }
 
-Eigen::Isometry3d getTransform(KDL::Chain const & chain, urpp::joint_position const & joint_positions, const std::string & joint_name_prefix) {
-
-	std::vector<std::string> joint_names = {joint_name_prefix+"shoulder_pan_joint",
-											joint_name_prefix+"shoulder_lift_joint",
-											joint_name_prefix+"elbow_joint",
-											joint_name_prefix+"wrist_1_joint",
-											joint_name_prefix+"wrist_2_joint",
-											joint_name_prefix+"wrist_3_joint"};
-
-	KDL::Frame transform = KDL::Frame::Identity();
-
-	for (auto const & segment : chain.segments) {
-		// Fixed joints.
-		if (segment.getJoint().getType() == KDL::Joint::JointType::None) {
-			transform = transform * segment.pose(0);
-
-		// Look up non-fixed joints in map.
-		} else {
-			auto index = std::find(joint_names.begin(), joint_names.end(), segment.getJoint().getName());
-			if (index == joint_names.end()) throw std::runtime_error("Joint `" + segment.getName() + "' not found in joint list.");
-			transform = transform * segment.pose(joint_positions[index - joint_names.begin()]);
-		}
-	}
-	return toEigen(transform);
-}
-
 KDL::Chain KdlTree::getChain(std::string const & start, std::string const & end) const {
 	KDL::Chain chain;
 	if (KDL::Tree::getChain(start, end, chain)) {