Merge branch 'develop' into main

main.cpp

@@ -27,6 +27,21 @@ l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f));
 _oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition);
 }
 
+void gripperControl (float newPos)
+{
+    std::vector<uint16_t> l_vTargetJointPosition;
+    l_vTargetJointPosition.clear();
+    for (int l_joint = 0; l_joint < _nbJoints; l_joint++)
+    {
+        if (l_joint==5){
+            l_vTargetJointPosition.push_back(convertAnglesToJointCmd(newPos));
+        }
+        else {
+            l_vTargetJointPosition.push_back(convertAnglesToJointCmd(0.0f));
+        }
+    }
+    _oDxlHandler.sendTargetJointPosition(l_vTargetJointPosition);
+}
 int main()
 { std::cout << "===Initialization of the Dynamixel Motor communication====" << std::endl;
 _oDxlHandler.setDeviceName(_poppyDxlPortName);
@@ -43,6 +58,47 @@ std::cout << "vCurrentJointPosition= (" << std::endl;
 for (int l_joint=0; l_joint < l_vCurrentJointPosition.size(); l_joint++)
 std::cout << l_vCurrentJointPosition[l_joint] << ", ";
 std::cout << ")" << std::endl;
+goToHomePosition();
+// wait 1s
+std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+// read current joint position
+l_vCurrentJointPosition.clear();
+_oDxlHandler.readCurrentJointPosition(l_vCurrentJointPosition);
+// display current joint position
+std::cout << "vCurrentJointPosition= (" << std::endl;
+for (int l_joint=0; l_joint < l_vCurrentJointPosition.size(); l_joint++)
+std::cout << l_vCurrentJointPosition[l_joint] << ", ";
+std::cout << ")" << std::endl;
+//#####################################################################
+// wait 1s
+std::this_thread::sleep_for(std::chrono::milliseconds(1000));
+std::vector<uint16_t> l_vCurrentJointTorque;
+// display current joint torque
+int targetTorque = 1080;
+float angularDelta;
+int newPos = 0;
+float epsilon;
+float p = 0.01f;
+for (int samples=0; samples<50;samples++){
+l_vCurrentJointTorque.clear();
+_oDxlHandler.readCurrentJointTorque(l_vCurrentJointTorque);
+std::cout << "vCurrentJointTorque= (" << std::endl;
+for (int l_joint=0; l_joint < l_vCurrentJointTorque.size(); l_joint++)
+std::cout << l_vCurrentJointTorque[l_joint] << ", ";
+std::cout << ")" << std::endl;
+
+epsilon = targetTorque - l_vCurrentJointTorque[5];
+angularDelta = epsilon*p;
+newPos += angularDelta;
+std::cout << newPos << "newPos" << epsilon << " epsilon" << angularDelta << " delate" << std::endl;
+gripperControl(newPos);
+
+// wait 1s
+std::this_thread::sleep_for(std::chrono::milliseconds(50));
+
+
+}
+//#####################################################################
 // wait 1s
 std::this_thread::sleep_for(std::chrono::milliseconds(1000));
 std::cout << "===Closing the Dynamixel Motor communication====" << std::endl;
@@ -50,21 +106,4 @@ _oDxlHandler.enableTorque(false);
 _oDxlHandler.closePort();
 std::cout << std::endl;
 return 0;
-goToHomePosition();
-// wait 1s
-std::this_thread::sleep_for(std::chrono::milliseconds(1000));
-std::cout << "===Closing the Dynamixel Motor communication====" << std::endl;
-_oDxlHandler.enableTorque(false);
-_oDxlHandler.closePort();
-std::cout << std::endl;
-// read current joint position
-std::vector<uint16_t> l_vCurrentJointPosition;
-_oDxlHandler.readCurrentJointPosition(l_vCurrentJointPosition);
-// display current joint position
-std::cout << "vCurrentJointPosition= (" << std::endl;
-for (int l_joint=0; l_joint < l_vCurrentJointPosition.size(); l_joint++)
-std::cout << l_vCurrentJointPosition[l_joint] << ", ";
-std::cout << ")" << std::endl;
-
-
 }

makefile

@@ -1,12 +1,11 @@
 all : dynamixel main
-	g++ DynamixelHandler.o main.o –L/usr/local/lib -ldxl_x64_cpp -lrt
+	g++ DynamixelHandler.o main.o -L/usr/local/lib -ldxl_x64_cpp -lrt
 dynamixel : DynamixelHandler.cpp
 	g++ -c -I/home/ros/SOFTWARE/toolkit-dynamixel/include /home/ros/SOFTWARE/toolkit-dynamixel/src/DynamixelHandler.cpp
 main : main.cpp
-	g++ -c -I/home/ros/SOFTWARE/toolkit-dynamixel/include main.cpp
+	g++ -c main.cpp -I/home/ros/SOFTWARE/toolkit-dynamixel/
 
 clean :
 	rm * .o
 	rm * .out
 	rm * .exe
-