add forward kinematics program

Kinematics/CMakeLists.txt

@@ -0,0 +1,25 @@
+project(TestKinematics)
+
+set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -g")
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -g")
+#boost
+find_package(Boost COMPONENTS system thread filesystem)
+
+#Eigen
+set(EIGEN_DIR ${EIGEN_DIR} CACHE PATH "The directory of the Eigen library")
+set(EIGEN_INCLUDE_DIRS ${EIGEN_DIR})
+
+include_directories(
+	${BOOST_INCLUDE_DIRS}
+	${Boost_INCLUDE_DIR}
+	${EIGEN_INCLUDE_DIRS}
+)
+
+link_directories(
+	${Boost_LIBRARY_DIRS}
+	${Boost_LIBRARY_DIR}
+)
+
+add_executable(TestKinematics ForwardKinematics.cpp Link.cpp testKinematics.cpp ForwardKinematics.h Link.h LinkParameter.h)
+
+target_link_libraries(TestKinematics ${Boost_LIBRARIES})

Kinematics/ForwardKinematics.cpp

@@ -0,0 +1,36 @@
+#include "ForwardKinematics.h"
+
+using namespace MotionControl;
+
+Matrix<double,3,3> ForwardKinematics::Rodrigues(Matrix<double,3,1> a, double q)
+{
+	return AngleAxisd(q,a).toRotationMatrix();
+}
+
+vector<int> ForwardKinematics::FindRoute(int to)
+{
+	vector<int> idx;
+	int link_num = to;
+
+	while(link_num != 0)
+	{
+		idx.push_back(link_num);
+		link_num = ulink[link_num].parent;
+	}
+	reverse(idx.begin(), idx.end());
+	return idx;
+}
+
+void ForwardKinematics::calcForwardKinematics(int rootlink)
+{
+	if(rootlink == -1)
+		return ;
+	if(rootlink != 0)
+	{
+		int parent = ulink[rootlink].parent;
+		ulink[rootlink].p = ulink[parent].R * ulink[rootlink].b + ulink[parent].p;
+		ulink[rootlink].R = ulink[parent].R * Rodrigues(ulink[rootlink].a, ulink[rootlink].q);
+	}
+	calcForwardKinematics(ulink[rootlink].sister);
+	calcForwardKinematics(ulink[rootlink].child);
+}

Kinematics/ForwardKinematics.h

@@ -0,0 +1,30 @@
+/*
+ * @file		ForwardKinematics.h
+ * @brief		Calculation ForwardKinematics
+ * @author	Ryu Yamamoto
+ * @date		2016/03/03
+ */
+
+#ifndef _FORWARD_KINEMATICS_
+#define _FORWARD_KINEMATICS_
+
+#include "Link.h"
+
+namespace MotionControl
+{
+	class ForwardKinematics
+	{
+	public:
+		struct Link *ulink;
+	public:
+		ForwardKinematics(Link *ulink)
+		{
+			this->ulink = ulink;
+		}
+		void calcForwardKinematics(int rootlink);
+		vector<int> FindRoute(int to);
+		Matrix<double,3,3> Rodrigues(Matrix<double,3,1> a, double q);
+	};
+};
+
+#endif

Kinematics/Link.cpp

@@ -0,0 +1,19 @@
+#include "Link.h"
+
+using namespace MotionControl;
+
+extern "C" void SetJointInfo(struct Link *link)
+{
+	for(int i=0;i<LEG_JOINT_NUM;i++)
+	{
+		link[i].joint_name = joint_name[i];
+		link[i].parent = parent[i];
+		link[i].child = child[i];
+		link[i].sister = sister[i];
+		for(int j=0;j<3;j++)
+		{
+			link[i].a(j) = LinkAxis[i][j];
+			link[i].b(j) = LinkPos[i][j];
+		}
+	}
+}

Kinematics/Link.h

@@ -0,0 +1,50 @@
+/*
+ * @file		Link.h
+ * @brief		Link Information
+ * @author	Ryu Yamamoto
+ * @date		2016/02/26
+ */
+
+#ifndef _LINK_
+#define _LINK_
+
+#include <iostream>
+#include <string>
+#include <vector>
+#include <Eigen/Dense>
+#include "LinkParameter.h"
+
+using namespace std;
+using namespace Eigen;
+
+namespace MotionControl
+{
+	struct Link
+	{
+		string joint_name;
+		int sister;
+		int child;
+		int parent;
+		Matrix<double,3,1> p;
+		Matrix<double,3,3> R;
+		Matrix<double,3,1> a;
+		Matrix<double,3,1> b;	
+		double q;						
+		double dq;						
+		double ddq;
+
+		Link() : 
+			p(Matrix<double,3,1>::Zero()),
+			R(Matrix<double,3,3>::Identity()),
+			a(Matrix<double,3,1>::Zero()),
+			b(Matrix<double,3,1>::Zero()),
+			q(0.0),
+			dq(0.0),
+			ddq(0.0)
+		{
+		}
+	};
+	extern "C" void SetJointInfo(struct Link *link);
+};
+
+#endif

Kinematics/LinkParameter.h

@@ -0,0 +1,58 @@
+/*
+ * @file		LinkParameter.h
+ * @brief		Constant Value of Link for Accelite
+ * @author	Ryu Yamamoto
+ * @date		2016/02/26
+ */
+
+#ifndef _LINK_PARAMETER_
+#define _LINK_PARAMETER_
+
+#include <iostream>
+#include <string>
+#include <Eigen/Dense>
+#include <limits>
+#include <boost/math/constants/constants.hpp>
+
+using namespace std;
+using namespace Eigen;
+
+static const double eps = numeric_limits<double>::epsilon();
+static const double pi = boost::math::constants::pi<double>();
+
+static const string joint_name[] = {
+	"WAIST",
+	"LINK_1",
+	"LINK_2",
+	"LINK_3"
+};
+
+enum
+{
+	WAIST=0,
+	LINK_1,
+	LINK_2,
+	LINK_3,
+	LEG_JOINT_NUM
+};
+
+static const int parent[LEG_JOINT_NUM] = {-1,0,1,2};
+static const int child[LEG_JOINT_NUM] = {1,2,3,-1};
+static const int sister[LEG_JOINT_NUM] = {-1,-1,-1,-1};
+
+static const double LinkAxis[LEG_JOINT_NUM][3] = {
+	{0,0,0},	//WASIT
+	{0,1,0},	//LINK_1
+	{0,1,0},	//LINK_2
+	{0,1,0}		//LINK_3
+};
+
+//For test
+static const double LinkPos[LEG_JOINT_NUM][3] = {
+	{0.0f, 0.0f, 0.0f},		//WAIST
+	{0.0f, 0.0f, 0.0f},	//LINK_1
+	{50.0f, 0.0f, 0.0f},	//LINK_2
+	{50.0f, 0.0f, 0.0f}	//LINK_3
+};
+
+#endif

Kinematics/testKinematics.cpp

@@ -0,0 +1,19 @@
+#include "ForwardKinematics.h"
+
+using namespace std;
+using namespace MotionControl;
+
+int main(int argc, char* argv[])
+{
+	//Initialize Link info
+
+	//Consturctor of the ForwardKinematics Class
+
+	//Calculation Forward Kinematics
+
+	//Set Joint Angle
+
+	//Calculation Forward Kinematics
+
+	return 0;
+}