diff --git a/pose_estimation/CMakeLists.txt b/pose_estimation/CMakeLists.txt
index 9020edfb32943dc4a908c7ebb7fbd1c095513f7b..8a75491b941b34c959fac7ba1f0f6213074816ff 100644
--- a/pose_estimation/CMakeLists.txt
+++ b/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})
diff --git a/pose_estimation/include/pose_estimation/inverse_kinematics_solver.h b/pose_estimation/include/pose_estimation/inverse_kinematics_solver.h
new file mode 100644
index 0000000000000000000000000000000000000000..ee92208e165a50c2ea2baa7e0206f5f30fdf41e8
--- /dev/null
+++ b/pose_estimation/include/pose_estimation/inverse_kinematics_solver.h
@@ -0,0 +1,29 @@
+#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
diff --git a/pose_estimation/msg/SphericalCoordinates.msg b/pose_estimation/msg/SphericalCoordinates.msg
index 1bf8d76b7d15c82b5763ed1d1283b1878d411a93..7e14a368a901bfbc4913ec75a16b0e1e2ac7e86e 100644
--- a/pose_estimation/msg/SphericalCoordinates.msg
+++ b/pose_estimation/msg/SphericalCoordinates.msg
@@ -1,4 +1,3 @@
float64 distance
float64 theta
-float64 phi
-float64 wrist_angle
\ No newline at end of file
+float64 phi
\ No newline at end of file
diff --git a/pose_estimation/package.xml b/pose_estimation/package.xml
index c99581201b0d6eb4873cf45fd609f36fe1ffcead..95a8c873710f1dfaa543326e73c7093f8223e374 100644
--- a/pose_estimation/package.xml
+++ b/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>
diff --git a/pose_estimation/scripts/data_collection.py b/pose_estimation/scripts/data_collection.py
index 92cc47ad5c025663e63c9cb69a71eee3d72ee622..346a72c754eaab2d98b784955119d3a06f6eefae 100755
--- a/pose_estimation/scripts/data_collection.py
+++ b/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")
diff --git a/pose_estimation/src/inverse_kinematics_solver.cpp b/pose_estimation/src/inverse_kinematics_solver.cpp
new file mode 100755
index 0000000000000000000000000000000000000000..6e266aa06ba10cd3b0e6b4eb4bc65951e3ad1909
--- /dev/null
+++ b/pose_estimation/src/inverse_kinematics_solver.cpp
@@ -0,0 +1,72 @@
+
+#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