diff --git a/src/rrrobot_ws/src/rrrobot/CMakeLists.txt b/src/rrrobot_ws/src/rrrobot/CMakeLists.txt index 90d48c8..f4f4440 100644 --- a/src/rrrobot_ws/src/rrrobot/CMakeLists.txt +++ b/src/rrrobot_ws/src/rrrobot/CMakeLists.txt @@ -73,10 +73,14 @@ add_executable(rrrobot_node src/rrrobot_node.cpp) add_dependencies(rrrobot_node ${catkin_EXPORTED_TARGETS} rrrobot_generate_messages_cpp) target_link_libraries(rrrobot_node ${catkin_LIBRARIES}) -add_executable(arm_controller_node src/arm_controller_node.cpp) +add_executable(arm_controller_node src/arm_controller_node.cpp src/arm_representation.cpp) add_dependencies(arm_controller_node ${catkin_EXPORTED_TARGETS}) target_link_libraries(arm_controller_node ${catkin_LIBRARIES} ${orocos_kdl_LIBRARIES}) +add_executable(test_arm test/test_arm.cpp src/arm_representation.cpp) +add_dependencies(test_arm ${catkin_EXPORTED_TARGETS}) +target_link_libraries(test_arm ${catkin_LIBRARIES} ${orocos_kdl_LIBRARIES}) + add_executable(depth_camera_node src/depth_camera_node.cpp) add_dependencies(depth_camera_node ${catkin_EXPORTED_TARGETS}) target_link_libraries(depth_camera_node ${catkin_LIBRARIES} ${PCL_LIBRARIES}) diff --git a/src/rrrobot_ws/src/rrrobot/include/arm_representation.h b/src/rrrobot_ws/src/rrrobot/include/arm_representation.h new file mode 100644 index 0000000..d58ab73 --- /dev/null +++ b/src/rrrobot_ws/src/rrrobot/include/arm_representation.h @@ -0,0 +1,59 @@ +// arm_controller_node.cpp + +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include +#include + +#include "topic_names.h" +#include "rrrobot/arm_command.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +// using namespace std; +using std::string; + +class ArmRepresentation +{ +public: + ArmRepresentation(const KDL::Frame &base_pose = KDL::Frame(KDL::Vector(0.3, 0.0, 0.9))); //KDL::Frame(KDL::Vector(0.3, 0.92, 1))); + + int calculateForwardKinematics(const KDL::JntArray &joint_positions, KDL::Frame &end_effector_pose); + + int calculateInverseKinematics(const KDL::JntArray &cur_configuration, + const KDL::Frame &desired_end_effector_pose, + KDL::JntArray &final_joint_configuration); + + std::vector get_joint_names(); + + KDL::Chain *getChain(); + +private: + KDL::Chain chain; + + // KDL::ChainFkSolverPos_recursive fk_solver; + // KDL::ChainIkSolverPos_LMA ik_solver; +}; \ No newline at end of file diff --git a/src/rrrobot_ws/src/rrrobot/scripts/arm_pub_test.sh b/src/rrrobot_ws/src/rrrobot/scripts/arm_pub_test.sh old mode 100644 new mode 100755 index 2353aa5..4447663 --- a/src/rrrobot_ws/src/rrrobot/scripts/arm_pub_test.sh +++ b/src/rrrobot_ws/src/rrrobot/scripts/arm_pub_test.sh @@ -1,3 +1,24 @@ #!/bin/bash -rostopic pub /target_pose geometry_msgs/Pose '{position: {x: 0.875, y: 0.75, z: 1.5}, orientation: {x: 0, y: 0, z: 0, w: 0}}' +#rostopic pub /target_pose geometry_msgs/Pose '{position: {x: 0.875, y: 0.75, z: 1.5}, orientation: {x: 0, y: 0, z: 0, w: 0}}' +rostopic pub /arm_controller/destination rrrobot/arm_command "grab_location: + position: + x: 1.2 + y: 0.2 + z: 1.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 0.0 +drop_location: + position: + x: -0.3 + y: 1.15 + z: 1.5 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 0.0" \ +-1 diff --git a/src/rrrobot_ws/src/rrrobot/scripts/arm_pub_test_continuous.sh b/src/rrrobot_ws/src/rrrobot/scripts/arm_pub_test_continuous.sh new file mode 100755 index 0000000..6ce4da3 --- /dev/null +++ b/src/rrrobot_ws/src/rrrobot/scripts/arm_pub_test_continuous.sh @@ -0,0 +1,23 @@ +#!/bin/bash + +#rostopic pub /target_pose geometry_msgs/Pose '{position: {x: 0.875, y: 0.75, z: 1.5}, orientation: {x: 0, y: 0, z: 0, w: 0}}' +rostopic pub /arm_controller/destination rrrobot/arm_command "grab_location: + position: + x: 1.2 + y: 0.2 + z: 1.0 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 0.0 +drop_location: + position: + x: -0.3 + y: 1.15 + z: 1.5 + orientation: + x: 0.0 + y: 0.0 + z: 0.0 + w: 0.0" diff --git a/src/rrrobot_ws/src/rrrobot/src/arm_controller_node.cpp b/src/rrrobot_ws/src/rrrobot/src/arm_controller_node.cpp index 3d26a38..c495bf4 100644 --- a/src/rrrobot_ws/src/rrrobot/src/arm_controller_node.cpp +++ b/src/rrrobot_ws/src/rrrobot/src/arm_controller_node.cpp @@ -18,6 +18,7 @@ #include "topic_names.h" #include "rrrobot/arm_command.h" +#include "arm_representation.h" #include #include @@ -31,191 +32,227 @@ #include #include +#include + #include using namespace std; -class ArmRepresentation +// class ArmRepresentation +// { +// public: +// ArmRepresentation(const KDL::Frame &base_pose = KDL::Frame(KDL::Vector(0.3, 0.92, 1))) +// { +// const double base_len = 0.2; +// const double shoulder_height = 0.1273; +// const double upper_arm_length = 0.612; +// const double forearm_length = 0.5723; +// const double shoulder_offset = 0.220941; +// const double elbow_offset = -0.1719; +// const double wrist_1_length = 0.163941 - elbow_offset - shoulder_offset; +// const double wrist_2_length = 0.1157; +// const double wrist_3_length = 0.0922; + +// // KDL::Vector pos; //(0, 0, base_len/2); +// // KDL::Rotation rot; + +// // The joints might be off by one +// KDL::Segment linear_arm_actuator("linear_arm_actuator_joint", +// KDL::Joint(KDL::Joint::JointType::None), base_pose); +// arm.addSegment(linear_arm_actuator); + +// const KDL::Vector pos_base(0, 0, base_len / 2); +// const KDL::Rotation rot_base(KDL::Rotation::RPY(0, 0, 0)); +// KDL::Segment base_link("base_link", KDL::Joint("linear_arm_actuator_joint", KDL::Joint::JointType::TransY), +// KDL::Frame(rot_base, pos_base)); +// arm.addSegment(base_link); + +// const KDL::Vector pos_shoulder(0, 0, shoulder_height); +// const KDL::Rotation rot_shoulder(KDL::Rotation::RPY(0, 0, 0)); +// KDL::Segment shoulder_link("shoulder_link", KDL::Joint("shoulder_pan_joint", KDL::Joint::JointType::RotZ), +// KDL::Frame(rot_shoulder, pos_shoulder)); +// arm.addSegment(shoulder_link); + +// const KDL::Vector pos_upper_arm(0, shoulder_offset, 0); +// const KDL::Rotation rot_upper_arm(KDL::Rotation::RPY(0.0, M_PI / 2.0, 0.0)); +// KDL::Segment upper_arm_link("upper_arm_link", KDL::Joint("shoulder_lift_joint", KDL::Joint::JointType::RotY), +// KDL::Frame(rot_upper_arm, pos_upper_arm)); +// arm.addSegment(upper_arm_link); + +// const KDL::Vector pos_forearm(0, elbow_offset, upper_arm_length); +// const KDL::Rotation rot_forearm(KDL::Rotation::RPY(0.0, 0.0, 0.0)); +// KDL::Segment forearm_link("forearm_link", KDL::Joint("elbow_joint", KDL::Joint::JointType::RotY), +// KDL::Frame(rot_forearm, pos_forearm)); +// arm.addSegment(forearm_link); + +// const KDL::Vector pos_wrist_1(0, 0, forearm_length); +// const KDL::Rotation rot_wrist_1(KDL::Rotation::RPY(0.0, M_PI / 2.0, 0.0)); +// KDL::Segment wrist_1_link("wrist_1_link", KDL::Joint("wrist_1_joint", KDL::Joint::JointType::RotY), +// KDL::Frame(rot_wrist_1, pos_wrist_1)); +// arm.addSegment(wrist_1_link); + +// const KDL::Vector pos_wrist_2(0, wrist_1_length, 0); +// const KDL::Rotation rot_wrist_2(KDL::Rotation::RPY(0.0, 0.0, 0.0)); +// KDL::Segment wrist_2_link("wrist_2_link", KDL::Joint("wrist_2_joint", KDL::Joint::JointType::RotZ), +// KDL::Frame(rot_wrist_2, pos_wrist_2)); +// arm.addSegment(wrist_2_link); + +// const KDL::Vector pos_wrist_3(0, 0, wrist_2_length); +// const KDL::Rotation rot_wrist_3(KDL::Rotation::RPY(0.0, 0.0, 0.0)); +// KDL::Segment wrist_3_link("wrist_3_link", KDL::Joint("wrist_3_joint", KDL::Joint::JointType::RotY), +// KDL::Frame(rot_wrist_3, pos_wrist_3)); +// arm.addSegment(wrist_3_link); + +// const KDL::Vector pos_ee(0, wrist_3_length, 0.0); +// const KDL::Rotation rot_ee(KDL::Rotation::RPY(0.0, 0.0, M_PI / 2.0)); +// KDL::Segment ee_link("ee_link", KDL::Joint(KDL::Joint::JointType::None), +// KDL::Frame(rot_ee, pos_ee)); +// arm.addSegment(ee_link); + +// // arm.addSegment(base_link); +// // arm.addSegment(shoulder_link); +// // arm.addSegment(upper_arm_link); +// // arm.addSegment(forearm_link); +// // arm.addSegment(wrist_1_link); +// // arm.addSegment(wrist_2_link); +// // arm.addSegment(wrist_3_link); +// // arm.addSegment(ee_link); + +// ROS_INFO("Arm has %d joints", arm->getNrOfJoints()); +// fk_solver.reset(new KDL::ChainFkSolverPos_recursive(arm)); +// ik_solver.reset(new KDL::ChainIkSolverPos_LMA(arm)); +// } + +// int calculateForwardKinematics(const KDL::JntArray &joint_positions, KDL::Frame &end_effector_pose) +// { +// return fk_solver->JntToCart(joint_positions, end_effector_pose); +// } + +// int calculateInverseKinematics(const KDL::JntArray &cur_configuration, +// const KDL::Frame &desired_end_effector_pose, +// KDL::JntArray &final_joint_configuration) +// { +// return ik_solver->CartToJnt(cur_configuration, desired_end_effector_pose, final_joint_configuration); +// } + +// vector get_joint_names() +// { +// vector joints; + +// for (int segment = 0; segment < arm->getNrOfSegments(); ++segment) +// { +// KDL::Joint cur = arm.getSegment(segment).getJoint(); + +// if (cur.getType() != KDL::Joint::JointType::None) +// { +// joints.push_back(cur.getName()); +// } +// } + +// return joints; +// } + +// const KDL::Chain &getChain() const +// { +// return arm; +// } + +// private: +// KDL::Chain arm; + +// std::unique_ptr fk_solver; +// std::unique_ptr ik_solver; +// }; + +class ArmController { public: - ArmRepresentation(const KDL::Frame &base_pose = KDL::Frame(KDL::Vector(0.3, 0.92, 1))) + ArmController(ros::NodeHandle &node, ArmRepresentation *arm_) : gripper_enabled_(false), item_attached_(false), arm(arm_) { - const double base_len = 0.2; - const double shoulder_height = 0.1273; - const double upper_arm_length = 0.612; - const double forearm_length = 0.5723; - const double shoulder_offset = 0.220941; - const double elbow_offset = -0.1719; - const double wrist_1_length = 0.163941 - elbow_offset - shoulder_offset; - const double wrist_2_length = 0.1157; - const double wrist_3_length = 0.0922; - - // KDL::Vector pos; //(0, 0, base_len/2); - // KDL::Rotation rot; - - // The joints might be off by one - KDL::Segment linear_arm_actuator("linear_arm_actuator_joint", - KDL::Joint(KDL::Joint::JointType::None), base_pose); - arm.addSegment(linear_arm_actuator); - - const KDL::Vector pos_base(0, 0, base_len / 2); - const KDL::Rotation rot_base(KDL::Rotation::RPY(0, 0, 0)); - KDL::Segment base_link("base_link", KDL::Joint(KDL::Joint::JointType::TransY), - KDL::Frame(rot_base, pos_base)); - arm.addSegment(base_link); - - const KDL::Vector pos_shoulder(0, 0, shoulder_height); - const KDL::Rotation rot_shoulder(KDL::Rotation::RPY(0, 0, 0)); - KDL::Segment shoulder_link("shoulder_link", KDL::Joint(KDL::Joint::JointType::RotZ), - KDL::Frame(rot_shoulder, pos_shoulder)); - arm.addSegment(shoulder_link); - - const KDL::Vector pos_upper_arm(0, shoulder_offset, 0); - const KDL::Rotation rot_upper_arm(KDL::Rotation::RPY(0.0, M_PI / 2.0, 0.0)); - KDL::Segment upper_arm_link("upper_arm_link", KDL::Joint(KDL::Joint::JointType::RotY), - KDL::Frame(rot_upper_arm, pos_upper_arm)); - arm.addSegment(upper_arm_link); - - const KDL::Vector pos_forearm(0, elbow_offset, upper_arm_length); - const KDL::Rotation rot_forearm(KDL::Rotation::RPY(0.0, 0.0, 0.0)); - KDL::Segment forearm_link("forearm_link", KDL::Joint(KDL::Joint::JointType::RotY), - KDL::Frame(rot_forearm, pos_forearm)); - arm.addSegment(forearm_link); - - const KDL::Vector pos_wrist_1(0, 0, forearm_length); - const KDL::Rotation rot_wrist_1(KDL::Rotation::RPY(0.0, M_PI / 2.0, 0.0)); - KDL::Segment wrist_1_link("wrist_1_link", KDL::Joint(KDL::Joint::JointType::RotY), - KDL::Frame(rot_wrist_1, pos_wrist_1)); - arm.addSegment(wrist_1_link); - - const KDL::Vector pos_wrist_2(0, wrist_1_length, 0); - const KDL::Rotation rot_wrist_2(KDL::Rotation::RPY(0.0, 0.0, 0.0)); - KDL::Segment wrist_2_link("wrist_2_link", KDL::Joint(KDL::Joint::JointType::RotZ), - KDL::Frame(rot_wrist_2, pos_wrist_2)); - arm.addSegment(wrist_2_link); - - const KDL::Vector pos_wrist_3(0, 0, wrist_2_length); - const KDL::Rotation rot_wrist_3(KDL::Rotation::RPY(0.0, 0.0, 0.0)); - KDL::Segment wrist_3_link("wrist_3_link", KDL::Joint(KDL::Joint::JointType::RotY), - KDL::Frame(rot_wrist_3, pos_wrist_3)); - arm.addSegment(wrist_3_link); - - const KDL::Vector pos_ee(0, wrist_3_length, 0.0); - const KDL::Rotation rot_ee(KDL::Rotation::RPY(0.0, 0.0, M_PI / 2.0)); - KDL::Segment ee_link("ee_link", KDL::Joint(KDL::Joint::JointType::None), - KDL::Frame(rot_ee, pos_ee)); - arm.addSegment(ee_link); - - // arm.addSegment(base_link); - // arm.addSegment(shoulder_link); - // arm.addSegment(upper_arm_link); - // arm.addSegment(forearm_link); - // arm.addSegment(wrist_1_link); - // arm.addSegment(wrist_2_link); - // arm.addSegment(wrist_3_link); - // arm.addSegment(ee_link); - - fk_solver.reset(new KDL::ChainFkSolverPos_recursive(arm)); - ik_solver.reset(new KDL::ChainIkSolverPos_LMA(arm)); - } - - bool calculateForwardKinematics(const KDL::JntArray &joint_positions, KDL::Frame &end_effector_pose) - { - return fk_solver->JntToCart(joint_positions, end_effector_pose); - } - - bool calculateInverseKinematics(const KDL::JntArray &cur_configuration, - const KDL::Frame &desired_end_effector_pose, - KDL::JntArray &final_joint_configuration) - { - return ik_solver->CartToJnt(cur_configuration, desired_end_effector_pose, final_joint_configuration); - } - - vector get_joint_names() { - vector joints; - - for (int segment = 0; segment < arm.getNrOfSegments(); ++segment) { - KDL::Joint cur = arm.getSegment(segment).getJoint(); - - if (cur.getType() != KDL::Joint::JointType::None) - { - joints.push_back(cur.getName()); - } - } - - return joints; - } - - const KDL::Chain &getArm() const - { - return arm; - } - -private: - KDL::Chain arm; - - std::unique_ptr fk_solver; - std::unique_ptr ik_solver; -}; - - -class ArmController { -public: - ArmController(ros::NodeHandle & node) : gripper_enabled_(false), item_attached_(false) { arm_joint_trajectory_publisher_ = node.advertise(ARM_COMMAND_CHANNEL, 10); gripper_ = node.serviceClient(GRIPPER_CONTROL_CHANNEL); - arm = ArmRepresentation(); + // arm = std::move(arm_); + ROS_INFO("Arm size (in constructor): %d", arm->getChain()->getNrOfJoints()); + arm_current_joint_states_ = KDL::JntArray(arm->getChain()->getNrOfJoints()); + KDL::SetToZero(arm_current_joint_states_); } - void joint_state_callback(const sensor_msgs::JointState & joint_state_msg) { - ROS_INFO_STREAM_THROTTLE(10, "Arm Joint States (throttled to 0.1 Hz):\n" << joint_state_msg); - - // ROS_INFO_STREAM("Joint States:\n" << joint_state_msg); - + void joint_state_callback(const sensor_msgs::JointState &joint_state_msg) + { + // ROS_INFO("Received joint state"); + // Convert std_msgs::double[] to KDL::JntArray - int nbr_joints = arm.getArm().getNrOfJoints(); + int nbr_joints = arm->getChain()->getNrOfJoints(); vector msg_joint_names = joint_state_msg.name; - vector cur_joint_names = arm.get_joint_names(); + vector cur_joint_names = arm->get_joint_names(); vector position = joint_state_msg.position; - for (int i = 0; i < nbr_joints; ++i) { - for (int j = 0; i < nbr_joints; ++j) { - if (msg_joint_names[i].compare(cur_joint_names[j]) == 0) { + // Print joint name vectors for debugging + // ROS_INFO_STREAM("msg_joint_names: {"); + // for (int i = 0; i < nbr_joints; ++i) + // { + // ROS_INFO_STREAM(msg_joint_names[i] << ", "); + // } + // ROS_INFO_STREAM("}\ncur_joint_names: {"); + // for (int j = 0; j < cur_joint_names.size(); ++j) + // { + // ROS_INFO_STREAM(cur_joint_names[j] << ", "); + // } + // ROS_INFO_STREAM("}"); + + for (int i = 0; i < nbr_joints; ++i) + { + arm_current_joint_states_(i) = 0.0; + } + + for (size_t i = 0; i < position.size(); ++i) + { + for (size_t j = 0; j < cur_joint_names.size(); ++j) + { + if (msg_joint_names[i].compare(cur_joint_names[j]) == 0) + { arm_current_joint_states_(j) = position[i]; } } } } - void gripper_state_callback(const osrf_gear::VacuumGripperState::ConstPtr & gripper_state_msg) { + void gripper_state_callback(const osrf_gear::VacuumGripperState::ConstPtr &gripper_state_msg) + { gripper_enabled_ = gripper_state_msg->enabled; item_attached_ = gripper_state_msg->attached; } - void arm_destination_callback(const rrrobot::arm_command & target_pose) { - int nbr_joints = arm.getArm().getNrOfJoints(); + void arm_destination_callback(const rrrobot::arm_command &target_pose) + { + ROS_INFO("Received target pose"); + + int nbr_joints = arm->getChain()->getNrOfJoints(); vector positions; double time_from_start = 5; // Seconds - + // Move arm to pickup item from conveyor belt positions = calc_joint_positions(target_pose.grab_location); send_joint_trajectory(positions, time_from_start); // Wait until target position is reached - while (!have_reached_target(frame_to_pose(calc_end_effector_pose()), target_pose.grab_location)) { + while (!have_reached_target(frame_to_pose(calc_end_effector_pose()), target_pose.grab_location)) + { // TODO: Do something if not able to reach target continue; } - + // Turn on suction - while (!gripper_enabled_) { + while (!gripper_enabled_) + { gripper_control(true); } // Wait until object is attached - while (!item_attached_) { + while (!item_attached_) + { // TODO: Do something if item is not able to attach continue; } @@ -223,20 +260,23 @@ public: // Move item to desired position positions = calc_joint_positions(target_pose.drop_location); send_joint_trajectory(positions, time_from_start); - + // Wait until target position is reached - while (!have_reached_target(frame_to_pose(calc_end_effector_pose()), target_pose.drop_location)) { + while (!have_reached_target(frame_to_pose(calc_end_effector_pose()), target_pose.drop_location)) + { // TODO: Do something if not able to reach target continue; } // Turn off suction - while (gripper_enabled_) { + while (gripper_enabled_) + { gripper_control(false); } // Wait until object is detached - while (item_attached_) { + while (item_attached_) + { // TODO: Do something if item doesn't detach continue; } @@ -248,39 +288,60 @@ private: ros::Publisher arm_joint_trajectory_publisher_; ros::ServiceClient gripper_; KDL::JntArray arm_current_joint_states_; - ArmRepresentation arm; + ArmRepresentation *arm; - vector calc_joint_positions(const geometry_msgs::Pose & pose) { - KDL::JntArray cur_configuration = arm_current_joint_states_; + vector calc_joint_positions(const geometry_msgs::Pose &pose) + { + // KDL::JntArray cur_configuration(arm->getChain()->getNrOfJoints()); // = arm_current_joint_states_; + // KDL::SetToZero(cur_configuration); KDL::Frame desired_end_effector_pose = pose_to_frame(pose); - KDL::JntArray final_joint_configuration = KDL::JntArray(); - + KDL::JntArray final_joint_configuration(arm->getChain()->getNrOfJoints()); + + // ROS_INFO("cur_configuration (%i x %i)", cur_configuration.rows(), cur_configuration.columns()); + + int error_code = arm->calculateInverseKinematics(arm_current_joint_states_, desired_end_effector_pose, final_joint_configuration); + // Check status of IK and print error message if there is a failure - if (!arm.calculateInverseKinematics(cur_configuration, desired_end_effector_pose, final_joint_configuration)) { - ROS_ERROR("Inverse Kinematics Failure"); + if (error_code != 0) + { + ROS_ERROR("Inverse Kinematics Failure: %i", error_code); } // Convert data attribute (Eigen::VectorXd) of KDL::JntArray to double[] via data() function - int nbr_joints = arm.getArm().getNrOfJoints(); + int nbr_joints = arm->getChain()->getNrOfJoints(); Eigen::VectorXd mat = final_joint_configuration.data; - vector positions(mat.data(), mat.data() + mat.rows() * mat.cols()); - + // cout << mat << endl; + vector positions; + for (size_t idx = 0; idx < arm->getChain()->getNrOfJoints(); ++idx) + { + positions.push_back(mat[idx]); + } + return positions; } - const KDL::Frame& calc_end_effector_pose() { + KDL::Frame calc_end_effector_pose() + { KDL::Frame end_effector_pose; - KDL::JntArray joint_positions = arm_current_joint_states_; - + // KDL::JntArray joint_positions = arm_current_joint_states_; + KDL::JntArray joint_positions(arm->getChain()->getNrOfJoints()); + KDL::SetToZero(joint_positions); + + // ROS_INFO("joint_positions (%i x %i)", joint_positions.rows(), joint_positions.columns()); + // ROS_INFO("arm chain # of joints: %d", arm->getChain()->getNrOfJoints()); + int error_code = arm->calculateForwardKinematics(joint_positions, end_effector_pose); + // Check status of FK and do something if there is a failure - if (!arm.calculateForwardKinematics(joint_positions, end_effector_pose)) { - ROS_ERROR("Forward Kinematics Failure"); + if (error_code != 0) + { + ROS_ERROR("Forward Kinematics Failure: %i", error_code); } return end_effector_pose; } - const KDL::Frame& pose_to_frame(const geometry_msgs::Pose & pose) { + KDL::Frame pose_to_frame(const geometry_msgs::Pose &pose) + { double p_x = pose.position.x; double p_y = pose.position.y; double p_z = pose.position.z; @@ -290,10 +351,13 @@ private: double q_z = pose.orientation.z; double q_w = pose.orientation.w; - return KDL::Frame(KDL::Rotation::Quaternion(q_x, q_y, q_z, q_w), KDL::Vector(p_x, p_y, p_z)); + KDL::Rotation rot(KDL::Rotation::Quaternion(q_x, q_y, q_z, q_w)); + KDL::Vector pos(p_x, p_y, p_z); + return KDL::Frame(rot, pos); } - const geometry_msgs::Pose& frame_to_pose(const KDL::Frame & frame) { + geometry_msgs::Pose frame_to_pose(const KDL::Frame &frame) + { double p_x = frame.p.x(); double p_y = frame.p.y(); double p_z = frame.p.z(); @@ -315,35 +379,42 @@ private: return pose; } - void send_joint_trajectory(vector & positions, double time_from_start) { + void send_joint_trajectory(const vector &positions, double time_from_start) + { // Declare JointTrajectory message trajectory_msgs::JointTrajectory msg; - + // Fill the names of the joints to be controlled - msg.joint_names = arm.get_joint_names(); - + msg.joint_names = arm->get_joint_names(); + // Create one point in the trajectory msg.points.resize(1); + msg.points[0].positions.resize(msg.joint_names.size(), 0.0); // Set joint positions - msg.points[0].positions = positions; - + for (size_t idx = 0; idx < positions.size(); ++idx) + { + msg.points[0].positions[idx] = positions[idx]; + } + // How long to take getting to the point (floating point seconds) msg.points[0].time_from_start = ros::Duration(time_from_start); - ROS_INFO_STREAM("Sending command:\n" << msg); + //ROS_INFO("Sending command: \n%s", msg); arm_joint_trajectory_publisher_.publish(msg); } - void gripper_control(bool state) { + void gripper_control(bool state) + { osrf_gear::VacuumGripperControl srv; srv.request.enable = state; gripper_.call(srv); } - bool have_reached_target(geometry_msgs::Pose cur, geometry_msgs::Pose target) { + bool have_reached_target(geometry_msgs::Pose cur, geometry_msgs::Pose target) + { // TODO: Tune threshold values float pos_thresh = 0.01; // Meters float rot_thresh = 0.02; // Radians @@ -351,8 +422,9 @@ private: float pos_err = fabs(cur.position.x - target.position.x) + fabs(cur.position.y - target.position.y) + fabs(cur.position.z - target.position.z); - - if (pos_err > pos_thresh) { + + if (pos_err > pos_thresh) + { return false; } @@ -360,20 +432,24 @@ private: float qy_err = fabs(cur.orientation.y - target.orientation.y); float qz_err = fabs(cur.orientation.z - target.orientation.z); float qw_err = fabs(cur.orientation.w - target.orientation.w); - - if (qx_err > rot_thresh) { + + if (qx_err > rot_thresh) + { return false; } - if (qy_err > rot_thresh) { + if (qy_err > rot_thresh) + { return false; } - if (qz_err > rot_thresh) { + if (qz_err > rot_thresh) + { return false; } - if (qw_err > rot_thresh) { + if (qw_err > rot_thresh) + { return false; } @@ -381,22 +457,24 @@ private: } }; - -int main(int argc, char ** argv) { +int main(int argc, char **argv) +{ cout << "Starting arm_controller_node" << endl; - + // Last argument is the default name of the node. ros::init(argc, argv, "arm_controller_node"); ros::NodeHandle node; - ArmController ac(node); - + ArmRepresentation arm; + + ArmController ac(node, &arm); + // Subscribe to arm destination and joint states channels ros::Subscriber arm_destination_sub = node.subscribe(ARM_DESTINATION_CHANNEL, 1, &ArmController::arm_destination_callback, &ac); ros::Subscriber gripper_state_sub = node.subscribe(GRIPPER_STATE_CHANNEL, 10, &ArmController::gripper_state_callback, &ac); - + ros::Subscriber joint_state_sub = node.subscribe(ARM_JOINT_STATES_CHANNEL, 10, &ArmController::joint_state_callback, &ac); ROS_INFO("Setup complete"); diff --git a/src/rrrobot_ws/src/rrrobot/src/arm_representation.cpp b/src/rrrobot_ws/src/rrrobot/src/arm_representation.cpp new file mode 100644 index 0000000..64c2dfd --- /dev/null +++ b/src/rrrobot_ws/src/rrrobot/src/arm_representation.cpp @@ -0,0 +1,127 @@ +// arm_controller_node.cpp + +#include "arm_representation.h" + +// using namespace std; +using std::string; +using std::vector; + +ArmRepresentation::ArmRepresentation(const KDL::Frame &base_pose) +{ + const double base_len = 0.2; + const double shoulder_height = 0.1273; + const double upper_arm_length = 0.612; + const double forearm_length = 0.5723; + const double shoulder_offset = 0.220941; + const double elbow_offset = -0.1719; + const double wrist_1_length = 0.163941 - elbow_offset - shoulder_offset; + const double wrist_2_length = 0.1157; + const double wrist_3_length = 0.0922; + + // KDL::Vector pos; //(0, 0, base_len/2); + // KDL::Rotation rot; + + // The joints might be off by one + KDL::Segment linear_arm_actuator("linear_arm_actuator_joint", + KDL::Joint(KDL::Joint::JointType::None), base_pose); + chain.addSegment(linear_arm_actuator); + + const KDL::Vector pos_base(0, 0, base_len / 2); + const KDL::Rotation rot_base(KDL::Rotation::RPY(0, 0, 0)); + KDL::Segment base_link("base_link", KDL::Joint("linear_arm_actuator_joint", KDL::Joint::JointType::TransY), + KDL::Frame(rot_base, pos_base)); + chain.addSegment(base_link); + + const KDL::Vector pos_shoulder(0, 0, shoulder_height); + const KDL::Rotation rot_shoulder(KDL::Rotation::RPY(0, 0, 0)); + KDL::Segment shoulder_link("shoulder_link", KDL::Joint("shoulder_pan_joint", KDL::Joint::JointType::RotZ), + KDL::Frame(rot_shoulder, pos_shoulder)); + chain.addSegment(shoulder_link); + + const KDL::Vector pos_upper_arm(0, shoulder_offset, 0); + const KDL::Rotation rot_upper_arm(KDL::Rotation::RPY(0.0, M_PI / 2.0, 0.0)); + KDL::Segment upper_arm_link("upper_arm_link", KDL::Joint("shoulder_lift_joint", KDL::Joint::JointType::RotY), + KDL::Frame(rot_upper_arm, pos_upper_arm)); + chain.addSegment(upper_arm_link); + + const KDL::Vector pos_forearm(0, elbow_offset, upper_arm_length); + const KDL::Rotation rot_forearm(KDL::Rotation::RPY(0.0, 0.0, 0.0)); + KDL::Segment forearm_link("forearm_link", KDL::Joint("elbow_joint", KDL::Joint::JointType::RotY), + KDL::Frame(rot_forearm, pos_forearm)); + chain.addSegment(forearm_link); + + const KDL::Vector pos_wrist_1(0, 0, forearm_length); + const KDL::Rotation rot_wrist_1(KDL::Rotation::RPY(0.0, M_PI / 2.0, 0.0)); + KDL::Segment wrist_1_link("wrist_1_link", KDL::Joint("wrist_1_joint", KDL::Joint::JointType::RotY), + KDL::Frame(rot_wrist_1, pos_wrist_1)); + chain.addSegment(wrist_1_link); + + const KDL::Vector pos_wrist_2(0, wrist_1_length, 0); + const KDL::Rotation rot_wrist_2(KDL::Rotation::RPY(0.0, 0.0, 0.0)); + KDL::Segment wrist_2_link("wrist_2_link", KDL::Joint("wrist_2_joint", KDL::Joint::JointType::RotZ), + KDL::Frame(rot_wrist_2, pos_wrist_2)); + chain.addSegment(wrist_2_link); + + const KDL::Vector pos_wrist_3(0, 0, wrist_2_length); + const KDL::Rotation rot_wrist_3(KDL::Rotation::RPY(0.0, 0.0, 0.0)); + KDL::Segment wrist_3_link("wrist_3_link", KDL::Joint("wrist_3_joint", KDL::Joint::JointType::RotY), + KDL::Frame(rot_wrist_3, pos_wrist_3)); + chain.addSegment(wrist_3_link); + + const KDL::Vector pos_ee(0, wrist_3_length, 0.0); + const KDL::Rotation rot_ee(KDL::Rotation::RPY(0.0, 0.0, M_PI / 2.0)); + KDL::Segment ee_link("ee_link", KDL::Joint(KDL::Joint::JointType::None), + KDL::Frame(rot_ee, pos_ee)); + chain.addSegment(ee_link); + + // arm.addSegment(base_link); + // arm.addSegment(shoulder_link); + // arm.addSegment(upper_arm_link); + // arm.addSegment(forearm_link); + // arm.addSegment(wrist_1_link); + // arm.addSegment(wrist_2_link); + // arm.addSegment(wrist_3_link); + // arm.addSegment(ee_link); + + // fk_solver = KDL::ChainFkSolverPos_recursive(chain); + // ik_solver = KDL::ChainIkSolverPos_LMA(chain); + + // fk_solver.reset(new KDL::ChainFkSolverPos_recursive(arm)); + // ik_solver.reset(new KDL::ChainIkSolverPos_LMA(arm)); +} + +int ArmRepresentation::calculateForwardKinematics(const KDL::JntArray &joint_positions, KDL::Frame &end_effector_pose) +{ + KDL::ChainFkSolverPos_recursive fk_solver = KDL::ChainFkSolverPos_recursive(chain); + int status = fk_solver.JntToCart(joint_positions, end_effector_pose); + + return status; +} + +int ArmRepresentation::calculateInverseKinematics(const KDL::JntArray &cur_configuration, + const KDL::Frame &desired_end_effector_pose, + KDL::JntArray &final_joint_configuration) +{ + KDL::ChainIkSolverPos_LMA ik_solver = KDL::ChainIkSolverPos_LMA(chain); + int status = ik_solver.CartToJnt(cur_configuration, desired_end_effector_pose, final_joint_configuration); + + return status; +} + +vector ArmRepresentation::get_joint_names() +{ + vector joint_names; + + for (auto it : chain.segments) + { + if (it.getJoint().getType() != KDL::Joint::JointType::None) + joint_names.push_back(it.getJoint().getName()); + } + + return joint_names; +} + +KDL::Chain *ArmRepresentation::getChain() +{ + return &chain; +} \ No newline at end of file diff --git a/src/rrrobot_ws/src/rrrobot/test/test_arm.cpp b/src/rrrobot_ws/src/rrrobot/test/test_arm.cpp new file mode 100644 index 0000000..74554bc --- /dev/null +++ b/src/rrrobot_ws/src/rrrobot/test/test_arm.cpp @@ -0,0 +1,231 @@ +// arm_controller_node.cpp + +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include +#include + +#include "topic_names.h" +#include "rrrobot/arm_command.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "arm_representation.h" + +// using namespace std; +using std::cin; +using std::cout; +using std::endl; +using std::string; + +// class ArmRepresentation +// { +// public: +// ArmRepresentation(const KDL::Frame &base_pose = KDL::Frame(KDL::Vector(0.3, 0.92, 1))) +// { +// const double base_len = 0.2; +// const double shoulder_height = 0.1273; +// const double upper_arm_length = 0.612; +// const double forearm_length = 0.5723; +// const double shoulder_offset = 0.220941; +// const double elbow_offset = -0.1719; +// const double wrist_1_length = 0.163941 - elbow_offset - shoulder_offset; +// const double wrist_2_length = 0.1157; +// const double wrist_3_length = 0.0922; + +// // KDL::Vector pos; //(0, 0, base_len/2); +// // KDL::Rotation rot; + +// // The joints might be off by one +// KDL::Segment linear_arm_actuator("linear_arm_actuator_joint", +// KDL::Joint(KDL::Joint::JointType::None), base_pose); +// arm.addSegment(linear_arm_actuator); + +// const KDL::Vector pos_base(0, 0, base_len / 2); +// const KDL::Rotation rot_base(KDL::Rotation::RPY(0, 0, 0)); +// KDL::Segment base_link("base_link", KDL::Joint(KDL::Joint::JointType::TransY), +// KDL::Frame(rot_base, pos_base)); +// arm.addSegment(base_link); + +// const KDL::Vector pos_shoulder(0, 0, shoulder_height); +// const KDL::Rotation rot_shoulder(KDL::Rotation::RPY(0, 0, 0)); +// KDL::Segment shoulder_link("shoulder_link", KDL::Joint(KDL::Joint::JointType::RotZ), +// KDL::Frame(rot_shoulder, pos_shoulder)); +// arm.addSegment(shoulder_link); + +// const KDL::Vector pos_upper_arm(0, shoulder_offset, 0); +// const KDL::Rotation rot_upper_arm(KDL::Rotation::RPY(0.0, M_PI / 2.0, 0.0)); +// KDL::Segment upper_arm_link("upper_arm_link", KDL::Joint(KDL::Joint::JointType::RotY), +// KDL::Frame(rot_upper_arm, pos_upper_arm)); +// arm.addSegment(upper_arm_link); + +// const KDL::Vector pos_forearm(0, elbow_offset, upper_arm_length); +// const KDL::Rotation rot_forearm(KDL::Rotation::RPY(0.0, 0.0, 0.0)); +// KDL::Segment forearm_link("forearm_link", KDL::Joint(KDL::Joint::JointType::RotY), +// KDL::Frame(rot_forearm, pos_forearm)); +// arm.addSegment(forearm_link); + +// const KDL::Vector pos_wrist_1(0, 0, forearm_length); +// const KDL::Rotation rot_wrist_1(KDL::Rotation::RPY(0.0, M_PI / 2.0, 0.0)); +// KDL::Segment wrist_1_link("wrist_1_link", KDL::Joint(KDL::Joint::JointType::RotY), +// KDL::Frame(rot_wrist_1, pos_wrist_1)); +// arm.addSegment(wrist_1_link); + +// const KDL::Vector pos_wrist_2(0, wrist_1_length, 0); +// const KDL::Rotation rot_wrist_2(KDL::Rotation::RPY(0.0, 0.0, 0.0)); +// KDL::Segment wrist_2_link("wrist_2_link", KDL::Joint(KDL::Joint::JointType::RotZ), +// KDL::Frame(rot_wrist_2, pos_wrist_2)); +// arm.addSegment(wrist_2_link); + +// const KDL::Vector pos_wrist_3(0, 0, wrist_2_length); +// const KDL::Rotation rot_wrist_3(KDL::Rotation::RPY(0.0, 0.0, 0.0)); +// KDL::Segment wrist_3_link("wrist_3_link", KDL::Joint(KDL::Joint::JointType::RotY), +// KDL::Frame(rot_wrist_3, pos_wrist_3)); +// arm.addSegment(wrist_3_link); + +// const KDL::Vector pos_ee(0, wrist_3_length, 0.0); +// const KDL::Rotation rot_ee(KDL::Rotation::RPY(0.0, 0.0, M_PI / 2.0)); +// KDL::Segment ee_link("ee_link", KDL::Joint(KDL::Joint::JointType::None), +// KDL::Frame(rot_ee, pos_ee)); +// arm.addSegment(ee_link); + +// // arm.addSegment(base_link); +// // arm.addSegment(shoulder_link); +// // arm.addSegment(upper_arm_link); +// // arm.addSegment(forearm_link); +// // arm.addSegment(wrist_1_link); +// // arm.addSegment(wrist_2_link); +// // arm.addSegment(wrist_3_link); +// // arm.addSegment(ee_link); + +// fk_solver.reset(new KDL::ChainFkSolverPos_recursive(arm)); +// ik_solver.reset(new KDL::ChainIkSolverPos_LMA(arm)); +// } + +// int calculateForwardKinematics(const KDL::JntArray &joint_positions, KDL::Frame &end_effector_pose) +// { +// return fk_solver->JntToCart(joint_positions, end_effector_pose); +// } + +// int calculateInverseKinematics(const KDL::JntArray &cur_configuration, +// const KDL::Frame &desired_end_effector_pose, +// KDL::JntArray &final_joint_configuration) +// { +// return ik_solver->CartToJnt(cur_configuration, desired_end_effector_pose, final_joint_configuration); +// } + +// string *get_joint_names() +// { +// // TODO: return ["linear_arm_actuator_joint", +// // "shoulder_pan_joint", +// // "shoulder_lift_joint", +// // "elbow_joint", +// // "wrist_1_joint", +// // "wrist_2_joint", +// // "wrist_3_joint"]; +// } + +// const KDL::Chain &getArm() const +// { +// return arm; +// } + +// private: +// KDL::Chain arm; + +// std::unique_ptr fk_solver; +// std::unique_ptr ik_solver; +// }; + +int main(int argc, char **argv) +{ + ArmRepresentation arm; + + KDL::JntArray pos(arm.getChain()->getNrOfJoints()); + KDL::SetToZero(pos); + + int joint; + + // std::cout << "Enter joint to exercise: "; + // std::cin >> joint; + + KDL::Frame end_effector_pose; + std::ofstream f("data.txt"); + int error_code; + for (joint = 0; joint < arm.getChain()->getNrOfJoints(); ++joint) + { + for (double pos_val = 0.0; pos_val <= 2 * M_PI; pos_val += 0.1) + { + pos(joint) = pos_val; + error_code = arm.calculateForwardKinematics(pos, end_effector_pose); + + if (error_code != 0) + { + ROS_ERROR("Forward Kinematics Failure: %i", error_code); + } + + f << end_effector_pose.p.x() << "," << end_effector_pose.p.y() << "," << end_effector_pose.p.z() << '\n'; + + error_code = arm.calculateInverseKinematics(pos, end_effector_pose, pos); + + if (error_code != 0) + { + ROS_ERROR("Inverse Kinematics Failure: %i", error_code); + } + } + } + + double linear_arm_actuator_joint, shoulder_pan_joint, shoulder_lift_joint, elbow_joint, wrist_1_joint, wrist_2_joint, wrist_3_joint; + + while (true) + { + cout << "linear_arm_actuator_joint: "; + cin >> pos(0); //linear_arm_actuator_joint; + cout << "shoulder_pan_joint: "; + cin >> pos(1); //shoulder_pan_joint; + cout << "shoulder_lift_joint: "; + cin >> pos(2); //shoulder_lift_joint; + cout << "elbow_joint: "; + cin >> pos(3); //elbow_joint; + cout << "wrist_1_joint: "; + cin >> pos(4); //wrist_1_joint; + cout << "wrist_2_joint: "; + cin >> pos(5); //wrist_2_joint; + cout << "wrist_3_joint: "; + cin >> pos(6); //wrist_3_joint; + + error_code = arm.calculateForwardKinematics(pos, end_effector_pose); + + if (error_code != 0) + { + ROS_ERROR("Forward kinematics failure: %i", error_code); + } + + cout << "x,y,z: " << end_effector_pose.p.x() << ", " << end_effector_pose.p.y() << ", " << end_effector_pose.p.z() << endl; + cout << end_effector_pose.M << endl; + } +} \ No newline at end of file