diff --git a/.gitignore b/.gitignore index 42771c3..f6fca57 100644 --- a/.gitignore +++ b/.gitignore @@ -34,3 +34,4 @@ core *.old *.custom *.run +*.debug diff --git a/docs/gear.md b/docs/gear.md index 88700a1..ace8587 100644 --- a/docs/gear.md +++ b/docs/gear.md @@ -86,18 +86,38 @@ rostopic pub /ariac/arm1/arm/command trajectory_msgs/JointTrajectory "{joint_ ## Running Full Simulation -### ARIAC Environment +### Terminal 1: ARIAC Environment - `cd /app/rrrobot_ws/src/rrrobot/scripts` -- `./rrrobot_run.sh` +- `./rrrobot_run_no_build.sh` -### Build RRRobot Package +### Terminal 2: Build RRRobot Package & Arm Controller Node - `cd /app/rrrobot_ws` - `catkin_make clean` - `catkin_make` - `source devel/setup.bash` - -### Arm Controller Node - - `rosrun rrrobot arm_controller_node` + +### Terminal 3: CV Model + +- `cd /app/rrrobot_ws/src/rrrobot/src/` +- `python3 cv_model.py` + +### Terminal 4: Depth Camera Node (for getting pick up location) + +- `cd /app/rrrobot_ws/` +- `source devel/setup.bash` +- `rosrun rrrobot depth_camera_node` + +### Terminal 5: Run the main rrrobot node + +- `cd /app/rrrobot_ws/` +- `source devel/setup.bash` +- `rosrun rrrobot rrrobot_node` + +### Terminal 6: Run the node to spawn random objects onto the conveyor belt + +- `cd /app/rrrobot_ws/` +- `source devel/setup.bash` +- `rosrun rrrobot object_spawner_node` diff --git a/src/rrrobot_ws/src/rrrobot/include/arm_representation.h b/src/rrrobot_ws/src/rrrobot/include/arm_representation.h index 6dc4a37..98dbdb4 100644 --- a/src/rrrobot_ws/src/rrrobot/include/arm_representation.h +++ b/src/rrrobot_ws/src/rrrobot/include/arm_representation.h @@ -43,7 +43,7 @@ public: int calculateForwardKinematics(const KDL::JntArray &joint_positions, KDL::Frame &end_effector_pose, int joint_nbr = -1); - int calculateInverseKinematics(const KDL::JntArray &cur_configuration, + int calculateInverseKinematics(const std::vector &cur_configuration, const KDL::Frame &desired_end_effector_pose, KDL::JntArray &final_joint_configuration); diff --git a/src/rrrobot_ws/src/rrrobot/scripts/arm_pub_test.sh b/src/rrrobot_ws/src/rrrobot/scripts/arm_pub_test.sh index 9560048..1dc4c79 100755 --- a/src/rrrobot_ws/src/rrrobot/scripts/arm_pub_test.sh +++ b/src/rrrobot_ws/src/rrrobot/scripts/arm_pub_test.sh @@ -1,11 +1,12 @@ #!/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}}' +source /app/rrrobot_ws/devel/setup.bash + rostopic pub /arm_controller/destination rrrobot/arm_command "grab_location: position: - x: 1.2 - y: -1 - z: 1.5 + x: 1.22 + y: 1.22 + z: 0.9725 orientation: x: 0.0 y: 0.707 @@ -14,11 +15,11 @@ rostopic pub /arm_controller/destination rrrobot/arm_command "grab_location: drop_location: position: x: -0.3 - y: 1.15 - z: 1.5 + y: 0.383 + z: 1.0 orientation: x: 0.0 - y: 0.0 + y: 0.707 z: 0.0 - w: 0.0" \ + w: 0.707" \ -1 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 e3b7353..c5495b6 100644 --- a/src/rrrobot_ws/src/rrrobot/src/arm_controller_node.cpp +++ b/src/rrrobot_ws/src/rrrobot/src/arm_controller_node.cpp @@ -48,10 +48,12 @@ public: gripper_ = node.serviceClient(GRIPPER_CONTROL_CHANNEL); + arm_current_joint_states_.resize(arm->getChain()->getNrOfJoints(), 0.0); + // 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_); + // 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) @@ -64,22 +66,9 @@ public: vector cur_joint_names = arm->get_joint_names(); vector position = joint_state_msg.position; - // 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; + arm_current_joint_states_[i] = 0.0; } for (size_t i = 0; i < position.size(); ++i) @@ -88,7 +77,7 @@ public: { if (msg_joint_names[i].compare(cur_joint_names[j]) == 0) { - arm_current_joint_states_(j) = position[i]; + arm_current_joint_states_[j] = position[i]; } } } @@ -122,7 +111,7 @@ public: // Move arm to pickup item from conveyor belt attempts = 1; - positions = calc_joint_positions(target_pose.grab_location); + positions = calc_joint_positions(target_pose.grab_location, above_conveyor); send_joint_trajectory(positions, arm_action_phase::bin_to_belt); // Check if target has been reached @@ -151,7 +140,7 @@ public: // If item is not able to attach, adjust z and try again target_location.position.z -= item_attach_z_adjustment; // meters - positions = calc_joint_positions(target_location); + positions = calc_joint_positions(target_location, arm_current_joint_states_); send_joint_trajectory(positions, arm_action_phase::item_pickup_adjustment); attempts++; } @@ -160,11 +149,11 @@ public: // Move item to desired position attempts = 1; - positions = calc_joint_positions(target_pose.drop_location); + positions = calc_joint_positions(target_pose.drop_location, above_bins); send_joint_trajectory(positions, arm_action_phase::belt_to_bin); // Check if target has been 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.drop_location)) { if (attempts == retry_attempts) { ROS_ERROR("STEP 4: Did not reach target in allotted time"); return; @@ -198,7 +187,7 @@ public: ROS_ERROR("STEP 6: Item not detaching.. ¯\\_(ツ)_/¯"); } - ros::Duration(0.1).sleep(); + ros::Duration(1).sleep(); attempts++; } @@ -206,11 +195,12 @@ public: } private: + // Private variables bool gripper_enabled_; bool item_attached_; ros::Publisher arm_joint_trajectory_publisher_; ros::ServiceClient gripper_; - KDL::JntArray arm_current_joint_states_; + vector arm_current_joint_states_; ArmRepresentation *arm; enum arm_action_phase { belt_to_bin, @@ -220,8 +210,11 @@ private: double time_per_step; int retry_attempts; double item_attach_z_adjustment; + // Define intermediate joint positions + const vector above_conveyor = {0, 0, -M_PI/2, M_PI/2, -M_PI/2, -M_PI/2, 0}; + const vector above_bins = {0, M_PI, -M_PI/2, M_PI/2, -M_PI/2, -M_PI/2, 0}; - vector calc_joint_positions(const geometry_msgs::Pose &pose) + vector calc_joint_positions(const geometry_msgs::Pose &pose, const vector &start_state) { // KDL::JntArray cur_configuration(arm->getChain()->getNrOfJoints()); // = arm_current_joint_states_; // KDL::SetToZero(cur_configuration); @@ -230,7 +223,7 @@ private: // 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); + int error_code = arm->calculateInverseKinematics(start_state, desired_end_effector_pose, final_joint_configuration); // Check status of IK and print error message if there is a failure if (error_code != 0) @@ -253,13 +246,16 @@ private: KDL::Frame calc_end_effector_pose() { + int num_joints = arm->getChain()->getNrOfJoints(); + KDL::Frame end_effector_pose; - // KDL::JntArray joint_positions = arm_current_joint_states_; - KDL::JntArray joint_positions(arm->getChain()->getNrOfJoints()); - KDL::SetToZero(joint_positions); + KDL::JntArray joint_positions(num_joints); + + // Copy current joint states values into JntArray + for (int i = 0; i < num_joints; ++i) { + joint_positions(i) = arm_current_joint_states_[i]; + } - // 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 @@ -315,10 +311,6 @@ private: // Declare JointTrajectory message trajectory_msgs::JointTrajectory msg; - // Define intermediate joint positions - const vector above_conveyor = {0, 0, -M_PI/2, M_PI/2, -M_PI/2, -M_PI/2, 0}; - const vector above_bins = {0, M_PI, -M_PI/2, M_PI/2, -M_PI/2, -M_PI/2, 0}; - // Fill the names of the joints to be controlled msg.joint_names = arm->get_joint_names(); @@ -383,51 +375,52 @@ private: bool have_reached_target(geometry_msgs::Pose cur, geometry_msgs::Pose target) { - // TODO: Tune threshold values - float pos_thresh = 0.1; // Meters - float rot_thresh = 0.0875; // Radians + // Tune threshold values + float pos_thresh = 0.15; // Meters + float rot_thresh = 0.05; float pos_err = fabs(cur.position.x - target.position.x) + fabs(cur.position.y - target.position.y) + fabs(cur.position.z - target.position.z); + // ROS_INFO_STREAM("pos_err: " << pos_err << " (" << pos_thresh << ")"); + if (pos_err > pos_thresh) { return false; } - vector rpy1 = QuatToEuler(cur.orientation); - vector rpy2 = QuatToEuler(target.orientation); + float qx_err = fabs(fabs(cur.orientation.x) - fabs(target.orientation.x)); + float qy_err = fabs(fabs(cur.orientation.y) - fabs(target.orientation.y)); + float qz_err = fabs(fabs(cur.orientation.z) - fabs(target.orientation.z)); + float qw_err = fabs(fabs(cur.orientation.w) - fabs(target.orientation.w)); - float rx_err = fabs(fmod(rpy1[0] - rpy2[0] + M_PI, M_PI)); - float ry_err = fabs(fmod(rpy1[1] - rpy2[1] + M_PI, M_PI)); - float rz_err = fabs(fmod(rpy1[2] - rpy2[2] + M_PI, M_PI)); + ROS_INFO_STREAM("qx_err: " << qx_err << " (" << rot_thresh << ")"); + ROS_INFO_STREAM("qy_err: " << qy_err << " (" << rot_thresh << ")"); + ROS_INFO_STREAM("qz_err: " << qz_err << " (" << rot_thresh << ")"); + ROS_INFO_STREAM("qw_err: " << qw_err << " (" << rot_thresh << ")"); - if (rx_err > rot_thresh) + if (qx_err > rot_thresh) { return false; } - if (ry_err > rot_thresh) + if (qy_err > rot_thresh) { return false; } - if (rz_err > rot_thresh) + if (qz_err > rot_thresh) { return false; } + if (qw_err > rot_thresh) { + return false; + } + return true; } - - vector QuatToEuler(geometry_msgs::Quaternion q) { - KDL::Rotation r = KDL::Rotation::Quaternion(q.x, q.y, q.z, q.w); - vector rpy = {0, 0, 0}; - r.GetRPY(rpy[0], rpy[1], rpy[2]); - - return rpy; - } }; int main(int argc, char **argv) @@ -443,20 +436,23 @@ int main(int argc, char **argv) double time_per_step = 3.0; // seconds int retry_attempts = 3; - double item_attach_z_adjustment = 0.025; // meters + double item_attach_z_adjustment = 0.05; // meters ArmController ac(node, &arm, time_per_step, retry_attempts, item_attach_z_adjustment); + ros::AsyncSpinner spinner(0); + spinner.start(); + // 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 gripper_state_sub = node.subscribe(GRIPPER_STATE_CHANNEL, 1, &ArmController::gripper_state_callback, &ac); - ros::Subscriber joint_state_sub = node.subscribe(ARM_JOINT_STATES_CHANNEL, 10, &ArmController::joint_state_callback, &ac); + ros::Subscriber joint_state_sub = node.subscribe(ARM_JOINT_STATES_CHANNEL, 1, &ArmController::joint_state_callback, &ac); ROS_INFO("Setup complete"); // Execute callbacks on new data until ctrl-c - ros::spin(); + ros::waitForShutdown(); return 0; } diff --git a/src/rrrobot_ws/src/rrrobot/src/arm_representation.cpp b/src/rrrobot_ws/src/rrrobot/src/arm_representation.cpp index 7caf630..5fa6e2e 100644 --- a/src/rrrobot_ws/src/rrrobot/src/arm_representation.cpp +++ b/src/rrrobot_ws/src/rrrobot/src/arm_representation.cpp @@ -99,12 +99,22 @@ int ArmRepresentation::calculateForwardKinematics(const KDL::JntArray &joint_pos return status; } -int ArmRepresentation::calculateInverseKinematics(const KDL::JntArray &cur_configuration, +int ArmRepresentation::calculateInverseKinematics(const vector &cur_configuration, const KDL::Frame &desired_end_effector_pose, KDL::JntArray &final_joint_configuration) { KDL::ChainIkSolverPos_LMA ik_solver(chain, 1e-3, 2000, 1e-8); - int status = ik_solver.CartToJnt(cur_configuration, desired_end_effector_pose, final_joint_configuration); + + // Convert cur_configuration vector to JntArray + int num_joints = chain.getNrOfJoints(); + KDL::JntArray pos(num_joints); + KDL::SetToZero(pos); + + for (int i = 0; i < num_joints; ++i) { + pos(i) = cur_configuration[i]; + } + + int status = ik_solver.CartToJnt(pos, desired_end_effector_pose, final_joint_configuration); return status; } diff --git a/src/rrrobot_ws/src/rrrobot/src/cv_model.py b/src/rrrobot_ws/src/rrrobot/src/cv_model.py index b02b735..e9fd9ef 100644 --- a/src/rrrobot_ws/src/rrrobot/src/cv_model.py +++ b/src/rrrobot_ws/src/rrrobot/src/cv_model.py @@ -38,7 +38,7 @@ def call_back(filename): # query model predicted_logits = model(input_tensor).squeeze() # print(predicted_logits.shape) - predicted_label = torch.argmax(predicted_logits).numpy().tolist() + predicted_label = torch.argmax(predicted_logits).detach().numpy().tolist() type_dict = {0: 'cardboard', 1: 'glass' , diff --git a/src/rrrobot_ws/src/rrrobot/src/depth_camera_node.cpp b/src/rrrobot_ws/src/rrrobot/src/depth_camera_node.cpp index 812cd89..f735cae 100644 --- a/src/rrrobot_ws/src/rrrobot/src/depth_camera_node.cpp +++ b/src/rrrobot_ws/src/rrrobot/src/depth_camera_node.cpp @@ -218,7 +218,7 @@ void depth_camera_callback(const sensor_msgs::PointCloud::ConstPtr &cloud_msg) geometry_msgs::Point p; geometry_msgs::Quaternion q; - // pick up from top + // pick up from top (-z direction) if (area_top > .002) { std::cout << "picking up object from top" << std::endl; @@ -228,23 +228,23 @@ void depth_camera_callback(const sensor_msgs::PointCloud::ConstPtr &cloud_msg) // rpy = 0 0 0 q.x = 0; - q.y = 0; + q.y = 0.707; q.z = 0; - q.w = 1; + q.w = 0.707; } else { - // pick up from front + // pick up from front (+x direction) std::cout << "picking up object from front" << std::endl; p.x = xmin; p.y = (ymin + ymax) / 2; p.z = (zmax + zmin) / 2; // rpy = -pi/2 0 pi/2 - q.x = -0.63; - q.y = 0; - q.z = 0.63; - q.w = 0.44; + q.x = 0.707; + q.y = 0.0; + q.z = 0.0; + q.w = 0.707; } pose.position = p; @@ -271,18 +271,16 @@ void depth_camera_callback(const sensor_msgs::PointCloud::ConstPtr &cloud_msg) int main(int argc, char **argv) { - // TODO: Subscribe to depth camera topic // Last argument is the default name of the node. ros::init(argc, argv, "depth_camera_node"); ros::NodeHandle node; + // Subscribe to depth camera topic ros::Subscriber sub = node.subscribe("/ariac/depth_camera_1", 1, depth_camera_callback); + // Publish object's current location to topic for RRRobot node to listen to pub = node.advertise(DESIRED_GRASP_POSE_CHANNEL, 1); ros::spin(); - // TODO: When item is in view, work with point cloud to get location (in world frame) for arm to reach to pickup item - - // TODO: Publish object's current location to topic for RRRobot node to listen to } \ No newline at end of file diff --git a/src/rrrobot_ws/src/rrrobot/src/object_spawner_node.cpp b/src/rrrobot_ws/src/rrrobot/src/object_spawner_node.cpp index 5eacaf2..0b3a6fb 100644 --- a/src/rrrobot_ws/src/rrrobot/src/object_spawner_node.cpp +++ b/src/rrrobot_ws/src/rrrobot/src/object_spawner_node.cpp @@ -99,7 +99,7 @@ private: image_file_msg.data = models[rand_idx].image_file; cout << "Spawning " << msg.model_name << endl; - // TODO: Determine position to spawn items + // Determine position to spawn items // can place objects on conveyor belt just upstream from the arms // at roughly (0.2516105, 5.474367, 0.935669) // x range: 1.022548-1.413952 diff --git a/src/rrrobot_ws/src/rrrobot/src/rrrobot_node.cpp b/src/rrrobot_ws/src/rrrobot/src/rrrobot_node.cpp index a24fa4b..071586d 100644 --- a/src/rrrobot_ws/src/rrrobot/src/rrrobot_node.cpp +++ b/src/rrrobot_ws/src/rrrobot/src/rrrobot_node.cpp @@ -126,6 +126,8 @@ public: set_conveyor(0); desired_grasp_pose = grasp_pose; + // TODO: Tune z offset so end effector doesn't hit object + desired_grasp_pose.position.z -= 0.025; if (current_robot_state & RobotState::WAITING_FOR_CLASSIFICATION) { diff --git a/src/rrrobot_ws/src/rrrobot/test/test_arm.cpp b/src/rrrobot_ws/src/rrrobot/test/test_arm.cpp index 74554bc..28400b5 100644 --- a/src/rrrobot_ws/src/rrrobot/test/test_arm.cpp +++ b/src/rrrobot_ws/src/rrrobot/test/test_arm.cpp @@ -36,130 +36,8 @@ #include "arm_representation.h" -// using namespace std; -using std::cin; -using std::cout; -using std::endl; -using std::string; +using namespace std; -// 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) { @@ -174,7 +52,7 @@ int main(int argc, char **argv) // std::cin >> joint; KDL::Frame end_effector_pose; - std::ofstream f("data.txt"); + std::ofstream f("arm_test.debug"); int error_code; for (joint = 0; joint < arm.getChain()->getNrOfJoints(); ++joint) { @@ -190,7 +68,7 @@ int main(int argc, char **argv) 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); + error_code = arm.calculateInverseKinematics(vector(arm.getChain()->getNrOfJoints(), 0.0), end_effector_pose, pos); if (error_code != 0) {