ik corrected

catkin_ws/src/poppy_ros/src/poppy_pong_iv.cpp

@@ -1,6 +1,8 @@
 #include "ros/ros.h"
 #include "geometry_msgs/Twist.h"
-#include "std_msgs/Float32MultiArray.h"
+#include <std_msgs/MultiArrayLayout.h>
+#include <std_msgs/MultiArrayDimension.h>
+#include <std_msgs/Float32MultiArray.h>
 #include <array> // Inclure la bibliothèque std::array pour déclarer des tableaux statiques
 #include <cmath> // Inclure la bibliothèque cmath pour atan2
 
@@ -12,62 +14,66 @@ float _minJointCmd = 0;
 float _maxJointCmd = 1023;
 float _minJointAngle = -180.0f;
 float _maxJointAngle = 180.0f;
-float L1 = 25;
-float L2 = 25;
-// create vector unit 16_t
-std::vector<uint16_t> jointVec;
+float L1 = 65;
+float L2 = 55;
+float angleBase = 100;
+ros::Publisher _jointPositionPublisher;
 
 
-std::array<float, 2> get_q2(float x, float y)
+
+
+float get_q2(float x, float y)
 {
-	std::array<float, 2> q2; // Déclarer un tableau statique de 2 éléments de type float
-	q2[0] = (x * x + y * y - L1 * L1 - L2 * L2) / (2 * L1 * L2);
-	q2[1] = -(x * x + y * y - L1 * L1 - L2 * L2) / (2 * L1 * L2);
+	float q2; // Déclarer un tableau statique de 2 éléments de type float
+	q2 = acos((x * x + y * y - L1 * L1 - L2 * L2) / (2 * L1 * L2));
 
 	return q2;
 }
 
 
-std::array<float, 2> get_q1(float x, float y)
+float get_q1(float x, float y)
 {
-	std::array<float, 2> q1;
-	std::array<float, 2> q2 = get_q2(x, y);
+	float q1;
+	float q2 = get_q2(x, y);
 
-	q1[0] = atan2(y, x) - atan2(L2 * sin(q2[0]), L1 + L2 * cos(q2[0]));
-	q1[1] = atan2(y, x) - atan2(L2 * sin(q2[1]), L1 + L2 * cos(q2[1]));
+	q1 = atan2(y, x) - atan2(L2 * sin(q2), L1 + L2 * cos(q2));
 
 	return q1;
 }
 
-
-
-void jointCMDCallback(const std_msgs/Float32MultiArray& pos_cmd)
+void posCMDCallback(const geometry_msgs::Twist& joint_pos)
 {
-	jointVec.clear();
-	jointVec.push_back(convertPosToJointCmd(joint_cmd.linear.y));
+	geometry_msgs::Twist joint_cmd;
+	float q1 = get_q1(joint_pos.linear.x, joint_pos.linear.y);
+	float q2 = get_q2(joint_pos.linear.x, joint_pos.linear.y);
+	// stores them into a msg
+	joint_cmd.linear.x = q1*(180.0f/3.141592);
+	joint_cmd.linear.y = angleBase;
+	joint_cmd.linear.z = q2*(180.0f/3.141592);
+	joint_cmd.angular.x = (-q1-q2)*(180.0f/3.141592);
+	
+
+	// publish the Twist message to the joint_position topic
+	_jointPositionPublisher.publish(joint_cmd);
 }
 
 int main(int argc, char** argv)
 {
+	//ros::init(argc, argv, "autopilot");
 	// create a node called poppy_ros
 	ros::init(argc, argv, "poppy_ros_pos");
-	//ros::init(argc, argv, "autopilot");
-	
+
 	// create a node handle
 	ros::NodeHandle nh;
-	
+
 	// create a publisher to joint_position topic
-	_jointPositionPublisher = nh.advertise<geometry_msgs::Twist>("joint_position", 1);
-	
+	_jointPositionPublisher = nh.advertise<geometry_msgs::Twist>("joint_cmd", 1);
 	//create a subscriber
 	ros::Subscriber sub = nh.subscribe("position_cmd",1, posCMDCallback);
 	
 	// create a loop rate
 	ros::Rate loopRate(_fps);
 	
-	// create a Twist message
-	geometry_msgs::Twist jointPositionMsg;
-	
 	
 	
 	
@@ -76,21 +82,6 @@ int main(int argc, char** argv)
 	// loop until Ctrl+C is pressed or ROS connectivity issues
 	while(ros::ok())
 	{
-		//===RETRIEVE Dynamixel Motor positions====
-		std::vector<uint16_t> l_vCurrentJointPosition;
-		bool bIsReadSuccessfull = _oDxlHandler.readCurrentJointPosition(l_vCurrentJointPosition);
-
-		// stores them into a msg
-		if (bIsReadSuccessfull)
-		{
-			jointPositionMsg.linear.x = l_vCurrentJointPosition[0];
-			jointPositionMsg.linear.y = l_vCurrentJointPosition[1];
-			jointPositionMsg.linear.z = l_vCurrentJointPosition[2];
-			jointPositionMsg.angular.x = l_vCurrentJointPosition[3];
-		}
-	
-		// publish the Twist message to the joint_position topic
-		_jointPositionPublisher.publish(jointPositionMsg);
 		
 		// spin once to let the process handle callback ad key stroke
 		ros::spinOnce();