diff --git a/src/lite6_controller/launch/lite6_gazebo.launch.py b/src/lite6_controller/launch/lite6_gazebo.launch.py index ac1a559..c72f93c 100644 --- a/src/lite6_controller/launch/lite6_gazebo.launch.py +++ b/src/lite6_controller/launch/lite6_gazebo.launch.py @@ -382,15 +382,8 @@ def launch_setup(context, *args, **kwargs): 'publish_geometry_updates': True, 'publish_state_updates': True, 'publish_transforms_updates': True, - # "planning_scene_monitor_options": { - # "name": "planning_scene_monitor", - # "robot_description": "robot_description", - # "joint_state_topic": "/joint_states", - # "attached_collision_object_topic": "/move_group/planning_scene_monitor", - # "publish_planning_scene_topic": "/move_group/publish_planning_scene", - # "monitored_planning_scene_topic": "/move_group/monitored_planning_scene", - # "wait_for_initial_state_timeout": 10.0, - # }, + 'publish_robot_description': True, + 'publish_robot_description_semantic' :True, } # Start the actual move_group node/action server diff --git a/src/lite6_controller/launch/lite6_real.launch.py b/src/lite6_controller/launch/lite6_real.launch.py index c349d32..04a0302 100644 --- a/src/lite6_controller/launch/lite6_real.launch.py +++ b/src/lite6_controller/launch/lite6_real.launch.py @@ -227,15 +227,8 @@ def launch_setup(context, *args, **kwargs): 'publish_geometry_updates': True, 'publish_state_updates': True, 'publish_transforms_updates': True, - # "planning_scene_monitor_options": { - # "name": "planning_scene_monitor", - # "robot_description": "robot_description", - # "joint_state_topic": "/joint_states", - # "attached_collision_object_topic": "/move_group/planning_scene_monitor", - # "publish_planning_scene_topic": "/move_group/publish_planning_scene", - # "monitored_planning_scene_topic": "/move_group/monitored_planning_scene", - # "wait_for_initial_state_timeout": 10.0, - # }, + 'publish_robot_description': True, + 'publish_robot_description_semantic' :True, } diff --git a/src/lite6_controller/launch/lite6_real_no_gui.launch.py b/src/lite6_controller/launch/lite6_real_no_gui.launch.py index cdf5c6b..df866fc 100644 --- a/src/lite6_controller/launch/lite6_real_no_gui.launch.py +++ b/src/lite6_controller/launch/lite6_real_no_gui.launch.py @@ -233,15 +233,8 @@ def launch_setup(context, *args, **kwargs): 'publish_geometry_updates': True, 'publish_state_updates': True, 'publish_transforms_updates': True, - # "planning_scene_monitor_options": { - # "name": "planning_scene_monitor", - # "robot_description": "robot_description", - # "joint_state_topic": "/joint_states", - # "attached_collision_object_topic": "/move_group/planning_scene_monitor", - # "publish_planning_scene_topic": "/move_group/publish_planning_scene", - # "monitored_planning_scene_topic": "/move_group/monitored_planning_scene", - # "wait_for_initial_state_timeout": 10.0, - # }, + 'publish_robot_description': True, + 'publish_robot_description_semantic' :True, } # Start the actual move_group node/action server diff --git a/src/lite6_controller/src/lite6_calibration.cpp b/src/lite6_controller/src/lite6_calibration.cpp index 3a9add2..1e2fa34 100644 --- a/src/lite6_controller/src/lite6_calibration.cpp +++ b/src/lite6_controller/src/lite6_calibration.cpp @@ -5,6 +5,15 @@ #include #include +// MoveIt +#include +#include +#include + + +const std::string MOVE_GROUP = "lite6"; + +std::shared_ptr node; void exit_sig_handler(int signum) { @@ -16,24 +25,31 @@ void wait() { do { - std::cout << '\n' << "Press a key to continue..."; + std::cout << "Press a key to continue..."; } while (std::cin.get() != '\n'); } -void println(std::string s) { +void println(std::string s) +{ std::cout << s << std::endl; } -void print_joints(std::vector jnts) { +void print_joints(std::vector jnts) +{ + std::string out = ""; + out = out + "{ "; for (auto i : jnts) - std::cout << i << std::endl; + out = out + std::to_string(i) + ", "; + out = out.substr(0, out.size()-2); //remove trailing comma + out = out + " }"; + std::cout << out << std::endl; } int main(int argc, char **argv) { rclcpp::init(argc, argv); std::string hw_ns = "ufactory"; - std::shared_ptr node = rclcpp::Node::make_shared("lite6_calibration"); + node = rclcpp::Node::make_shared("lite6_calibration"); int ret; signal(SIGINT, exit_sig_handler); @@ -55,52 +71,61 @@ int main(int argc, char **argv) client.set_state(0); rclcpp::sleep_for(std::chrono::seconds(2)); - std::vector jnt_drawing_pose = {-0.813972, -0.0103697, 0.554617, 3.09979, -0.627334, -0.397301, 0}; + //std::vector jnt_drawing_pose = { -0.975004, 0.0734182, 0.443928, 3.14102, -0.370552, -0.85577, 0 }; + std::vector jnt_drawing_pose = { -0.975008, 0.00889134, 0.453255, 3.1414, -0.444295, -0.85692, 0 } ; std::vector jnt_pose = { 0, 0, 0, 0, 0, 0, 0 }; client.get_servo_angle(jnt_pose); - println("Moving to start drawing pose"); -//XArmROSClient::set_servo_angle(const std::vector& angles, fp32 speed, fp32 acc, fp32 mvtime, bool wait, fp32 timeout, fp32 radius) - client.set_servo_angle(jnt_drawing_pose, 1, 1, 1, true, 100, -1); + + print_joints(jnt_pose); + + + //client.set_servo_angle_j(jnt_drawing_pose, 1, 1, 100); //rclcpp::sleep_for(std::chrono::seconds(5)); + // Compute position of pen from joint state + robot_model_loader::RobotModelLoader robot_model_loader(node); + const moveit::core::RobotModelPtr& kinematic_model = robot_model_loader.getModel(); + moveit::core::RobotStatePtr kinematic_state(new moveit::core::RobotState(kinematic_model)); + const moveit::core::JointModelGroup* joint_model_group = kinematic_model->getJointModelGroup(MOVE_GROUP); + std::string ee_link = "pen_link"; + + std::vector jnts; + for (auto j : jnt_pose) + jnts.push_back(j); + + jnts.resize(joint_model_group->getVariableNames().size()); + kinematic_state->setJointGroupPositions(joint_model_group, jnts); + + // from tutorial https://ros-planning.github.io/moveit_tutorials/doc/robot_model_and_robot_state/robot_model_and_robot_state_tutorial.html + // https://github.com/ros-planning/moveit_tutorials/blob/master/doc/robot_model_and_robot_state/src/robot_model_and_robot_state_tutorial.cpp + //robot_model_loader::RobotModelLoader robot_model_loader(node, "robot_description"); + + kinematic_state->setJointGroupPositions(joint_model_group, jnts); + // https://mycourses.aalto.fi/pluginfile.php/1433193/mod_resource/content/2/Intro_to_ROS_Eigen.pdf + const Eigen::Isometry3d& end_effector_state = kinematic_state->getGlobalLinkTransform(ee_link); + + auto x = std::to_string(end_effector_state.translation().x()); + auto y = std::to_string(end_effector_state.translation().y()); + auto z = std::to_string(end_effector_state.translation().z()); + //println("x for '" + ee_link + "': " + x); + //println("y for '" + ee_link + "': " + y); + println("z-offset value for '" + ee_link + "' (use this value for the current pen): " + z); + + + println("Moving to start drawing pose"); + wait(); + client.set_servo_angle(jnt_drawing_pose, 1, 1, 1, true, 100, -1); + client.set_mode(0); client.set_state(0); - return 0; - client.set_mode(4); - client.set_state(0); - std::vector jnt_v = { 1, 0, 0, 0, 0, 0, 0 }; - ret = client.vc_set_joint_velocity(jnt_v); - RCLCPP_INFO(rclcpp::get_logger("test_xarm_velo_move"), "velo_move_joint: %d", ret); - rclcpp::sleep_for(std::chrono::seconds(2)); - jnt_v[0] = -1; - ret = client.vc_set_joint_velocity(jnt_v); - RCLCPP_INFO(rclcpp::get_logger("test_xarm_velo_move"), "velo_move_joint: %d", ret); - rclcpp::sleep_for(std::chrono::seconds(2)); - // stop - jnt_v[0] = 0; - ret = client.vc_set_joint_velocity(jnt_v); - RCLCPP_INFO(rclcpp::get_logger("test_xarm_velo_move"), "velo_move_joint: %d", ret); + rclcpp::shutdown(); - std::vector line_v = { 1, 0, 0, 0, 0, 0}; - client.set_mode(5); - client.set_state(0); - ret = client.vc_set_cartesian_velocity(line_v); - RCLCPP_INFO(rclcpp::get_logger("test_xarm_velo_move"), "velo_move_line: %d", ret); - rclcpp::sleep_for(std::chrono::seconds(2)); - line_v[0] = -1; - ret = client.vc_set_cartesian_velocity(line_v); - RCLCPP_INFO(rclcpp::get_logger("test_xarm_velo_move"), "velo_move_line: %d", ret); - rclcpp::sleep_for(std::chrono::seconds(2)); - // stop - line_v[0] = 0; - ret = client.vc_set_cartesian_velocity(line_v); - RCLCPP_INFO(rclcpp::get_logger("test_xarm_velo_move"), "velo_move_line: %d", ret); - - RCLCPP_INFO(rclcpp::get_logger("test_xarm_velo_move"), "test_xarm_velo_move over"); + println("Done, ignore any errors after this"); + wait(); return 0; }