Skip to content
Snippets Groups Projects
Commit 2f807246 authored by Sitarz, Michal A (UG - Comp Sci & Elec Eng)'s avatar Sitarz, Michal A (UG - Comp Sci & Elec Eng)
Browse files

Merge branch 'main' of...

parents 46b6aea6 9c895a35
No related branches found
No related tags found
No related merge requests found
...@@ -8,6 +8,8 @@ find_package(catkin REQUIRED COMPONENTS ...@@ -8,6 +8,8 @@ find_package(catkin REQUIRED COMPONENTS
open_manipulator_msgs open_manipulator_msgs
geometry_msgs geometry_msgs
message_generation message_generation
open_manipulator_libs
robotis_manipulator
) )
################################################ ################################################
...@@ -20,19 +22,29 @@ add_message_files( ...@@ -20,19 +22,29 @@ add_message_files(
# Keypoint.msg # Keypoint.msg
# Keypoints.msg # Keypoints.msg
SphericalCoordinates.msg SphericalCoordinates.msg
JointAngle.msg
JointPositions.msg
) )
## Generate added messages and services with any dependencies listed here ## Generate added messages and services with any dependencies listed here
generate_messages( generate_messages(
DEPENDENCIES DEPENDENCIES
std_msgs std_msgs
open_manipulator_msgs
) )
################################### ###################################
## catkin specific configuration ## ## catkin specific configuration ##
################################### ###################################
catkin_package( 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( ...@@ -42,6 +54,7 @@ catkin_package(
## Specify additional locations of header files ## Specify additional locations of header files
## Your package locations should be listed before other locations ## Your package locations should be listed before other locations
include_directories( include_directories(
include
${catkin_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS}
) )
...@@ -53,5 +66,10 @@ include_directories( ...@@ -53,5 +66,10 @@ include_directories(
## in contrast to setup.py, you can choose the destination ## in contrast to setup.py, you can choose the destination
catkin_install_python(PROGRAMS catkin_install_python(PROGRAMS
scripts/pose_estimation_control.py scripts/arm_keypoint_capture.py scripts/pose_estimation_control.py scripts/arm_keypoint_capture.py
scripts/data_collection.py scripts/training_data.py
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 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})
#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
float64 distance float64 distance
float64 theta float64 theta
float64 phi float64 phi
float64 wrist_angle \ No newline at end of file
\ No newline at end of file
...@@ -66,6 +66,12 @@ ...@@ -66,6 +66,12 @@
<!-- Message Generation --> <!-- Message Generation -->
<build_depend>message_generation</build_depend> <build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_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 --> <!-- The export tag contains other, unspecified, tags -->
<export> <export>
......
...@@ -29,8 +29,7 @@ else: ...@@ -29,8 +29,7 @@ else:
MODEL = rospy.get_param('/keypoint_collection/gesture_model') MODEL = rospy.get_param('/keypoint_collection/gesture_model')
model_file = os.path.expanduser(f"~/catkin_ws/src/pose_estimation/data/{MODEL}") model_file = os.path.expanduser(f"~/catkin_ws/src/pose_estimation/data/{MODEL}")
# Dictionary of different modes/stages of data collection # Dictionary of different modes/stages of data collection
COLLECTION_MODES = { COLLECTION_MODES = {
"Setup": 0, "Setup": 0,
...@@ -221,7 +220,7 @@ def skeleton_estimation_pose(img, pose): ...@@ -221,7 +220,7 @@ def skeleton_estimation_pose(img, pose):
rospy.logerr(f"Exception: {e}") rospy.logerr(f"Exception: {e}")
# Displaying the video feed and the landmarks # Displaying the video feed and the landmarks
# show_image(img, pose_landmarks) show_image(img, pose_landmarks)
return results return results
...@@ -449,10 +448,7 @@ def verify_params(): ...@@ -449,10 +448,7 @@ def verify_params():
if __name__ == '__main__': if __name__ == '__main__':
rospy.init_node("keypoint_collection") rospy.init_node("keypoint_collection")
print(CONTROL)
print(type(CONTROL))
if verify_params(): if verify_params():
rospy.loginfo("All the parameters were verified") rospy.loginfo("All the parameters were verified")
...@@ -463,4 +459,4 @@ if __name__ == '__main__': ...@@ -463,4 +459,4 @@ if __name__ == '__main__':
else: else:
rospy.logfatal(f"Check if all the provided model names are correct or if they are in the correct directories") rospy.logfatal(f"Check if all the provided model names are correct or if they are in the correct directories")
rospy.loginfo("Finished") rospy.loginfo("Finished")
\ No newline at end of file
#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
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment