Merge branch 'main' of...

Merge branch 'main' of https://gitlab.surrey.ac.uk/ms02264/manipulator-control-with-computer-vision into main

pose_estimation/CMakeLists.txt

@@ -8,6 +8,8 @@ find_package(catkin REQUIRED COMPONENTS
   open_manipulator_msgs
   geometry_msgs
   message_generation
+  open_manipulator_libs
+  robotis_manipulator
 )
 
 ################################################
@@ -20,19 +22,29 @@ add_message_files(
   # Keypoint.msg
   # Keypoints.msg
   SphericalCoordinates.msg
+  JointAngle.msg
+  JointPositions.msg
 )
 
 ## Generate added messages and services with any dependencies listed here
 generate_messages(
   DEPENDENCIES
   std_msgs
+  open_manipulator_msgs
 )
 
 ###################################
 ## catkin specific configuration ##
 ###################################
 catkin_package(
-  CATKIN_DEPENDS message_runtime
+  INCLUDE_DIRS include
+  CATKIN_DEPENDS
+    message_runtime
+    roscpp
+    std_msgs
+    open_manipulator_msgs
+    robotis_manipulator
+    open_manipulator_libs
 )
 
 ###########
@@ -42,6 +54,7 @@ catkin_package(
 ## Specify additional locations of header files
 ## Your package locations should be listed before other locations
 include_directories(
+  include
   ${catkin_INCLUDE_DIRS}
 )
 
@@ -53,5 +66,10 @@ include_directories(
 ## in contrast to setup.py, you can choose the destination
 catkin_install_python(PROGRAMS
   scripts/pose_estimation_control.py scripts/arm_keypoint_capture.py
+  scripts/data_collection.py scripts/training_data.py
   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
\ No newline at end of file
+)
+
+
+add_executable(inverse_kinematics_solver src/inverse_kinematics_solver.cpp)
+target_link_libraries(inverse_kinematics_solver ${catkin_LIBRARIES})

pose_estimation/include/pose_estimation/inverse_kinematics_solver.h

+#pragma once
+
+#include "open_manipulator_libs/kinematics.h"
+#include "open_manipulator_libs/open_manipulator.h"
+#include "open_manipulator_msgs/GetKinematicsPose.h"
+#include "open_manipulator_msgs/SetKinematicsPose.h"
+#include "pose_estimation/JointPositions.h"
+#include <robotis_manipulator/robotis_manipulator.h>
+#include "ros/ros.h"
+   
+class InverseKinematicsSolver
+{
+ public:
+   InverseKinematicsSolver(bool using_platform, std::string usb_port, std::string baud_rate, double control_period);
+   
+   void keypointsCallback(const open_manipulator_msgs::KinematicsPose& msg);
+   
+   ros::NodeHandle getNodeHandle() const { return n_; } 
+
+ private:
+   OpenManipulator open_manipulator_;
+   std::vector<JointValue>* goal_joint_value_;
+   
+   ros::NodeHandle n_;
+   ros::Publisher ik_pub_;
+   robotis_manipulator::Kinematics *kinematics_;
+
+   void solveIK(Pose target_pose, const open_manipulator_msgs::KinematicsPose& manipulator_pose);
+};
\ No newline at end of file

pose_estimation/msg/SphericalCoordinates.msg

 float64 distance
 float64 theta
-float64 phi
-float64 wrist_angle
\ No newline at end of file
+float64 phi
\ No newline at end of file

pose_estimation/package.xml

@@ -66,6 +66,12 @@
   <!-- Message Generation -->
   <build_depend>message_generation</build_depend>
   <exec_depend>message_runtime</exec_depend>
+
+  <build_depend>robotis_manipulator</build_depend>
+  <exec_depend>robotis_manipulator</exec_depend>
+  <build_depend>open_manipulator_libs</build_depend>
+  <exec_depend>open_manipulator_libs</exec_depend>
+  
   
   <!-- The export tag contains other, unspecified, tags -->
   <export>

pose_estimation/scripts/data_collection.py

@@ -29,8 +29,7 @@ else:
 
 MODEL = rospy.get_param('/keypoint_collection/gesture_model')
 model_file = os.path.expanduser(f"~/catkin_ws/src/pose_estimation/data/{MODEL}")
-
-
+    
 # Dictionary of different modes/stages of data collection
 COLLECTION_MODES = {
     "Setup": 0,
@@ -221,7 +220,7 @@ def skeleton_estimation_pose(img, pose):
         rospy.logerr(f"Exception: {e}")
 
     # Displaying the video feed and the landmarks
-    # show_image(img, pose_landmarks)
+    show_image(img, pose_landmarks)
     
     return results
 
@@ -449,10 +448,7 @@ def verify_params():
 
 if __name__ == '__main__':
     rospy.init_node("keypoint_collection")
-
-    print(CONTROL)
-    print(type(CONTROL))
-
+    
     if verify_params():
         rospy.loginfo("All the parameters were verified")
 
@@ -463,4 +459,4 @@ if __name__ == '__main__':
     else:
         rospy.logfatal(f"Check if all the provided model names are correct or if they are in the correct directories")
     
-    rospy.loginfo("Finished")
\ No newline at end of file
+    rospy.loginfo("Finished")

pose_estimation/src/inverse_kinematics_solver.cpp

+
+#include "pose_estimation/inverse_kinematics_solver.h"
+
+InverseKinematicsSolver::InverseKinematicsSolver(bool using_platform, std::string usb_port, std::string baud_rate, double control_period)
+{
+    log::info("Setting up the IK Solver for Open Manipulator");
+
+    open_manipulator_.initOpenManipulator(using_platform, usb_port, baud_rate, control_period);
+    goal_joint_value_ = new std::vector<JointValue>();
+
+    kinematics_ = new kinematics::SolverUsingCRAndSRPositionOnlyJacobian();
+    open_manipulator_.addKinematics(kinematics_);
+    log::info("Kinematics Solver Set 'SolverUsingCRandSRPoisionOnlyJacobian'");
+
+    ik_pub_ = n_.advertise<pose_estimation::JointPositions>("inverse_kinematics_keypoints", 1000);
+
+    log::info("Completed setting up the IK Solver");
+}
+
+void InverseKinematicsSolver::solveIK(Pose target_pose, const open_manipulator_msgs::KinematicsPose& manipulator_pose) {
+    pose_estimation::JointPositions msg;
+    
+    // TODO: Debug Mode
+    // std::cout << target_pose.kinematic.position << std::endl;
+    bool solved = open_manipulator_.solveInverseKinematics("gripper", target_pose, goal_joint_value_);
+    
+
+    // ? IF FAILED, CHECK THE CLOSEST POSITION
+
+
+    int idx = 0;
+    auto names = open_manipulator_.getManipulator()->getAllActiveJointComponentName();	
+    for(auto &point : *goal_joint_value_) {
+        pose_estimation::JointAngle joint;
+        joint.angle = point.position;
+        joint.name = names.at(idx);
+        msg.jointPositions.push_back(joint);
+
+        idx++;
+    }
+
+    msg.success = solved;
+
+    msg.manipulatorPose = manipulator_pose;
+    ik_pub_.publish(msg);
+    log::info("Published the point");
+
+}
+
+void InverseKinematicsSolver::keypointsCallback(const open_manipulator_msgs::KinematicsPose& msg) {
+    Eigen::Vector3d position;
+    position[0] = msg.pose.position.x;
+    position[1] = msg.pose.position.y;
+    position[2] = msg.pose.position.z;
+
+    Pose target_pose = { position };
+    solveIK(target_pose, msg);
+}
+
+
+int main(int argc, char **argv)
+{
+    ros::init(argc, argv, "inverse_kinematics_solver");
+    
+    InverseKinematicsSolver ik_solver(false, "/dev/ttyUSB0", "1000000", 0.010);
+    
+    auto n = ik_solver.getNodeHandle();
+    ros::Subscriber sub = n.subscribe("captured_keypoints", 100, &InverseKinematicsSolver::keypointsCallback, &ik_solver);
+    
+    ros::spin();
+    return 0;
+}
\ No newline at end of file