error compiling while nothing had been changed ?

CMakeLists.txt

@@ -130,8 +130,11 @@ catkin_package(
 include_directories(
     /home/lucas/toolkit-dynamixel/include
     /home/lucas/toolkit-kinematics/include
+    /home/lucas/toolkit-dynamixel
+    /home/lucas/toolkit-kinematics
    ${OpenCV_INCLUDE_DIRS}
    /home/lucas/eigen/Eigen/include
+   /home/lucas/eigen/Eigen
   ${catkin_INCLUDE_DIRS}
 )
 
@@ -154,6 +157,8 @@ add_executable(poppy_ros src/poppy_ros.cpp /home/lucas/toolkit-dynamixel/src/Dyn
 add_executable(ik_server src/ik_server.cpp /home/lucas/toolkit-kinematics/src/Kinematics.cpp /home/lucas/toolkit-kinematics/src/RotationMatrix.cpp /home/lucas/toolkit-kinematics/src/TranslationMatrix.cpp /home/lucas/toolkit-kinematics/src/TransformationMatrix.cpp)
 
 add_executable(ik_client src/ik_client.cpp)
+
+add_executable(sim_poppy_joint src/poppy_ros.cpp)
 ## Rename C++ executable without prefix
 ## The above recommended prefix causes long target names, the following renames the
 ## target back to the shorter version for ease of user use
@@ -169,10 +174,11 @@ add_executable(ik_client src/ik_client.cpp)
 #   ${catkin_LIBRARIES}
 # )
 target_link_libraries(poppy_ros dxl_x64_cpp rt ${catkin_LIBRARIES})
+target_link_libraries(sim_poppy_joint ${catkin_LIBRARIES})
 
-#target_link_libraries(ik_server ${catkin_LIBRARIES} ${OpenCV_LIBS})
+target_link_libraries(ik_server ${catkin_LIBRARIES} ${OpenCV_LIBS})
 
-#target_link_libraries(ik_client ${catkin_LIBRARIES})
+target_link_libraries(ik_client ${catkin_LIBRARIES})
 #############
 ## Install ##
 #############

src/ik_client.cpp

@@ -1,6 +1,7 @@
 #include <iostream>
 #include "ros/ros.h"
 #include "poppy_ros/ik.h"
+#include "geometry_msgs/Twist.h"
 
 
 int main(int argc, char** argv)
@@ -8,13 +9,27 @@ int main(int argc, char** argv)
 	ros::init(argc, argv, "ik_client");
 	ros::NodeHandle nh;
 	ros::ServiceClient client = nh.serviceClient<poppy_ros::ik>("ik");
+	ros::Publisher joint_cmd_pub = nh.advertise<geometry_msgs::Twist>("/joint_cmd", 1);
 	poppy_ros::ik srv;
-	srv.request.x = 1;
+	srv.request.x = atoll(argv[1]);
-	srv.request.y = 2;
+	srv.request.y = atoll(argv[2]);
-	srv.request.z = 2;
+	srv.request.z = atoll(argv[3]);
 	if (client.call(srv))
 	{
-		std::cout << srv.response.j1 << std::endl;
+		// Assuming joint values are to be published as a geometry_msgs::Twist message
+		geometry_msgs::Twist joint_cmd;
+		
+		joint_cmd.linear.x = srv.response.joints[0];
+		joint_cmd.linear.y = srv.response.joints[1];
+		joint_cmd.linear.z = srv.response.joints[2];
+		joint_cmd.angular.x = srv.response.joints[3];
+		joint_cmd.angular.y = srv.response.joints[4];
+		joint_cmd.angular.z = srv.response.joints[5];
+
+		joint_cmd_pub.publish(joint_cmd);
+		ROS_INFO("Command passed");
+	    } else {
+		ROS_ERROR("Failed to call service ik_service");
 	}
 	return 0;
 }

src/ik_server.cpp

@@ -1,15 +1,49 @@
 #include "ros/ros.h"
-#include "poppy_ros/ik.h
+#include "poppy_ros/ik.h"
-bool add( poppy_ros::ik::Request& req,poppy_ros::ik::Response& res)
+#include "Kinematics.h"
-{
+#include <opencv2/opencv.hpp>
-	res.j1 = req.a + req.b;
+
-	return true;
+Kinematics kinematics;
+
+bool ProcessIK(poppy_ros::ik::Request &req, poppy_ros::ik::Response &res) {
+    // Create a cv::Mat for Cartesian coordinates
+    cv::Mat cartesian_coords = (cv::Mat_<double>(3, 1) << req.x, req.y, req.z);
+
+    // Create a cv::Mat for joint values
+    cv::Mat joint_values;
+
+    // Perform inverse kinematics calculations and get the result directly
+    kinematics.inverseKinematics(cartesian_coords);
+    
+    // Get the joint values after inverse kinematics
+    joint_values = kinematics.computeCurrentEndEffectorPosition();
+
+
+    // Set the response
+    if (!joint_values.empty()) {
+        // Convert cv::Mat to std::vector<double>
+        std::vector<double> joint_values_vector(joint_values.begin<double>(), joint_values.end<double>());
+
+        // Set the response
+        res.joints = joint_values_vector;
+        return true;
+    } else {
+        ROS_ERROR("Inverse kinematics calculation failed.");
+        return false;
+    }
 }
+
 int main(int argc, char** argv)
 {
 	ros::init(argc, argv, "ik_server");
 	ros::NodeHandle nh;
-	ros::ServiceServer server = nh.advertiseService("ik", add);
+	
+	//load to the kinematics lib
+	kinematics.loadDHParameters("/home/lucas/catkin_ws/src/poppy_ros/src/poppy_dh.yaml");
+	kinematics.loadIKParameters("/home/lucas/catkin_ws/src/poppy_ros/src/poppy_ik.yaml");
+	
+	ros::ServiceServer server = nh.advertiseService("ik", ProcessIK);
 	ros::spin();
 	return 0;
 }
+

src/poppy_dh.yaml

@@ -0,0 +1,11 @@
+%YAML:1.0
+---
+dhThetaParams: [ 0., 0., 0., 0., 0., 0., 0. ]
+dhDParams: [ 2.5000000000000000e+00, 2.5000000000000000e+00, 0., 0., 0.,
+    0., 0. ]
+dhAParams: [ 0., 0., 5.5000000000000000e+00, 3., 0., 5., 12. ]
+dhAlphaParams: [ 0., 90., 0., 0., -90., 90., 0. ]
+jointType: [ 0, 1, 1, 1, 0, 1, 1 ]
+nbJoints: 5
+jointOffset: [ 0., 0., 90., 0., -90., 0., 0. ]
+qiValues: [ 0., 0., 0., 0., 90. ]

src/poppy_ik.yaml

@@ -0,0 +1,6 @@
+%YAML:1.0
+---
+maxNbIterations: 1000
+pController: 0.5
+rankThreshold: 0.2
+distanceThreshold: 0.01

src/sim_poppy_joint.cpp

@@ -0,0 +1,52 @@
+#include "ros/ros.h"
+#include "geometry_msgs/Twist.h"
+
+std::vector<uint16_t> jointVec;
+void jointCMDCallback(const geometry_msgs::Twist& joint_cmd)
+{
+	jointVec.clear();
+	//fill it w joint_cmd_values
+	jointVec.push_back(joint_cmd.linear.x);
+	jointVec.push_back(joint_cmd.linear.y);
+	jointVec.push_back(joint_cmd.linear.z);
+	jointVec.push_back(joint_cmd.angular.x);
+	jointVec.push_back(joint_cmd.angular.y);
+	jointVec.push_back(joint_cmd.angular.z);
+}
+double deg2rad(double angle)
+{
+	return -angle / 180.0 * M_PI;
+}
+int main(int argc, char** argv)
+{
+	ros::init(argc, argv, "poppy_ros", ros::init_options::NoSigintHandler);
+	
+	// create a node handle
+	ros::NodeHandle nh;
+	
+	
+	//create a subscriber
+	ros::Subscriber sub = nh.subscribe("joint_cmd",1, jointCMDCallback);
+	
+	// create a loop rate
+	ros::Rate loopRate(10);
+	
+	
+	
+	ROS_INFO("===Launching Poppy node 2===");
+	ros::Publisher jointCmdPublisher = nh.advertise<sensor_msgs::JointState>("joint_states", 1);
+	std::vector<std::string> jointCmdNameArray = {"m1", "m2", "m3", "m4", "m5", "m6"};
+	while (ros::ok())
+	{
+		sensor_msgs::JointState jointCmdMsg;
+		jointCmdMsg.header.stamp = ros::Time::now();
+		jointCmdMsg.header.seq++;
+		jointCmdMsg.position = jointVec;
+		jointCmdMsg.name = jointCmdNameArray;
+		jointCmdPublisher.publish(jointCmdMsg);
+		ROS_INFO("===data published===");
+		ros::spinOnce();
+		loopRate.sleep();
+	}
+	return 0;
+}

srv/ik.srv

@@ -2,9 +2,4 @@ int64 x
 int64 y
 int64 z
 ---
-int64 j1
+float64[] joints
-int64 j2
-int64 j3
-int64 j4
-int64 j5
-int64 j6