diff --git a/auto_exchange_planner/CMakeLists.txt b/auto_exchange_planner/CMakeLists.txt new file mode 100644 index 0000000..dee6355 --- /dev/null +++ b/auto_exchange_planner/CMakeLists.txt @@ -0,0 +1,79 @@ +cmake_minimum_required(VERSION 3.10) +project(auto_exchange_planner) + +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +add_definitions(-Wall -Werror) + +find_package(catkin REQUIRED COMPONENTS + moveit_core + pluginlib + roscpp + std_msgs +) + +find_package(Eigen3 REQUIRED) + +catkin_package( + INCLUDE_DIRS + include + ${EIGEN3_INCLUDE_DIR} + LIBRARIES + CATKIN_DEPENDS + roscpp + std_msgs + DEPENDS +) + +include_directories(include) +include_directories( + SYSTEM + ${catkin_INCLUDE_DIRS} + ${Boost_INCLUDE_DIRS} +) + +#add_executable(test_auto_exchange src/test_auto_exchange.cpp) +#target_link_libraries(test_auto_exchange +# ${catkin_LIBRARIES} +# ${Boost_LIBRARIES} +#) + +add_library(moveit_auto_exchange_planner_plugin + src/auto_exchange_manager.cpp + src/auto_exchange_planner.cpp + src/auto_exchange_context.cpp) +set_target_properties(moveit_auto_exchange_planner_plugin PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +target_link_libraries(moveit_auto_exchange_planner_plugin ${catkin_LIBRARIES} ${Boost_LIBRARIES}) + +# Mark executables and/or libraries for installation +install( + TARGETS + moveit_auto_exchange_planner_plugin + ARCHIVE DESTINATION + ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION + ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION + ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +# Mark cpp header files for installation +install( + DIRECTORY + include + DESTINATION + ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +# Mark roslaunch files for installation +install( + DIRECTORY + launch + DESTINATION + ${CATKIN_PACKAGE_SHARE_DESTINATION} +) +#catkin_lint ignore uninstalled_plugin +install(FILES auto_exchange_planner_plugin_description.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + diff --git a/auto_exchange_planner/auto_exchange_planner_plugin_description.xml b/auto_exchange_planner/auto_exchange_planner_plugin_description.xml new file mode 100644 index 0000000..f926cac --- /dev/null +++ b/auto_exchange_planner/auto_exchange_planner_plugin_description.xml @@ -0,0 +1,7 @@ + + + + The motion planner plugin which plans a particular path for RM exchanger. + + + diff --git a/auto_exchange_planner/include/auto_exchange_planner/auto_exchange_context.h b/auto_exchange_planner/include/auto_exchange_planner/auto_exchange_context.h new file mode 100644 index 0000000..9cfd159 --- /dev/null +++ b/auto_exchange_planner/include/auto_exchange_planner/auto_exchange_context.h @@ -0,0 +1,35 @@ +// +// Created by ch on 24-12-6. +// + +#pragma once + +#include +#include "auto_exchange_planner/auto_exchange_planner.h" + +namespace auto_exchange_planner +{ + MOVEIT_CLASS_FORWARD(AutoExchangeContext); + + class AutoExchangeContext : public planning_interface::PlanningContext + { + public: + AutoExchangeContext(const std::string& name, const std::string& ns, const std::string& group, + const moveit::core::RobotModelConstPtr& model); + ~AutoExchangeContext() override + { + } + + bool solve(planning_interface::MotionPlanResponse& res) override; + bool solve(planning_interface::MotionPlanDetailedResponse& res) override; + + bool terminate() override; + void clear() override; + + private: + moveit::core::RobotModelConstPtr robot_model_; + moveit::core::RobotStatePtr robot_state_; + AutoExchangePlannerPtr auto_exchange_planner_; + }; + +} // namespace auto_exchange_planner diff --git a/auto_exchange_planner/include/auto_exchange_planner/auto_exchange_planner.h b/auto_exchange_planner/include/auto_exchange_planner/auto_exchange_planner.h new file mode 100644 index 0000000..347af48 --- /dev/null +++ b/auto_exchange_planner/include/auto_exchange_planner/auto_exchange_planner.h @@ -0,0 +1,37 @@ +// +// Created by ch on 24-12-1. +// + +#pragma once + +#include +#include + +namespace auto_exchange_planner +{ + MOVEIT_CLASS_FORWARD(AutoExchangePlanner); + + class AutoExchangePlanner + { + public: + AutoExchangePlanner(const ros::NodeHandle& nh = ros::NodeHandle("~")); + + bool solve(const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req, moveit_msgs::MotionPlanDetailedResponse& res); + + protected: + ros::NodeHandle nh_; + std::string name_; + int num_steps_; + int dof_; + double trajectory2_length_; + + private: + void interpolate(const std::vector& joint_names, moveit::core::RobotStatePtr& robot_state, + const moveit::core::JointModelGroup* joint_model_group, const std::vector& start_joint_vals, + const std::vector& goal_joint_vals, trajectory_msgs::JointTrajectory& joint_trajectory); + void auto_exchange_interpolate(const std::vector& joint_names, moveit::core::RobotStatePtr& robot_state, + const moveit::core::JointModelGroup* joint_model_group, const geometry_msgs::Pose& start_pose, + const geometry_msgs::Pose& goal_pose, trajectory_msgs::JointTrajectory& joint_trajectory); + }; +} // namespace auto_exchange_planner diff --git a/auto_exchange_planner/package.xml b/auto_exchange_planner/package.xml new file mode 100644 index 0000000..4318caa --- /dev/null +++ b/auto_exchange_planner/package.xml @@ -0,0 +1,22 @@ + + + auto_exchange_planner + 0.0.0 + The auto_exchange_planner package + ch + BSD + catkin + pluginlib + moveit_core + roscpp + std_msgs + pluginlib + moveit_core + roscpp + std_msgs + + + + + + diff --git a/auto_exchange_planner/src/auto_exchange_context.cpp b/auto_exchange_planner/src/auto_exchange_context.cpp new file mode 100644 index 0000000..02b0350 --- /dev/null +++ b/auto_exchange_planner/src/auto_exchange_context.cpp @@ -0,0 +1,72 @@ +// +// Created by ch on 24-12-6. +// +#include +#include +#include +#include + +#include "auto_exchange_planner/auto_exchange_context.h" +#include "auto_exchange_planner/auto_exchange_planner.h" +namespace auto_exchange_planner +{ +AutoExchangeContext::AutoExchangeContext(const std::string& context_name, const std::string& ns, + const std::string& group_name, const moveit::core::RobotModelConstPtr& model) + : planning_interface::PlanningContext(context_name, group_name), robot_model_(model) +{ + auto_exchange_planner_ = AutoExchangePlannerPtr(new AutoExchangePlanner(ros::NodeHandle(ns))); +} + +bool AutoExchangeContext::solve(planning_interface::MotionPlanDetailedResponse& res) +{ + moveit_msgs::MotionPlanDetailedResponse res_msg; + bool auto_exchange_solved = auto_exchange_planner_->solve(planning_scene_, request_, res_msg); + + if (auto_exchange_solved) + { + res.trajectory_.resize(1); + res.trajectory_[0] = + robot_trajectory::RobotTrajectoryPtr(new robot_trajectory::RobotTrajectory(robot_model_, getGroupName())); + + moveit::core::RobotState start_state(robot_model_); + moveit::core::robotStateMsgToRobotState(res_msg.trajectory_start, start_state); + + res.trajectory_[0]->setRobotTrajectoryMsg(start_state, res_msg.trajectory[0]); + res.description_.push_back("plan"); + res.processing_time_ = res_msg.processing_time; + res.error_code_ = res_msg.error_code; + + return true; + } + + res.error_code_ = res_msg.error_code; + return false; +}; + +bool AutoExchangeContext::solve(planning_interface::MotionPlanResponse& res) +{ + planning_interface::MotionPlanDetailedResponse res_detailed; + bool planning_success = solve(res_detailed); + + res.error_code_ = res_detailed.error_code_; + + if (planning_success) + { + res.trajectory_ = res_detailed.trajectory_[0]; + res.planning_time_ = res_detailed.processing_time_[0]; + } + + return planning_success; +} + +bool AutoExchangeContext::terminate() +{ + return true; +} + +void AutoExchangeContext::clear() +{ + // This planner has no state, so has nothing to clear +} + +} // namespace auto_exchange_planner diff --git a/auto_exchange_planner/src/auto_exchange_manager.cpp b/auto_exchange_planner/src/auto_exchange_manager.cpp new file mode 100644 index 0000000..42e06b8 --- /dev/null +++ b/auto_exchange_planner/src/auto_exchange_manager.cpp @@ -0,0 +1,85 @@ +// +// Created by ch on 24-12-6. +// + +#include +#include +#include +#include +#include +#include "auto_exchange_planner/auto_exchange_context.h" + +namespace auto_exchange_planner +{ +class AutoExchangeManager : public planning_interface::PlannerManager +{ +public: + AutoExchangeManager() : planning_interface::PlannerManager() + { + } + + bool initialize(const moveit::core::RobotModelConstPtr& model, const std::string& ns) override + { + for (const std::string& gpName : model->getJointModelGroupNames()) + { + planning_contexts_[gpName] = + AutoExchangeContextPtr(new AutoExchangeContext("auto_exchange_context", ns, gpName, model)); + } + return true; + } + + bool canServiceRequest(const moveit_msgs::MotionPlanRequest& req) const override + { + return req.trajectory_constraints.constraints.empty(); + } + + std::string getDescription() const override + { + return "AutoExchange"; + } + + void getPlanningAlgorithms(std::vector& algs) const override + { + algs.clear(); + algs.push_back("auto_exchange"); + } + + planning_interface::PlanningContextPtr getPlanningContext(const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req, + moveit_msgs::MoveItErrorCodes& error_code) const override + { + error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS; + + if (req.group_name.empty()) + { + ROS_ERROR("No group specified to plan for"); + error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME; + return planning_interface::PlanningContextPtr(); + } + + if (!planning_scene) + { + ROS_ERROR("No planning scene supplied as input"); + error_code.val = moveit_msgs::MoveItErrorCodes::FAILURE; + return planning_interface::PlanningContextPtr(); + } + + // retrieve and configure existing context + const AutoExchangeContextPtr& context = planning_contexts_.at(req.group_name); + ROS_INFO_STREAM_NAMED("auto_exchange_manager", "===>>> context is made "); + + context->setPlanningScene(planning_scene); + context->setMotionPlanRequest(req); + + error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS; + return context; + } + +protected: + std::map planning_contexts_; +}; + +} // namespace auto_exchange_planner + +// register the AutoExchangeManager class as a plugin +CLASS_LOADER_REGISTER_CLASS(auto_exchange_planner::AutoExchangeManager, planning_interface::PlannerManager); diff --git a/auto_exchange_planner/src/auto_exchange_planner.cpp b/auto_exchange_planner/src/auto_exchange_planner.cpp new file mode 100644 index 0000000..6af5fde --- /dev/null +++ b/auto_exchange_planner/src/auto_exchange_planner.cpp @@ -0,0 +1,171 @@ +// +// Created by ch on 24-12-1. +// + +#include +#include +#include + +#include + +#include + +#include +#include +#include +#include + +#include "auto_exchange_planner/auto_exchange_planner.h" + +namespace auto_exchange_planner +{ + AutoExchangePlanner::AutoExchangePlanner(const ros::NodeHandle& nh) : nh_(nh),name_("AutoExchangePlanner"), num_steps_(1), dof_(0) + { + } + + bool AutoExchangePlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req, + moveit_msgs::MotionPlanDetailedResponse& res) + { + // Load the planner-specific parameters + nh_.param("num_steps", num_steps_, 50); + + ros::Time start_time = ros::Time::now(); + moveit::core::RobotModelConstPtr robot_model = planning_scene->getRobotModel(); + moveit::core::RobotStatePtr start_state(new moveit::core::RobotState(robot_model)), middle_state(new moveit::core::RobotState(robot_model)); + *start_state = planning_scene->getCurrentState(); + const moveit::core::JointModelGroup* joint_model_group = start_state->getJointModelGroup(req.group_name); + const moveit::core::JointModelGroup* mid_joint_model_group = middle_state->getJointModelGroup(req.group_name); + std::vector joint_names = joint_model_group->getVariableNames(); + dof_ = joint_names.size(); + std::vector start_joint_values; + start_state->copyJointGroupPositions(joint_model_group, start_joint_values); + trajectory_msgs::JointTrajectory joint_trajectory; + + if (req.goal_constraints.size() > 1) + { + geometry_msgs::Pose goal_pose_final; + goal_pose_final.position = req.goal_constraints[0].position_constraints[0].constraint_region.primitive_poses[0].position; + goal_pose_final.orientation = req.goal_constraints[0].orientation_constraints[0].orientation; + + geometry_msgs::Pose goal_pose_mid; + goal_pose_mid.position = req.goal_constraints[1].position_constraints[0].constraint_region.primitive_poses[0].position; + goal_pose_mid.orientation = req.goal_constraints[1].orientation_constraints[0].orientation; + + std::vector middle_joint_values; + bool found_mid_ik = middle_state->setFromIK(mid_joint_model_group, goal_pose_mid); + if (found_mid_ik) + { + middle_state->copyJointGroupPositions(mid_joint_model_group, middle_joint_values); + } + else + { + ROS_INFO("Did not find middle_state's IK solution for middle state!"); + return false; + } + + interpolate(joint_names, start_state, joint_model_group, start_joint_values, middle_joint_values, joint_trajectory); + auto_exchange_interpolate(joint_names, start_state, joint_model_group, goal_pose_mid, goal_pose_final, joint_trajectory); + + } + else + { + const std::vector& goal_constraints = req.goal_constraints; + const std::vector& goal_joint_constraint = goal_constraints[0].joint_constraints; + + std::vector goal_joint_values; + goal_joint_values.reserve(goal_joint_constraint.size()); + + for (const auto& constraint : goal_joint_constraint) + { + goal_joint_values.push_back(constraint.position); + } + + interpolate(joint_names, start_state, joint_model_group, start_joint_values, goal_joint_values, joint_trajectory); + + } + + res.trajectory.resize(1); + res.trajectory[0].joint_trajectory.joint_names = joint_names; + res.trajectory[0].joint_trajectory.header = req.start_state.joint_state.header; + res.trajectory[0].joint_trajectory = joint_trajectory; + + res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS; + res.processing_time.push_back((ros::Time::now() - start_time).toSec()); + + res.group_name = req.group_name; + res.trajectory_start.joint_state.name = joint_names; + res.trajectory_start.joint_state.position = start_joint_values; + + return true; + } + + void AutoExchangePlanner::interpolate(const std::vector& joint_names, moveit::core::RobotStatePtr& rob_state, + const moveit::core::JointModelGroup* joint_model_group, + const std::vector& start_joint_vals, const std::vector& goal_joint_vals, + trajectory_msgs::JointTrajectory& joint_trajectory) + { + std::vector dt_vector, m_start_joint_vals; + for (int joint_index = 0; joint_index < dof_; ++joint_index) + { + double dt = (goal_joint_vals[joint_index] - start_joint_vals[joint_index]) / num_steps_; + dt_vector.push_back(dt); + } + + for (double start_joint_val : start_joint_vals) + m_start_joint_vals.push_back(start_joint_val + 0.001 ); + + for (int step = 0; step <= num_steps_; ++step) + { + std::vector joint_values; + for (int k = 0; k < dof_; ++k) + { + double joint_value = m_start_joint_vals[k] + step * dt_vector[k]; + joint_values.push_back(joint_value); + } + rob_state->setJointGroupPositions(joint_model_group, joint_values); + rob_state->update(); + + joint_trajectory.joint_names = joint_names; + trajectory_msgs::JointTrajectoryPoint trajectory_point; + trajectory_point.positions = joint_values; + joint_trajectory.points.push_back(trajectory_point); + } + } + + void AutoExchangePlanner::auto_exchange_interpolate(const std::vector& joint_names, moveit::core::RobotStatePtr& rob_state, + const moveit::core::JointModelGroup* joint_model_group, const geometry_msgs::Pose& start_pose, + const geometry_msgs::Pose& goal_pose, trajectory_msgs::JointTrajectory& joint_trajectory) + { + double dt_x = (goal_pose.position.x - start_pose.position.x)/num_steps_, + dt_y = (goal_pose.position.y - start_pose.position.y)/num_steps_, + dt_z = (goal_pose.position.z - start_pose.position.z)/num_steps_; + + for (int step = 0; step <= num_steps_; ++step) + { + std::vector joint_values; + geometry_msgs::Pose waypoint_pose; + waypoint_pose.orientation = start_pose.orientation; + waypoint_pose.position.x = start_pose.position.x + dt_x * step; + waypoint_pose.position.y = start_pose.position.y + dt_y * step; + waypoint_pose.position.z = start_pose.position.z + dt_z * step; + // ROS_INFO("waypoint%d x:%f y:%f z:%f",step,waypoint_pose.position.x,waypoint_pose.position.y,waypoint_pose.position.z); + bool found_waypoint_ik = rob_state->setFromIK(joint_model_group, waypoint_pose); + if (found_waypoint_ik) + { + rob_state->copyJointGroupPositions(joint_model_group, joint_values); + rob_state->update(); + + joint_trajectory.joint_names = joint_names; + trajectory_msgs::JointTrajectoryPoint trajectory_point; + trajectory_point.positions = joint_values; + joint_trajectory.points.push_back(trajectory_point); + } + else + { + ROS_INFO("Did not find IK solution in step %d !", step); + } + } + } + +} // namespace auto_exchange_planner \ No newline at end of file diff --git a/dist_sensor/CMakeLists.txt b/dist_sensor/CMakeLists.txt new file mode 100644 index 0000000..56642f2 --- /dev/null +++ b/dist_sensor/CMakeLists.txt @@ -0,0 +1,107 @@ +cmake_minimum_required(VERSION 3.0.2) +project(dist_sensor) + +## Use C++14 +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +## By adding -Wall and -Werror, the compiler does not ignore warnings anymore, +## enforcing cleaner code. +add_definitions(-Wall -Werror -Wno-address-of-packed-member) + +## Find catkin macros and libraries +find_package(serial REQUIRED) + +find_package(catkin REQUIRED + COMPONENTS + roscpp + rm_msgs + serial + rm_common +) + +## Find system libraries +#find_package(Eigen3 REQUIRED) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + INCLUDE_DIRS + include + LIBRARIES + CATKIN_DEPENDS + roscpp + rm_msgs + rm_common + DEPENDS +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +## Declare cpp executables +FILE(GLOB ALL_SOURCES "src/*.cpp" "src/common/*.cpp" ) +add_executable(${PROJECT_NAME} ${ALL_SOURCES}) + +## Add dependencies to exported targets, like ROS msgs or srvs +add_dependencies(${PROJECT_NAME} + ${catkin_EXPORTED_TARGETS} +) + +## Specify libraries to link executable targets against +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} +) + +############# +## Install ## +############# + +# Mark executables and/or libraries for installation +install( + TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +# Mark cpp header files for installation +install( + DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" +) + +## Mark other files for installation +install( + DIRECTORY launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +############# +## Testing ## +############# + +#if (${CATKIN_ENABLE_TESTING}) +# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread") +# ## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test +# test/test_ros_package_template.cpp +# test/AlgorithmTest.cpp) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}_core) +#endif () diff --git a/dist_sensor/config/dist_sensor_config.yaml b/dist_sensor/config/dist_sensor_config.yaml new file mode 100644 index 0000000..3032ce1 --- /dev/null +++ b/dist_sensor/config/dist_sensor_config.yaml @@ -0,0 +1,3 @@ +sensor_interval: 0.44 +dist_lower: 0.1 +dist_upper: 2.5 \ No newline at end of file diff --git a/dist_sensor/include/dist_sensor/common/data.h b/dist_sensor/include/dist_sensor/common/data.h new file mode 100644 index 0000000..f2c7404 --- /dev/null +++ b/dist_sensor/include/dist_sensor/common/data.h @@ -0,0 +1,119 @@ +// +// Created by ch on 25-6-4. +// + +#pragma once + +#include +#include +#include + +namespace dist_sensor +{ +class Base +{ +public: + serial::Serial serial_; + bool sensor_is_online_ = false; + + void initSerial(const std::string &port) + { + serial::Timeout timeout = serial::Timeout::simpleTimeout(50); + serial_.setPort(port); + serial_.setBaudrate(115200); + serial_.setTimeout(timeout); + if (serial_.isOpen()) + return; + try + { + serial_.open(); + } + catch (serial::IOException& e) + { + ROS_ERROR("Cannot open port %s", port.c_str()); + } + } + + // CheckSum + uint8_t getChecksum(unsigned char* pch_message, unsigned int dw_length) + { + uint8_t checksum = 0; + while (dw_length--) + { + checksum += *pch_message++; + } + return checksum; + } + + uint32_t verifyCheckSum(unsigned char* pch_message, unsigned int dw_length) + { + unsigned char expected_checksum; + if ((pch_message == nullptr) || (dw_length <= 2)) + return 0; + expected_checksum = getChecksum(pch_message, dw_length - 1); + return (expected_checksum == pch_message[dw_length - 1]); + } + + void appendCheckSum(unsigned char* pch_message, unsigned int dw_length) + { + unsigned char checksum; + if ((pch_message == nullptr) || (dw_length <= 2)) + return; + checksum = getChecksum(pch_message, dw_length - 1); + pch_message[dw_length - 1] = checksum; + } +}; + +class SensorData +{ +public: + explicit SensorData(ros::NodeHandle& nh, const std::string& sensor_name, const std::string& port) + : dist_lower_(nh.param("dist_lower", 0.2)), + dist_upper_(nh.param("dist_upper", 2.0)) + { + base_.initSerial(port); + publisher_ = nh.advertise(sensor_name, 1); + } + void setDistance(const double distance) + { + if (getAmp() >= 200 && getAmp() <= 60000 && distance >= dist_lower_ && distance <= dist_upper_) + { + dist_sensor_data_.distance = distance; + error_ = false; + } + else + error_ = true; + } + void setAmp(const double amp) + { + dist_sensor_data_.amp = amp; + } + void setTemperature(const double temperature) + { + dist_sensor_data_.temperature = temperature; + } + void publish() + { + publisher_.publish(dist_sensor_data_); + } + double getDistance() + { + return dist_sensor_data_.distance; + } + double getAmp() + { + return dist_sensor_data_.amp; + } + bool getError() + { + return error_; + } + Base base_; + uint8_t unpack_buffer_[256]{}; +private: + double dist_lower_, dist_upper_; + ros::Publisher publisher_; + rm_msgs::EngineerDistSensor dist_sensor_data_; + bool error_ = true; +}; +} // namespace dist_sensor diff --git a/dist_sensor/include/dist_sensor/common/protocol.h b/dist_sensor/include/dist_sensor/common/protocol.h new file mode 100644 index 0000000..75d25be --- /dev/null +++ b/dist_sensor/include/dist_sensor/common/protocol.h @@ -0,0 +1,17 @@ +// +// Created by ch on 25-6-4. +// + +#pragma once +#define __packed __attribute__((packed)) + +#include +namespace dist_sensor +{ +typedef struct +{ + uint8_t dist[2]; + uint8_t amp[2]; + uint8_t temp[2]; +} __packed DistanceSensorData; +} // namespace dist_sensor \ No newline at end of file diff --git a/dist_sensor/include/dist_sensor/dist_sensor.h b/dist_sensor/include/dist_sensor/dist_sensor.h new file mode 100644 index 0000000..ad9ccfd --- /dev/null +++ b/dist_sensor/include/dist_sensor/dist_sensor.h @@ -0,0 +1,57 @@ +// +// Created by ch on 25-6-4. +// + +#pragma once + +#include +#include +#include +#include + +#include "dist_sensor/common/data.h" +#include "dist_sensor/common/protocol.h" + +namespace dist_sensor +{ + class DistSensor + { + public: + explicit DistSensor(ros::NodeHandle& nh) : + sensor_l_(nh, "dist_sensor_l", "/dev/usbDistSensorLeft"), + sensor_r_(nh, "dist_sensor_r", "/dev/usbDistSensorRight"), + last_get_data_time_(ros::Time::now()) + { + sensor_interval_ = nh.param("sensor_interval", 0.6); + yaw_angle_pub_ = nh.advertise("yaw_angle",1); + ROS_INFO("Dist sensor load."); + } + + void read(SensorData& sensor); + void clearRxBuffer() + { + rx_buffer_.clear(); + rx_len_ = 0; + } + void calculateYawAngle() + { + std_msgs::Float64 yaw_angle; + if (sensor_l_.getError() || sensor_r_.getError()) + yaw_angle.data = 0; + else + yaw_angle.data = atan((sensor_r_.getDistance() - sensor_l_.getDistance()) / sensor_interval_); + yaw_angle_pub_.publish(yaw_angle); + } + SensorData sensor_l_, sensor_r_; + std::vector rx_buffer_; + int rx_len_; + + private: + int unpack(uint8_t* rx_data, SensorData& sensor); + ros::Time last_get_data_time_; + ros::Publisher yaw_angle_pub_; + double sensor_interval_; + const int k_frame_length_ = 128, k_header_length_ = 2, k_data_length_ = 6, k_tail_length_ = 1; + const int k_unpack_buffer_length_ = 256; + }; +} // namespace dist_sensor diff --git a/dist_sensor/launch/load.launch b/dist_sensor/launch/load.launch new file mode 100644 index 0000000..ec2df19 --- /dev/null +++ b/dist_sensor/launch/load.launch @@ -0,0 +1,4 @@ + + + + diff --git a/dist_sensor/package.xml b/dist_sensor/package.xml new file mode 100644 index 0000000..b0ae83b --- /dev/null +++ b/dist_sensor/package.xml @@ -0,0 +1,16 @@ + + + dist_sensor + 0.0.0 + The dist_sensor package + + BSD + ch + + catkin + + roscpp + rm_msgs + rm_common + serial + diff --git a/dist_sensor/src/dist_sensor.cpp b/dist_sensor/src/dist_sensor.cpp new file mode 100644 index 0000000..26585d7 --- /dev/null +++ b/dist_sensor/src/dist_sensor.cpp @@ -0,0 +1,61 @@ +// +// Created by ch on 25-6-4. +// + +#include "dist_sensor/dist_sensor.h" + +namespace dist_sensor +{ +void DistSensor::read(SensorData& sensor) +{ + if (sensor.base_.serial_.available()) + { + rx_len_ = static_cast(sensor.base_.serial_.available()); + sensor.base_.serial_.read(rx_buffer_, rx_len_); + } + else + return; + uint8_t temp_buffer[256] = { 0 }; + if (ros::Time::now() - last_get_data_time_ > ros::Duration(0.1)) + sensor.base_.sensor_is_online_ = false; + if (rx_len_ < k_unpack_buffer_length_) + { + for (int k_i = 0; k_i < k_unpack_buffer_length_ - rx_len_; ++k_i) + temp_buffer[k_i] = sensor.unpack_buffer_[k_i + rx_len_]; + for (int k_i = 0; k_i < rx_len_; ++k_i) + temp_buffer[k_i + k_unpack_buffer_length_ - rx_len_] = rx_buffer_[k_i]; + for (int k_i = 0; k_i < k_unpack_buffer_length_; ++k_i) + sensor.unpack_buffer_[k_i] = temp_buffer[k_i]; + } + for (int k_i = 0; k_i < k_unpack_buffer_length_ - k_frame_length_; ++k_i) + { + if (sensor.unpack_buffer_[k_i] == 0x59 && sensor.unpack_buffer_[k_i + 1] == 0x59) + { + int frame_len = unpack(&sensor.unpack_buffer_[k_i], sensor); + if (frame_len != -1) + k_i += frame_len; + } + } + clearRxBuffer(); +} + +int DistSensor::unpack(uint8_t* rx_data, SensorData& sensor) +{ + int frame_len = k_header_length_ + k_data_length_ + k_tail_length_; + if (sensor.base_.verifyCheckSum(rx_data, frame_len) == 1) + { + dist_sensor::DistanceSensorData dist_sensor_data_ref; + memcpy(&dist_sensor_data_ref, rx_data + 2, sizeof(dist_sensor::DistanceSensorData)); + sensor.setDistance((dist_sensor_data_ref.dist[1] << 8 | dist_sensor_data_ref.dist[0]) / 1000.0); + sensor.setAmp(dist_sensor_data_ref.amp[1] << 8 | dist_sensor_data_ref.amp[0]); + sensor.setTemperature((dist_sensor_data_ref.temp[1] << 8 | dist_sensor_data_ref.temp[0]) / 8.0 - 256); + sensor.publish(); + + sensor.base_.sensor_is_online_ = true; + last_get_data_time_ = ros::Time::now(); + return frame_len; + } + return -1; +} + +} // namespace dist_sensor \ No newline at end of file diff --git a/dist_sensor/src/main.cpp b/dist_sensor/src/main.cpp new file mode 100644 index 0000000..10066b3 --- /dev/null +++ b/dist_sensor/src/main.cpp @@ -0,0 +1,21 @@ +// +// Created by ch on 25-6-4. +// + +#include "dist_sensor/dist_sensor.h" + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "dist_sensor"); + ros::NodeHandle nh("~"); + dist_sensor::DistSensor dist_sensor(nh); + ros::Rate loop_rate(100); + while (ros::ok()) + { + ros::spinOnce(); + dist_sensor.read(dist_sensor.sensor_l_); + dist_sensor.read(dist_sensor.sensor_r_); + dist_sensor.calculateYawAngle(); + loop_rate.sleep(); + } +} diff --git a/engineer2_arm_config/.setup_assistant b/engineer2_arm_config/.setup_assistant index 7ab7a69..9c88099 100644 --- a/engineer2_arm_config/.setup_assistant +++ b/engineer2_arm_config/.setup_assistant @@ -6,6 +6,6 @@ moveit_setup_assistant_config: SRDF: relative_path: config/engineer2.srdf CONFIG: - author_name: Chonghong Cai - author_email: 792166012@qq.com - generated_timestamp: 1720344342 + author_name: ch + author_email: 3631676002@qq.com + generated_timestamp: 1775277360 \ No newline at end of file diff --git a/engineer2_arm_config/config/auto_exchange_planning.yaml b/engineer2_arm_config/config/auto_exchange_planning.yaml new file mode 100644 index 0000000..d82580e --- /dev/null +++ b/engineer2_arm_config/config/auto_exchange_planning.yaml @@ -0,0 +1 @@ +num_steps: 40 \ No newline at end of file diff --git a/engineer2_arm_config/config/engineer2.srdf b/engineer2_arm_config/config/engineer2.srdf index 12140bf..3de3820 100644 --- a/engineer2_arm_config/config/engineer2.srdf +++ b/engineer2_arm_config/config/engineer2.srdf @@ -10,27 +10,119 @@ - + + + + + + + + + + - + - - + + + + + - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/engineer2_arm_config/config/engineer2_moveit_controllers.yaml b/engineer2_arm_config/config/engineer2_moveit_controllers.yaml new file mode 100644 index 0000000..c4b157e --- /dev/null +++ b/engineer2_arm_config/config/engineer2_moveit_controllers.yaml @@ -0,0 +1,13 @@ +controller_list: + - name: controllers/gravity_compensation_controller + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: True + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 + - joint7 \ No newline at end of file diff --git a/engineer2_arm_config/config/fake_controllers.yaml b/engineer2_arm_config/config/fake_controllers.yaml index 42c4827..5debc52 100644 --- a/engineer2_arm_config/config/fake_controllers.yaml +++ b/engineer2_arm_config/config/fake_controllers.yaml @@ -1,5 +1,5 @@ controller_list: - - name: arm_trajectory_controller + - name: fake_engineer_arm_controller type: $(arg fake_execution_type) joints: - joint1 @@ -8,6 +8,11 @@ controller_list: - joint4 - joint5 - joint6 + - joint7 + - name: fake_gripper_controller + type: $(arg fake_execution_type) + joints: + - gripper_joint initial: # Define initial robot poses per group - group: engineer_arm - pose: home + pose: home \ No newline at end of file diff --git a/engineer2_arm_config/config/gazebo_engineer2.urdf b/engineer2_arm_config/config/gazebo_engineer2.urdf index e04abdc..9d5e13a 100644 --- a/engineer2_arm_config/config/gazebo_engineer2.urdf +++ b/engineer2_arm_config/config/gazebo_engineer2.urdf @@ -1,14 +1,14 @@ - + - - - + + + @@ -33,7 +33,13 @@ + + + + + + @@ -46,2583 +52,691 @@ - - - - + + - + - + - - + + - - - - - - - - - - - - - - Gazebo/Black - - / - - - - - - - + - - - - - - - - - - - + + + + + + + + + - - - - - - Gazebo/Black - - - - - + + - + - + - - + + - - - - - - - - - - - - - - Gazebo/Black - - - - - - + - - - - - - - - - - - + + + + + + + + + - - - - - - Gazebo/Black - - - - - + + - + - + - - + + - - - - - - - - - - - - - - Gazebo/Black - - - - - - + - - - - - - - - - - - + + + + + + + + + - - - - - - Gazebo/Black - + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + 19.2032 + + + hardware_interface/EffortJointInterface + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + -19.2032 + + + hardware_interface/EffortJointInterface + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + 19.2032 + + + hardware_interface/EffortJointInterface + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + -19.2032 + + + hardware_interface/EffortJointInterface + + - - - - + + + - + - + - - - + + + - - - - - - - - - - - - - - Gazebo/Black - - - - - - + - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - + + + - + - + - - - + + + - - - - - - - - - - - - - - Gazebo/Black - - - - - - + - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - + + + - + - + - - - + + + - - - - - - - - - - - - - - Gazebo/Black - - - - - + - + - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - + + + - + - + - - - + + + - - - - - - - - - - - - - - Gazebo/Black - - - - - + - + - - - - - - - - - - - - - - - - - - Gazebo/Black - - - + + - + - - + + + - - - - - - - - - - - - - - - + - + - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - + + + - + - + - - - + + + - - - - - - - - - - - - - - Gazebo/Black - - - - - + - + - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - + + + - + - + - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - + + + - - - - - - - - - - - - - - Gazebo/Black - - - - - + - + - - - - - - - - - - - - - - - - - - Gazebo/Black - - - + + - + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - + + + - - - - - - - - - - - - - - Gazebo/Black - - - - - + - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - - - - - - - - - - - - - - - - - - - - - - - - - - - Gazebo/Black - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - 19.2032 - - - hardware_interface/EffortJointInterface - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - -19.2032 - - - hardware_interface/EffortJointInterface - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - 19.2032 - - - hardware_interface/EffortJointInterface - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - -19.2032 - - - hardware_interface/EffortJointInterface - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - transmission_interface/SimpleTransmission - - 421.49 - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 0. - - - - transmission_interface/SimpleTransmission - - 1 - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 0. - - - - transmission_interface/SimpleTransmission - - 1 - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 0 - - - - transmission_interface/SimpleTransmission - - 1 - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 0 - - - - transmission_interface/DifferentialTransmission - - actuator1 - 1 - - - actuator2 - 1 - - - hardware_interface/EffortJointInterface - 0 - joint1 - - - hardware_interface/EffortJointInterface - 0 - joint2 - - - - - - - - - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - + + 492.488 + hardware_interface/EffortJointInterface - 1 - - - transmission_interface/SimpleTransmission - + hardware_interface/EffortJointInterface + 0.552 - - hardware_interface/EffortJointInterface - 1 - - + transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface + 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - hardware_interface/EffortJointInterface - 1 - - - transmission_interface/SimpleTransmission - + hardware_interface/EffortJointInterface + -0.588 - - hardware_interface/EffortJointInterface - 1 - - + transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - + + -1 hardware_interface/EffortJointInterface - 1 - - - transmission_interface/SimpleTransmission - + hardware_interface/EffortJointInterface + 0.514 - - hardware_interface/EffortJointInterface - 1 - - + transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface + 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - hardware_interface/EffortJointInterface - 1 - - - transmission_interface/SimpleTransmission - + hardware_interface/EffortJointInterface + -0.539 - - hardware_interface/EffortJointInterface - 1 - - + transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - + + -1 hardware_interface/EffortJointInterface - 1 - - - transmission_interface/SimpleTransmission - + hardware_interface/EffortJointInterface + 0.323 - - hardware_interface/EffortJointInterface - 1 - - + transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface + 1 - - - - transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - hardware_interface/EffortJointInterface - 1 - - - transmission_interface/SimpleTransmission - + hardware_interface/EffortJointInterface + -0.036 - - hardware_interface/EffortJointInterface - 1 - - + transmission_interface/SimpleTransmission - + + -1 hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - transmission_interface/SimpleTransmission - + hardware_interface/EffortJointInterface + 0.419 - - hardware_interface/EffortJointInterface - 1 - - + transmission_interface/SimpleTransmission - + + -1 hardware_interface/EffortJointInterface - - - hardware_interface/EffortJointInterface - 1 - - - transmission_interface/SimpleTransmission - + hardware_interface/EffortJointInterface + 0.0 - - hardware_interface/EffortJointInterface - 1 - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - + + 984.976 hardware_interface/EffortJointInterface - 1 - - - transmission_interface/SimpleTransmission - + hardware_interface/EffortJointInterface + 0.0 - - hardware_interface/EffortJointInterface - 1 - - + transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - + + -984.976 hardware_interface/EffortJointInterface - 1 - - - transmission_interface/SimpleTransmission - + hardware_interface/EffortJointInterface + 0.0 - - hardware_interface/EffortJointInterface - 1 - - + transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - + + 36 hardware_interface/EffortJointInterface - 1 - - - transmission_interface/SimpleTransmission - + hardware_interface/EffortJointInterface + 0.0 - - hardware_interface/EffortJointInterface - 1 - - + transmission_interface/SimpleTransmission - - hardware_interface/EffortJointInterface - - + + -36 hardware_interface/EffortJointInterface - 1 - - - transmission_interface/SimpleTransmission - + hardware_interface/EffortJointInterface + 0.0 - - hardware_interface/EffortJointInterface - 1 - + + + + + + + + + / + + + diff --git a/engineer2_arm_config/config/joint_limits.yaml b/engineer2_arm_config/config/joint_limits.yaml index 7031779..52d5010 100644 --- a/engineer2_arm_config/config/joint_limits.yaml +++ b/engineer2_arm_config/config/joint_limits.yaml @@ -2,39 +2,49 @@ # For beginners, we downscale velocity and acceleration limits. # You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed. -default_velocity_scaling_factor: 0.1 -default_acceleration_scaling_factor: 0.1 +default_velocity_scaling_factor: 0.5 +default_acceleration_scaling_factor: 0.4 # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] joint_limits: + gripper_joint: + has_velocity_limits: true + max_velocity: 20.94 + has_acceleration_limits: true + max_acceleration: 10 joint1: has_velocity_limits: true - max_velocity: 1.25 + max_velocity: 1.929 has_acceleration_limits: true - max_acceleration: 1 + max_acceleration: 1.5 joint2: has_velocity_limits: true - max_velocity: 10.46 + max_velocity: 3.76 has_acceleration_limits: true max_acceleration: 10 joint3: has_velocity_limits: true - max_velocity: 5.44 + max_velocity: 10.46 has_acceleration_limits: true - max_acceleration: 15 + max_acceleration: 10 joint4: has_velocity_limits: true - max_velocity: 20.94 + max_velocity: 3.76 has_acceleration_limits: true - max_acceleration: 15 + max_acceleration: 10 joint5: has_velocity_limits: true - max_velocity: 10 + max_velocity: 20.94 has_acceleration_limits: true - max_acceleration: 15 + max_acceleration: 10 joint6: has_velocity_limits: true - max_velocity: 10 + max_velocity: 20.94 + has_acceleration_limits: true + max_acceleration: 10 + joint7: + has_velocity_limits: true + max_velocity: 20.94 has_acceleration_limits: true - max_acceleration: 15 + max_acceleration: 10 \ No newline at end of file diff --git a/engineer2_arm_config/config/kinematics.yaml b/engineer2_arm_config/config/kinematics.yaml index c50afb8..7d6dacf 100644 --- a/engineer2_arm_config/config/kinematics.yaml +++ b/engineer2_arm_config/config/kinematics.yaml @@ -2,3 +2,6 @@ engineer_arm: kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin kinematics_solver_search_resolution: 0.005 kinematics_solver_timeout: 0.005 + goal_joint_tolerance: 0.001 + goal_position_tolerance: 0.001 + goal_orientation_tolerance: 0.01 \ No newline at end of file diff --git a/engineer2_arm_config/config/ompl_planning.yaml b/engineer2_arm_config/config/ompl_planning.yaml index 205b9ad..1dd9651 100644 --- a/engineer2_arm_config/config/ompl_planning.yaml +++ b/engineer2_arm_config/config/ompl_planning.yaml @@ -195,4 +195,38 @@ engineer_arm: - ABITstar - BITstar projection_evaluator: joints(joint1,joint2) - longest_valid_segment_fraction: 0.005 + longest_valid_segment_fraction: 0.01 +gripper: + planner_configs: + - AnytimePathShortening + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo + - AITstar + - ABITstar + - BITstar +special_joint_constraint: + enabled: true + joint_a: joint4 + joint_b: joint3 + constant: -0.27 \ No newline at end of file diff --git a/engineer2_arm_config/config/ros_controllers.yaml b/engineer2_arm_config/config/ros_controllers.yaml index a4837dc..fda1626 100644 --- a/engineer2_arm_config/config/ros_controllers.yaml +++ b/engineer2_arm_config/config/ros_controllers.yaml @@ -1,13 +1,5 @@ -# Simulation settings for using moveit_sim_controllers -moveit_sim_hw_interface: - joint_model_group: engineer_arm - joint_model_group_pose: home -# Settings for ros_control_boilerplate control loop -generic_hw_control_loop: - loop_hz: 300 - cycle_time_error_threshold: 0.01 -# Settings for ros_control hardware interface -hardware_interface: +engineer_arm_controller: + type: effort_controllers/JointTrajectoryController joints: - joint1 - joint2 @@ -15,17 +7,50 @@ hardware_interface: - joint4 - joint5 - joint6 - sim_control_mode: 1 # 0: position, 1: velocity - -controller_list: - - name: controllers/arm_trajectory_controller - action_ns: follow_joint_trajectory - default: True - type: FollowJointTrajectory - joints: - - joint1 - - joint2 - - joint3 - - joint4 - - joint5 - - joint6 + - joint7 + constraints: + goal_time: 5.0 + stopped_velocity_tolerance: 0.05 + joint1: { trajectory: 0.08, goal: 0.05 } + joint2: { trajectory: 0.08, goal: 0.05 } + joint3: { trajectory: 0.08, goal: 0.05 } + joint4: { trajectory: 0.08, goal: 0.05 } + joint5: { trajectory: 0.08, goal: 0.05 } + joint6: { trajectory: 0.08, goal: 0.05 } + joint7: { trajectory: 0.08, goal: 0.05 } + gains: + joint1: + p: 16000 + d: 500 + i: 100 + i_clamp: 10 + joint2: + p: 100 + d: 10 + i: 5 + i_clamp: 5 + joint3: + p: 1000 + d: 20 + i: 100 + i_clamp: 10 + joint4: + p: 500 + d: 5 + i: 1 + i_clamp: 1 + joint5: + p: 200 + d: 2 + i: 1 + i_clamp: 1 + joint6: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + joint7: + p: 100 + d: 1 + i: 1 + i_clamp: 1 \ No newline at end of file diff --git a/engineer2_arm_config/config/sensors_3d.yaml b/engineer2_arm_config/config/sensors_3d.yaml index b29057e..51010a3 100644 --- a/engineer2_arm_config/config/sensors_3d.yaml +++ b/engineer2_arm_config/config/sensors_3d.yaml @@ -1 +1,2 @@ sensors: + [] \ No newline at end of file diff --git a/engineer2_arm_config/config/servo.yaml b/engineer2_arm_config/config/servo.yaml index 76270c2..e0c53fe 100644 --- a/engineer2_arm_config/config/servo.yaml +++ b/engineer2_arm_config/config/servo.yaml @@ -8,10 +8,10 @@ command_in_type: "speed_units" # "unitless"> in the range [-1:1], as if from joy scale: # Scale parameters are only used if command_in_type=="unitless" linear: 0.4 # Max linear velocity. Meters per publish_period. Unit is [m/s]. Only used for Cartesian commands. - rotational: 0.2 # Max angular velocity. Rads per publish_period. Unit is [rad/s]. Only used for Cartesian commands. + rotational: 1.5 # Max angular velocity. Rads per publish_period. Unit is [rad/s]. Only used for Cartesian commands. # Max joint angular/linear velocity. Rads or Meters per publish period. Only used for joint commands on joint_command_in_topic. joint: 0.005 -low_pass_filter_coeff: 1. # Larger --> trust the filtered data more, trust the measurements less. +low_pass_filter_coeff: 2. # Larger --> trust the filtered data more, trust the measurements less. ## Properties of outgoing commands publish_period: 0.5 # 1/Nominal publish rate [seconds] @@ -25,15 +25,15 @@ command_out_type: trajectory_msgs/JointTrajectory # What to publish? Can save some bandwidth as most robots only require positions or velocities publish_joint_positions: true publish_joint_velocities: true -publish_joint_accelerations: false +publish_joint_accelerations: true ## MoveIt properties move_group_name: engineer_arm # Often 'manipulator' or 'arm' planning_frame: base_link # The MoveIt planning frame. Often 'base_link' or 'world' ## Other frames -ee_frame_name: link6 # The name of the end effector link, used to return the EE pose -robot_link_command_frame: link6 # commands must be given in the frame of a robot link. Usually either the base or end effector +ee_frame_name: link7 # The name of the end effector link, used to return the EE pose +robot_link_command_frame: link7 # commands must be given in the frame of a robot link. Usually either the base or end effector ## Stopping behaviour incoming_command_timeout: 0.1 # Stop servoing if X seconds elapse without a new command @@ -44,7 +44,7 @@ num_outgoing_halt_msgs_to_publish: 4 ## Configure handling of singularities and joint limits lower_singularity_threshold: 17 # Start decelerating when the condition number hits this (close to singularity) hard_stop_singularity_threshold: 30 # Stop when the condition number hits this -joint_limit_margin: 0.001 # added as a buffer to joint limits [radians]. If moving quickly, make this larger. +joint_limit_margin: 0.005 # added as a buffer to joint limits [radians]. If moving quickly, make this larger. ## Topic names cartesian_command_in_topic: delta_twist_cmds # Topic for incoming Cartesian twist commands @@ -54,7 +54,7 @@ status_topic: status # Publish status to this topic command_out_topic: /controllers/arm_trajectory_controller/command # Publish outgoing commands here ## Collision checking for the entire robot body -check_collisions: false # Check collisions? +check_collisions: true # Check collisions? collision_check_rate: 50 # [Hz] Collision-checking can easily bog down a CPU if done too often. # Two collision check algorithms are available: # "threshold_distance" begins slowing down when nearer than a specified distance. Good if you want to tune collision thresholds manually. diff --git a/engineer2_arm_config/config/simple_moveit_controllers.yaml b/engineer2_arm_config/config/simple_moveit_controllers.yaml index 1da75d2..a4ca2dc 100644 --- a/engineer2_arm_config/config/simple_moveit_controllers.yaml +++ b/engineer2_arm_config/config/simple_moveit_controllers.yaml @@ -1,5 +1,5 @@ controller_list: - - name: arm_trajectory_controller + - name: engineer_arm_controller action_ns: follow_joint_trajectory type: FollowJointTrajectory default: True @@ -10,3 +10,4 @@ controller_list: - joint4 - joint5 - joint6 + - joint7 \ No newline at end of file diff --git a/engineer2_arm_config/config/stomp_planning.yaml b/engineer2_arm_config/config/stomp_planning.yaml index d0e6815..71ef836 100644 --- a/engineer2_arm_config/config/stomp_planning.yaml +++ b/engineer2_arm_config/config/stomp_planning.yaml @@ -11,7 +11,7 @@ stomp/engineer_arm: task: noise_generator: - class: stomp_moveit/NormalDistributionSampling - stddev: [0.05, 0.05, 0.05, 0.05, 0.05, 0.05] + stddev: [0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05] cost_functions: - class: stomp_moveit/CollisionCheck collision_penalty: 1.0 @@ -37,3 +37,42 @@ stomp/engineer_arm: publish_intermediate: True marker_topic: stomp_trajectory marker_namespace: optimized +stomp/gripper: + group_name: gripper + optimization: + num_timesteps: 60 + num_iterations: 40 + num_iterations_after_valid: 0 + num_rollouts: 30 + max_rollouts: 30 + initialization_method: 1 # [1 : LINEAR_INTERPOLATION, 2 : CUBIC_POLYNOMIAL, 3 : MININUM_CONTROL_COST] + control_cost_weight: 0.0 + task: + noise_generator: + - class: stomp_moveit/NormalDistributionSampling + stddev: [0.05] + cost_functions: + - class: stomp_moveit/CollisionCheck + collision_penalty: 1.0 + cost_weight: 1.0 + kernel_window_percentage: 0.2 + longest_valid_joint_move: 0.05 + noisy_filters: + - class: stomp_moveit/JointLimits + lock_start: True + lock_goal: True + - class: stomp_moveit/MultiTrajectoryVisualization + line_width: 0.02 + rgb: [255, 255, 0] + marker_array_topic: stomp_trajectories + marker_namespace: noisy + update_filters: + - class: stomp_moveit/PolynomialSmoother + poly_order: 6 + - class: stomp_moveit/TrajectoryVisualization + line_width: 0.05 + rgb: [0, 191, 255] + error_rgb: [255, 0, 0] + publish_intermediate: True + marker_topic: stomp_trajectory + marker_namespace: optimized \ No newline at end of file diff --git a/engineer2_arm_config/launch/auto_exchange_planning_pipeline.launch.xml b/engineer2_arm_config/launch/auto_exchange_planning_pipeline.launch.xml new file mode 100644 index 0000000..eba37b8 --- /dev/null +++ b/engineer2_arm_config/launch/auto_exchange_planning_pipeline.launch.xml @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + diff --git a/engineer2_arm_config/launch/default_warehouse_db.launch b/engineer2_arm_config/launch/default_warehouse_db.launch new file mode 100644 index 0000000..2188279 --- /dev/null +++ b/engineer2_arm_config/launch/default_warehouse_db.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/engineer2_arm_config/launch/demo.launch b/engineer2_arm_config/launch/demo.launch new file mode 100644 index 0000000..123a14c --- /dev/null +++ b/engineer2_arm_config/launch/demo.launch @@ -0,0 +1,66 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + [move_group/fake_controller_joint_states] + + + + [move_group/fake_controller_joint_states] + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/engineer2_arm_config/launch/demo_gazebo.launch b/engineer2_arm_config/launch/demo_gazebo.launch new file mode 100644 index 0000000..0ef8f95 --- /dev/null +++ b/engineer2_arm_config/launch/demo_gazebo.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/engineer2_arm_config/launch/empty_world.launch b/engineer2_arm_config/launch/empty_world.launch new file mode 100644 index 0000000..bf2e5a9 --- /dev/null +++ b/engineer2_arm_config/launch/empty_world.launch @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/engineer2_arm_config/launch/engineer2_moveit_controller_manager.launch.xml b/engineer2_arm_config/launch/engineer2_moveit_controller_manager.launch.xml index db614c0..c730acd 100644 --- a/engineer2_arm_config/launch/engineer2_moveit_controller_manager.launch.xml +++ b/engineer2_arm_config/launch/engineer2_moveit_controller_manager.launch.xml @@ -1,10 +1,7 @@ + + - - - - - - - + + + \ No newline at end of file diff --git a/engineer2_arm_config/launch/fake_moveit_controller_manager.launch.xml b/engineer2_arm_config/launch/fake_moveit_controller_manager.launch.xml index 4d5bbea..64a5b70 100644 --- a/engineer2_arm_config/launch/fake_moveit_controller_manager.launch.xml +++ b/engineer2_arm_config/launch/fake_moveit_controller_manager.launch.xml @@ -7,6 +7,6 @@ - + diff --git a/engineer2_arm_config/launch/gazebo.launch b/engineer2_arm_config/launch/gazebo.launch new file mode 100644 index 0000000..e0006af --- /dev/null +++ b/engineer2_arm_config/launch/gazebo.launch @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/engineer2_arm_config/launch/joystick_control.launch b/engineer2_arm_config/launch/joystick_control.launch new file mode 100644 index 0000000..9411f6e --- /dev/null +++ b/engineer2_arm_config/launch/joystick_control.launch @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/engineer2_arm_config/launch/load_controllers.launch b/engineer2_arm_config/launch/load_controllers.launch index 2b45884..f1aa6bd 100644 --- a/engineer2_arm_config/launch/load_controllers.launch +++ b/engineer2_arm_config/launch/load_controllers.launch @@ -21,13 +21,8 @@ controllers/robot_state_controller controllers/joint_state_controller controllers/arm_trajectory_controller - controllers/gimbal_controller + controllers/gravity_compensation_controller controllers/chassis_controller - controllers/silver_lifter_controller - controllers/silver_pusher_controller - controllers/silver_rotator_controller - controllers/gold_lifter_controller - controllers/gold_pusher_controller " /> diff --git a/engineer2_arm_config/launch/load_move_group.launch b/engineer2_arm_config/launch/load_move_group.launch index a66a2a9..d4c3309 100644 --- a/engineer2_arm_config/launch/load_move_group.launch +++ b/engineer2_arm_config/launch/load_move_group.launch @@ -9,7 +9,7 @@ - + diff --git a/engineer2_arm_config/launch/move_group.launch b/engineer2_arm_config/launch/move_group.launch index c8771cc..0cb8840 100644 --- a/engineer2_arm_config/launch/move_group.launch +++ b/engineer2_arm_config/launch/move_group.launch @@ -1,52 +1,89 @@ - - - - + + + + - - - - + + + + - + + + + + - + - - - + + + + + + + + + + + + + + + + - - - - - + + + + + + + + + + + + + + + + - + - + diff --git a/engineer2_arm_config/launch/moveit.rviz b/engineer2_arm_config/launch/moveit.rviz index 2d517dd..6a876dd 100644 --- a/engineer2_arm_config/launch/moveit.rviz +++ b/engineer2_arm_config/launch/moveit.rviz @@ -4,12 +4,9 @@ Panels: Name: Displays Property Tree Widget: Expanded: - - /Global Options1 - /MotionPlanning1 - - /TF1 - - /TF1/Frames1 Splitter Ratio: 0.5 - Tree Height: 1301 + Tree Height: 226 - Class: rviz/Help Name: Help - Class: rviz/Views @@ -17,10 +14,6 @@ Panels: - /Current View1 Name: Views Splitter Ratio: 0.5 -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 Visualization Manager: Class: "" Displays: @@ -30,7 +23,7 @@ Visualization Manager: Color: 160; 160; 164 Enabled: true Line Style: - Line Width: 0.029999999329447746 + Line Width: 0.03 Value: Lines Name: Grid Normal Cell Count: 0 @@ -42,61 +35,30 @@ Visualization Manager: Plane Cell Count: 10 Reference Frame: Value: true - - Acceleration_Scaling_Factor: 0.1 - Class: moveit_rviz_plugin/MotionPlanning - Enabled: false - Move Group Namespace: "" - MoveIt_Allow_Approximate_IK: false - MoveIt_Allow_External_Program: false - MoveIt_Allow_Replanning: false - MoveIt_Allow_Sensor_Positioning: false - MoveIt_Planning_Attempts: 10 - MoveIt_Planning_Time: 5 - MoveIt_Use_Cartesian_Path: false - MoveIt_Use_Constraint_Aware_IK: false - MoveIt_Workspace: - Center: - X: 0 - Y: 0 - Z: 0 - Size: - X: 2 - Y: 2 - Z: 2 + - Class: moveit_rviz_plugin/MotionPlanning + Enabled: true Name: MotionPlanning Planned Path: - Color Enabled: false - Interrupt Display: false - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order + Links: ~ Loop Animation: true Robot Alpha: 0.5 - Robot Color: 150; 50; 150 Show Robot Collision: false Show Robot Visual: true Show Trail: false State Display Time: 0.05 s - Trail Step Size: 1 Trajectory Topic: move_group/display_planned_path - Use Sim Time: false Planning Metrics: Payload: 1 Show Joint Torques: false Show Manipulability: false Show Manipulability Index: false Show Weight Limit: false - TextHeight: 0.07999999821186066 Planning Request: Colliding Link Color: 255; 0; 0 Goal State Alpha: 1 Goal State Color: 250; 128; 0 Interactive Marker Size: 0 Joint Violation Color: 255; 0; 255 - Planning Group: engineer_arm Query Goal State: true Query Start State: false Show Workspace: false @@ -106,236 +68,19 @@ Visualization Manager: Robot Description: robot_description Scene Geometry: Scene Alpha: 1 - Scene Color: 50; 230; 50 - Scene Display Time: 0.009999999776482582 Show Scene Geometry: true Voxel Coloring: Z-Axis Voxel Rendering: Occupied Voxels Scene Robot: Attached Body Color: 150; 50; 150 - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order + Links: ~ Robot Alpha: 0.5 - Show Robot Collision: false - Show Robot Visual: true - Value: false - Velocity_Scaling_Factor: 0.1 - - Class: rviz/TF - Enabled: true - Filter (blacklist): "" - Filter (whitelist): "" - Frame Timeout: 15 - Frames: - All Enabled: false - base_link: - Value: true - fake_link4: - Value: false - gold_lifter_link: - Value: false - gold_pusher_link: - Value: false - left_back_wheel: - Value: false - left_front_wheel: - Value: false - link1: - Value: false - link2: - Value: false - link3: - Value: false - link4: - Value: false - link5: - Value: true - link6: - Value: true - map: - Value: true - odom: - Value: true - pitch: - Value: true - right_back_wheel: - Value: false - right_front_wheel: - Value: false - silver_gripper_link: - Value: false - silver_lifter_link: - Value: false - silver_pusher_link: - Value: false - tools_link: - Value: false - yaw: - Value: false - Marker Alpha: 1 - Marker Scale: 1 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: true - Tree: - map: - odom: - base_link: - gold_pusher_link: - gold_lifter_link: - {} - left_back_wheel: - {} - left_front_wheel: - {} - link1: - link2: - link3: - fake_link4: - {} - link4: - link5: - link6: - tools_link: - {} - yaw: - pitch: - {} - right_back_wheel: - {} - right_front_wheel: - {} - silver_lifter_link: - silver_pusher_link: - silver_gripper_link: - {} - Update Interval: 0 + Show Scene Robot: true Value: true - - Alpha: 1 - Class: rviz/RobotModel - Collision Enabled: false - Enabled: true - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - fake_link4: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - gold_lifter_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - gold_pusher_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - left_back_wheel: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - left_front_wheel: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link3: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link4: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link5: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link6: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - pitch: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - right_back_wheel: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - right_front_wheel: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - silver_gripper_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - silver_lifter_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - silver_pusher_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - tools_link: - Alpha: 1 - Show Axes: false - Show Trail: false - yaw: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Name: RobotModel - Robot Description: robot_description - TF Prefix: "" - Update Interval: 0 - Value: true - Visual Enabled: true Enabled: true Global Options: Background Color: 48; 48; 48 - Default Light: true - Fixed Frame: odom - Frame Rate: 30 + Fixed Frame: base_link Name: root Tools: - Class: rviz/Interact @@ -346,37 +91,41 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 1.3873012065887451 + Distance: 2.0 Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 + Stereo Eye Separation: 0.06 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Field of View: 0.75 Focal Point: - X: 0.22694315016269684 - Y: -0.3219256103038788 - Z: 0.15961049497127533 + X: -0.1 + Y: 0.25 + Z: 0.30 Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 + Focal Shape Size: 0.05 Invert Z Axis: false Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 1.3847957849502563 + Near Clip Distance: 0.01 + Pitch: 0.5 Target Frame: base_link - Yaw: 4.408130168914795 + Yaw: -0.6232355833053589 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1536 + Height: 848 Help: collapsed: false Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000100000000000001f3000005a6fc0200000007fb000000100044006900730070006c006100790073010000003d000005a6000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006700000002f4000002ef0000000000000000000007bf000005a600000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + MotionPlanning: + collapsed: false + MotionPlanning - Trajectory Slider: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000100000000000001f0000002f6fc0200000007fb000000100044006900730070006c006100790073010000003d00000173000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001b60000017d0000017d00ffffff00000315000002f600000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Views: collapsed: false - Width: 2488 - X: 72 - Y: 27 + Width: 1291 + X: 454 + Y: 25 diff --git a/engineer2_arm_config/launch/ompl_chomp_planning_pipeline.launch.xml b/engineer2_arm_config/launch/ompl_chomp_planning_pipeline.launch.xml new file mode 100644 index 0000000..c1e6c0c --- /dev/null +++ b/engineer2_arm_config/launch/ompl_chomp_planning_pipeline.launch.xml @@ -0,0 +1,21 @@ + + + + + + + + + + + + diff --git a/engineer2_arm_config/launch/ompl_planning_pipeline.launch.xml b/engineer2_arm_config/launch/ompl_planning_pipeline.launch.xml index ed05662..b2aa188 100644 --- a/engineer2_arm_config/launch/ompl_planning_pipeline.launch.xml +++ b/engineer2_arm_config/launch/ompl_planning_pipeline.launch.xml @@ -5,6 +5,7 @@ default="default_planner_request_adapters/LimitMaxCartesianLinkSpeed default_planner_request_adapters/AddTimeParameterization default_planner_request_adapters/ResolveConstraintFrames + engineer_planning_adapters/SpecialJointConstraintAdapter default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision diff --git a/engineer2_arm_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml b/engineer2_arm_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml new file mode 100644 index 0000000..c7c4cf5 --- /dev/null +++ b/engineer2_arm_config/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + diff --git a/engineer2_arm_config/launch/planning_pipeline.launch.xml b/engineer2_arm_config/launch/planning_pipeline.launch.xml index 0929525..4b4d0d6 100644 --- a/engineer2_arm_config/launch/planning_pipeline.launch.xml +++ b/engineer2_arm_config/launch/planning_pipeline.launch.xml @@ -5,6 +5,6 @@ - + diff --git a/engineer2_arm_config/launch/ros_control_moveit_controller_manager.launch.xml b/engineer2_arm_config/launch/ros_control_moveit_controller_manager.launch.xml new file mode 100644 index 0000000..9ebc91c --- /dev/null +++ b/engineer2_arm_config/launch/ros_control_moveit_controller_manager.launch.xml @@ -0,0 +1,4 @@ + + + + diff --git a/engineer2_arm_config/launch/ros_controllers.launch b/engineer2_arm_config/launch/ros_controllers.launch new file mode 100644 index 0000000..7565713 --- /dev/null +++ b/engineer2_arm_config/launch/ros_controllers.launch @@ -0,0 +1,11 @@ + + + + + + + + + + diff --git a/engineer2_arm_config/launch/run_benchmark_ompl.launch b/engineer2_arm_config/launch/run_benchmark_ompl.launch new file mode 100644 index 0000000..8873abe --- /dev/null +++ b/engineer2_arm_config/launch/run_benchmark_ompl.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/engineer2_arm_config/launch/trajectory_execution.launch.xml b/engineer2_arm_config/launch/trajectory_execution.launch.xml index ae4b5af..fa6ef00 100644 --- a/engineer2_arm_config/launch/trajectory_execution.launch.xml +++ b/engineer2_arm_config/launch/trajectory_execution.launch.xml @@ -2,7 +2,7 @@ - + @@ -10,15 +10,14 @@ - + - + - + - - - + + diff --git a/engineer2_arm_config/launch/warehouse.launch b/engineer2_arm_config/launch/warehouse.launch new file mode 100644 index 0000000..0712e67 --- /dev/null +++ b/engineer2_arm_config/launch/warehouse.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/engineer2_arm_config/launch/warehouse_settings.launch.xml b/engineer2_arm_config/launch/warehouse_settings.launch.xml new file mode 100644 index 0000000..e473b08 --- /dev/null +++ b/engineer2_arm_config/launch/warehouse_settings.launch.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/engineer2_arm_config/package.xml b/engineer2_arm_config/package.xml index 8c33136..3464459 100644 --- a/engineer2_arm_config/package.xml +++ b/engineer2_arm_config/package.xml @@ -5,14 +5,14 @@ An automatically generated package with all the configuration and launch files for using the engineer2 with the MoveIt Motion Planning Framework - Chonghong Cai - Chonghong Cai + ch + ch BSD http://moveit.ros.org/ - https://github.com/ros-planning/moveit/issues - https://github.com/ros-planning/moveit + https://github.com/moveit/moveit/issues + https://github.com/moveit/moveit catkin diff --git a/engineer_arm_config/.setup_assistant b/engineer_arm_config/.setup_assistant index 27ac3a9..2cebde3 100644 --- a/engineer_arm_config/.setup_assistant +++ b/engineer_arm_config/.setup_assistant @@ -6,6 +6,6 @@ moveit_setup_assistant_config: SRDF: relative_path: config/engineer.srdf CONFIG: - author_name: QiayuanLiao - author_email: liaoqiayuan@gmail.com - generated_timestamp: 1617294211 + author_name: CH + author_email: 3631676002@qq.com + generated_timestamp: 1784690695 \ No newline at end of file diff --git a/engineer_arm_config/CMakeLists.txt b/engineer_arm_config/CMakeLists.txt index 287bc11..0cbbfdf 100644 --- a/engineer_arm_config/CMakeLists.txt +++ b/engineer_arm_config/CMakeLists.txt @@ -6,5 +6,5 @@ find_package(catkin REQUIRED) catkin_package() install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} - PATTERN "setup_assistant.launch" EXCLUDE) + PATTERN "setup_assistant.launch" EXCLUDE) install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) diff --git a/engineer_arm_config/config/cartesian_limits.yaml b/engineer_arm_config/config/cartesian_limits.yaml new file mode 100644 index 0000000..7df72f6 --- /dev/null +++ b/engineer_arm_config/config/cartesian_limits.yaml @@ -0,0 +1,5 @@ +cartesian_limits: + max_trans_vel: 1 + max_trans_acc: 2.25 + max_trans_dec: -5 + max_rot_vel: 1.57 diff --git a/engineer_arm_config/config/chomp_planning.yaml b/engineer_arm_config/config/chomp_planning.yaml index 0831fda..eb9c912 100644 --- a/engineer_arm_config/config/chomp_planning.yaml +++ b/engineer_arm_config/config/chomp_planning.yaml @@ -7,12 +7,12 @@ learning_rate: 0.01 smoothness_cost_velocity: 0.0 smoothness_cost_acceleration: 1.0 smoothness_cost_jerk: 0.0 -ridge_factor: 0.01 +ridge_factor: 0.0 use_pseudo_inverse: false pseudo_inverse_ridge_factor: 1e-4 joint_update_limit: 0.1 collision_clearance: 0.2 collision_threshold: 0.07 use_stochastic_descent: true -enable_failure_recovery: true -max_recovery_attempts: 15 +enable_failure_recovery: false +max_recovery_attempts: 5 diff --git a/engineer_arm_config/config/engineer.srdf b/engineer_arm_config/config/engineer.srdf index c49a334..b6855d6 100644 --- a/engineer_arm_config/config/engineer.srdf +++ b/engineer_arm_config/config/engineer.srdf @@ -9,48 +9,200 @@ - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/engineer_arm_config/config/engineer_moveit_controllers.yaml b/engineer_arm_config/config/engineer_moveit_controllers.yaml new file mode 100644 index 0000000..138f592 --- /dev/null +++ b/engineer_arm_config/config/engineer_moveit_controllers.yaml @@ -0,0 +1,34 @@ +controller_list: + - name: controllers/left_arm_controller + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: True + joints: + - left_joint1 + - left_joint2 + - left_joint3 + - left_joint4 + - left_joint5 + - left_joint6 + - left_joint7 + - name: controllers/right_arm_controller + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: True + joints: + - right_joint1 + - right_joint2 + - right_joint3 + - right_joint4 + - right_joint5 + - right_joint6 + - right_joint7 + - name: controllers/gimbal_arm_controller + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: True + joints: + - main_pitch + - gimbal_joint1 + - gimbal_joint2 + - gimbal_joint3 \ No newline at end of file diff --git a/engineer_arm_config/config/fake_controllers.yaml b/engineer_arm_config/config/fake_controllers.yaml index 82506df..cdcbecf 100644 --- a/engineer_arm_config/config/fake_controllers.yaml +++ b/engineer_arm_config/config/fake_controllers.yaml @@ -1,14 +1,56 @@ controller_list: - - name: fake_engineer_arm_controller + - name: fake_left_arm_controller type: $(arg fake_execution_type) joints: - - joint1 - - joint2 - - joint3 - - joint4 - - joint5 - - joint6 - - joint7 + - left_joint1 + - left_joint2 + - left_joint3 + - left_joint4 + - left_joint5 + - left_joint6 + - left_joint7 + - name: fake_right_arm_controller + type: $(arg fake_execution_type) + joints: + - right_joint1 + - right_joint2 + - right_joint3 + - right_joint4 + - right_joint5 + - right_joint6 + - right_joint7 + - name: fake_gimbal_arm_controller + type: $(arg fake_execution_type) + joints: + - main_pitch + - gimbal_joint1 + - gimbal_joint2 + - gimbal_joint3 + - name: fake_engineer_arms_controller + type: $(arg fake_execution_type) + joints: + - main_pitch + - gimbal_joint1 + - gimbal_joint2 + - gimbal_joint3 + - left_joint1 + - left_joint2 + - left_joint3 + - left_joint4 + - left_joint5 + - left_joint6 + - left_joint7 + - right_joint1 + - right_joint2 + - right_joint3 + - right_joint4 + - right_joint5 + - right_joint6 + - right_joint7 initial: # Define initial robot poses per group - - group: engineer_arm - pose: home + - group: left_arm + pose: left_arm_home + - group: right_arm + pose: right_arm_home + - group: gimbal_arm + pose: gimbal_arm_home \ No newline at end of file diff --git a/engineer_arm_config/config/gazebo_controllers.yaml b/engineer_arm_config/config/gazebo_controllers.yaml new file mode 100644 index 0000000..e4d2eb0 --- /dev/null +++ b/engineer_arm_config/config/gazebo_controllers.yaml @@ -0,0 +1,4 @@ +# Publish joint_states +joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 50 diff --git a/engineer_arm_config/config/gazebo_engineer.urdf b/engineer_arm_config/config/gazebo_engineer.urdf new file mode 100644 index 0000000..c7cca2b --- /dev/null +++ b/engineer_arm_config/config/gazebo_engineer.urdf @@ -0,0 +1,1095 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + / + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + Gazebo/Black + + + transmission_interface/SimpleTransmission + + -1 + hardware_interface/EffortJointInterface + + + -1.815 + hardware_interface/EffortJointInterface + + + + transmission_interface/SimpleTransmission + + 19.2032 + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + + + + transmission_interface/SimpleTransmission + + -1 + hardware_interface/EffortJointInterface + + + 0.472 + hardware_interface/EffortJointInterface + + + + transmission_interface/SimpleTransmission + + -19.2032 + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + + + + transmission_interface/SimpleTransmission + + -1 + hardware_interface/EffortJointInterface + + + -0.312 + hardware_interface/EffortJointInterface + + + + transmission_interface/SimpleTransmission + + 19.2032 + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + + + + transmission_interface/SimpleTransmission + + -1 + hardware_interface/EffortJointInterface + + + 1.856 + hardware_interface/EffortJointInterface + + + + transmission_interface/SimpleTransmission + + -19.2032 + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + 1 + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 0.940 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + 1 + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + -0.402 + + + + transmission_interface/SimpleTransmission + + -1 + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 2.320 + + + + transmission_interface/SimpleTransmission + + 1 + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + -0.641 + + + + transmission_interface/SimpleTransmission + + -1 + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + -0.298 + + + + transmission_interface/SimpleTransmission + + 1 + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1.279 + + + + transmission_interface/SimpleTransmission + + -1 + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + -1.283 + + + + transmission_interface/SimpleTransmission + + 1 + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1.515 + + + + transmission_interface/SimpleTransmission + + -1 + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1.705 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + -1 + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 0.003 + + + + transmission_interface/SimpleTransmission + + 1 + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 3.420 + + + + transmission_interface/SimpleTransmission + + -1 + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 0.638 + + + + transmission_interface/SimpleTransmission + + 1 + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + -0.369 + + + + transmission_interface/SimpleTransmission + + -1 + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 0.832 + + + + transmission_interface/SimpleTransmission + + 1 + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + -1.697 + + + + transmission_interface/SimpleTransmission + + -1 + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 2.02 + + + + transmission_interface/SimpleTransmission + + 1 + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 2.14 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + 1 + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 0.0 + + + + transmission_interface/SimpleTransmission + + 1 + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 0.0 + + + + transmission_interface/SimpleTransmission + + 1 + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 0.0 + + + + + + + + + + + + + + + + diff --git a/engineer_arm_config/config/joint_limits.yaml b/engineer_arm_config/config/joint_limits.yaml index a1ea2d6..ba51a1d 100644 --- a/engineer_arm_config/config/joint_limits.yaml +++ b/engineer_arm_config/config/joint_limits.yaml @@ -8,38 +8,93 @@ default_acceleration_scaling_factor: 0.1 # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] joint_limits: - joint1: + left_joint1: has_velocity_limits: true - max_velocity: 1.80 + max_velocity: 10.46 has_acceleration_limits: true - max_acceleration: 3.5 - joint2: + max_acceleration: 10 + left_joint2: has_velocity_limits: true - max_velocity: 2.75 + max_velocity: 10.46 has_acceleration_limits: true - max_acceleration: 6 - joint3: + max_acceleration: 10 + left_joint3: has_velocity_limits: true - max_velocity: 1.80 + max_velocity: 3.76 has_acceleration_limits: true - max_acceleration: 5 - joint4: + max_acceleration: 10 + left_joint4: has_velocity_limits: true - max_velocity: 19 + max_velocity: 3.76 has_acceleration_limits: true - max_acceleration: 50 - joint5: + max_acceleration: 10 + left_joint5: has_velocity_limits: true - max_velocity: 15 + max_velocity: 12.56 has_acceleration_limits: true - max_acceleration: 20 - joint6: + max_acceleration: 10 + left_joint6: has_velocity_limits: true - max_velocity: 50 + max_velocity: 12.56 has_acceleration_limits: true - max_acceleration: 90 - joint7: + max_acceleration: 10 + left_joint7: has_velocity_limits: true - max_velocity: 50 + max_velocity: 12.56 has_acceleration_limits: true - max_acceleration: 90 + max_acceleration: 10 + right_joint1: + has_velocity_limits: true + max_velocity: 10.46 + has_acceleration_limits: true + max_acceleration: 10 + right_joint2: + has_velocity_limits: true + max_velocity: 10.46 + has_acceleration_limits: true + max_acceleration: 10 + right_joint3: + has_velocity_limits: true + max_velocity: 3.76 + has_acceleration_limits: true + max_acceleration: 10 + right_joint4: + has_velocity_limits: true + max_velocity: 3.76 + has_acceleration_limits: true + max_acceleration: 10 + right_joint5: + has_velocity_limits: true + max_velocity: 10.46 + has_acceleration_limits: true + max_acceleration: 10 + right_joint6: + has_velocity_limits: true + max_velocity: 10.46 + has_acceleration_limits: true + max_acceleration: 10 + right_joint7: + has_velocity_limits: true + max_velocity: 10.46 + has_acceleration_limits: true + max_acceleration: 10 + main_pitch: + has_velocity_limits: true + max_velocity: 10.46 + has_acceleration_limits: true + max_acceleration: 10 + gimbal_joint1: + has_velocity_limits: true + max_velocity: 3.76 + has_acceleration_limits: true + max_acceleration: 10 + gimbal_joint2: + has_velocity_limits: true + max_velocity: 10.46 + has_acceleration_limits: true + max_acceleration: 10 + gimbal_joint3: + has_velocity_limits: true + max_velocity: 10.46 + has_acceleration_limits: true + max_acceleration: 10 \ No newline at end of file diff --git a/engineer_arm_config/config/kinematics.yaml b/engineer_arm_config/config/kinematics.yaml index b298b37..fc7bb29 100644 --- a/engineer_arm_config/config/kinematics.yaml +++ b/engineer_arm_config/config/kinematics.yaml @@ -1,3 +1,21 @@ -engineer_arm: +left_arm: kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin + kinematics_solver_search_resolution: 0.005 kinematics_solver_timeout: 0.005 + goal_joint_tolerance: 0.001 + goal_position_tolerance: 0.001 + goal_orientation_tolerance: 0.01 +right_arm: + kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 + goal_joint_tolerance: 0.001 + goal_position_tolerance: 0.001 + goal_orientation_tolerance: 0.01 +gimbal_arm: + kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 + goal_joint_tolerance: 0.0001 + goal_position_tolerance: 0.0001 + goal_orientation_tolerance: 0.001 \ No newline at end of file diff --git a/engineer_arm_config/config/ompl_planning.yaml b/engineer_arm_config/config/ompl_planning.yaml index 8e69e09..6af2b90 100644 --- a/engineer_arm_config/config/ompl_planning.yaml +++ b/engineer_arm_config/config/ompl_planning.yaml @@ -3,8 +3,8 @@ planner_configs: type: geometric::AnytimePathShortening shortcut: true # Attempt to shortcut all new solution paths hybridize: true # Compute hybrid solution trajectories - max_hybrid_paths: 40 # Number of hybrid paths generated per iteration - num_planners: 20 # The number of default planners to use for planning + max_hybrid_paths: 24 # Number of hybrid paths generated per iteration + num_planners: 4 # The number of default planners to use for planning planners: "" # A comma-separated list of planner types (e.g., "PRM,EST,RRTConnect"Optionally, planner parameters can be passed to change the default:"PRM[max_nearest_neighbors=5],EST[goal_bias=.5],RRT[range=10. goal_bias=.1]" SBL: type: geometric::SBL @@ -51,8 +51,8 @@ planner_configs: temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0 min_temperature: 10e-10 # lower limit of temp change. default: 10e-10 init_temperature: 10e-6 # initial temperature. default: 10e-6 - frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() - frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + frontier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frontier_node_ratio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 k_constant: 0.0 # value used to normalize expresssion. default: 0.0 set in setup() PRM: type: geometric::PRM @@ -95,8 +95,8 @@ planner_configs: range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() temp_change_factor: 0.1 # how much to increase or decrease temp. default: 0.1 init_temperature: 100 # initial temperature. default: 100 - frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() - frountier_node_ratio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + frontier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frontier_node_ratio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 cost_threshold: 1e300 # the cost threshold. Any motion cost that is not better will not be expanded. default: inf LBTRRT: type: geometric::LBTRRT @@ -127,7 +127,44 @@ planner_configs: sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 max_failures: 5000 # maximum consecutive failure limit. default: 5000 -engineer_arm: + AITstar: + type: geometric::AITstar + use_k_nearest: 1 # whether to use a k-nearest RGG connection model (1) or an r-disc model (0). Default: 1 + rewire_factor: 1.001 # rewire factor of the RGG. Valid values: [1.0:0.01:3.0]. Default: 1.001 + samples_per_batch: 100 # batch size. Valid values: [1:1:1000]. Default: 100 + use_graph_pruning: 1 # enable graph pruning (1) or not (0). Default: 1 + find_approximate_solutions: 0 # track approximate solutions (1) or not (0). Default: 0 + set_max_num_goals: 1 # maximum number of goals sampled from sampleable goal regions. Valid values: [1:1:1000]. Default: 1 + ABITstar: + type: geometric::ABITstar + use_k_nearest: 1 # whether to use a k-nearest RGG connection model (1) or an r-disc model (0). Default: 1 + rewire_factor: 1.001 # rewire factor of the RGG. Valid values: [1.0:0.01:3.0]. Default: 1.001 + samples_per_batch: 100 # batch size. Valid values: [1:1:1000]. Default: 100 + use_graph_pruning: 1 # enable graph pruning (1) or not (0). Default: 1 + prune_threshold_as_fractional_cost_change: 0.1 # fractional change in the solution cost AND problem measure necessary for pruning to occur. Default: 0.1 + delay_rewiring_to_first_solution: 0 # delay (1) or not (0) rewiring until a solution is found. Default: 0 + use_just_in_time_sampling: 0 # delay the generation of samples until they are * necessary. Only works with r-disc connection and path length minimization. Default: 0 + drop_unconnected_samples_on_prune: 0 # drop unconnected samples when pruning, regardless of their heuristic value. Default: 0 + stop_on_each_solution_improvement: 0 # stop the planner each time a solution improvement is found. Useful for debugging. Default: 0 + use_strict_queue_ordering: 0 # sort edges in the queue at the end of the batch (0) or after each rewiring (1). Default: 0 + find_approximate_solutions: 0 # track approximate solutions (1) or not (0). Default: 0 + initial_inflation_factor: 1000000 # inflation factor for the initial search. Valid values: [1.0:0.01:1000000.0]. Default: 1000000 + inflation_scaling_parameter: 10 # scaling parameter for the inflation factor update policy. Valid values: [1.0:0.01:1000000.0]. Default: 0 + truncation_scaling_parameter: 5.0 # scaling parameter for the truncation factor update policy. Valid values: [1.0:0.01:1000000.0]. Default: 0 + BITstar: + type: geometric::BITstar + use_k_nearest: 1 # whether to use a k-nearest RGG connection model (1) or an r-disc model (0). Default: 1 + rewire_factor: 1.001 # rewire factor of the RGG. Valid values: [1.0:0.01:3.0]. Default: 1.001 + samples_per_batch: 100 # batch size. Valid values: [1:1:1000]. Default: 100 + use_graph_pruning: 1 # enable graph pruning (1) or not (0). Default: 1 + prune_threshold_as_fractional_cost_change: 0.1 # fractional change in the solution cost AND problem measure necessary for pruning to occur. Default: 0.1 + delay_rewiring_to_first_solution: 0 # delay (1) or not (0) rewiring until a solution is found. Default: 0 + use_just_in_time_sampling: 0 # delay the generation of samples until they are * necessary. Only works with r-disc connection and path length minimization. Default: 0 + drop_unconnected_samples_on_prune: 0 # drop unconnected samples when pruning, regardless of their heuristic value. Default: 0 + stop_on_each_solution_improvement: 0 # stop the planner each time a solution improvement is found. Useful for debugging. Default: 0 + use_strict_queue_ordering: 0 # sort edges in the queue at the end of the batch (0) or after each rewiring (1). Default: 0 + find_approximate_solutions: 0 # track approximate solutions (1) or not (0). Default: 0 +left_arm: default_planner_config: RRTConnect planner_configs: - AnytimePathShortening @@ -154,5 +191,102 @@ engineer_arm: - LazyPRMstar - SPARS - SPARStwo - projection_evaluator: joints(joint1,joint2) + - AITstar + - ABITstar + - BITstar + projection_evaluator: joints(left_joint1,left_joint2) + longest_valid_segment_fraction: 0.005 +right_arm: + default_planner_config: RRTConnect + planner_configs: + - AnytimePathShortening + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo + - AITstar + - ABITstar + - BITstar + projection_evaluator: joints(right_joint1,right_joint2) + longest_valid_segment_fraction: 0.005 +gimbal_arm: + planner_configs: + - AnytimePathShortening + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo + - AITstar + - ABITstar + - BITstar + projection_evaluator: joints(main_pitch,gimbal_joint1) + longest_valid_segment_fraction: 0.005 +engineer_arms: + planner_configs: + - AnytimePathShortening + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo + - AITstar + - ABITstar + - BITstar + projection_evaluator: joints(main_pitch,gimbal_joint1) longest_valid_segment_fraction: 0.005 diff --git a/engineer_arm_config/config/ros_controllers.yaml b/engineer_arm_config/config/ros_controllers.yaml index 52ad421..6cd8482 100644 --- a/engineer_arm_config/config/ros_controllers.yaml +++ b/engineer_arm_config/config/ros_controllers.yaml @@ -1,33 +1,112 @@ -# Simulation settings for using moveit_sim_controllers -moveit_sim_hw_interface: - joint_model_group: engineer_arm - joint_model_group_pose: home -# Settings for ros_control_boilerplate control loop -generic_hw_control_loop: - loop_hz: 300 - cycle_time_error_threshold: 0.01 -# Settings for ros_control hardware interface -hardware_interface: +left_arm_controller: + type: effort_controllers/JointTrajectoryController joints: - - joint1 - - joint2 - - joint3 - - joint4 - - joint5 - - joint6 - - joint7 - sim_control_mode: 1 # 0: position, 1: velocity - -controller_list: - - name: controllers/arm_trajectory_controller - action_ns: follow_joint_trajectory - default: True - type: FollowJointTrajectory - joints: - - joint1 - - joint2 - - joint3 - - joint4 - - joint5 - - joint6 - - joint7 + - left_joint1 + - left_joint2 + - left_joint3 + - left_joint4 + - left_joint5 + - left_joint6 + - left_joint7 + constraints: + goal_time: 5.0 + stopped_velocity_tolerance: 0.5 + left_joint1: { trajectory: 0.08, goal: 0.05 } + left_joint2: { trajectory: 0.08, goal: 0.05 } + left_joint3: { trajectory: 0.08, goal: 0.05 } + left_joint4: { trajectory: 0.08, goal: 0.05 } + left_joint5: { trajectory: 0.08, goal: 0.05 } + left_joint6: { trajectory: 0.08, goal: 0.05 } + left_joint7: { trajectory: 0.08, goal: 0.05 } + gains: + left_joint1: + p: 1000 + d: 1 + i: 1 + i_clamp: 1 + left_joint2: + p: 1000 + d: 1 + i: 1 + i_clamp: 1 + left_joint3: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + left_joint4: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + left_joint5: + p: 100 + d: 1 + i: 0.5 + i_clamp: 1 + left_joint6: + p: 50 + d: 1 + i: 0.5 + i_clamp: 1 + left_joint7: + p: 50 + d: 1 + i: 0.5 + i_clamp: 1 +right_arm_controller: + type: effort_controllers/JointTrajectoryController + joints: + - right_joint1 + - right_joint2 + - right_joint3 + - right_joint4 + - right_joint5 + - right_joint6 + - right_joint7 + constraints: + goal_time: 5.0 + stopped_velocity_tolerance: 0.05 + right_joint1: { trajectory: 0.08, goal: 0.05 } + right_joint2: { trajectory: 0.08, goal: 0.05 } + right_joint3: { trajectory: 0.08, goal: 0.05 } + right_joint4: { trajectory: 0.08, goal: 0.05 } + right_joint5: { trajectory: 0.08, goal: 0.05 } + right_joint6: { trajectory: 0.08, goal: 0.05 } + right_joint7: { trajectory: 0.08, goal: 0.05 } + gains: + right_joint1: + p: 1000 + d: 1 + i: 1 + i_clamp: 1 + right_joint2: + p: 1000 + d: 1 + i: 1 + i_clamp: 1 + right_joint3: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + right_joint4: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + right_joint5: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + right_joint6: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + right_joint7: + p: 100 + d: 1 + i: 1 + i_clamp: 1 \ No newline at end of file diff --git a/engineer_arm_config/config/sensors_3d.yaml b/engineer_arm_config/config/sensors_3d.yaml index add9f47..51010a3 100644 --- a/engineer_arm_config/config/sensors_3d.yaml +++ b/engineer_arm_config/config/sensors_3d.yaml @@ -1,2 +1,2 @@ sensors: - - { } + [] \ No newline at end of file diff --git a/engineer_arm_config/config/simple_moveit_controllers.yaml b/engineer_arm_config/config/simple_moveit_controllers.yaml new file mode 100644 index 0000000..da7a627 --- /dev/null +++ b/engineer_arm_config/config/simple_moveit_controllers.yaml @@ -0,0 +1,25 @@ +controller_list: + - name: left_arm_controller + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: True + joints: + - left_joint1 + - left_joint2 + - left_joint3 + - left_joint4 + - left_joint5 + - left_joint6 + - left_joint7 + - name: right_arm_controller + action_ns: follow_joint_trajectory + type: FollowJointTrajectory + default: True + joints: + - right_joint1 + - right_joint2 + - right_joint3 + - right_joint4 + - right_joint5 + - right_joint6 + - right_joint7 \ No newline at end of file diff --git a/engineer_arm_config/config/stomp_planning.yaml b/engineer_arm_config/config/stomp_planning.yaml new file mode 100644 index 0000000..1971dee --- /dev/null +++ b/engineer_arm_config/config/stomp_planning.yaml @@ -0,0 +1,156 @@ +stomp/left_arm: + group_name: left_arm + optimization: + num_timesteps: 60 + num_iterations: 40 + num_iterations_after_valid: 0 + num_rollouts: 30 + max_rollouts: 30 + initialization_method: 1 # [1 : LINEAR_INTERPOLATION, 2 : CUBIC_POLYNOMIAL, 3 : MININUM_CONTROL_COST] + control_cost_weight: 0.0 + task: + noise_generator: + - class: stomp_moveit/NormalDistributionSampling + stddev: [0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05] + cost_functions: + - class: stomp_moveit/CollisionCheck + collision_penalty: 1.0 + cost_weight: 1.0 + kernel_window_percentage: 0.2 + longest_valid_joint_move: 0.05 + noisy_filters: + - class: stomp_moveit/JointLimits + lock_start: True + lock_goal: True + - class: stomp_moveit/MultiTrajectoryVisualization + line_width: 0.02 + rgb: [255, 255, 0] + marker_array_topic: stomp_trajectories + marker_namespace: noisy + update_filters: + - class: stomp_moveit/PolynomialSmoother + poly_order: 6 + - class: stomp_moveit/TrajectoryVisualization + line_width: 0.05 + rgb: [0, 191, 255] + error_rgb: [255, 0, 0] + publish_intermediate: True + marker_topic: stomp_trajectory + marker_namespace: optimized +stomp/right_arm: + group_name: right_arm + optimization: + num_timesteps: 60 + num_iterations: 40 + num_iterations_after_valid: 0 + num_rollouts: 30 + max_rollouts: 30 + initialization_method: 1 # [1 : LINEAR_INTERPOLATION, 2 : CUBIC_POLYNOMIAL, 3 : MININUM_CONTROL_COST] + control_cost_weight: 0.0 + task: + noise_generator: + - class: stomp_moveit/NormalDistributionSampling + stddev: [0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05] + cost_functions: + - class: stomp_moveit/CollisionCheck + collision_penalty: 1.0 + cost_weight: 1.0 + kernel_window_percentage: 0.2 + longest_valid_joint_move: 0.05 + noisy_filters: + - class: stomp_moveit/JointLimits + lock_start: True + lock_goal: True + - class: stomp_moveit/MultiTrajectoryVisualization + line_width: 0.02 + rgb: [255, 255, 0] + marker_array_topic: stomp_trajectories + marker_namespace: noisy + update_filters: + - class: stomp_moveit/PolynomialSmoother + poly_order: 6 + - class: stomp_moveit/TrajectoryVisualization + line_width: 0.05 + rgb: [0, 191, 255] + error_rgb: [255, 0, 0] + publish_intermediate: True + marker_topic: stomp_trajectory + marker_namespace: optimized +stomp/gimbal_arm: + group_name: gimbal_arm + optimization: + num_timesteps: 60 + num_iterations: 40 + num_iterations_after_valid: 0 + num_rollouts: 30 + max_rollouts: 30 + initialization_method: 1 # [1 : LINEAR_INTERPOLATION, 2 : CUBIC_POLYNOMIAL, 3 : MININUM_CONTROL_COST] + control_cost_weight: 0.0 + task: + noise_generator: + - class: stomp_moveit/NormalDistributionSampling + stddev: [0.05, 0.05, 0.05, 0.05] + cost_functions: + - class: stomp_moveit/CollisionCheck + collision_penalty: 1.0 + cost_weight: 1.0 + kernel_window_percentage: 0.2 + longest_valid_joint_move: 0.05 + noisy_filters: + - class: stomp_moveit/JointLimits + lock_start: True + lock_goal: True + - class: stomp_moveit/MultiTrajectoryVisualization + line_width: 0.02 + rgb: [255, 255, 0] + marker_array_topic: stomp_trajectories + marker_namespace: noisy + update_filters: + - class: stomp_moveit/PolynomialSmoother + poly_order: 6 + - class: stomp_moveit/TrajectoryVisualization + line_width: 0.05 + rgb: [0, 191, 255] + error_rgb: [255, 0, 0] + publish_intermediate: True + marker_topic: stomp_trajectory + marker_namespace: optimized +stomp/engineer_arms: + group_name: engineer_arms + optimization: + num_timesteps: 60 + num_iterations: 40 + num_iterations_after_valid: 0 + num_rollouts: 30 + max_rollouts: 30 + initialization_method: 1 # [1 : LINEAR_INTERPOLATION, 2 : CUBIC_POLYNOMIAL, 3 : MININUM_CONTROL_COST] + control_cost_weight: 0.0 + task: + noise_generator: + - class: stomp_moveit/NormalDistributionSampling + stddev: [0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05] + cost_functions: + - class: stomp_moveit/CollisionCheck + collision_penalty: 1.0 + cost_weight: 1.0 + kernel_window_percentage: 0.2 + longest_valid_joint_move: 0.05 + noisy_filters: + - class: stomp_moveit/JointLimits + lock_start: True + lock_goal: True + - class: stomp_moveit/MultiTrajectoryVisualization + line_width: 0.02 + rgb: [255, 255, 0] + marker_array_topic: stomp_trajectories + marker_namespace: noisy + update_filters: + - class: stomp_moveit/PolynomialSmoother + poly_order: 6 + - class: stomp_moveit/TrajectoryVisualization + line_width: 0.05 + rgb: [0, 191, 255] + error_rgb: [255, 0, 0] + publish_intermediate: True + marker_topic: stomp_trajectory + marker_namespace: optimized \ No newline at end of file diff --git a/engineer_arm_config/launch/chomp_planning_pipeline.launch.xml b/engineer_arm_config/launch/chomp_planning_pipeline.launch.xml index 0863402..83f7dcb 100644 --- a/engineer_arm_config/launch/chomp_planning_pipeline.launch.xml +++ b/engineer_arm_config/launch/chomp_planning_pipeline.launch.xml @@ -1,23 +1,21 @@ - - - - + + + + /> - - - - + + + + - - - - + diff --git a/engineer_arm_config/launch/default_warehouse_db.launch b/engineer_arm_config/launch/default_warehouse_db.launch new file mode 100644 index 0000000..41bdc13 --- /dev/null +++ b/engineer_arm_config/launch/default_warehouse_db.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/engineer_arm_config/launch/demo.launch b/engineer_arm_config/launch/demo.launch new file mode 100644 index 0000000..1f3d99a --- /dev/null +++ b/engineer_arm_config/launch/demo.launch @@ -0,0 +1,66 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + [move_group/fake_controller_joint_states] + + + + [move_group/fake_controller_joint_states] + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/engineer_arm_config/launch/demo_gazebo.launch b/engineer_arm_config/launch/demo_gazebo.launch new file mode 100644 index 0000000..0ef8f95 --- /dev/null +++ b/engineer_arm_config/launch/demo_gazebo.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/engineer_arm_config/launch/empty_world.launch b/engineer_arm_config/launch/empty_world.launch new file mode 100644 index 0000000..3b8b37b --- /dev/null +++ b/engineer_arm_config/launch/empty_world.launch @@ -0,0 +1,25 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/engineer_arm_config/launch/engineer_moveit_controller_manager.launch.xml b/engineer_arm_config/launch/engineer_moveit_controller_manager.launch.xml index bbb90dc..65af328 100644 --- a/engineer_arm_config/launch/engineer_moveit_controller_manager.launch.xml +++ b/engineer_arm_config/launch/engineer_moveit_controller_manager.launch.xml @@ -1,10 +1,7 @@ + + - - - - - - - + + + \ No newline at end of file diff --git a/engineer_arm_config/launch/fake_moveit_controller_manager.launch.xml b/engineer_arm_config/launch/fake_moveit_controller_manager.launch.xml index 62baeea..f160d13 100644 --- a/engineer_arm_config/launch/fake_moveit_controller_manager.launch.xml +++ b/engineer_arm_config/launch/fake_moveit_controller_manager.launch.xml @@ -1,9 +1,12 @@ + + + - + diff --git a/engineer_arm_config/launch/gazebo.launch b/engineer_arm_config/launch/gazebo.launch new file mode 100644 index 0000000..429bca7 --- /dev/null +++ b/engineer_arm_config/launch/gazebo.launch @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/engineer_arm_config/launch/joystick_control.launch b/engineer_arm_config/launch/joystick_control.launch new file mode 100644 index 0000000..9411f6e --- /dev/null +++ b/engineer_arm_config/launch/joystick_control.launch @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/engineer_arm_config/launch/load_controllers.launch b/engineer_arm_config/launch/load_controllers.launch index 39c6842..4a592bd 100644 --- a/engineer_arm_config/launch/load_controllers.launch +++ b/engineer_arm_config/launch/load_controllers.launch @@ -20,27 +20,21 @@ args="load controllers/robot_state_controller controllers/joint_state_controller - controllers/joint1_calibration_controller - controllers/joint2_calibration_controller - controllers/joint3_calibration_controller - controllers/joint4_calibration_controller - controllers/joint5_calibration_controller - controllers/joint6_calibration_controller - controllers/joint7_calibration_controller - controllers/arm_trajectory_controller + controllers/left_arm_controller + controllers/right_arm_controller + controllers/gimbal_arm_controller + controllers/chassis_controller + " + /> + + diff --git a/engineer_arm_config/launch/load_move_group.launch b/engineer_arm_config/launch/load_move_group.launch index 2f9b1f7..f1edc8c 100644 --- a/engineer_arm_config/launch/load_move_group.launch +++ b/engineer_arm_config/launch/load_move_group.launch @@ -9,7 +9,7 @@ - + diff --git a/engineer_arm_config/launch/move_group.launch b/engineer_arm_config/launch/move_group.launch index 07af4a0..19bace8 100644 --- a/engineer_arm_config/launch/move_group.launch +++ b/engineer_arm_config/launch/move_group.launch @@ -1,65 +1,105 @@ - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - + + + - - - - - + + + - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/engineer_arm_config/launch/moveit.rviz b/engineer_arm_config/launch/moveit.rviz index a0a4838..6a876dd 100644 --- a/engineer_arm_config/launch/moveit.rviz +++ b/engineer_arm_config/launch/moveit.rviz @@ -5,13 +5,8 @@ Panels: Property Tree Widget: Expanded: - /MotionPlanning1 - - /MotionPlanning1/Planning Request1 - - /MotionPlanning1/Planning Metrics1 - - /MotionPlanning1/Planned Path1 - - /TF1 - - /TF1/Frames1 Splitter Ratio: 0.5 - Tree Height: 315 + Tree Height: 226 - Class: rviz/Help Name: Help - Class: rviz/Views @@ -19,10 +14,6 @@ Panels: - /Current View1 Name: Views Splitter Ratio: 0.5 -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 Visualization Manager: Class: "" Displays: @@ -32,7 +23,7 @@ Visualization Manager: Color: 160; 160; 164 Enabled: true Line Style: - Line Width: 0.029999999329447746 + Line Width: 0.03 Value: Lines Name: Grid Normal Cell Count: 0 @@ -44,177 +35,30 @@ Visualization Manager: Plane Cell Count: 10 Reference Frame: Value: true - - Acceleration_Scaling_Factor: 0.1 - Class: moveit_rviz_plugin/MotionPlanning + - Class: moveit_rviz_plugin/MotionPlanning Enabled: true - Move Group Namespace: "" - MoveIt_Allow_Approximate_IK: false - MoveIt_Allow_External_Program: false - MoveIt_Allow_Replanning: false - MoveIt_Allow_Sensor_Positioning: false - MoveIt_Planning_Attempts: 10 - MoveIt_Planning_Time: 5 - MoveIt_Use_Cartesian_Path: false - MoveIt_Use_Constraint_Aware_IK: false - MoveIt_Workspace: - Center: - X: 0 - Y: 0 - Z: 0 - Size: - X: 2 - Y: 2 - Z: 2 Name: MotionPlanning Planned Path: - Color Enabled: false - Interrupt Display: false - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - camera_optical_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - extend_arm_back: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - extend_arm_front: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - gimbal_lifter: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - left_back_wheel: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - left_front_wheel: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link3: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link4: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link5: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link6: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link7: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - mimic_link1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - ore_bin_lifter: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - ore_bin_rotator: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - pitch: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - right_back_wheel: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - right_front_wheel: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - tools_link: - Alpha: 1 - Show Axes: false - Show Trail: false - tools_link_st: - Alpha: 1 - Show Axes: false - Show Trail: false - yaw: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Loop Animation: false + Links: ~ + Loop Animation: true Robot Alpha: 0.5 - Robot Color: 150; 50; 150 Show Robot Collision: false Show Robot Visual: true Show Trail: false State Display Time: 0.05 s - Trail Step Size: 1 Trajectory Topic: move_group/display_planned_path - Use Sim Time: false Planning Metrics: Payload: 1 Show Joint Torques: false Show Manipulability: false Show Manipulability Index: false Show Weight Limit: false - TextHeight: 0.07999999821186066 Planning Request: Colliding Link Color: 255; 0; 0 Goal State Alpha: 1 Goal State Color: 250; 128; 0 Interactive Marker Size: 0 Joint Violation Color: 255; 0; 255 - Planning Group: engineer_arm Query Goal State: true Query Start State: false Show Workspace: false @@ -224,272 +68,19 @@ Visualization Manager: Robot Description: robot_description Scene Geometry: Scene Alpha: 1 - Scene Color: 50; 230; 50 - Scene Display Time: 0.009999999776482582 Show Scene Geometry: true Voxel Coloring: Z-Axis Voxel Rendering: Occupied Voxels Scene Robot: Attached Body Color: 150; 50; 150 - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - base_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - camera_optical_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - extend_arm_back: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - extend_arm_front: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - gimbal_lifter: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - left_back_wheel: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - left_front_wheel: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link3: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link4: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link5: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link6: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - link7: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - mimic_link1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - ore_bin_lifter: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - ore_bin_rotator: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - pitch: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - right_back_wheel: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - right_front_wheel: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - tools_link: - Alpha: 1 - Show Axes: false - Show Trail: false - tools_link_st: - Alpha: 1 - Show Axes: false - Show Trail: false - yaw: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true + Links: ~ Robot Alpha: 0.5 - Show Robot Collision: false - Show Robot Visual: true - Value: true - Velocity_Scaling_Factor: 0.1 - - Class: rviz/TF - Enabled: true - Filter (blacklist): "" - Filter (whitelist): "" - Frame Timeout: 15 - Frames: - All Enabled: false - base_link: - Value: true - camera_link: - Value: false - camera_optical_frame: - Value: false - exchanger: - Value: true - extend_arm_back: - Value: false - extend_arm_front: - Value: false - gimbal_lifter: - Value: false - left_back_wheel: - Value: false - left_front_wheel: - Value: false - link1: - Value: false - link2: - Value: false - link3: - Value: false - link4: - Value: false - link5: - Value: false - link6: - Value: false - link7: - Value: true - map: - Value: false - mimic_link1: - Value: false - odom: - Value: false - ore_bin_lifter: - Value: false - ore_bin_rotator: - Value: false - pitch: - Value: false - real_world: - Value: false - right_back_wheel: - Value: false - right_front_wheel: - Value: false - tools_link: - Value: false - tools_link_st: - Value: false - yaw: - Value: false - Marker Alpha: 1 - Marker Scale: 1 - Name: TF - Show Arrows: true - Show Axes: true - Show Names: true - Tree: - base_link: - {} - exchanger: - {} - extend_arm_back: - {} - extend_arm_front: - {} - gimbal_lifter: - camera_link: - {} - camera_optical_frame: - {} - left_back_wheel: - {} - left_front_wheel: - {} - link1: - {} - link2: - {} - link3: - {} - link4: - {} - link5: - {} - link6: - {} - link7: - tools_link: - {} - tools_link_st: - {} - map: - {} - mimic_link1: - {} - odom: - {} - ore_bin_lifter: - {} - ore_bin_rotator: - {} - pitch: - {} - real_world: - {} - right_back_wheel: - {} - right_front_wheel: - {} - yaw: - {} - Update Interval: 0 + Show Scene Robot: true Value: true Enabled: true Global Options: Background Color: 48; 48; 48 - Default Light: true Fixed Frame: base_link - Frame Rate: 30 Name: root Tools: - Class: rviz/Interact @@ -500,30 +91,30 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 1.4108070135116577 + Distance: 2.0 Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 + Stereo Eye Separation: 0.06 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Field of View: 0.75 Focal Point: - X: 0.23874804377555847 - Y: 0.04174364358186722 - Z: 0.6224369406700134 + X: -0.1 + Y: 0.25 + Z: 0.30 Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 + Focal Shape Size: 0.05 Invert Z Axis: false Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.48979732394218445 + Near Clip Distance: 0.01 + Pitch: 0.5 Target Frame: base_link - Yaw: 3.2827558517456055 + Yaw: -0.6232355833053589 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1016 + Height: 848 Help: collapsed: false Hide Left Dock: false @@ -532,9 +123,9 @@ Window Geometry: collapsed: false MotionPlanning - Trajectory Slider: collapsed: false - QMainWindow State: 000000ff00000000fd0000000100000000000001f30000039efc0200000007fb000000100044006900730070006c006100790073010000003d000001cc000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000020f000001cc0000017d00ffffff000004880000039e00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000100000000000001f0000002f6fc0200000007fb000000100044006900730070006c006100790073010000003d00000173000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001b60000017d0000017d00ffffff00000315000002f600000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Views: collapsed: false - Width: 1665 - X: 757 - Y: 27 + Width: 1291 + X: 454 + Y: 25 diff --git a/engineer_arm_config/launch/moveit_rviz.launch b/engineer_arm_config/launch/moveit_rviz.launch new file mode 100644 index 0000000..a4605c0 --- /dev/null +++ b/engineer_arm_config/launch/moveit_rviz.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + diff --git a/engineer_arm_config/launch/ompl_planning_pipeline.launch.xml b/engineer_arm_config/launch/ompl_planning_pipeline.launch.xml index 7578dd5..e923462 100644 --- a/engineer_arm_config/launch/ompl_planning_pipeline.launch.xml +++ b/engineer_arm_config/launch/ompl_planning_pipeline.launch.xml @@ -1,25 +1,24 @@ - - + + - - + + - + + + + - - - - - + diff --git a/engineer_arm_config/launch/planning_context.launch b/engineer_arm_config/launch/planning_context.launch index 81bbf7d..cf9d2c8 100644 --- a/engineer_arm_config/launch/planning_context.launch +++ b/engineer_arm_config/launch/planning_context.launch @@ -1,25 +1,26 @@ - - + + - - + + - - + + - - + + - - - - + + + + + - - - - + + + + + diff --git a/engineer_arm_config/launch/planning_pipeline.launch.xml b/engineer_arm_config/launch/planning_pipeline.launch.xml index 3689d56..4b4d0d6 100644 --- a/engineer_arm_config/launch/planning_pipeline.launch.xml +++ b/engineer_arm_config/launch/planning_pipeline.launch.xml @@ -1,10 +1,10 @@ - + - + - + diff --git a/engineer_arm_config/launch/ros_control_moveit_controller_manager.launch.xml b/engineer_arm_config/launch/ros_control_moveit_controller_manager.launch.xml new file mode 100644 index 0000000..9ebc91c --- /dev/null +++ b/engineer_arm_config/launch/ros_control_moveit_controller_manager.launch.xml @@ -0,0 +1,4 @@ + + + + diff --git a/engineer_arm_config/launch/ros_controllers.launch b/engineer_arm_config/launch/ros_controllers.launch new file mode 100644 index 0000000..aad62da --- /dev/null +++ b/engineer_arm_config/launch/ros_controllers.launch @@ -0,0 +1,11 @@ + + + + + + + + + + diff --git a/engineer_arm_config/launch/run_benchmark_ompl.launch b/engineer_arm_config/launch/run_benchmark_ompl.launch new file mode 100644 index 0000000..0914c6c --- /dev/null +++ b/engineer_arm_config/launch/run_benchmark_ompl.launch @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + + + + + diff --git a/engineer_arm_config/launch/sensor_manager.launch.xml b/engineer_arm_config/launch/sensor_manager.launch.xml new file mode 100644 index 0000000..5f635ad --- /dev/null +++ b/engineer_arm_config/launch/sensor_manager.launch.xml @@ -0,0 +1,17 @@ + + + + + + + + + + + + + + + + + diff --git a/engineer_arm_config/launch/setup_assistant.launch b/engineer_arm_config/launch/setup_assistant.launch new file mode 100644 index 0000000..dae2d15 --- /dev/null +++ b/engineer_arm_config/launch/setup_assistant.launch @@ -0,0 +1,16 @@ + + + + + + + + + + + + diff --git a/engineer_arm_config/launch/simple_moveit_controller_manager.launch.xml b/engineer_arm_config/launch/simple_moveit_controller_manager.launch.xml new file mode 100644 index 0000000..321b88f --- /dev/null +++ b/engineer_arm_config/launch/simple_moveit_controller_manager.launch.xml @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/engineer_arm_config/launch/stomp_planning_pipeline.launch.xml b/engineer_arm_config/launch/stomp_planning_pipeline.launch.xml new file mode 100644 index 0000000..50fb33a --- /dev/null +++ b/engineer_arm_config/launch/stomp_planning_pipeline.launch.xml @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + diff --git a/engineer_arm_config/launch/trajectory_execution.launch.xml b/engineer_arm_config/launch/trajectory_execution.launch.xml index a8c0ac0..0fff921 100644 --- a/engineer_arm_config/launch/trajectory_execution.launch.xml +++ b/engineer_arm_config/launch/trajectory_execution.launch.xml @@ -1,21 +1,23 @@ + - + + + - - - + + + - - - - - - + + + + + + - - - + + diff --git a/engineer_arm_config/launch/warehouse.launch b/engineer_arm_config/launch/warehouse.launch new file mode 100644 index 0000000..0712e67 --- /dev/null +++ b/engineer_arm_config/launch/warehouse.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + diff --git a/engineer_arm_config/launch/warehouse_settings.launch.xml b/engineer_arm_config/launch/warehouse_settings.launch.xml new file mode 100644 index 0000000..e473b08 --- /dev/null +++ b/engineer_arm_config/launch/warehouse_settings.launch.xml @@ -0,0 +1,16 @@ + + + + + + + + + + + + + + + + diff --git a/engineer_arm_config/package.xml b/engineer_arm_config/package.xml index eb4cd9c..dc51f38 100644 --- a/engineer_arm_config/package.xml +++ b/engineer_arm_config/package.xml @@ -1,27 +1,41 @@ - engineer_arm_config - 1.0.0 - - An automatically generated package with all the configuration and launch files for using the engineer with the - MoveIt Motion Planning Framework - - QiayuanLiao - QiayuanLiao - BSD + engineer_arm_config + 0.3.0 + + An automatically generated package with all the configuration and launch files for using the engineer with the MoveIt Motion Planning Framework + + CH + CH - http://moveit.ros.org/ - https://github.com/ros-planning/moveit/issues - https://github.com/ros-planning/moveit + BSD + + http://moveit.ros.org/ + https://github.com/moveit/moveit/issues + https://github.com/moveit/moveit + + catkin + + moveit_ros_move_group + moveit_fake_controller_manager + moveit_kinematics + moveit_planners + moveit_ros_visualization + moveit_setup_assistant + moveit_simple_controller_manager + joint_state_publisher + joint_state_publisher_gui + robot_state_publisher + rviz + tf2_ros + xacro + + + + + + rm_description - catkin - moveit_ros_move_group - moveit_fake_controller_manager - moveit_kinematics - moveit_planners_ompl - moveit_ros_visualization - moveit_setup_assistant - tf2_ros - xacro diff --git a/engineer_custom_controller/CMakeLists.txt b/engineer_custom_controller/CMakeLists.txt new file mode 100644 index 0000000..36816f1 --- /dev/null +++ b/engineer_custom_controller/CMakeLists.txt @@ -0,0 +1,118 @@ +cmake_minimum_required(VERSION 3.5.0) +project(engineer_custom_controller) + +## Use C++14 +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +## By adding -Wall and -Werror, the compiler does not ignore warnings anymore, +## enforcing cleaner code. +add_definitions(-Wall -Werror -Wno-address-of-packed-member) + +## Find catkin macros and libraries +find_package(serial REQUIRED) + +find_package(catkin REQUIRED + COMPONENTS + sensor_msgs + roscpp + rm_msgs + rm_ecat_msgs + serial + rm_common + tf2_geometry_msgs + std_msgs + actionlib + nav_msgs +) + +## Find system libraries +#find_package(Eigen3 REQUIRED) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + INCLUDE_DIRS + include + LIBRARIES + CATKIN_DEPENDS + roscpp + sensor_msgs + rm_msgs + rm_common + tf2_geometry_msgs + std_msgs + actionlib + nav_msgs + DEPENDS +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +## Declare cpp executables +FILE(GLOB ALL_SOURCES "src/*.cpp") +add_executable(${PROJECT_NAME} ${ALL_SOURCES}) + +## Add dependencies to exported targets, like ROS msgs or srvs +add_dependencies(${PROJECT_NAME} + ${catkin_EXPORTED_TARGETS} +) + +## Specify libraries to link executable targets against +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} +) + +############# +## Install ## +############# + +# Mark executables and/or libraries for installation +install( + TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +# Mark cpp header files for installation +install( + DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" +) + +## Mark other files for installation +install( + DIRECTORY launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +############# +## Testing ## +############# + +#if (${CATKIN_ENABLE_TESTING}) +# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread") +# ## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test +# test/test_ros_package_template.cpp +# test/AlgorithmTest.cpp) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}_core) +#endif () diff --git a/engineer_custom_controller/config/engineer_custom_controller.yaml b/engineer_custom_controller/config/engineer_custom_controller.yaml new file mode 100644 index 0000000..9923f1f --- /dev/null +++ b/engineer_custom_controller/config/engineer_custom_controller.yaml @@ -0,0 +1,18 @@ +engineer_custom_controller: + joint_names: + - left_joint1 + - left_joint2 + - left_joint3 + - left_joint4 + - left_joint5 + - left_joint6 + - left_joint7 + - right_joint1 + - right_joint2 + - right_joint3 + - right_joint4 + - right_joint5 + - right_joint6 + - right_joint7 + maxVal: 12.56 + minVal: -12.56 \ No newline at end of file diff --git a/engineer_custom_controller/include/engineer_custom_controller/common/data.h b/engineer_custom_controller/include/engineer_custom_controller/common/data.h new file mode 100644 index 0000000..e1932ba --- /dev/null +++ b/engineer_custom_controller/include/engineer_custom_controller/common/data.h @@ -0,0 +1,107 @@ +// +// Created by ch on 24-11-23. +// + +#pragma once + +#include +#include +#include +#include +#include "rm_msgs/RobotCustomData.h" +#include "rm_ecat_msgs/RmEcatMitSlaveReadings.h" + +#include "engineer_custom_controller/common/protocol.h" + +namespace engineer_custom_controller +{ +class Base +{ +public: + serial::Serial serial_; + bool engineer_custom_controller_is_online_ = false; + + void initSerial() + { + serial::Timeout timeout = serial::Timeout::simpleTimeout(50); + serial_.setPort("/dev/usbCustomController"); + serial_.setBaudrate(115200); + // serial_.setBaudrate(921600); + serial_.setTimeout(timeout); + if (serial_.isOpen()) + return; + try + { + serial_.open(); + } + catch (serial::IOException& e) + { + ROS_ERROR("Cannot open custom controller port"); + } + } + + // CRC check + uint8_t getCRC8CheckSum(unsigned char* pch_message, unsigned int dw_length, unsigned char uc_crc_8) + { + unsigned char uc_index; + while (dw_length--) + { + uc_index = uc_crc_8 ^ (*pch_message++); + uc_crc_8 = engineer_custom_controller::kCrc8Table[uc_index]; + } + return (uc_crc_8); + } + + uint32_t verifyCRC8CheckSum(unsigned char* pch_message, unsigned int dw_length) + { + unsigned char uc_expected; + if ((pch_message == nullptr) || (dw_length <= 2)) + return 0; + uc_expected = getCRC8CheckSum(pch_message, dw_length - 1, engineer_custom_controller::kCrc8Init); + return (uc_expected == pch_message[dw_length - 1]); + } + + void appendCRC8CheckSum(unsigned char* pch_message, unsigned int dw_length) + { + unsigned char uc_crc; + if ((pch_message == nullptr) || (dw_length <= 2)) + return; + uc_crc = getCRC8CheckSum((unsigned char*)pch_message, dw_length - 1, engineer_custom_controller::kCrc8Init); + pch_message[dw_length - 1] = uc_crc; + } + + uint16_t getCRC16CheckSum(uint8_t* pch_message, uint32_t dw_length, uint16_t w_crc) + { + uint8_t chData; + if (pch_message == nullptr) + return 0xFFFF; + while (dw_length--) + { + chData = *pch_message++; + (w_crc) = (static_cast(w_crc) >> 8) ^ + engineer_custom_controller::wCRC_table[(static_cast(w_crc) ^ static_cast(chData)) & 0x00ff]; + } + return w_crc; + } + + uint32_t verifyCRC16CheckSum(uint8_t* pch_message, uint32_t dw_length) + { + uint16_t w_expected; + if ((pch_message == nullptr) || (dw_length <= 2)) + return 0; + w_expected = getCRC16CheckSum(pch_message, dw_length - 2, engineer_custom_controller::kCrc16Init); + return ((w_expected & 0xff) == pch_message[dw_length - 2] && + ((w_expected >> 8) & 0xff) == pch_message[dw_length - 1]); + } + + void appendCRC16CheckSum(uint8_t* pch_message, uint32_t dw_length) + { + uint16_t wCRC; + if ((pch_message == nullptr) || (dw_length <= 2)) + return; + wCRC = getCRC16CheckSum(static_cast(pch_message), dw_length - 2, engineer_custom_controller::kCrc16Init); + pch_message[dw_length - 2] = static_cast((wCRC & 0x00ff)); + pch_message[dw_length - 1] = static_cast(((wCRC >> 8) & 0x00ff)); + } +}; +} // namespace engineer_custom_controller diff --git a/engineer_custom_controller/include/engineer_custom_controller/common/protocol.h b/engineer_custom_controller/include/engineer_custom_controller/common/protocol.h new file mode 100644 index 0000000..ed1f69b --- /dev/null +++ b/engineer_custom_controller/include/engineer_custom_controller/common/protocol.h @@ -0,0 +1,86 @@ +// +// Created by ch on 24-11-23. +// +#pragma once +#define __packed __attribute__((packed)) + +#include +namespace engineer_custom_controller +{ +typedef enum +{ + CUSTOM_CONTROLLER_CMD = 0x0302, // custom_controller + SIM_KEYBOARD_MOUSE_CMD = 0x0306, + ROBOT_TO_CUSTOM_CONTROLLER_CMD = 0x0309, +} VideoTransmissionCmdId; + +typedef struct +{ + uint8_t sof; + uint16_t data_length; + uint8_t seq; + uint8_t crc_8; +} __packed FrameHeader; + +typedef struct +{ + uint8_t data[30]; + +} __packed CustomControllerData; + +typedef struct +{ + uint16_t key_value; + uint16_t x_position : 12; + uint16_t mouse_left : 4; + uint16_t y_position : 12; + uint16_t mouse_right : 4; + uint16_t reserved; +} __packed SimKeyboardMouseData; + +typedef struct +{ + uint8_t data[30]; +} __packed RobotToCustomData; + +/***********************Frame tail(CRC8_CRC16)********************************************/ +const uint8_t kCrc8Init = 0xff; +const uint8_t kCrc8Table[256] = { + 0x00, 0x5e, 0xbc, 0xe2, 0x61, 0x3f, 0xdd, 0x83, 0xc2, 0x9c, 0x7e, 0x20, 0xa3, 0xfd, 0x1f, 0x41, 0x9d, 0xc3, 0x21, + 0x7f, 0xfc, 0xa2, 0x40, 0x1e, 0x5f, 0x01, 0xe3, 0xbd, 0x3e, 0x60, 0x82, 0xdc, 0x23, 0x7d, 0x9f, 0xc1, 0x42, 0x1c, + 0xfe, 0xa0, 0xe1, 0xbf, 0x5d, 0x03, 0x80, 0xde, 0x3c, 0x62, 0xbe, 0xe0, 0x02, 0x5c, 0xdf, 0x81, 0x63, 0x3d, 0x7c, + 0x22, 0xc0, 0x9e, 0x1d, 0x43, 0xa1, 0xff, 0x46, 0x18, 0xfa, 0xa4, 0x27, 0x79, 0x9b, 0xc5, 0x84, 0xda, 0x38, 0x66, + 0xe5, 0xbb, 0x59, 0x07, 0xdb, 0x85, 0x67, 0x39, 0xba, 0xe4, 0x06, 0x58, 0x19, 0x47, 0xa5, 0xfb, 0x78, 0x26, 0xc4, + 0x9a, 0x65, 0x3b, 0xd9, 0x87, 0x04, 0x5a, 0xb8, 0xe6, 0xa7, 0xf9, 0x1b, 0x45, 0xc6, 0x98, 0x7a, 0x24, 0xf8, 0xa6, + 0x44, 0x1a, 0x99, 0xc7, 0x25, 0x7b, 0x3a, 0x64, 0x86, 0xd8, 0x5b, 0x05, 0xe7, 0xb9, 0x8c, 0xd2, 0x30, 0x6e, 0xed, + 0xb3, 0x51, 0x0f, 0x4e, 0x10, 0xf2, 0xac, 0x2f, 0x71, 0x93, 0xcd, 0x11, 0x4f, 0xad, 0xf3, 0x70, 0x2e, 0xcc, 0x92, + 0xd3, 0x8d, 0x6f, 0x31, 0xb2, 0xec, 0x0e, 0x50, 0xaf, 0xf1, 0x13, 0x4d, 0xce, 0x90, 0x72, 0x2c, 0x6d, 0x33, 0xd1, + 0x8f, 0x0c, 0x52, 0xb0, 0xee, 0x32, 0x6c, 0x8e, 0xd0, 0x53, 0x0d, 0xef, 0xb1, 0xf0, 0xae, 0x4c, 0x12, 0x91, 0xcf, + 0x2d, 0x73, 0xca, 0x94, 0x76, 0x28, 0xab, 0xf5, 0x17, 0x49, 0x08, 0x56, 0xb4, 0xea, 0x69, 0x37, 0xd5, 0x8b, 0x57, + 0x09, 0xeb, 0xb5, 0x36, 0x68, 0x8a, 0xd4, 0x95, 0xcb, 0x29, 0x77, 0xf4, 0xaa, 0x48, 0x16, 0xe9, 0xb7, 0x55, 0x0b, + 0x88, 0xd6, 0x34, 0x6a, 0x2b, 0x75, 0x97, 0xc9, 0x4a, 0x14, 0xf6, 0xa8, 0x74, 0x2a, 0xc8, 0x96, 0x15, 0x4b, 0xa9, + 0xf7, 0xb6, 0xe8, 0x0a, 0x54, 0xd7, 0x89, 0x6b, 0x35, +}; +const uint16_t kCrc16Init = 0xffff; +const uint16_t wCRC_table[256] = { + 0x0000, 0x1189, 0x2312, 0x329b, 0x4624, 0x57ad, 0x6536, 0x74bf, 0x8c48, 0x9dc1, 0xaf5a, 0xbed3, 0xca6c, 0xdbe5, + 0xe97e, 0xf8f7, 0x1081, 0x0108, 0x3393, 0x221a, 0x56a5, 0x472c, 0x75b7, 0x643e, 0x9cc9, 0x8d40, 0xbfdb, 0xae52, + 0xdaed, 0xcb64, 0xf9ff, 0xe876, 0x2102, 0x308b, 0x0210, 0x1399, 0x6726, 0x76af, 0x4434, 0x55bd, 0xad4a, 0xbcc3, + 0x8e58, 0x9fd1, 0xeb6e, 0xfae7, 0xc87c, 0xd9f5, 0x3183, 0x200a, 0x1291, 0x0318, 0x77a7, 0x662e, 0x54b5, 0x453c, + 0xbdcb, 0xac42, 0x9ed9, 0x8f50, 0xfbef, 0xea66, 0xd8fd, 0xc974, 0x4204, 0x538d, 0x6116, 0x709f, 0x0420, 0x15a9, + 0x2732, 0x36bb, 0xce4c, 0xdfc5, 0xed5e, 0xfcd7, 0x8868, 0x99e1, 0xab7a, 0xbaf3, 0x5285, 0x430c, 0x7197, 0x601e, + 0x14a1, 0x0528, 0x37b3, 0x263a, 0xdecd, 0xcf44, 0xfddf, 0xec56, 0x98e9, 0x8960, 0xbbfb, 0xaa72, 0x6306, 0x728f, + 0x4014, 0x519d, 0x2522, 0x34ab, 0x0630, 0x17b9, 0xef4e, 0xfec7, 0xcc5c, 0xddd5, 0xa96a, 0xb8e3, 0x8a78, 0x9bf1, + 0x7387, 0x620e, 0x5095, 0x411c, 0x35a3, 0x242a, 0x16b1, 0x0738, 0xffcf, 0xee46, 0xdcdd, 0xcd54, 0xb9eb, 0xa862, + 0x9af9, 0x8b70, 0x8408, 0x9581, 0xa71a, 0xb693, 0xc22c, 0xd3a5, 0xe13e, 0xf0b7, 0x0840, 0x19c9, 0x2b52, 0x3adb, + 0x4e64, 0x5fed, 0x6d76, 0x7cff, 0x9489, 0x8500, 0xb79b, 0xa612, 0xd2ad, 0xc324, 0xf1bf, 0xe036, 0x18c1, 0x0948, + 0x3bd3, 0x2a5a, 0x5ee5, 0x4f6c, 0x7df7, 0x6c7e, 0xa50a, 0xb483, 0x8618, 0x9791, 0xe32e, 0xf2a7, 0xc03c, 0xd1b5, + 0x2942, 0x38cb, 0x0a50, 0x1bd9, 0x6f66, 0x7eef, 0x4c74, 0x5dfd, 0xb58b, 0xa402, 0x9699, 0x8710, 0xf3af, 0xe226, + 0xd0bd, 0xc134, 0x39c3, 0x284a, 0x1ad1, 0x0b58, 0x7fe7, 0x6e6e, 0x5cf5, 0x4d7c, 0xc60c, 0xd785, 0xe51e, 0xf497, + 0x8028, 0x91a1, 0xa33a, 0xb2b3, 0x4a44, 0x5bcd, 0x6956, 0x78df, 0x0c60, 0x1de9, 0x2f72, 0x3efb, 0xd68d, 0xc704, + 0xf59f, 0xe416, 0x90a9, 0x8120, 0xb3bb, 0xa232, 0x5ac5, 0x4b4c, 0x79d7, 0x685e, 0x1ce1, 0x0d68, 0x3ff3, 0x2e7a, + 0xe70e, 0xf687, 0xc41c, 0xd595, 0xa12a, 0xb0a3, 0x8238, 0x93b1, 0x6b46, 0x7acf, 0x4854, 0x59dd, 0x2d62, 0x3ceb, + 0x0e70, 0x1ff9, 0xf78f, 0xe606, 0xd49d, 0xc514, 0xb1ab, 0xa022, 0x92b9, 0x8330, 0x7bc7, 0x6a4e, 0x58d5, 0x495c, + 0x3de3, 0x2c6a, 0x1ef1, 0x0f78 +}; +} // namespace engineer_custom_controller diff --git a/engineer_custom_controller/include/engineer_custom_controller/custom_controller_data_sender.h b/engineer_custom_controller/include/engineer_custom_controller/custom_controller_data_sender.h new file mode 100644 index 0000000..2274140 --- /dev/null +++ b/engineer_custom_controller/include/engineer_custom_controller/custom_controller_data_sender.h @@ -0,0 +1,58 @@ +// +// Created by ljyi on 2026/4/6. +// + +#pragma once + +#include +#include +#include + +#include "common/data.h" +#include "common/protocol.h" + +namespace engineer_custom_controller +{ +class CustomControllerDataSender +{ +public: + explicit CustomControllerDataSender(ros::NodeHandle& nh, Base& base) : base_(base) + { + ROS_INFO("Custom data sender load."); + joint_names_ = nh.param("joint_names", std::vector()); + maxVal = nh.param("maxVal", 12.56); + minVal = nh.param("minVal", -12.56); + joint_state_sub_ = nh.subscribe("/joint_states", 1, &CustomControllerDataSender::jointStateCallback, this); + ecat_data_sub_ = nh.subscribe("/rm_ecat_hw/mit_readings", 1, &CustomControllerDataSender::ecatDataCallback, this); + } + virtual ~CustomControllerDataSender() = default; + +private: + void jointStateCallback(const sensor_msgs::JointState::ConstPtr& data); + void ecatDataCallback(const rm_ecat_msgs::RmEcatMitSlaveReadings::ConstPtr& data); + void sendCustomControllerData(const sensor_msgs::JointState::ConstPtr& data); + void pack(uint8_t* tx_buffer, uint8_t* data, int cmd_id, int len); + void clearTxBuffer() + { + for (int i = 0; i < k_frame_length_; i++) + tx_buffer_[i] = 0; + tx_len_ = 0; + } + + int16_t rawData2Int16(double value); + + double maxVal, minVal; + std::vector joint_names_{}; + std::map joint2position_{}; + ros::Subscriber joint_state_sub_, ecat_data_sub_; + ros::Time last_send_time_{}; + Base& base_; + uint8_t tx_buffer_[128]{}; + realtime_tools::RealtimeBuffer button_rt_buffer_{}; + int tx_len_{}; + bool r_pressed_ = false; + + const int k_frame_length_ = 128, k_header_length_ = 5, k_cmd_id_length_ = 2, k_tail_length_ = 2; +}; + +} // namespace engineer_custom_controller diff --git a/engineer_custom_controller/include/engineer_custom_controller/engineer_custom_controller.h b/engineer_custom_controller/include/engineer_custom_controller/engineer_custom_controller.h new file mode 100644 index 0000000..c3dd2f5 --- /dev/null +++ b/engineer_custom_controller/include/engineer_custom_controller/engineer_custom_controller.h @@ -0,0 +1,44 @@ +// +// Created by ch on 24-11-23. +// +#pragma once + +#include + +#include "engineer_custom_controller/common/data.h" +#include "engineer_custom_controller/custom_controller_data_sender.h" + +namespace engineer_custom_controller +{ +class EngineerCustomController +{ +public: + explicit EngineerCustomController(ros::NodeHandle& nh) : custom_controller_data_sender(nh, base_), last_get_data_time_(ros::Time::now()) + { + ROS_INFO("Engineer custom controller load."); + robot_custom_data_pub_ = nh.advertise("robot_custom_data", 1); + + base_.initSerial(); + } + void read(); + void clearRxBuffer() + { + rx_buffer_.clear(); + rx_len_ = 0; + } + + ros::Publisher robot_custom_data_pub_; + + Base base_; + CustomControllerDataSender custom_controller_data_sender; + std::vector rx_buffer_; + int rx_len_{}; + +private: + int unpack(uint8_t* rx_data); + ros::Time last_get_data_time_; + const int k_frame_length_ = 64, k_header_length_ = 5, k_cmd_id_length_ = 2, k_tail_length_ = 2; + const int k_unpack_buffer_length_ = 128; + uint8_t unpack_buffer_[128]{}; +}; +} // namespace engineer_custom_controller diff --git a/engineer_custom_controller/launch/load.launch b/engineer_custom_controller/launch/load.launch new file mode 100644 index 0000000..b013923 --- /dev/null +++ b/engineer_custom_controller/launch/load.launch @@ -0,0 +1,4 @@ + + + + diff --git a/engineer_custom_controller/package.xml b/engineer_custom_controller/package.xml new file mode 100644 index 0000000..135cbf2 --- /dev/null +++ b/engineer_custom_controller/package.xml @@ -0,0 +1,22 @@ + + + engineer_custom_controller + 0.0.0 + The engineer_custom_controller package + + BSD + ch + + catkin + + roscpp + sensor_msgs + rm_msgs + rm_ecat_msgs + std_msgs + nav_msgs + actionlib + rm_common + tf2_geometry_msgs + serial + diff --git a/engineer_custom_controller/src/custom_controller_data_sender.cpp b/engineer_custom_controller/src/custom_controller_data_sender.cpp new file mode 100644 index 0000000..56a4c43 --- /dev/null +++ b/engineer_custom_controller/src/custom_controller_data_sender.cpp @@ -0,0 +1,94 @@ +// +// Created by ljyi on 2026/4/6. +// + +#include "engineer_custom_controller/custom_controller_data_sender.h" + +namespace engineer_custom_controller +{ +void CustomControllerDataSender::pack(uint8_t* tx_buffer, uint8_t* data, int cmd_id, int data_len) +{ + memset(tx_buffer, 0, k_frame_length_); + auto* frame_header = reinterpret_cast(tx_buffer); + + frame_header->sof = 0xA5; + frame_header->data_length = data_len; + memcpy(&tx_buffer[k_header_length_], reinterpret_cast(&cmd_id), k_cmd_id_length_); + base_.appendCRC8CheckSum(tx_buffer, k_header_length_); + memcpy(&tx_buffer[k_header_length_ + k_cmd_id_length_], data, data_len); + base_.appendCRC16CheckSum(tx_buffer, k_header_length_ + k_cmd_id_length_ + data_len + k_tail_length_); +} + +void CustomControllerDataSender::sendCustomControllerData(const sensor_msgs::JointState::ConstPtr& data) { + uint8_t tx_data[sizeof(CustomControllerData)] = { 0 }; + for (size_t i = 0; i < data->name.size(); ++i) + { + joint2position_[data->name[i]] = data->position[i]; + } + std::vector positions; + for (size_t i = 0; i < joint2position_.size(); ++i) { + double position = joint2position_[joint_names_[i]]; + positions.push_back(rawData2Int16(position)); + } + positions.push_back(*button_rt_buffer_.readFromRT()); + memcpy(tx_data, positions.data(), sizeof(int16_t) * positions.size()); + pack(tx_buffer_, tx_data, CUSTOM_CONTROLLER_CMD, sizeof(CustomControllerData)); + tx_len_ = k_header_length_ + k_cmd_id_length_ + static_cast(sizeof(CustomControllerData) + k_tail_length_); + try { + base_.serial_.write(tx_buffer_, tx_len_); + } catch (serial::PortNotOpenedException& e) { + ROS_ERROR_STREAM(e.what()); + } + if ((*button_rt_buffer_.readFromRT() >> 9) & 0x01 || (*button_rt_buffer_.readFromRT() >> 1) & 0x01) { + clearTxBuffer(); + SimKeyboardMouseData keyboard_mouse_data; + int len = k_header_length_ + k_cmd_id_length_ + static_cast(sizeof(SimKeyboardMouseData) + k_tail_length_);; + keyboard_mouse_data.key_value = 0x0059; + pack(tx_buffer_, reinterpret_cast(&keyboard_mouse_data), SIM_KEYBOARD_MOUSE_CMD, sizeof(SimKeyboardMouseData)); + try { + base_.serial_.write(tx_buffer_, len); + r_pressed_ = true; + } catch (serial::PortNotOpenedException& e) { + ROS_ERROR_STREAM(e.what()); + } + } + else if (r_pressed_) { + clearTxBuffer(); + SimKeyboardMouseData keyboard_mouse_data; + int len = k_header_length_ + k_cmd_id_length_ + static_cast(sizeof(SimKeyboardMouseData) + k_tail_length_); + keyboard_mouse_data.key_value = 0x0000; + pack(tx_buffer_, reinterpret_cast(&keyboard_mouse_data), SIM_KEYBOARD_MOUSE_CMD, sizeof(SimKeyboardMouseData)); + try { + for (int i = 0; i < 5; ++i) { + base_.serial_.write(tx_buffer_, len); + } + r_pressed_ = false; + } catch (serial::PortNotOpenedException& e) { + ROS_ERROR_STREAM(e.what()); + } + } + clearTxBuffer(); +} + +int16_t CustomControllerDataSender::rawData2Int16(double value) { + if (value > maxVal) { + value = maxVal; + } else if (value < minVal) { + value = minVal; + } + double norm = (value - minVal) / (maxVal - minVal); + return static_cast(norm * 65535.0 - 32768.0); +} + +void CustomControllerDataSender::jointStateCallback(const sensor_msgs::JointState::ConstPtr& data) { + sendCustomControllerData(data); + last_send_time_ = ros::Time::now(); +} + +void CustomControllerDataSender::ecatDataCallback(const rm_ecat_msgs::RmEcatMitSlaveReadings::ConstPtr &data) { + double raw_button_l = (data->readings[1].velocity[0] + 45.0) / (45. * 2. / 4095) + 0.00001; + double raw_button_r = (data->readings[1].velocity[1] + 45.0) / (45. * 2. / 4095) + 0.00001; + int16_t button_data = (static_cast(raw_button_l) << 8) | static_cast(raw_button_r); + button_rt_buffer_.writeFromNonRT(button_data); +} +} // namespace engineer_custom_controller diff --git a/engineer_custom_controller/src/engineer_custom_controller.cpp b/engineer_custom_controller/src/engineer_custom_controller.cpp new file mode 100644 index 0000000..56d4431 --- /dev/null +++ b/engineer_custom_controller/src/engineer_custom_controller.cpp @@ -0,0 +1,87 @@ +// +// Created by ch on 24-11-23. +// +#include "engineer_custom_controller/engineer_custom_controller.h" + +namespace engineer_custom_controller +{ +void EngineerCustomController::read() +{ + if (base_.serial_.available()) + { + rx_len_ = static_cast(base_.serial_.available()); + base_.serial_.read(rx_buffer_, rx_len_); + } + else + return; + uint8_t temp_buffer[128] = { 0 }; + int frame_len; + if (ros::Time::now() - last_get_data_time_ > ros::Duration(0.1)) + base_.engineer_custom_controller_is_online_ = false; + // for (int k_i = 0; k_i < rx_len_; ++k_i) + // ROS_INFO("%02X", rx_buffer_[k_i]); + if (rx_len_ < k_unpack_buffer_length_) + { + for (int k_i = 0; k_i < k_unpack_buffer_length_ - rx_len_; ++k_i) + temp_buffer[k_i] = unpack_buffer_[k_i + rx_len_]; + for (int k_i = 0; k_i < rx_len_; ++k_i) + temp_buffer[k_i + k_unpack_buffer_length_ - rx_len_] = rx_buffer_[k_i]; + for (int k_i = 0; k_i < k_unpack_buffer_length_; ++k_i) + unpack_buffer_[k_i] = temp_buffer[k_i]; + } + for (int k_i = 0; k_i < k_unpack_buffer_length_ - k_frame_length_; ++k_i) + { + if (unpack_buffer_[k_i] == 0xA5) + { + frame_len = unpack(&unpack_buffer_[k_i]); + if (frame_len != -1) + k_i += frame_len; + } + } + clearRxBuffer(); +} + +int EngineerCustomController::unpack(uint8_t* rx_data) +{ + uint16_t cmd_id; + int frame_len; + engineer_custom_controller::FrameHeader frame_header; + + memcpy(&frame_header, rx_data, k_header_length_); + if (static_cast(base_.verifyCRC8CheckSum(rx_data, k_header_length_))) + { + if (frame_header.data_length > 128) // temporary and inaccurate value + { + ROS_INFO("discard possible wrong frames, data length: %d", frame_header.data_length); + return 0; + } + frame_len = frame_header.data_length + k_header_length_ + k_cmd_id_length_ + k_tail_length_; + if (base_.verifyCRC16CheckSum(rx_data, frame_len) == 1) + { + cmd_id = (rx_data[6] << 8 | rx_data[5]); + switch (cmd_id) + { + case engineer_custom_controller::ROBOT_TO_CUSTOM_CONTROLLER_CMD: + { + engineer_custom_controller::RobotToCustomData robot_custom_ref; + rm_msgs::RobotCustomData robot_custom_data; + memcpy(&robot_custom_ref, rx_data + 7, sizeof(engineer_custom_controller::RobotToCustomData)); + for (int i = 0; i < 30; ++i) + robot_custom_data.data[i] = robot_custom_ref.data[i]; + robot_custom_data.stamp = last_get_data_time_; + robot_custom_data_pub_.publish(robot_custom_data); + break; + } + default: + ROS_WARN("Referee command ID 0x%02X not found.", cmd_id); + break; + } + base_.engineer_custom_controller_is_online_ = true; + last_get_data_time_ = ros::Time::now(); + return frame_len; + } + } + return -1; +} + +} // namespace engineer_custom_controller diff --git a/engineer_custom_controller/src/main.cpp b/engineer_custom_controller/src/main.cpp new file mode 100644 index 0000000..b118582 --- /dev/null +++ b/engineer_custom_controller/src/main.cpp @@ -0,0 +1,21 @@ +// +// Created by ch on 24-11-23. +// +#include +#include "engineer_custom_controller/engineer_custom_controller.h" +#include "engineer_custom_controller/custom_controller_data_sender.h" +#include "engineer_custom_controller/common/data.h" + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "engineer_custom_controller"); + ros::NodeHandle nh("~"); + engineer_custom_controller::EngineerCustomController enngineer_custom_controller(nh); + ros::Rate loop_rate(100); + while (ros::ok()) + { + ros::spinOnce(); + // enngineer_custom_controller.read(); + loop_rate.sleep(); + } +} diff --git a/engineer_middleware/CMakeLists.txt b/engineer_middleware/CMakeLists.txt index b6764bb..c93c3c8 100644 --- a/engineer_middleware/CMakeLists.txt +++ b/engineer_middleware/CMakeLists.txt @@ -10,6 +10,8 @@ set(CMAKE_CXX_STANDARD_REQUIRED ON) add_definitions(-Wall -Werror) ## Find catkin macros and libraries +find_package(yaml-cpp REQUIRED) + find_package(catkin REQUIRED COMPONENTS roscpp @@ -79,6 +81,7 @@ add_dependencies(${PROJECT_NAME} ## Specify libraries to link executable targets against target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} + yaml-cpp ) ############# diff --git a/engineer_middleware/config/engineer.yaml b/engineer_middleware/config/engineer.yaml index 24d6f86..f38c687 100644 --- a/engineer_middleware/config/engineer.yaml +++ b/engineer_middleware/config/engineer.yaml @@ -6,4 +6,5 @@ chassis: yaw: pid: { p: 5., i: 0.0, d: 0.0, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true } yaw_start_threshold: 0.05 + linear_start_threshold: 0.05 max_vel: 10 diff --git a/engineer_middleware/config/engineer2_simulation_controllers.yaml b/engineer_middleware/config/engineer2_simulation_controllers.yaml index a43ad6f..98a6a44 100644 --- a/engineer_middleware/config/engineer2_simulation_controllers.yaml +++ b/engineer_middleware/config/engineer2_simulation_controllers.yaml @@ -15,20 +15,58 @@ controllers: - joint4 - joint5 - joint6 + - joint7 + constraints: + goal_time: 5.0 + stopped_velocity_tolerance: 0.05 + joint1: { trajectory: 0.08, goal: 0.05 } + joint2: { trajectory: 0.08, goal: 0.05 } + joint3: { trajectory: 0.08, goal: 0.05 } + joint4: { trajectory: 0.08, goal: 0.05 } + joint5: { trajectory: 0.08, goal: 0.05 } + joint6: { trajectory: 0.08, goal: 0.05 } + joint7: { trajectory: 0.08, goal: 0.05 } + gains: + joint1: { p: 16000, i: 100, d: 500.0, i_clamp_max: 10, i_clamp_min: -10, antiwindup: true, publish_state: true } + joint2: { p: 100, i: 5, d: 10, i_clamp_max: 5, i_clamp_min: -5, antiwindup: true, publish_state: true } + joint3: { p: 1000, i: 100, d: 20, i_clamp_max: 10, i_clamp_min: -10, antiwindup: true, publish_state: true } + joint4: { p: 500, i: 1, d: 5, i_clamp_max: 1, i_clamp_min: -1, antiwindup: true, publish_state: true } + joint5: { p: 200, i: 1, d: 2, i_clamp_max: 1, i_clamp_min: -1, antiwindup: true, publish_state: true } + joint6: { p: 100, i: 1, d: 1, i_clamp_max: 1, i_clamp_min: -1, antiwindup: true, publish_state: true } + joint7: { p: 100, i: 1, d: 1, i_clamp_max: 1, i_clamp_min: -1, antiwindup: true, publish_state: true } + + gravity_compensation_controller: + type: gravity_compensation_controller/GravityCompensationJointTrajectoryController + joints: + - joint1 + - joint2 + - joint3 + - joint4 + - joint5 + - joint6 + - joint7 gains: - joint1: { p: 20000.0, i: 0, d: 400.0, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true } - joint2: { p: 100000, i: 0, d: 0.5, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true } - joint3: { p: 20.0, i: 0, d: 0.1, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true } - joint4: { p: 20.0, i: 0.0, d: 0.1, i_clamp_max: 0, i_clamp_min: 0.0, antiwindup: true, publish_state: true } - joint5: { p: 20.0, i: 0, d: 0.1, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true } - joint6: { p: 5.0, i: 0, d: 0.1, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true } + joint1: { p: 16000.0, i: 10.0, d: 580.0, i_clamp_max: 20, i_clamp_min: -20, antiwindup: true, publish_state: true } + joint2: { p: 60.0, i: 0.015, d: 1.8, i_clamp_max: 7, i_clamp_min: -7, antiwindup: true, publish_state: true } + joint3: { p: 135.0, i: 35.0, d: 3.5, i_clamp_max: 0, i_clamp_min: -16, antiwindup: true, publish_state: true } + joint4: { p: 85.0, i: 9.0, d: 2.0, i_clamp_max: 0, i_clamp_min: -9, antiwindup: true, publish_state: true } + joint5: { p: 24.0, i: 1.5, d: 0.4, i_clamp_max: 2, i_clamp_min: -2, antiwindup: true, publish_state: true } + joint6: { p: 18.0, i: 4.0, d: 0.5, i_clamp_max: 1, i_clamp_min: -1, antiwindup: true, publish_state: true } + joint7: { p: 17.0, i: 1.2, d: 0.4, i_clamp_max: 2, i_clamp_min: -2, antiwindup: true, publish_state: true } constraints: - joint1: { trajectory: 0.05, goal: 0.05 } - joint2: { trajectory: 0.05, goal: 0.05 } - joint3: { trajectory: 0.05, goal: 0.05 } - joint4: { trajectory: 0.05, goal: 0.05 } - joint5: { trajectory: 0.05, goal: 0.05 } - joint6: { trajectory: 0.05, goal: 0.05 } + goal_time: 5.0 + stopped_velocity_tolerance: 0.05 + joint1: { trajectory: 0.08, goal: 0.05 } + joint2: { trajectory: 0.08, goal: 0.05 } + joint3: { trajectory: 0.08, goal: 0.05 } + joint4: { trajectory: 0.08, goal: 0.05 } + joint5: { trajectory: 0.08, goal: 0.05 } + joint6: { trajectory: 0.08, goal: 0.05 } + joint7: { trajectory: 0.08, goal: 0.05 } + offset_torques: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + tau_g_scale: [0.0, 1.0, 0.57, 0.625, 1.0, 1.0, 1.0] + chain_root: base_link + chain_tip: gripper_link chassis_controller: type: rm_chassis_controllers/OmniController @@ -42,7 +80,7 @@ controllers: power_offset: -9.8 twist_angular: 0.5233 timeout: 0.1 - pid_follow: { p: 4.0, i: 0, d: 0.02, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } + pid_follow: { p: 4.0, i: 0, d: 0.02, i_clamp_max: 0.0, i_clamp_min: 0.0, antiwindup: true, publish_state: false } twist_covariance_diagonal: [ 0.001, 0.001, 0.001, 0.001, 0.001, 0.001 ] max_odom_vel: 10.0 # OmniController @@ -53,7 +91,7 @@ controllers: joint: left_front_wheel_joint <<: &wheel_setting radius: 0.07 - pid: { p: 0.4, i: 0, d: 0.0, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } + pid: { p: 0.3, i: 0.2, d: 0.0, i_clamp_max: 1.0, i_clamp_min: -1.0, antiwindup: true, publish_state: false } right_front: pose: [ 0.213, -0.1945, 0. ] roller_angle: 0.785 @@ -70,63 +108,31 @@ controllers: joint: right_back_wheel_joint <<: *wheel_setting - gimbal_controller: - type: rm_gimbal_controllers/Controller - yaw: - joint: "yaw_joint" - pid: { p: 0.5, i: 0, d: 0.0, i_clamp_max: 0.0, i_clamp_min: -0.0, antiwindup: true, publish_state: false } - pid_pos: { p: 15.0, i: 0, d: 0.0, i_clamp_max: 0.0, i_clamp_min: 0.0, antiwindup: true, publish_state: true } - k_chassis_vel: 0.035 - k_v: 1.0 - pitch: - joint: "pitch_joint" - pid: { p: 0., i: 0.0, d: 0.0, i_clamp_max: 0.0, i_clamp_min: -0.0, antiwindup: true, publish_state: false } - pid_pos: { p: 0.0, i: 0, d: 0.0, i_clamp_max: 0.0, i_clamp_min: 0.0, antiwindup: true, publish_state: true } - k_v: 1.0 - feedforward: - gravity: 0.0 - enable_gravity_compensation: false - mass_origin: [ 0.0,0.0,0.0 ] - bullet_solver: - resistance_coff_qd_10: 0.45 - resistance_coff_qd_15: 1.0 - resistance_coff_qd_16: 0.7 - resistance_coff_qd_18: 0.55 - resistance_coff_qd_30: 3.0 - g: 9.81 - delay: 0.1 - dt: 0.001 - timeout: 0.001 - publish_rate: 50 - - middle_pitch_controller: - type: effort_controllers/JointPositionController - joint: middle_pitch_joint - pid: { p: 5., i: 0., d: 1, i_max: 0.01, i_min: 0.01, antiwindup: true, publish_state: true } - - silver_lifter_controller: - type: effort_controllers/JointPositionController - joint: silver_lifter_joint - pid: { p: 1000., i: 0., d: 100, i_max: 0.01, i_min: 0.01, antiwindup: true, publish_state: true } - - silver_pusher_controller: - type: effort_controllers/JointPositionController - joint: silver_pusher_joint - pid: { p: 1000., i: 0., d: 100, i_max: 0.01, i_min: 0.01, antiwindup: true, publish_state: true } - - silver_rotator_controller: - type: effort_controllers/JointPositionController - joint: silver_rotator_joint - pid: { p: 50., i: 0., d: 1, i_max: 0.01, i_min: 0.01, antiwindup: true, publish_state: true } - - ######################### Gold Ore Controllers ######################## - - gold_lifter_controller: - type: effort_controllers/JointPositionController - joint: gold_lifter_joint - pid: { p: 100., i: 0., d: 10, i_max: 0.01, i_min: 0.01, antiwindup: true, publish_state: true } - - gold_pusher_controller: - type: effort_controllers/JointPositionController - joint: gold_pusher_joint - pid: { p: 1000., i: 0., d: 100, i_max: 0.01, i_min: 0.01, antiwindup: true, publish_state: true } +# gimbal_controller: +# type: rm_gimbal_controllers/Controller +# yaw: +# joint: "yaw_joint" +# pid: { p: 0.5, i: 0, d: 0.0, i_clamp_max: 0.0, i_clamp_min: -0.0, antiwindup: true, publish_state: false } +# pid_pos: { p: 15.0, i: 0, d: 0.0, i_clamp_max: 0.0, i_clamp_min: 0.0, antiwindup: true, publish_state: true } +# k_chassis_vel: 0.035 +# k_v: 1.0 +# pitch: +# joint: "pitch_joint" +# pid: { p: 0., i: 0.0, d: 0.0, i_clamp_max: 0.0, i_clamp_min: -0.0, antiwindup: true, publish_state: false } +# pid_pos: { p: 0.0, i: 0, d: 0.0, i_clamp_max: 0.0, i_clamp_min: 0.0, antiwindup: true, publish_state: true } +# k_v: 1.0 +# feedforward: +# gravity: 0.0 +# enable_gravity_compensation: false +# mass_origin: [ 0.0,0.0,0.0 ] +# bullet_solver: +# resistance_coff_qd_10: 0.45 +# resistance_coff_qd_15: 1.0 +# resistance_coff_qd_16: 0.7 +# resistance_coff_qd_18: 0.55 +# resistance_coff_qd_30: 3.0 +# g: 9.81 +# delay: 0.1 +# dt: 0.001 +# timeout: 0.001 +# publish_rate: 50 diff --git a/engineer_middleware/config/engineer2_steps_list.yaml b/engineer_middleware/config/engineer2_steps_list.yaml index ce4df3c..90759bd 100644 --- a/engineer_middleware/config/engineer2_steps_list.yaml +++ b/engineer_middleware/config/engineer2_steps_list.yaml @@ -5,212 +5,69 @@ common: accel: 0.1 timeout: 15. normally: &NORMALLY - speed: 0.4 - accel: 0.3 - timeout: 6 - quickly: &QUICKLY - speed: 0.6 - accel: 0.5 + speed: 0.5 + accel: 0.4 timeout: 6. + quickly: &QUICKLY + speed: 0.8 + accel: 0.6 + timeout: 5. + maximum: &MAXIMUM + speed: 1.0 + accel: 1.0 + timeout: 4. tolerance: small_tolerance: &SMALL_TOLERANCE - tolerance_joints: [ 0.005, 0.008, 0.015, 0.1, 0.1, 0.1, 0.03 ] + tolerance_joints: [ 0.005, 0.02, 0.01, 0.02, 0.02, 0.02, 0.02 ] normal_tolerance: &NORMAL_TOLERANCE - tolerance_joints: [ 0.012, 0.02, 0.015, 0.1, 0.2, 0.15, 0.07 ] + tolerance_joints: [ 0.012, 0.03, 0.03, 0.04, 0.04, 0.03, 0.03 ] bigger_tolerance: &BIGGER_TOLERANCE - tolerance_joints: [ 0.015, 0.04, 0.03, 0.35, 0.2, 0.2, 0.1 ] + tolerance_joints: [ 0.015, 0.05, 0.06, 0.08, 0.06, 0.07, 0.05 ] max_tolerance: &MAX_TOLERANCE - tolerance_joints: [ 0.03, 0.03, 0.03, 0.3, 0.3, 0.3 , 0.3] + tolerance_joints: [ 0.02, 0.08, 0.07, 0.1, 0.07, 0.07, 0.06 ] joint1: home: &JOINT1_HOME_POS - 0.1 - ready_get_ground: &JOINT1_GET_GROUND_POS - 0.005 - exchange: &JOINT1_EXCHANGE_POS - 0.35 - ready_get_small: &JOINT1_READY_GET_SMALL_POS - 0.42 - get_small: &JOINT1_GET_SMALL_POS - 0.325 - ready_get_stored: &JOINT1_READY_GET_STORED_POS - 0.54 - get_stored: &JOINT1_GET_STORED_POS - 0.478 - get_stored_gold: &JOINT1_GET_STORED_GOLD_POS - 0.04 - ready_store: &JOINT1_READY_STORE_POS - 0.44 - store: &JOINT1_STORE_POS - 0.34 - + 0.121 joint2: home: &JOINT2_HOME_POS - 1.0 - avoid_controller: &JOINT2_AVOID_CONTROLLER_POS - 0.6 - exchange: &JOINT2_EXCHANGE_POS - -1.57 - l_exchange: &JOINT2_L_EXCHANGE_POS - -0.3 - r_exchange: &JOINT2_R_EXCHANGE_POS - -2.5 - get_small_island_l: &JOINT2_GET_SMALL_ISLAND_L_POS - 0.858 - get_small_island_m: &JOINT2_GET_SMALL_ISLAND_M_POS - -1.101 - get_small_island_r: &JOINT2_GET_SMALL_ISLAND_R_POS - -2.052 - store_to_s1: &JOINT2_STORE_TO_S1_POS - -3.22 - store_to_s2: &JOINT2_STORE_TO_S2_POS - -2.364 - store_to_s3: &JOINT2_STORE_TO_S3_POS - -1.763 - get_stored_silver3: &JOINT2_GET_STORED_S3_POS - -2.055 - get_stored_silver2: &JOINT2_GET_STORED_S2_POS - -2.802 - get_stored_silver1: &JOINT2_GET_STORED_S1_POS - -3.833 - get_stored_gold: &JOINT2_GET_STORED_GOLD_POS - -1.062 - zero_pos: &JOINTN2_ZERO_POS - 0.000001 + 1.553 joint3: home: &JOINT3_HOME_POS - -2.3 - zero: &JOINT3_ZERO_POS - 0.00000001 - l_exchange: &JOINT3_L_EXCHANGE_POS - -0.7 - r_exchange: &JOINT3_R_EXCHANGE_POS - 0.7 - get_small_island_l: &JOINT3_GET_SMALL_ISLAND_L_POS - -1.643 - get_small_island_m: &JOINT3_GET_SMALL_ISLAND_M_POS - -1.833 - get_small_island_r: &JOINT3_GET_SMALL_ISLAND_R_POS - -0.815 - store_to_s1: &JOINT3_STORE_TO_S1_POS - -1.024 - store_to_s2: &JOINT3_STORE_TO_S2_POS - -1.723 - store_to_s3: &JOINT3_STORE_TO_S3_POS - -1.467 - get_stored_silver3: &JOINT3_GET_STORED_SILVER3_POS - -1.741 - get_stored_silver2: &JOINT3_GET_STORED_SILVER2_POS - -1.57 - get_stored_silver1: &JOINT3_GET_STORED_SILVER1_POS - -0.307 - ready_get_stored_gold: &JOINT3_READY_GET_STORED_GOLD_POS - -1.67 - get_stored_gold: &JOINT3_GET_STORED_GOLD_POS - -1.868 + -1.06 joint4: home: &JOINT4_HOME_POS - 1.022 - zero: &JOINT4_ZERO_POS - 0.000001 - r90: &JOINT4_R90_POS - -1.57 - l90: &JOINT4_L90_POS - 1.57 - get_stored_gold: &JOINT4_GET_STORED_GOLD_POS - -1.627 - + 1.007 joint5: home: &JOINT5_HOME_POS - 0.00001 - down_45: &JOINT5_DOWN_45 - 0.77 - down_90: &JOINT5_DOWN_90 - 1.57 - get_stored_gold: &JOINT5_GET_STORED_GOLD_POS - 1.765 - store_to_s1: &JOINT5_STORE_TO_S1_POS - -0.215 - store_to_s2: &JOINT5_STORE_TO_S2_POS - -0.314 - store_to_s3: &JOINT5_STORE_TO_S3_POS - -1.153 + -0.033 joint6: home: &JOINT6_HOME_POS - 0.00001 - l_90: &JOINT6_L_90 - 1.57 - r_90: &JOINT6_R_90 - -1.57 - get_small3: &JOINT6_SMALL3 - 4.387 - get_small2: &JOINT6_SMALL2 - 4.387 - get_small1: &JOINT6_SMALL1 - -0.481 - get_own_s3: &JOINT6_GET_S3 - 0.6536 - get_own_s2: &JOINT6_GET_S2 - 1.22727 - get_own_s1: &JOINT6_GET_S1 - 0.9794 + 0.197 - middle_pitch: - ground_stone: &MIDDLE_PITCH_GROUND_STONE - 1. - normal: &MIDDLE_PITCH_NORMAL - 0.0 - highest: &MIDDLE_PITCH_HIGHEST - -0.1 + joint7: + home: &JOINT7_HOME_POS + 0.0001 arm: home: &HOME - joints: [ *JOINT1_HOME_POS, *JOINT2_HOME_POS, *JOINT3_HOME_POS, *JOINT4_ZERO_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS ] - common: - <<: *NORMALLY - tolerance: - <<: *BIGGER_TOLERANCE - - lv4_exchange: &LV4_EXCHANGE - joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_R_EXCHANGE_POS, *JOINT3_R_EXCHANGE_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_45, *JOINT6_HOME_POS ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - lv5_r_exchange: &LV5_L_EXCHANGE - joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_L_EXCHANGE_POS, *JOINT3_L_EXCHANGE_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_L_90] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - lv5_l_exchange: &LV5_R_EXCHANGE - joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_R_EXCHANGE_POS, *JOINT3_R_EXCHANGE_POS, *JOINT4_L90_POS, *JOINT5_DOWN_90, *JOINT6_R_90] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - lv5_r_straight_exchange: &LV5_R_STRAIGHT_EXCHANGE - joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_EXCHANGE_POS, *JOINT3_ZERO_POS, *JOINT4_L90_POS, *JOINT5_DOWN_90, *JOINT6_R_90 ] + joints: [ *JOINT1_HOME_POS, *JOINT2_HOME_POS, *JOINT3_HOME_POS, *JOINT4_HOME_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS, *JOINT7_HOME_POS] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - lv5_l_straight_exchange: &LV5_L_STRAIGHT_EXCHANGE - joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_EXCHANGE_POS, *JOINT3_ZERO_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_L_90 ] + ready_assemble: &READY_ASSEMBLE + joints: [ 0.43, 0.001, *JOINT3_HOME_POS, *JOINT4_HOME_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS, *JOINT7_HOME_POS ] common: <<: *NORMALLY tolerance: - <<: *NORMAL_TOLERANCE + <<: *BIGGER_TOLERANCE gimbal: right_pos: &RIGHT_POS @@ -226,50 +83,81 @@ common: frame: base_link position: [ -3., -0.3, 0. ] follow_ee: &FOLLOW_EE - frame: link6 - position: [ 0.15, 0.00, 0. ] - follow_gold_gripper: &FOLLOW_GOLD - frame: gold_lifter_link - position: [ 0.0, 0.0, 0.0 ] + frame: gripper_link + position: [ 0.20, 0.00, 0. ] gripper: - open_main_gripper: &OPEN_MAIN_GRIPPER - pin: 0 - state: true - close_main_gripper: &CLOSE_MAIN_GRIPPER - pin: 0 - state: false - open_s1: &OPEN_S1 - pin: 1 - state: true - close_s1: &CLOSE_S1 - pin: 1 - state: false - open_s2: &OPEN_S2 - pin: 2 - state: true - close_s2: &CLOSE_S2 - pin: 2 - state: false - open_s3: &OPEN_S3 - pin: 3 - state: true - close_s3: &CLOSE_S3 - pin: 3 - state: false - open_gold: &OPEN_GOLD - pin: 4 - state: true - close_gold: &CLOSE_GOLD - pin: 4 - state: false - open_silver_gripper: &OPEN_S_GRIPPER - pin: 5 - state: true - close_silver_gripper: &CLOSE_S_GRIPPER - pin: 5 - state: false + open_gripper: &OPEN_GRIPPER + command: false + clode_gripper: &CLOSE_GRIPPER + command: true + + chassis: + chassis_left: &CHASSIS_LEFT_30 + frame: base_link + position: [ 0.0, 0.30 ] + yaw: 0.0 + chassis_tolerance_position: 0.05 + chassis_tolerance_angular: 0.1 + common: + timeout: 6. +# chassis_left_60: &CHASSIS_LEFT_70 +# frame: base_link +# position: [ 0.0, 0.7 ] +# yaw: 0.0 +# chassis_tolerance_position: 0.1 +# chassis_tolerance_angular: 0.2 +# common: +# timeout: 6. +# chassis_left60: &CHASSIS_LEFT_60 +# frame: base_link +# position: [ 0.0, 0.6 ] +# yaw: 0.0 +# chassis_tolerance_position: 0.1 +# chassis_tolerance_angular: 0.2 +# common: +# timeout: 6. +# chassis_left50: &CHASSIS_LEFT_50 +# frame: base_link +# position: [ 0.0, 0.5 ] +# yaw: 0.0 +# chassis_tolerance_position: 0.1 +# chassis_tolerance_angular: 0.2 +# common: +# timeout: 6. +# chassis_back_60: &CHASSIS_BACK_60 +# frame: base_link +# position: [ -0.6, 0.0 ] +# yaw: 0.0 +# chassis_tolerance_position: 0.1 +# chassis_tolerance_angular: 0.2 +# common: +# timeout: 6. +# chassis_turn_left: &CHASSIS_TURN_LEFT +# frame: base_link +# position: [ 0.0, 0.0 ] +# yaw: 1.57 +# chassis_tolerance_position: 0.1 +# chassis_tolerance_angular: 0.2 +# common: +# timeout: 6. +# chassis_turn_right: &CHASSIS_TURN_RIGHT +# frame: base_link +# position: [ 0.0, 0.0 ] +# yaw: -1.07 +# chassis_tolerance_position: 0.1 +# chassis_tolerance_angular: 0.2 +# common: +# timeout: 6. + chassis_turn_back: &CHASSIS_TURN_BACK + frame: base_link + position: [ 0.0, 0.0 ] + yaw: 3.14 + chassis_tolerance_position: 0.1 + chassis_tolerance_angular: 0.2 + common: + timeout: 2. steps_list: GIMBAL_F: @@ -286,1973 +174,577 @@ steps_list: - step: "gimbal look back" gimbal: <<: *BACK_POS - GIMBAL_EE: - step: "gimbal follow ee" gimbal: <<: *FOLLOW_EE - MIDDLE_PITCH_UP: - - step: "middle pitch up" - middle_pitch: - target: *MIDDLE_PITCH_NORMAL - HOME: - - step: "main close" - gripper: - <<: *CLOSE_MAIN_GRIPPER - - step: "pusher back" - silver_pusher: - target: 0. - - step: "lifter down" - silver_lifter: - target: 0.0 - - step: "rotate up" - silver_rotator: - target: 1.64 - - step: "get gold" - gold_pusher: - target: 0. - - step: "lift gold" - gold_lifter: - target: 0. - - step: "gimbal look front" - gimbal: - <<: *RIGHT_POS - - step: "gimbal look front" - gimbal: - <<: *FRONT_POS - - step: "main close" - gripper: - <<: *CLOSE_MAIN_GRIPPER - - step: "arm home pos" - arm: - <<: *HOME - - step: "middle pitch down" - middle_pitch: - target: *MIDDLE_PITCH_GROUND_STONE - CALIBRATION: - - step: "middle pitch up" - middle_pitch: - target: *MIDDLE_PITCH_NORMAL - - step: "main close" - gripper: - <<: *CLOSE_MAIN_GRIPPER - - step: "s1 close" - gripper: - <<: *CLOSE_S1 - - step: "s2 close" - gripper: - <<: *CLOSE_S2 - - step: "s3 close" - gripper: - <<: *CLOSE_S3 - - step: "g1 close" - gripper: - <<: *CLOSE_GOLD - - step: "close silver gripper" - gripper: - <<: *CLOSE_S_GRIPPER - step: "gimbal look front" gimbal: <<: *FRONT_POS - - step: "main close" - gripper: - <<: *CLOSE_MAIN_GRIPPER - HOME_WITH_PITCH: - - step: "rotate up" - silver_rotator: - target: 1.64 - - step: "gimbal look front" - gimbal: - <<: *FRONT_POS + HOME: - step: "arm home pos" arm: <<: *HOME - - step: "main close" - gripper: - <<: *CLOSE_MAIN_GRIPPER - - step: "middle pitch down" - middle_pitch: - target: *MIDDLE_PITCH_GROUND_STONE +# - step: "gripper open" +# gripper: +# <<: *OPEN_GRIPPER +# - step: "gimbal look front" +# gimbal: +# <<: *FRONT_POS + + LIFTER_INIT: + - step: "lifter init" + lifter: + state: 0 + LIFTER_UP: + - step: "lifter up" + lifter: + state: 1 + LIFTER_UP0: + - step: "lifter init" + lifter: + state: 0 + LIFTER_DOWN: + - step: "lifter down" + lifter: + state: 2 + LIFTER_DOWN0: + - step: "lifter init" + lifter: + state: 0 - LV4_EXCHANGE: - - step: "middle pitch up" - middle_pitch: - target: *MIDDLE_PITCH_NORMAL - delay: 0.3 - - step: "arm enter exchange pos" - arm: - <<: *LV4_EXCHANGE - - step: "gimbal ready" - gimbal: - <<: *RIGHT_POS - LV5_L_EXCHANGE: - - step: "middle pitch up" - middle_pitch: - target: *MIDDLE_PITCH_NORMAL - delay: 1.0 - - step: "arm enter exchange pos" - arm: - <<: *LV5_L_EXCHANGE - - step: "gimbal ready" - gimbal: - <<: *RIGHT_POS + GRIPPER_OPEN: + - step: "gripper open" + gripper: + <<: *OPEN_GRIPPER - L2R_EXCHANGE: - - step: "arm move back" - arm: - joints: [*JOINT1_EXCHANGE_POS, 0.92, -2.38, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS] - common: - <<: *NORMALLY - tolerance: - <<: *BIGGER_TOLERANCE - - step: "reverse joint2 and joint3" - arm: - joints: [*JOINT1_EXCHANGE_POS, -0.846, 2.20, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS] - common: - <<: *NORMALLY - tolerance: - <<: *BIGGER_TOLERANCE - - step: "rotate joint2 to r exchange pos" + GRIPPER_CLOSE: + - step: "gripper close" + gripper: + <<: *CLOSE_GRIPPER +# TEST_PLAYBACK: +# - step: "test playback" +# trajectory_playback: +# filename: "/home/ch/rm_ws/src/rm_engineer/engineer_trajectory_teaching/trajectory/test.yaml" +# speed: 1.5 +# sync_time: 0.5 +# + GET_UNITS_UP: + - step: "gripper open" + gripper: + <<: *OPEN_GRIPPER + - step: "arm ready get second unit" arm: - joints: [*JOINT1_EXCHANGE_POS, *JOINT2_R_EXCHANGE_POS, 2.20, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS] + joints: [ 0.215895, -0.003815, -0.312477, 0.193771, -1.581238, -0.002813, 2.618987 ] common: <<: *NORMALLY tolerance: - <<: *BIGGER_TOLERANCE - - step: "arm enter exchange pos" - arm: - <<: *LV5_R_EXCHANGE - - R2L_EXCHANGE: - - step: "fold arm" + <<: *NORMAL_TOLERANCE + GET_UNITS_UP0: + - step: "gripper close" + gripper: + <<: *CLOSE_GRIPPER + - step: "arm up unit" arm: - joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_R_EXCHANGE_POS, 2.20, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ 0.421690, 0.390625, -0.198036, 0.487916, -0.722261, -0.511475, 1.735095 ] common: <<: *NORMALLY tolerance: - <<: *BIGGER_TOLERANCE - - step: "rotate joint2 to l exchange pos" + <<: *NORMAL_TOLERANCE + - step: "chassis back" + chassis: + <<: *CHASSIS_LEFT_30 + - step: "ready store" arm: - joints: [ *JOINT1_EXCHANGE_POS, -0.846, 2.20, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ 0.445447, -2.713721, 0.194878, 0.043058, 1.530397, 1.536000, -2.027769 ] common: <<: *NORMALLY tolerance: <<: *BIGGER_TOLERANCE - - step: "reverse joint2 and joint3" + - step: "arm down unit" arm: - joints: [ *JOINT1_EXCHANGE_POS, 0.92, -2.38, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ "KEEP", "KEEP", 0.768227, -0.149203, 1.362128, 1.536000, -2.259214 ] common: <<: *NORMALLY tolerance: <<: *BIGGER_TOLERANCE - - step: "arm enter exchange pos" - arm: - <<: *LV5_L_EXCHANGE - - - LV5_R_EXCHANGE: - - step: "middle pitch up" - middle_pitch: - target: *MIDDLE_PITCH_NORMAL - delay: 1.0 - - step: "arm enter exchange pos" - arm: - <<: *LV5_R_EXCHANGE - - step: "gimbal ready" - gimbal: - <<: *RIGHT_POS - - LV5_L_STRAIGHT_EXCHANGE: - - step: "middle pitch up" - middle_pitch: - target: *MIDDLE_PITCH_NORMAL - delay: 0.3 - - step: "arm enter exchange pos" - arm: - <<: *LV5_L_STRAIGHT_EXCHANGE - - step: "gimbal ready" - gimbal: - <<: *RIGHT_POS - - LV5_R_STRAIGHT_EXCHANGE: - - step: "middle pitch up" - middle_pitch: - target: *MIDDLE_PITCH_NORMAL - delay: 0.3 - - step: "arm enter exchange pos" - arm: - <<: *LV5_R_STRAIGHT_EXCHANGE - - step: "gimbal ready" - gimbal: - <<: *RIGHT_POS - - LV5_R_DROP_GOLD_EXCHANGE: - - step: "middle pitch up" - middle_pitch: - target: *MIDDLE_PITCH_NORMAL - delay: 0.3 - - step: "drop side gold" - gripper: - <<: *CLOSE_GOLD - - step: "add stone" - stone_num: - change: "-g" - - step: "arm enter exchange pos" - arm: - <<: *LV5_R_EXCHANGE - - step: "gimbal ready" - gimbal: - <<: *RIGHT_POS - - LV5_L_DROP_GOLD_EXCHANGE: - - step: "middle pitch up" - middle_pitch: - target: *MIDDLE_PITCH_NORMAL - delay: 0.3 - - step: "drop side gold" + - step: "gripper open" gripper: - <<: *CLOSE_GOLD - - step: "add stone" - stone_num: - change: "-g" - - step: "arm enter exchange pos" - arm: - <<: *LV5_L_EXCHANGE - - step: "gimbal ready" - gimbal: - <<: *RIGHT_POS - - GET_SMALL_ISLAND: - - step: "middle pitch up" - middle_pitch: - target: *MIDDLE_PITCH_NORMAL - delay: 0.3 - - step: "gimbal look front" - gimbal: - <<: *FRONT_POS - - step: "ready get small island silver" + <<: *OPEN_GRIPPER + - step: "arm leave unit" arm: - joints: [*JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_L_POS, *JOINT3_GET_SMALL_ISLAND_L_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL1] + joints: [ "KEEP", -2.3271, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "open main gripper" - gripper: - <<: *OPEN_MAIN_GRIPPER - GET_SMALL_ISLAND0: - - step: "get small island silver" + - step: "arm ready get third unit" arm: - joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_L_POS, *JOINT3_GET_SMALL_ISLAND_L_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL1 ] + joints: [ 0.214920, -0.003815, -0.312477, 0.172921, -1.596671, 0.0001, 0.583660 ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "pull up silver" + GET_UNITS_UP00: + - step: "gripper close" + gripper: + <<: *CLOSE_GRIPPER + - step: "arm up unit" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_SMALL_ISLAND_L_POS, *JOINT3_GET_SMALL_ISLAND_L_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL1 ] + joints: [ 0.314685, -0.379, -0.226646, 0.074980, -1.414886, 0.153958, 0.366711 ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - GET_SMALL_ISLAND00: - - step: "ready stuck stone" + - step: "chassis back" + chassis: + <<: *CHASSIS_LEFT_30 + - step: "ready store" arm: - joints: [ 0.24, 1.049, -2.4010, -0.225, 1.112, -0.11 ] + joints: [ 0.445447, -2.517585, -0.464470, 0.684690, 1.505160, 1.6, -1.960691 ] common: <<: *NORMALLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "stuck stone" + <<: *BIGGER_TOLERANCE + - step: "arm down unit" arm: - joints: [ 0.16, 1.049, -2.4010, -0.225, 1.112, -0.11 ] + joints: [ "KEEP", -2.517585, 0.194878, 0.788450, 1.525415, 1.6, -2.6 ] common: <<: *NORMALLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "get ground stone" - middle_pitch: - target: 0.49 - delay: 0.5 - - - STORE_SILVER_TO_GRIPPER1: - - step: "silver rotator up" - silver_rotator: - target: 1.57 - - step: "open silver gripper" - gripper: - <<: *OPEN_S_GRIPPER - - - step: "open gripper1" + <<: *BIGGER_TOLERANCE + - step: "gripper open" gripper: - <<: *OPEN_S1 - - step: "ready to store" + <<: *OPEN_GRIPPER + - step: "arm leave unit" arm: - joints: [*JOINT1_READY_STORE_POS, *JOINT2_STORE_TO_S1_POS, *JOINT3_STORE_TO_S1_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S1_POS, *JOINT6_HOME_POS] + joints: [ "KEEP", -1.855548, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "store" + - step: "ready pos" arm: - joints: [ *JOINT1_STORE_POS, *JOINT2_STORE_TO_S1_POS, *JOINT3_STORE_TO_S1_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S1_POS, *JOINT6_HOME_POS ] + joints: [ 0.307, -0.013888, -0.188118, 0.060229, -0.018521, -0.020241, 0.008777 ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "close main gripper" + GET_UNITS_UP000: + - step: "gripper close" gripper: - <<: *CLOSE_MAIN_GRIPPER - - step: "add stone" - stone_num: - change: "+s1" - - step: "lift up arm" + <<: *CLOSE_GRIPPER + - step: "arm up unit" arm: - joints: [ *JOINT1_READY_STORE_POS, *JOINT2_STORE_TO_S1_POS, *JOINT3_STORE_TO_S1_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S1_POS, *JOINT6_HOME_POS ] + joints: [ 0.405447, -0.013888, -0.217, -0.01, -0.018521, -0.05, 0.008777 ] common: <<: *NORMALLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "left arm" + <<: *BIGGER_TOLERANCE + - step: "chassis back" + chassis: + <<: *CHASSIS_LEFT_30 + - step: "home" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE + <<: *HOME + - step: "chassis back" + chassis: + <<: *CHASSIS_TURN_BACK - STORE_SILVER_TO_GRIPPER2: - - step: "silver rotator up" - silver_rotator: - target: 1.57 - - step: "open silver gripper" - gripper: - <<: *OPEN_S_GRIPPER - - step: "open gripper2" + GET_UNITS_DOWN: + - step: "gripper open" gripper: - <<: *OPEN_S2 - - step: "ready to store" + <<: *OPEN_GRIPPER + - step: "ready pos" arm: - joints: [ *JOINT1_READY_STORE_POS, *JOINT2_STORE_TO_S2_POS, *JOINT3_STORE_TO_S2_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S2_POS, *JOINT6_HOME_POS ] + joints: [ 0.032178, -0.0028, -0.1699, 0.143689, -0.007022, -0.056858, 0.003342 ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "store" + GET_UNITS_DOWN0: + - step: "gripper close" + gripper: + <<: *CLOSE_GRIPPER + - step: "arm down unit" arm: - joints: [ *JOINT1_STORE_POS, *JOINT2_STORE_TO_S2_POS, *JOINT3_STORE_TO_S2_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S2_POS, *JOINT6_HOME_POS ] + joints: [ 0.003, "KEEP", 0.024193, 0.499239, -0.007022, -0.568923, 0.003342 ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "close main gripper" - gripper: - <<: *CLOSE_MAIN_GRIPPER - - step: "add stone" - stone_num: - change: "+s2" - - step: "lift up arm" + - step: "chassis back" + chassis: + <<: *CHASSIS_LEFT_30 + - step: "ready store" arm: - joints: [ *JOINT1_READY_STORE_POS, *JOINT2_STORE_TO_S2_POS, *JOINT3_STORE_TO_S2_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S2_POS, *JOINT6_HOME_POS ] + joints: [ 0.445447, -2.72, 0.194878, 0.043058, 1.530397, 1.536000, 1.153721 ] common: <<: *NORMALLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "left arm" + <<: *BIGGER_TOLERANCE + - step: "arm down unit" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ "KEEP", "KEEP", 0.768227, -0.149203, 1.362128, 1.536000, 0.844977 ] common: <<: *NORMALLY tolerance: - <<: *NORMAL_TOLERANCE - - STORE_SILVER_TO_GRIPPER3: - - step: "silver rotator up" - silver_rotator: - target: 1.57 - - step: "open silver gripper" - gripper: - <<: *OPEN_S_GRIPPER - - step: "open gripper3" + <<: *BIGGER_TOLERANCE + - step: "gripper open" gripper: - <<: *OPEN_S3 - - step: "ready to store" + <<: *OPEN_GRIPPER + - step: "arm leave unit" arm: - joints: [ *JOINT1_READY_STORE_POS, *JOINT2_STORE_TO_S3_POS, *JOINT3_STORE_TO_S3_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S3_POS, *JOINT6_HOME_POS ] + joints: [ "KEEP", -2.3271, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "store" + - step: "arm ready get second unit" arm: - joints: [ *JOINT1_STORE_POS, *JOINT2_STORE_TO_S3_POS, *JOINT3_STORE_TO_S3_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S3_POS, *JOINT6_HOME_POS ] + joints: [ 0.136601, -0.002062, -0.165398, 0.164752, 0.607200, -0.00001, 0.471600 ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "close main gripper" + GET_UNITS_DOWN00: + - step: "gripper close" gripper: - <<: *CLOSE_MAIN_GRIPPER - - step: "add stone" - stone_num: - change: "+s3" - - step: "lift up arm" - arm: - joints: [ *JOINT1_READY_STORE_POS, *JOINT2_STORE_TO_S3_POS, *JOINT3_STORE_TO_S3_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S3_POS, *JOINT6_HOME_POS ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "left arm" + <<: *CLOSE_GRIPPER + - step: "arm down unit" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ 0.130791, -0.460589, 0.141, 0.121, 0.624274, -0.601120, 0.583819 ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - GROUND_STONE: - - step: "middle pitch up" - middle_pitch: - target: *MIDDLE_PITCH_NORMAL - delay: 0.3 - - step: "arm ready get ground stone" + - step: "ready store" arm: - joints: [*JOINT1_GET_GROUND_POS, 0.94, -1.266, *JOINT4_ZERO_POS, 1.112 , -0.397] + joints: [ 0.445447, -2.517585, -0.464470, 0.684690, 1.505160, 1.6, 1.2576 ] common: <<: *NORMALLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "gimbal front" - gimbal: - <<: *FRONT_POS - GROUND_STONE0: - - step: "move arm to get ground stone" + <<: *BIGGER_TOLERANCE + chassis: + <<: *CHASSIS_LEFT_30 + - step: "arm down unit" arm: - joints: [*JOINT1_GET_GROUND_POS, 0.943, -1.863, *JOINT4_ZERO_POS, 1.112 , -0.397] + joints: [ "KEEP", -2.517585, 0.194878, 0.764, 1.525415, 1.6, 0.539 ] common: <<: *NORMALLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "open main gripper" + <<: *BIGGER_TOLERANCE + - step: "gripper open" gripper: - <<: *OPEN_MAIN_GRIPPER - - step: "get ground stone" - middle_pitch: - target: 0.49 - delay: 0.5 - - step: "arm pull up stone" + <<: *OPEN_GRIPPER + - step: "arm leave unit" arm: - joints: [0.2, 0.943, -1.863, *JOINT4_ZERO_POS, 1.112 , -0.397] + joints: [ "KEEP", -1.855548, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - GROUND_STONE00: - - step: "ready stuck stone" + - step: "arm ready get third unit" arm: - joints: [ 0.24, 1.049, -2.4010, -0.225, 1.112, -0.11] + joints: [ 0.098978, -0.0028, -0.1699, 0.095101, 2.07, -0.0001, 0.025798 ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "stuck stone" + GET_UNITS_DOWN000: + - step: "gripper close" + gripper: + <<: *CLOSE_GRIPPER + - step: "arm down unit" arm: - joints: [ 0.16, 1.049, -2.4010, -0.225, 1.112, -0.11] + joints: [ 0.098978, 0.475, 0.116, 0.097, 2.07, 0.575391, -0.202378 ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - - - - - GET_STORED_SILVER1: - - step: "middle pitch up" - middle_pitch: - target: *MIDDLE_PITCH_NORMAL - delay: 0.3 - - step: "ready to get silver from gripper1" + - step: "home" arm: - joints: [*JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE + <<: *HOME + - step: "chassis back" + chassis: + <<: *CHASSIS_TURN_BACK - - step: "open main gripper" - gripper: - <<: *OPEN_MAIN_GRIPPER - - step: "get silver" + READY_ASSEMBLE: + - step: "ready assemble" arm: - joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "close gripper1" + <<: *READY_ASSEMBLE + + GET_STORED_FRONT: + - step: "gripper open" gripper: - <<: *CLOSE_S1 - - step: "minus stone" - stone_num: - change: "-s1" - - step: "lift arm" - arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "left arm" + <<: *OPEN_GRIPPER + - step: "ready get stored front1" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] + joints: [ 0.445447, -1.638026, 0.163229, 0.5, 1.418200, 1.599, -2.6199 ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - GET_STORED_SILVER2: &GET_STORED_S2 - - step: "middle pitch up" - middle_pitch: - target: *MIDDLE_PITCH_NORMAL - delay: 0.3 - - step: "ready to get silver from gripper2" + - step: "ready get stored front2" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ] + joints: [ 0.445447, -1.638026, 0.163229, 0.791120, 1.418200, 1.599, -2.6199 ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "open main gripper" - gripper: - <<: *OPEN_MAIN_GRIPPER - - step: "store" + - step: "arm forward unit" arm: - joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ] + joints: [ "KEEP", -2.459872, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "close gripper2" + - step: "gripper close" gripper: - <<: *CLOSE_S2 - - step: "minus stone" - stone_num: - change: "-s2" - - step: "lift arm" + <<: *CLOSE_GRIPPER + - step: "arm up unit" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ] + joints: [ "KEEP", -2.517585, -0.464470, 0.684690, 1.505160, 1.6, -1.960691 ] common: <<: *NORMALLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "left arm" + <<: *BIGGER_TOLERANCE + - step: "ready assemble" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE + <<: *READY_ASSEMBLE - GET_STORED_SILVER3: &GET_STORED_S3 - - step: "middle pitch up" - middle_pitch: - target: *MIDDLE_PITCH_NORMAL - delay: 0.3 - - step: "lift arm" + GET_STORED_BACK: + - step: "gripper open" + gripper: + <<: *OPEN_GRIPPER + - step: "ready get stored back" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ] + joints: [ 0.445447, -2.263069, 0.654099, -0.073931, 1.362128, 1.59, -2.306812 ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "ready to get silver from gripper3" + - step: "arm forward unit" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ] + joints: [ "KEEP", -2.724, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "open main gripper" + - step: "gripper close" gripper: - <<: *OPEN_MAIN_GRIPPER - - step: "get ore" + <<: *CLOSE_GRIPPER + - step: "arm up unit" arm: - joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ] + joints: [ "KEEP", -2.724, 0.194878, 0.043058, 1.530397, 1.566000, -2.027769 ] common: <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "close gripper3" - gripper: - <<: *CLOSE_S3 - - step: "minus stone" - stone_num: - change: "-s3" - - step: "left arm" - arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ] - common: - <<: *SLOWLY tolerance: <<: *BIGGER_TOLERANCE - - step: "left arm" + - step: "ready assemble" arm: - joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE + <<: *READY_ASSEMBLE - GET_STORED_GOLD: - - step: "middle pitch up" - middle_pitch: - target: *MIDDLE_PITCH_NORMAL - delay: 0.3 - - step: "open main gripper" - gripper: - <<: *OPEN_MAIN_GRIPPER - - step: "ready to get stored gold" + STORE_UNIT_BACK: + - step: "ready assemble" arm: - joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_L_90 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "get stored gold" + <<: *READY_ASSEMBLE + - step: "ready store" arm: - joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_GET_STORED_GOLD_POS, *JOINT4_GET_STORED_GOLD_POS, *JOINT5_GET_STORED_GOLD_POS, *JOINT6_L_90] + joints: [ 0.445447, -2.733721, 0.194878, 0.043058, 1.530397, 1.536000, -2.027769 ] common: <<: *NORMALLY tolerance: <<: *BIGGER_TOLERANCE - - step: "close gold gripper" - gripper: - <<: *CLOSE_GOLD - - step: "move gold gripper back" - gold_pusher: - target: 0.00001 - - step: "add stone" - stone_num: - change: "+g" - - step: "move out gold" + STORE_UNIT_BACK0: + - step: "arm down unit" arm: - joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_L_90 ] + joints: [ 0.445447, -2.733721, 0.768227, -0.149203, 1.362128, 1.536000, -2.259214 ] common: <<: *NORMALLY tolerance: <<: *BIGGER_TOLERANCE - - step: "lift up gold" + - step: "gripper open" + gripper: + <<: *OPEN_GRIPPER + - step: "arm leave unit" arm: - joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_L_90 ] + joints: [ "KEEP", -2.3271, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE + - step: "ready assemble" + arm: + <<: *READY_ASSEMBLE - ARM_THREE_SILVER: - - step: "middle pitch up" - middle_pitch: - target: *MIDDLE_PITCH_NORMAL - - step: "gimbal look right" - gimbal: - <<: *RIGHT_POS - - step: "silver rotator up" - silver_rotator: - target: 1.57 - - step: "open silver gripper" - gripper: - <<: *OPEN_S_GRIPPER - - step: "ready get small island L silver" + STORE_UNIT_FRONT: + - step: "ready assemble" arm: - joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_R_POS, *JOINT3_GET_SMALL_ISLAND_R_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL3 ] + <<: *READY_ASSEMBLE + - step: "ready store" + arm: + joints: [ 0.445447, -2.517585, -0.464470, 0.684690, 1.505160, 1.6, -1.960691 ] common: <<: *NORMALLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "open s2" - gripper: - <<: *OPEN_S2 - - step: "open main gripper" - gripper: - <<: *OPEN_MAIN_GRIPPER - ARM_THREE_SILVER0: - - step: "get small island silver" + <<: *BIGGER_TOLERANCE + STORE_UNIT_FRONT0: + - step: "arm down unit" arm: - joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_R_POS, *JOINT3_GET_SMALL_ISLAND_R_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL3 ] + joints: [ "KEEP", -2.517585, 0.194878, 0.788450, 1.525415, 1.6, -2.59 ] common: <<: *NORMALLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "pull up silver" + <<: *BIGGER_TOLERANCE + - step: "gripper open" + gripper: + <<: *OPEN_GRIPPER + - step: "arm leave unit" arm: - joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_R_POS, *JOINT3_GET_SMALL_ISLAND_R_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL3 ] + joints: [ "KEEP", -1.855548, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "ready to store" + - step: "ready assemble" arm: - joints: [ *JOINT1_READY_STORE_POS, *JOINT2_STORE_TO_S2_POS, *JOINT3_STORE_TO_S2_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S2_POS, *JOINT6_HOME_POS ] + <<: *READY_ASSEMBLE + + STORE_UNIT_BACK_R: + - step: "ready store" + arm: + joints: [ 0.445447, -2.733721, 0.194878, 0.043058, 1.530397, 1.536000, 1.153721 ] common: <<: *NORMALLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "store" + <<: *BIGGER_TOLERANCE + STORE_UNIT_BACK_R0: + - step: "arm down unit" arm: - joints: [ *JOINT1_STORE_POS, *JOINT2_STORE_TO_S2_POS, *JOINT3_STORE_TO_S2_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S2_POS, *JOINT6_HOME_POS ] + joints: [ 0.445447, -2.733721, 0.768227, -0.149203, 1.362128, 1.536000, 0.844977 ] common: <<: *NORMALLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "close main gripper" + <<: *BIGGER_TOLERANCE + - step: "gripper open" gripper: - <<: *CLOSE_MAIN_GRIPPER - - step: "add stone" - stone_num: - change: "+s2" - - step: "lift up arm" + <<: *OPEN_GRIPPER + - step: "arm leave unit" arm: - joints: [ *JOINT1_READY_STORE_POS, *JOINT2_STORE_TO_S2_POS, *JOINT3_STORE_TO_S2_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S2_POS, *JOINT6_HOME_POS ] + joints: [ "KEEP", -2.3271, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - ############################################# 2nd silver - - - step: "ready get small island M silver" + - step: "ready assemble" arm: - joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_M_POS, *JOINT3_GET_SMALL_ISLAND_M_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL2 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "open main gripper" - gripper: - <<: *OPEN_MAIN_GRIPPER + <<: *READY_ASSEMBLE - - step: "get small island silver" + STORE_UNIT_FRONT_R: + - step: "ready store" arm: - joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_M_POS, *JOINT3_GET_SMALL_ISLAND_M_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL2 ] + joints: [ 0.445447, -2.517585, -0.464470, 0.684690, 1.505160, 1.6, 1.2576 ] common: <<: *NORMALLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "pull up silver" + <<: *BIGGER_TOLERANCE + STORE_UNIT_FRONT_R0: + - step: "arm down unit" arm: - joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_M_POS, *JOINT3_GET_SMALL_ISLAND_M_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL2 ] + joints: [ "KEEP", -2.517585, 0.194878, 0.788450, 1.525415, 1.6, 0.539 ] common: <<: *NORMALLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "ready to store" + <<: *BIGGER_TOLERANCE + - step: "gripper open" + gripper: + <<: *OPEN_GRIPPER + - step: "arm leave unit" arm: - joints: [ *JOINT1_READY_STORE_POS, *JOINT2_STORE_TO_S3_POS, *JOINT3_STORE_TO_S3_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S3_POS, *JOINT6_HOME_POS ] + joints: [ "KEEP", -1.855548, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] common: <<: *NORMALLY tolerance: <<: *NORMAL_TOLERANCE - - step: "store" + - step: "ready assemble" + arm: + <<: *READY_ASSEMBLE + + + + TEST_EE_MOTION0: + - step: "test ee motion" arm: - joints: [ *JOINT1_STORE_POS, *JOINT2_STORE_TO_S3_POS, *JOINT3_STORE_TO_S3_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S3_POS, *JOINT6_HOME_POS ] + end_effector: + tolerance_position: 0.05 + tolerance_orientation: 0.1 + frame: "link7" + xyz: [ 0.00, 0.00, 0.10 ] + rpy: [ 0.00, 0.00, 0.00 ] + cartesian: true common: <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "open s3" - gripper: - <<: *OPEN_S3 - - step: "close main gripper" - gripper: - <<: *CLOSE_MAIN_GRIPPER - - step: "add stone" - stone_num: - change: "+s3" - - step: "lift up arm" + TEST_EE_MOTION1: + - step: "test ee motion" arm: - joints: [ *JOINT1_READY_STORE_POS, *JOINT2_STORE_TO_S3_POS, *JOINT3_STORE_TO_S3_POS, *JOINT4_L90_POS, *JOINT5_STORE_TO_S3_POS, *JOINT6_HOME_POS ] + end_effector: + tolerance_position: 0.05 + tolerance_orientation: 0.1 + frame: "link7" + xyz: [ 0.00, 0.10, 0.00 ] + rpy: [ 0.00, 0.00, 0.00 ] + cartesian: true common: <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - ####################################################### 3rd silver - - - step: "ready get small island silver" + TEST_EE_MOTION2: + - step: "test ee motion" arm: - joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_L_POS, *JOINT3_GET_SMALL_ISLAND_L_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL1 ] + end_effector: + tolerance_position: 0.05 + tolerance_orientation: 0.1 + frame: "link7" + xyz: [ 0.10, 0.00, 0.00 ] + rpy: [ 0.00, 0.00, 0.00 ] + cartesian: true common: <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "open main gripper" - gripper: - <<: *OPEN_MAIN_GRIPPER - - step: "get small island silver" + TEST_EE_MOTION3: + - step: "test ee motion" arm: - joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_L_POS, *JOINT3_GET_SMALL_ISLAND_L_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL1 ] + end_effector: + tolerance_position: 0.05 + tolerance_orientation: 0.1 + frame: "link7" + xyz: [ 0.00, 0.00, -0.10 ] + rpy: [ 0.00, 0.00, 0.00 ] + cartesian: true common: <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "pull up silver" + TEST_EE_MOTION4: + - step: "test ee motion" arm: - joints: [ *JOINT1_READY_GET_SMALL_POS, *JOINT2_GET_SMALL_ISLAND_L_POS, *JOINT3_GET_SMALL_ISLAND_L_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_SMALL1 ] + end_effector: + tolerance_position: 0.05 + tolerance_orientation: 0.1 + frame: "link7" + xyz: [ 0.00, -0.10, 0.00 ] + rpy: [ 0.00, 0.00, 0.00 ] + cartesian: true common: <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - GRIPPER_THREE_SILVER: - - step: "open gripper" - gripper: - <<: *OPEN_S1 - - step: "open gripper" - gripper: - <<: *OPEN_S2 - - step: "open gripper" - gripper: - <<: *OPEN_S3 - - step: "open silver gripper" - gripper: - <<: *OPEN_S_GRIPPER - - step: "lift gimbal up" + TEST_EE_MOTION5: + - step: "test ee motion" arm: - joints: [ *JOINT1_GET_SMALL_POS, *JOINT2_AVOID_CONTROLLER_POS, *JOINT3_HOME_POS, *JOINT4_HOME_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS ] + end_effector: + tolerance_position: 0.05 + tolerance_orientation: 0.1 + frame: "link7" + xyz: [ -0.10, 0.00, 0.00 ] + rpy: [ 0.00, 0.00, 0.00 ] + cartesian: true common: <<: *NORMALLY - tolerance: - <<: *BIGGER_TOLERANCE - - step: "gimbal look right" - gimbal: - <<: *RIGHT_POS - - step: "rotate down" - silver_rotator: - target: 0.0001 - - step: "lifter ready" - silver_lifter: - target: 0.075 - - step: "pusher back" - silver_pusher: - target: -0.00001 - - step: "gimbal look back" - gimbal: - <<: *BACK_POS - GRIPPER_THREE_SILVER0: - - step: "get silver" - silver_pusher: - target: -0.042 - GRIPPER_THREE_SILVER00: - - step: "lifter ready" - silver_lifter: - target: 0.08 - - step: "pusher back little" - silver_pusher: - target: -0.02 - delay: 0.5 - - step: "rotate up" - silver_rotator: - target: 0.1 - - step: "lifter up" - silver_lifter: - target: 0.2 - delay: 0.5 - GRIPPER_THREE_SILVER000: - - step: "pusher back" - silver_pusher: - target: 0.0001 - - step: "rotate up" - silver_rotator: - target: 1.64 - delay: 0.4 - - step: "lifter down" - silver_lifter: - target: 0.05 - delay: 0.3 - - step: "lifter down" - silver_lifter: - target: 0.0 - - step: "add stone1" - stone_num: - change: "+s1" - - step: "add stone1" - stone_num: - change: "+s2" - - step: "add stone1" - stone_num: - change: "+s3" - - step: "gimbal look right" - gimbal: - <<: *RIGHT_POS - - step: "gimbal look front" - gimbal: - <<: *FRONT_POS - - step: "arm home pos" - arm: - joints: [ *JOINT1_HOME_POS, *JOINT2_HOME_POS, *JOINT3_HOME_POS, *JOINT4_HOME_POS, *JOINT5_HOME_POS, *JOINT6_HOME_POS ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - MID_BIG_ISLAND: - - step: "gimbal look right" - gimbal: - <<: *RIGHT_POS - - step: "pusher back" - gold_pusher: - target: 0. - - step: "lifter ready" - gold_lifter: - target: 0.00001 - - step: "open gold gripper" - gripper: - <<: *OPEN_GOLD - MID_BIG_ISLAND0: - - step: "get gold" - gold_pusher: - target: -0.46 - MID_BIG_ISLAND00: - - step: "pull gold back" - gold_pusher: - target: -0.43 - - step: "lift gold" - gold_lifter: - target: 0.058 - MID_BIG_ISLAND000: - - step: "pull back gold" - gold_pusher: - target: -0.054 - - step: "add stone" - stone_num: - change: "+g" - - step: "gimbal look front" - gimbal: - <<: *FRONT_POS - - MID_BIG_ISLAND1: - - step: "get gold" - gold_pusher: - target: -0.44 - MID_BIG_ISLAND11: - - step: "pull gold back" - gold_pusher: - target: -0.42 - - step: "lift gold" - gold_lifter: - target: 0.062 - delay: 0.7 - - step: "pull back gold" - gold_pusher: - target: -0.054 - - step: "add stone" - stone_num: - change: "+g" - - step: "gimbal look front" - gimbal: - <<: *FRONT_POS - - - - SIDE_BIG_ISLAND: - - step: "gimbal look right" - gimbal: - <<: *RIGHT_POS - - step: "middle pitch up" - middle_pitch: - target: *MIDDLE_PITCH_NORMAL - delay: 0.3 - - step: "arm ready" - arm: - joints: [ 0.040,0.885,-2.107,*JOINT4_R90_POS,-0.00001, *JOINT6_L_90 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "open main gripper" - gripper: - <<: *OPEN_MAIN_GRIPPER - SIDE_BIG_ISLAND1: - - step: "gimbal look ee" - gimbal: - <<: *FOLLOW_EE - - step: "arm get gold" - arm: - joints: [ 0.040,0.147,-1.429,*JOINT4_R90_POS, 0.08, *JOINT6_L_90 ] - common: - <<: *NORMALLY - tolerance: - <<: *BIGGER_TOLERANCE - SIDE_BIG_ISLAND11: - - step: "gimbal look right" - gimbal: - <<: *RIGHT_POS - SIDE_BIG_ISLAND111: - - step: "move stone towards down" - arm: - joints: [ 0.127, 0.997, -1.163, 0.0001, 1.57, -1.57 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "gimbal look front" - gimbal: - <<: *FRONT_POS - - SIDE_BIG_ISLAND0: - - step: "arm get gold" - arm: - joints: [ 0.040,0.147,-1.429,*JOINT4_R90_POS, 0.08, *JOINT6_HOME_POS ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "arm lift gold" - arm: - joints: [ 0.114,0.147,-1.429,*JOINT4_R90_POS, 0.08, *JOINT6_HOME_POS ] - common: - <<: *SLOWLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "gimbal look front" - gimbal: - <<: *FRONT_POS - - step: "arm pull back gold" - arm: - joints: [ 0.114,0.988,-1.927, *JOINT4_R90_POS,0.344, *JOINT6_HOME_POS ] - common: - <<: *SLOWLY - tolerance: - <<: *BIGGER_TOLERANCE - SIDE_BIG_ISLAND00: - - step: "ready stuck stone" - arm: - joints: [ 0.24, 1.049, -2.4010, -0.225, 1.112, -0.11 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "stuck stone" - arm: - joints: [ 0.16, 1.049, -2.4010, -0.225, 1.112, -0.11 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "get ground stone" - middle_pitch: - target: 0.49 - delay: 0.5 - - BOTH_BIG_ISLAND: - - step: "middle pitch up" - middle_pitch: - target: *MIDDLE_PITCH_NORMAL - delay: 0.3 - - step: "gimbal look right" - gimbal: - <<: *RIGHT_POS - - step: "open main gripper" - gripper: - <<: *OPEN_MAIN_GRIPPER - - step: "gimbal look right" - gimbal: - <<: *RIGHT_POS - - step: "pusher back" - gold_pusher: - target: 0. - - step: "lifter ready" - gold_lifter: - target: 0.00001 - - step: "open gold gripper" - gripper: - <<: *OPEN_GOLD - - step: "middle pitch up" - middle_pitch: - target: *MIDDLE_PITCH_HIGHEST - delay: 0.3 - - step: "arm ready" - arm: - joints: [ 0.040,0.885,-2.107,*JOINT4_R90_POS,-0.00001, *JOINT6_L_90 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "open main gripper" - gripper: - <<: *OPEN_MAIN_GRIPPER - - BOTH_BIG_ISLAND0: - - step: "get gold" - gold_pusher: - target: -0.46 - - step: "arm get gold" - arm: - joints: [ 0.040,0.147,-1.429,*JOINT4_R90_POS, 0.08, *JOINT6_HOME_POS ] - common: - <<: *NORMALLY - tolerance: - <<: *BIGGER_TOLERANCE - BOTH_BIG_ISLAND00: - - step: "gimbal look right" - gimbal: - <<: *RIGHT_POS - - step: "pull gold" - gold_pusher: - target: -0.43 - - step: "lift gold" - gold_lifter: - target: 0.058 - - step: "arm lift gold" - arm: - joints: [ 0.114,0.147,-1.429,*JOINT4_R90_POS, 0.08, *JOINT6_HOME_POS ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - BOTH_BIG_ISLAND000: - - step: "pull back gold" - gold_pusher: - target: -0.054 - - step: "add stone" - stone_num: - change: "+g" - - step: "arm pull back gold" - arm: - joints: [ 0.104,0.988,-1.927, *JOINT4_R90_POS,0.344, *JOINT6_HOME_POS ] - common: - <<: *SLOWLY - tolerance: - <<: *BIGGER_TOLERANCE - BOTH_BIG_ISLAND0000: - - step: "gimbal look right" - gimbal: - <<: *FRONT_POS - - step: "pull back gold" - gold_pusher: - target: -0.054 - BOTH_BIG_ISLAND00000: - - step: "ready stuck stone" - arm: - joints: [ 0.24, 1.049, -2.4010, -0.225, 1.112, -0.11 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "stuck stone" - arm: - joints: [ 0.16, 1.049, -2.4010, -0.225, 1.112, -0.11 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "get ground stone" - middle_pitch: - target: 0.49 - delay: 0.5 - - - BOTH_BIG_ISLAND1: - - step: "get gold" - gold_pusher: - target: -0.5 - - step: "arm get gold" - arm: - joints: [ 0.040,0.147,-1.429,*JOINT4_R90_POS, 0.08, *JOINT6_L_90 ] - common: - <<: *NORMALLY - tolerance: - <<: *BIGGER_TOLERANCE - - step: "pull gold" - gold_pusher: - target: -0.444 - delay: 0.2 - - step: "lift gold" - gold_lifter: - target: 0.062 - - step: "gimbal look ee" - gimbal: - <<: *FOLLOW_EE - BOTH_BIG_ISLAND11: - - step: "gimbal look right" - gimbal: - <<: *RIGHT_POS - BOTH_BIG_ISLAND111: - - step: "pull back gold" - gold_pusher: - target: -0.054 - - step: "add stone" - stone_num: - change: "+g" - - step: "gimbal look front" - gimbal: - <<: *FRONT_POS - - step: "move stone towards down" - arm: - joints: [ 0.127, 0.997, -1.163, 0.0001, 1.57, -1.57 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - OM: - - step: "main open" - gripper: - <<: *OPEN_MAIN_GRIPPER - CM: - - step: "main close" - gripper: - <<: *CLOSE_MAIN_GRIPPER - OS1: - - step: "s1 open" - gripper: - <<: *OPEN_S1 - CS1: - - step: "s1 close" - gripper: - <<: *CLOSE_S1 - OS2: - - step: "s2 open" - gripper: - <<: *OPEN_S2 - CS2: - - step: "s2 close" - gripper: - <<: *CLOSE_S2 - OS3: - - step: "s3 open" - gripper: - <<: *OPEN_S3 - CS3: - - step: "s3 close" - gripper: - <<: *CLOSE_S3 - OG: - - step: "g1 open" - gripper: - <<: *OPEN_GOLD - CG: - - step: "g1 close" - gripper: - <<: *CLOSE_GOLD - OSG: - - step: "g1 close" - gripper: - <<: *OPEN_S_GRIPPER - CSG: - - step: "close silver gripper" - gripper: - <<: *CLOSE_S_GRIPPER - - CA: - - step: "main close" - gripper: - <<: *CLOSE_MAIN_GRIPPER - - step: "s1 close" - gripper: - <<: *CLOSE_S1 - - step: "s2 close" - gripper: - <<: *CLOSE_S2 - - step: "s3 close" - gripper: - <<: *CLOSE_S3 - - step: "g1 close" - gripper: - <<: *CLOSE_GOLD - - step: "close silver gripper" - gripper: - <<: *CLOSE_S_GRIPPER - - OA: - - step: "main OPEN" - gripper: - <<: *OPEN_MAIN_GRIPPER - - step: "s1 OPEN" - gripper: - <<: *OPEN_S1 - - step: "s2 OPEN" - gripper: - <<: *OPEN_S2 - - step: "s3 OPEN" - gripper: - <<: *OPEN_S3 - - step: "g1 OPEN" - gripper: - <<: *OPEN_GOLD - - step: "OPEN silver gripper" - gripper: - <<: *OPEN_S_GRIPPER - - LV5_L_S1: - - step: "middle pitch up" - middle_pitch: - target: *MIDDLE_PITCH_NORMAL - delay: 0.6 - - step: "lift arm" - arm: - joints: [ *JOINT1_READY_GET_STORED_POS, -1.2, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] - common: - <<: *NORMALLY - tolerance: - <<: *BIGGER_TOLERANCE - - step: "ready to get silver from gripper1" - arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - - step: "open main gripper" - gripper: - <<: *OPEN_MAIN_GRIPPER - - step: "get silver" - arm: - joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "close gripper1" - gripper: - <<: *CLOSE_S1 - - step: "minus stone" - stone_num: - change: "-s1" - - step: "lift arm" - arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "close silver gripper" - gripper: - <<: *CLOSE_S_GRIPPER - - step: "lift arm" - arm: - joints: [ *JOINT1_READY_GET_STORED_POS, -1.7, -1.263, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] - common: - <<: *NORMALLY - tolerance: - <<: *BIGGER_TOLERANCE - - step: "rotate down" - silver_rotator: - target: 0.0001 - - step: "arm move back" - arm: - joints: [ *JOINT1_EXCHANGE_POS, 0.92, -2.38, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "arm enter exchange pos" - arm: - <<: *LV5_L_EXCHANGE - - step: "gimbal ready" - gimbal: - <<: *RIGHT_POS - - - - LV5_L_S2: - - step: "middle pitch up" - middle_pitch: - target: *MIDDLE_PITCH_NORMAL - delay: 0.6 - - step: "lift arm" - arm: - joints: [ *JOINT1_READY_GET_STORED_POS, -1.2, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] - common: - <<: *NORMALLY - tolerance: - <<: *BIGGER_TOLERANCE - - step: "ready to get silver from gripper2" - arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "open main gripper" - gripper: - <<: *OPEN_MAIN_GRIPPER - - step: "store" - arm: - joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "close gripper2" - gripper: - <<: *CLOSE_S2 - - step: "minus stone" - stone_num: - change: "-s2" - - step: "lift arm" - arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "lift arm" - arm: - joints: [ *JOINT1_READY_GET_STORED_POS, -1.7, -1.263, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] - common: - <<: *NORMALLY - tolerance: - <<: *BIGGER_TOLERANCE - - step: "arm move back" - arm: - joints: [ *JOINT1_EXCHANGE_POS, 0.92, -2.38, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "arm enter exchange pos" - arm: - <<: *LV5_L_EXCHANGE - - step: "gimbal ready" - gimbal: - <<: *RIGHT_POS - - - - LV5_L_S3: - - step: "middle pitch up" - middle_pitch: - target: *MIDDLE_PITCH_NORMAL - delay: 0.6 - - step: "lift arm" - arm: - joints: [ *JOINT1_READY_GET_STORED_POS, -1.2, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] - common: - <<: *NORMALLY - tolerance: - <<: *BIGGER_TOLERANCE - - step: "ready to get silver from gripper3" - arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "open main gripper" - gripper: - <<: *OPEN_MAIN_GRIPPER - - step: "get ore" - arm: - joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "close gripper3" - gripper: - <<: *CLOSE_S3 - - step: "minus stone" - stone_num: - change: "-s3" - - step: "left arm" - arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ] - common: - <<: *NORMALLY - tolerance: - <<: *BIGGER_TOLERANCE - - step: "lift arm" - arm: - joints: [ *JOINT1_READY_GET_STORED_POS, -1.7, -1.263, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] - common: - <<: *NORMALLY - tolerance: - <<: *BIGGER_TOLERANCE - - step: "arm move back" - arm: - joints: [ *JOINT1_EXCHANGE_POS, 0.92, -2.38, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "arm enter exchange pos" - arm: - <<: *LV5_L_EXCHANGE - - step: "gimbal ready" - gimbal: - <<: *RIGHT_POS - LV5_L_G: - - step: "rotate down" - silver_rotator: - target: 0.000001 - - step: "middle pitch up" - middle_pitch: - target: *MIDDLE_PITCH_NORMAL - delay: 0.6 - - step: "ready to get stored gold" - arm: - joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_L_90 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "open main gripper" - gripper: - <<: *OPEN_MAIN_GRIPPER - - step: "get stored gold" - arm: - joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_GET_STORED_GOLD_POS, *JOINT6_L_90 ] - common: - <<: *NORMALLY - tolerance: - <<: *BIGGER_TOLERANCE - - step: "close gold gripper" - gripper: - <<: *CLOSE_GOLD - - step: "move gold gripper back" - gold_pusher: - target: 0.00001 - delay: 0.3 - - step: "add stone" - stone_num: - change: "-g" - - step: "move out gold" - arm: - joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_GET_STORED_GOLD_POS, *JOINT6_L_90 ] - common: - <<: *NORMALLY - tolerance: - <<: *BIGGER_TOLERANCE - - step: "lift up gold" - arm: - joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_L_90 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "arm enter exchange pos" - arm: - <<: *LV5_L_EXCHANGE - - step: "gimbal ready" - gimbal: - <<: *RIGHT_POS - LV5_R_S1: - - step: "middle pitch up" - middle_pitch: - target: *MIDDLE_PITCH_NORMAL - delay: 0.6 - - step: "lift arm" - arm: - joints: [ *JOINT1_READY_GET_STORED_POS, -1.2, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] - common: - <<: *NORMALLY - tolerance: - <<: *BIGGER_TOLERANCE - - step: "ready to get silver from gripper1" - arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - - step: "open main gripper" - gripper: - <<: *OPEN_MAIN_GRIPPER - - step: "get silver" - arm: - joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "close gripper1" - gripper: - <<: *CLOSE_S1 - - step: "minus stone" - stone_num: - change: "-s1" - - step: "lift arm" - arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "close silver gripper" - gripper: - <<: *CLOSE_S_GRIPPER - - step: "lift arm" - arm: - joints: [ *JOINT1_READY_GET_STORED_POS, -1.7, -1.263, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] - common: - <<: *NORMALLY - tolerance: - <<: *BIGGER_TOLERANCE - - step: "rotate down" - silver_rotator: - target: 0.0001 - - step: "arm enter exchange pos" - arm: - <<: *LV5_R_EXCHANGE - - step: "gimbal ready" - gimbal: - <<: *RIGHT_POS - LV5_R_S2: - - step: "middle pitch up" - middle_pitch: - target: *MIDDLE_PITCH_NORMAL - delay: 0.6 - - step: "lift arm" - arm: - joints: [ *JOINT1_READY_GET_STORED_POS, -1.2, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] - common: - <<: *NORMALLY - tolerance: - <<: *BIGGER_TOLERANCE - - step: "ready to get silver from gripper2" - arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "open main gripper" - gripper: - <<: *OPEN_MAIN_GRIPPER - - step: "store" - arm: - joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "close gripper2" - gripper: - <<: *CLOSE_S2 - - step: "minus stone" - stone_num: - change: "-s2" - - step: "lift arm" - arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "lift arm" - arm: - joints: [ *JOINT1_READY_GET_STORED_POS, -1.7, -1.263, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] - common: - <<: *NORMALLY - tolerance: - <<: *BIGGER_TOLERANCE - - step: "arm enter exchange pos" - arm: - <<: *LV5_R_EXCHANGE - - step: "gimbal ready" - gimbal: - <<: *RIGHT_POS - LV5_R_S3: - - step: "middle pitch up" - middle_pitch: - target: *MIDDLE_PITCH_NORMAL - delay: 0.6 - - step: "lift arm" - arm: - joints: [ *JOINT1_READY_GET_STORED_POS, -1.2, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] - common: - <<: *NORMALLY - tolerance: - <<: *BIGGER_TOLERANCE - - step: "ready to get silver from gripper3" - arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "open main gripper" - gripper: - <<: *OPEN_MAIN_GRIPPER - - step: "get ore" - arm: - joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "close gripper3" - gripper: - <<: *CLOSE_S3 - - step: "minus stone" - stone_num: - change: "-s3" - - step: "left arm" - arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ] - common: - <<: *NORMALLY - tolerance: - <<: *BIGGER_TOLERANCE - - step: "lift arm" - arm: - joints: [ *JOINT1_READY_GET_STORED_POS, -1.7, -1.263, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] - common: - <<: *NORMALLY - tolerance: - <<: *BIGGER_TOLERANCE - - step: "arm enter exchange pos" - arm: - <<: *LV5_R_EXCHANGE - - step: "gimbal ready" - gimbal: - <<: *RIGHT_POS - LV5_R_G: - - step: "rotate down" - silver_rotator: - target: 0.000001 - - step: "middle pitch up" - middle_pitch: - target: *MIDDLE_PITCH_NORMAL - delay: 0.6 - - step: "ready to get stored gold" - arm: - joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "open main gripper" - gripper: - <<: *OPEN_MAIN_GRIPPER - - step: "get stored gold" - arm: - joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_GET_STORED_GOLD_POS, *JOINT6_HOME_POS ] - common: - <<: *NORMALLY - tolerance: - <<: *BIGGER_TOLERANCE - - step: "close gold gripper" - gripper: - <<: *CLOSE_GOLD - - step: "move gold gripper back" - gold_pusher: - target: 0.00001 - delay: 0.3 - - step: "add stone" - stone_num: - change: "-g" - - step: "move out gold" - arm: - joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_GET_STORED_GOLD_POS, *JOINT6_HOME_POS ] - common: - <<: *NORMALLY - tolerance: - <<: *BIGGER_TOLERANCE - - step: "lift up gold" - arm: - joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "arm enter exchange pos" - arm: - <<: *LV5_R_EXCHANGE - - step: "gimbal ready" - gimbal: - <<: *RIGHT_POS - LV4_S1: - - step: "middle pitch up" - middle_pitch: - target: *MIDDLE_PITCH_NORMAL - delay: 0.6 - - step: "ready to get silver from gripper1" - arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - - step: "open main gripper" - gripper: - <<: *OPEN_MAIN_GRIPPER - - step: "get silver" - arm: - joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "close gripper1" - gripper: - <<: *CLOSE_S1 - - step: "minus stone" - stone_num: - change: "-s1" - - step: "lift arm" - arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S1_POS, *JOINT3_GET_STORED_SILVER1_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S1 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "close silver gripper" - gripper: - <<: *CLOSE_S_GRIPPER - - step: "arm enter exchange pos" - arm: - <<: *LV4_EXCHANGE - - step: "gimbal ready" - gimbal: - <<: *RIGHT_POS - LV4_S2: - - step: "middle pitch up" - middle_pitch: - target: *MIDDLE_PITCH_NORMAL - delay: 0.6 - - step: "ready to get silver from gripper2" - arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "open main gripper" - gripper: - <<: *OPEN_MAIN_GRIPPER - - step: "store" - arm: - joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "close gripper2" - gripper: - <<: *CLOSE_S2 - - step: "minus stone" - stone_num: - change: "-s2" - - step: "lift arm" - arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S2_POS, *JOINT3_GET_STORED_SILVER2_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S2 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "arm enter exchange pos" - arm: - <<: *LV4_EXCHANGE - - step: "gimbal ready" - gimbal: - <<: *RIGHT_POS - LV4_S3: - - step: "middle pitch up" - middle_pitch: - target: *MIDDLE_PITCH_NORMAL - delay: 0.6 - - step: "lift arm" - arm: - joints: [ *JOINT1_READY_GET_STORED_POS, 0.113, -2.063, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "ready to get silver from gripper3" - arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "open main gripper" - gripper: - <<: *OPEN_MAIN_GRIPPER - - step: "get ore" - arm: - joints: [ *JOINT1_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "close gripper3" - gripper: - <<: *CLOSE_S3 - - step: "minus stone" - stone_num: - change: "-s3" - - step: "left arm" - arm: - joints: [ *JOINT1_READY_GET_STORED_POS, *JOINT2_GET_STORED_S3_POS, *JOINT3_GET_STORED_SILVER3_POS, *JOINT4_ZERO_POS, *JOINT5_DOWN_90, *JOINT6_GET_S3 ] - common: - <<: *NORMALLY - tolerance: - <<: *BIGGER_TOLERANCE - - step: "arm enter exchange pos" - arm: - <<: *LV4_EXCHANGE - - step: "gimbal ready" - gimbal: - <<: *RIGHT_POS - LV4_G: - - step: "middle pitch up" - middle_pitch: - target: *MIDDLE_PITCH_NORMAL - delay: 0.6 - - step: "ready to get stored gold" - arm: - joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "open main gripper" - gripper: - <<: *OPEN_MAIN_GRIPPER - - step: "get stored gold" - arm: - joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_GET_STORED_GOLD_POS, *JOINT6_HOME_POS ] - common: - <<: *NORMALLY - tolerance: - <<: *BIGGER_TOLERANCE - - step: "close gold gripper" - gripper: - <<: *CLOSE_GOLD - - step: "move gold gripper back" - gold_pusher: - target: 0.00001 - - step: "add stone" - stone_num: - change: "-g" - - step: "move out gold" - arm: - joints: [ *JOINT1_GET_STORED_GOLD_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] - common: - <<: *NORMALLY - tolerance: - <<: *BIGGER_TOLERANCE - - step: "lift up gold" - arm: - joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_GET_STORED_GOLD_POS, *JOINT3_READY_GET_STORED_GOLD_POS, *JOINT4_R90_POS, *JOINT5_DOWN_90, *JOINT6_HOME_POS ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "arm enter exchange pos" - arm: - <<: *LV4_EXCHANGE - - step: "gimbal ready" - gimbal: - <<: *RIGHT_POS diff --git a/engineer_middleware/config/engineer_simulation_controllers.yaml b/engineer_middleware/config/engineer_simulation_controllers.yaml index 6271b5b..79eb7ef 100644 --- a/engineer_middleware/config/engineer_simulation_controllers.yaml +++ b/engineer_middleware/config/engineer_simulation_controllers.yaml @@ -6,120 +6,148 @@ controllers: type: robot_state_controller/RobotStateController publish_rate: 100 - arm_trajectory_controller: - type: effort_controllers/JointTrajectoryController + left_arm_controller: + type: gravity_compensation_controller/GravityCompensationJointTrajectoryController joints: - - joint1 - - joint2 - - joint3 - - joint4 - - joint5 - - joint6 - - joint7 + - left_joint1 + - left_joint2 + - left_joint3 + - left_joint4 + - left_joint5 + - left_joint6 + - left_joint7 gains: - joint1: { p: 20000.0, i: 0, d: 400.0, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true } - joint2: { p: 30000, i: 0, d: 1000.0, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true } - joint3: { p: 10000.0, i: 0, d: 320.0, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true } - joint4: { p: 1000.0, i: 0.0, d: 20, i_clamp_max: 0, i_clamp_min: 0.0, antiwindup: true, publish_state: true } - joint5: { p: 350.0, i: 0, d: 50.0, i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true } - joint6: { p: 1000.0, i: 0, d: 13., i_clamp_max: 0, i_clamp_min: 0, antiwindup: true, publish_state: true } - joint7: { p: 5000., i: 0., d: 10000.0, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } + left_joint1: { p: 100, i: 2.0, d: 2.0, i_clamp_max: 5, i_clamp_min: -5, antiwindup: true, publish_state: false } + left_joint2: { p: 150, i: 5.0, d: 5.0, i_clamp_max: 6, i_clamp_min: -6, antiwindup: true, publish_state: false } + left_joint3: { p: 100, i: 1.0, d: 1.0, i_clamp_max: 2, i_clamp_min: -2, antiwindup: true, publish_state: false } + left_joint4: { p: 120, i: 5.0, d: 2.0, i_clamp_max: 3, i_clamp_min: -3, antiwindup: true, publish_state: false } + left_joint5: { p: 30, i: 1.0, d: 0.5, i_clamp_max: 1, i_clamp_min: -1, antiwindup: true, publish_state: false } + left_joint6: { p: 30, i: 1.0, d: 0.5, i_clamp_max: 1, i_clamp_min: -1, antiwindup: true, publish_state： false } + left_joint7: { p: 30, i: 1.0, d: 0.5, i_clamp_max: 1, i_clamp_min: -1, antiwindup: true, publish_state: false } constraints: - joint1: { trajectory: 0.05, goal: 0.05 } - joint2: { trajectory: 0.05, goal: 0.05 } - joint3: { trajectory: 0.05, goal: 0.05 } - joint4: { trajectory: 0.05, goal: 0.05 } - joint5: { trajectory: 0.05, goal: 0.05 } - joint6: { trajectory: 0.05, goal: 0.05 } - joint7: { trajectory: 0.05, goal: 0.05 } + goal_time: 5.0 + stopped_velocity_tolerance: 0.05 + left_joint1: { trajectory: 0.08, goal: 0.05 } + left_joint2: { trajectory: 0.08, goal: 0.05 } + left_joint3: { trajectory: 0.08, goal: 0.05 } + left_joint4: { trajectory: 0.08, goal: 0.05 } + left_joint5: { trajectory: 0.08, goal: 0.05 } + left_joint6: { trajectory: 0.08, goal: 0.05 } + left_joint7: { trajectory: 0.08, goal: 0.05 } + tau_g_scale: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0] + + right_arm_controller: + type: gravity_compensation_controller/GravityCompensationJointTrajectoryController + joints: + - right_joint1 + - right_joint2 + - right_joint3 + - right_joint4 + - right_joint5 + - right_joint6 + - right_joint7 + gains: + right_joint1: { p: 100, i: 2.0, d: 2.0, i_clamp_max: 5, i_clamp_min: -5, antiwindup: true, publish_state: false } + right_joint2: { p: 150, i: 5.0, d: 5.0, i_clamp_max: 6, i_clamp_min: -6, antiwindup: true, publish_state: false } + right_joint3: { p: 100, i: 1.0, d: 1.0, i_clamp_max: 2, i_clamp_min: -2, antiwindup: true, publish_state: false } + right_joint4: { p: 120, i: 5.0, d: 2.0, i_clamp_max: 3, i_clamp_min: -3, antiwindup: true, publish_state: false } + right_joint5: { p: 30, i: 1.0, d: 0.5, i_clamp_max: 1, i_clamp_min: -1, antiwindup: true, publish_state: false } + right_joint6: { p: 30, i: 1.0, d: 0.5, i_clamp_max: 1, i_clamp_min: -1, antiwindup: true, publish_state: false } + right_joint7: { p: 30, i: 1.0, d: 0.5, i_clamp_max: 1, i_clamp_min: -1, antiwindup: true, publish_state: false } + constraints: + goal_time: 5.0 + stopped_velocity_tolerance: 0.05 + right_joint1: { trajectory: 0.08, goal: 0.05 } + right_joint2: { trajectory: 0.08, goal: 0.05 } + right_joint3: { trajectory: 0.08, goal: 0.05 } + right_joint4: { trajectory: 0.08, goal: 0.05 } + right_joint5: { trajectory: 0.08, goal: 0.05 } + right_joint6: { trajectory: 0.08, goal: 0.05 } + right_joint7: { trajectory: 0.08, goal: 0.05 } + tau_g_scale: [1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0] + + gimbal_arm_controller: + type: gravity_compensation_controller/GravityCompensationJointTrajectoryController + joints: + - main_pitch + - gimbal_joint1 + - gimbal_joint2 + - gimbal_joint3 + gains: + main_pitch: { p: 350, i: 25.0, d: 7.0, i_clamp_max: 10, i_clamp_min: -10, antiwindup: true, publish_state: false } + gimbal_joint1: { p: 100.0, i: 10.0, d: 5.0, i_clamp_max: 3, i_clamp_min: -3, antiwindup: true, publish_state: false } + gimbal_joint2: { p: 100.0, i: 10.0, d: 5.0, i_clamp_max: 3, i_clamp_min: -3, antiwindup: true, publish_state: false } + gimbal_joint3: { p: 10.0, i: 1.0, d: 0.5, i_clamp_max: 1, i_clamp_min: -1, antiwindup: true, publish_state: false } + constraints: + goal_time: 5.0 + stopped_velocity_tolerance: 0.05 + main_pitch: { trajectory: 0.08, goal: 0.05 } + gimbal_joint1: { trajectory: 0.08, goal: 0.05 } + gimbal_joint2: { trajectory: 0.08, goal: 0.05 } + gimbal_joint3: { trajectory: 0.08, goal: 0.05 } + tau_g_scale: [0.0, 1.0, 1.0, 1.0] chassis_controller: - type: rm_chassis_controllers/OmniController + type: rm_chassis_controllers/SwerveController # ChassisBase publish_rate: 100 - enable_odom_tf: true + publish_map_tf: false publish_odom_tf: true + slam_topic: "/Odometry" + localization_topic: "/hdl_global_localization/result" power: - effort_coeff: 2.0 - vel_coeff: 0.008 - power_offset: -9.8 + effort_coeff: 1.8 + vel_coeff: 0.01 + power_offset: 7.0 + pivot_max_power: 50 + pivot_effort_coeff: 50 + pivot_vel_coeff: 0.01 + pivot_power_ratio: 0.8 + power_factor: -0.2 + use_rls: false + use_K_angle: true twist_angular: 0.5233 + max_odom_vel: 10.0 timeout: 0.1 - pid_follow: { p: 4.0, i: 0, d: 0.02, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } + pid_follow: { p: 6.5, i: 0, d: 0.0, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } twist_covariance_diagonal: [ 0.001, 0.001, 0.001, 0.001, 0.001, 0.001 ] - max_odom_vel: 10.0 - # OmniController - wheels: + + # SwerveController + modules: left_front: - pose: [ 0.213, 0.1945, 0. ] - roller_angle: -0.785 - joint: left_front_wheel_joint - <<: &wheel_setting - radius: 0.07 - pid: { p: 0.4, i: 0, d: 0.0, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } + position: [ 0.205, 0.200 ] + pivot: + joint: left_front_pivot_joint + <<: &pivot_setting + buffer_threshold: 10.0 + offset: 0. + pid: { p: 4.5, i: 0, d: 0.03, i_clamp_max: 0.0, i_clamp_min: -0.0, antiwindup: true, publish_state: false } + wheel: + joint: left_front_wheel_joint + <<: &wheel_setting + radius: 0.061 + pid: { p: 0.35, i: 0, d: 0.0, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: false } right_front: - pose: [ 0.213, -0.1945, 0. ] - roller_angle: 0.785 - joint: right_front_wheel_joint - <<: *wheel_setting + position: [ 0.205, -0.200 ] + pivot: + joint: right_front_pivot_joint + <<: *pivot_setting + wheel: + joint: right_front_wheel_joint + <<: *wheel_setting left_back: - pose: [ -0.213, 0.1945, 0. ] - roller_angle: 0.785 - joint: left_back_wheel_joint - <<: *wheel_setting + position: [ -0.205, 0.200 ] + pivot: + joint: left_back_pivot_joint + <<: *pivot_setting + wheel: + joint: left_back_wheel_joint + <<: *wheel_setting right_back: - pose: [ -0.213, -0.1945, 0. ] - roller_angle: -0.785 - joint: right_back_wheel_joint - <<: *wheel_setting - - - gimbal_controller: - type: rm_gimbal_controllers/Controller - yaw: - joint: "yaw_joint" - pid: { p: 0.5, i: 0.0, d: 0.0, i_clamp_max: 0.0, i_clamp_min: -0.0, antiwindup: true, publish_state: true } - pitch: - joint: "pitch_joint" - pid: { p: 0., i: 0.0, d: 0.0, i_clamp_max: 0.0, i_clamp_min: -0.0, antiwindup: true, publish_state: true } - feedforward: - gravity: 0.0 - enable_gravity_compensation: false - mass_origin: [ 0.0,0.0,0.0 ] - bullet_solver: - resistance_coff_qd_10: 0.45 - resistance_coff_qd_15: 1.0 - resistance_coff_qd_16: 0.7 - resistance_coff_qd_18: 0.55 - resistance_coff_qd_30: 3.0 - g: 9.81 - delay: 0.1 - dt: 0.001 - timeout: 0.001 - publish_rate: 50 - - ####################### Position Controllers ###################### - ore_bin_lifter_controller: - type: effort_controllers/JointPositionController - joint: ore_bin_lifter_joint - pid: { p: 100000., i: 0., d: 5000, i_max: 0.01, i_min: 0.01, antiwindup: true, publish_state: true } - - ore_bin_rotate_controller: - type: effort_controllers/JointPositionController - joint: ore_bin_rotate_joint - pid: { p: 0.9, i: 0., d: 0.06, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } - - gimbal_lifter_controller: - type: effort_controllers/JointPositionController - joint: gimbal_lifter_joint - pid: { p: 20000., i: 0., d: 1200, i_max: 0.01, i_min: 0.01, antiwindup: true, publish_state: true } - - extend_arm_front_controller: - type: effort_controllers/JointPositionController - joint: extend_arm_front_joint - pid: { p: 3., i: 0.1, d: 0.05, i_max: 0.01, i_min: 0.01, antiwindup: true, publish_state: true } - - extend_arm_back_controller: - type: effort_controllers/JointPositionController - joint: extend_arm_back_joint - pid: { p: 3., i: 0.1, d: 0.05, i_max: 0.01, i_min: 0.01, antiwindup: true, publish_state: true } + position: [ -0.205, -0.200 ] + pivot: + joint: right_back_pivot_joint + <<: *pivot_setting + wheel: + joint: right_back_wheel_joint + <<: *wheel_setting diff --git a/engineer_middleware/config/engineer_steps_list.yaml b/engineer_middleware/config/engineer_steps_list.yaml index 257570e..c648506 100644 --- a/engineer_middleware/config/engineer_steps_list.yaml +++ b/engineer_middleware/config/engineer_steps_list.yaml @@ -1,1987 +1,1527 @@ common: speed: slowly: &SLOWLY - speed: 0.15 + speed: 0.1 accel: 0.1 - timeout: 15. + timeout: 9. normally: &NORMALLY - speed: 0.4 - accel: 0.3 - timeout: 3 + speed: 0.5 + accel: 0.4 + timeout: 6. quickly: &QUICKLY - speed: 0.6 + speed: 0.8 accel: 0.6 - timeout: 7. + timeout: 5. + maximum: &MAXIMUM + speed: 1.0 + accel: 1.0 + timeout: 4. tolerance: + # gimbal_arm/left_arm/right_arm + # main_pitch/gimbal_joint1/2/3/left_joint1/2/3/4/5/6/7/right_joint1/2/3/4/5/6/7 small_tolerance: &SMALL_TOLERANCE - tolerance_joints: [ 0.005, 0.008, 0.015, 0.1, 0.1, 0.1, 0.03 ] + tolerance_joints: [ 0.05, 0.01, 0.01, 0.01, + 0.08, 0.05, 0.03, 0.04, 0.01, 0.01, 0.01, + 0.08, 0.05, 0.03, 0.04, 0.01, 0.01, 0.01 ] normal_tolerance: &NORMAL_TOLERANCE - tolerance_joints: [ 0.012, 0.02, 0.015, 0.1, 0.2, 0.15, 0.07 ] + tolerance_joints: [ 0.08, 0.03, 0.03, 0.03, + 0.1, 0.08, 0.06, 0.08, 0.03, 0.03, 0.03, + 0.1, 0.08, 0.06, 0.08, 0.03, 0.03, 0.03 ] bigger_tolerance: &BIGGER_TOLERANCE - tolerance_joints: [ 0.015, 0.015, 0.02, 0.35, 0.2, 0.2, 0.1 ] + tolerance_joints: [ 0.1, 0.05, 0.05, 0.05, + 0.15, 0.1, 0.08, 0.1, 0.06, 0.05, 0.06, + 0.15, 0.1, 0.08, 0.1, 0.06, 0.05, 0.06 ] max_tolerance: &MAX_TOLERANCE - tolerance_joints: [ 0.03, 0.03, 0.03, 0.3, 0.3, 0.3 , 0.3] + tolerance_joints: [ 0.2, 0.08, 0.08, 0.08, + 0.2, 0.15, 0.1, 0.12, 0.08, 0.07, 0.07, + 0.2, 0.15, 0.1, 0.12, 0.08, 0.07, 0.07 ] - joint1: - mechanical: - down_position: &JOINT1_DOWN_POSITION - 0.02 - small_ready: &JOINT1_SMALL_READY - 0.487 - small_down: &JOINT1_SMALL_DOWN - 0.34 - small_ready_store: &JOINT1_SMALL_READY_STORE - 0.254 - small_store_down: &JOINT1_SMALL_STORE_DOWN - 0.1962 - small_ready_store_third: &JOINT1_SMALL_READY_STORE_THIRD - 0.387 - up_pos: &JOINT1_UP_POS - 0.312 - bin_get_stone_left: &JOINT1_BIN_GET_STONE_LEFT - 0.2 - bin_get_stone_right: &JOINT1_BIN_GET_STONE_RIGHT - 0.1248 - big_ready: &JOINT1_BIG_READY - 0.005 - big_adjust_mid: &JOINT1_BIG_ADJUST_MID - 0.079 - big_up_pos: &JOINT1_BIG_UP_POS - 0.320 - big_ready_store: &JOINT1_BIG_READY_STORE - 0.133 - bin_get_stone: &JOINT1_BIN_GET_STONE - 0.1521 - exchange_pos: &JOINT1_EXCHANGE_POS - 0.1 - home: - zero_stone_position: &JOINT1_ZERO_STONE_POSITION - 0.01 - one_stone_position: &JOINT1_ONE_STONE_POSITION - 0.156 - two_stone_position: &JOINT1_TWO_STONE_POSITION - 0.22 - three_stone_position: &JOINT1_THREE_STONE_POSITION - 0.22 - - joint2: - mechanical: - back_position: &JOINT2_BACK_POSITION - 0.047 - get_ore_position: &JOINT2_GET_ORE_POSITION - 0.1 - furthest_position: &JOINT2_FURTHEST_POSITION - 0.232 - exchange_position: &JOINT2_EXCHANGE_POSITION - 0.105 - small_ready: &JOINT2_SMALL_READY - 0.029 - small_store: &JOINT2_SMALL_STORE - 0.060290 - bin_get_stone: &JOINT2_BIN_GET_STONE - 0.1 - back_bin_position: &JOINT2_BACK_BIN_POSITION - 0.04551 - avoid_collision: &JOINT2_AVOID_COLLISION - 0.1 - big_ready: &JOINT2_BIG_READY - 0.03 - big_arm_up: &JOINT2_BIG_ARM_UP - 0.142 - big_ready_store: &JOINT2_BIG_READY_STORE - 0.0739 - - joint3: - mechanical: - left_position: &JOINT3_L_POSITION - 0.308 - mid_position: &JOINT3_MID_POSITION - 0.04 - right_position: &JOINT3_R_POSITION - -0.278 - home: &JOINT3_HOME_POS - 0.08 - small_ready_mid: &JOINT3_SMALL_READY_MID - 0.0402 - take_one: &JOINT3_TAKE_ONE - -0.052850 - small_ready_left: &JOINT3_SMALL_READY_LEFT - 0.308 - small_ready_right: &JOINT3_SMALL_READY_RIGHT - -0.233 - small_turn: &JOINT3_SMALL_TURN - 0.257 - bin_get_stone: &JOINT3_BIN_GET_STONE - 0.2528 - bin_get_lv4_stone: &JOINT3_BIN_GET_LV4_STONE - 0.294414 - big_ready: &JOINT3_BIG_READY - 0.071 - get_big_mid: &JOINT3_GET_BIG_MID - -0.299 - get_big_side: &JOINT3_GET_BIG_SIDE - -0.199 - big_pull_out: &JOINT3_BIG_PULL_OUT - 0.188 - big_arm_up: &JOINT3_BIG_ARM_UP - 0.308 - exchange: &JOINT3_EXCHANGE - 0.081 - - joint4: - mechanical: - left_position: &JOINT4_L_POSITION - 1.00 - mid_position: &JOINT4_MID_POSITION - 0.021 - right_position: &JOINT4_R_POSITION - -1.554 - small_ready: &JOINT4_SMALL_READY - 0.054 - to: &JOINT4_TAKE_ONE - 0.828930 - small_turn: &JOINT4_SMALL_TURN - -1.497 - big_store: &JOINT4_BIG_STORE - -1.525 - exchange: &JOINT4_EXCHANGE - -0.086 - pre_home: &JOINT4_PRE_HOME - -0.91 - - - joint5: - mechanical: - mid_position: &JOINT5_MID_POSITION - 0.011 - left_90_position: &JOINT5_L90_POSITION - -1.54 - right_90_position: &JOINT5_R90_POSITION - 1.59 - bin_get_stone: &JOINT5_BIN_GET_STONE - 1.53912 - big_ready: &JOINT5_BIG_READY - -0.017 - big_ready_store: &JOINT5_BIG_READY_STORE - -0.001 - big_store: &JOINT5_BIG_STORE - 1.49 - small_ready: &JOINT5_SMALL_READY - -3.1267 - small_up_stone: &JOINT5_SMALL_UP_STONE - -0.001 - small_store: &JOINT5_SMALL_STORE - 1.4662 - exchange: &JOINT5_EXCHANGE - 1.805 - - joint6: - mechanical: - mid_position: &JOINT6_MID_POSITION - 0.001 - up_position: &JOINT6_UP_POSITION - -1.569 - down_position: &JOINT6_DOWN_POSITION - 1.569 - small_ready: &JOINT6_SMALL_READY - -1.607 - small_back: &JOINT6_SMALL_BACK - -1.987 - small_ready_store: &JOINT6_SMALL_READY_STORE - -1.633 - bin_get_stone: &JOINT6_BIN_GET_STONE - -1.6036 - big_ready: &JOINT6_BIG_READY - 0.014 - big_adjust_mid: &JOINT6_BIG_ADJUST_MID - -0.119 - big_arm_up: &JOINT6_BIG_ARM_UP - -1.519 - big_ready_store: &JOINT6_BIG_READY_STORE - -1.561 - exchange: &JOINT6_EXCHANGE - 0.139 - - joint7: - mechanical: - mid_position: &JOINT7_MID_POSITION - -0.098300 - left_90_position: &JOINT7_L90_POSITION - -1.56 - right_90_position: &JOINT7_R90_POSITION - 1.62 - big_pos: &JOINT7_BIG_POS - -0.013 - big_ready_store: &JOINT7_BIG_READY_STORE - -1.522 - small_ready: &JOINT7_SMALL_READY - -0.083 - small_ready_store: &JOINT7_SMALL_READY_STORE - 1.722 - exchange: &JOINT7_EXCHANGE - -1.771 - lv4_ore: &JOINT7_LV4_ORE - -0.098300 - bin_get_left: &JOINT7_BIN_GET_LEFT - -1.5793 - to: &JOINT7_TAKE_ONE - -0.844500 - bin_get_right: &JOINT7_BIN_GET_RIGHT - 1.554 gimbal: - right_pos: &RIGHT_POS - frame: gimbal_lifter - position: [ 0.001 , -3., 0. ] - left_pos: &LEFT_POS - frame: gimbal_lifter - position: [ 0.001, 3., 0. ] - front_pos: &FRONT_POS - frame: gimbal_lifter - position: [ 2., 0.001, 0. ] - small_island_pos: &BIG_ISLAND_POS - frame: gimbal_lifter - position: [ 1., -1., 0.] - test: &GIMBAL_TEST - frame: link4 - position: [1., 0., 0.] - test2: &GIMBAL_TEST2 - frame: link7 - position: [0., 0., -1.] - - gimbal_lifter: - button_pos: &BUTTON_POS - 0.05 - mid_pos: &MID_POS - 0.14 - two_stone_pos: &TWO_STONE_POS - 0.05 - tallest_pos: &TALLEST_POS - 0.28 - big_island: &LIFTER_BIG_ISLAND - 0.230 - small_island: &LIFTER_SMALL_ISLAND - 0.05 - gl_test1: &GL_TEST1 - 0.10 - gl_test2: &GL_TEST2 - 0.15 - gl_test3: &GL_TEST3 - 0.20 - gripper: + mid_pos: &GIMBAL_MID + groups: + - gimbal_arm: [ 0.0, 0.0, 0.0, 0.0 ] + pack_up: &GIMBAL_PACK_UP + groups: + - gimbal_arm: [ "KEEP", -0.048, 0.647, 1.184 ] + front_pos: &GIMBAL_FRONT + groups: + - gimbal_arm: [ 0.0, 0.0, 0.0, 0.0 ] + back_pos: &GIMBAL_BACK + groups: + - gimbal_arm: [ 0.175, -1.177, -0.667, 2.085 ] + look_down: &GIMBAL_LOOK_DOWN + groups: + - gimbal_arm: [ 0.175, -0.786, -0.366, 2.111 ] + + left_gripper: open_gripper: &OPEN_GRIPPER - pin: 0 - state: true + command: false close_gripper: &CLOSE_GRIPPER - pin: 0 - state: false - - arm: - pre_home: &PRE_HOME - joints: [ *JOINT1_ZERO_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_HOME_POS, *JOINT4_PRE_HOME, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - home: &HOME - joints: [ *JOINT1_ZERO_STONE_POSITION, *JOINT2_BACK_POSITION, *JOINT3_HOME_POS, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] - common: - <<: *NORMALLY - tolerance: - <<: *SMALL_TOLERANCE + command: true + right_gripper: + open_gripper: &OPEN_GRIPPER + command: false + close_gripper: &CLOSE_GRIPPER + command: true - chassis_target: - test: &TEST - frame: base_link - chassis_tolerance_position: 0.01 - chassis_tolerance_angular: 0.01 - offset: [0., 0.] - yaw_scale: 1 - target_frame: exchanger - common: - timeout: 2. chassis: - chassis_left_315: &CHASSIS_LEFT_15 + chassis_left: &CHASSIS_LEFT_30 frame: base_link - position: [ 0., 15. ] + position: [ 0.0, 0.30 ] yaw: 0.0 - chassis_tolerance_position: 0.1 - chassis_tolerance_angular: 0.3 - common: - timeout: 2. - - chassis_180: &CHASSIS_180 - frame: base_link - position: [ 0., 0. ] - yaw: 2.80 - chassis_tolerance_position: 0.1 - chassis_tolerance_angular: 0.2 + chassis_tolerance_position: 0.05 + chassis_tolerance_angular: 0.1 common: - timeout: 2. - - chassis_90: &CHASSIS_TURN - frame: base_link - position: [ 0., 0. ] - yaw: -1.40 - chassis_tolerance_position: 0.1 - chassis_tolerance_angular: 0.2 - common: - timeout: 2. - - chassis_back: &CHASSIS_BACK + timeout: 6. +# chassis_left_60: &CHASSIS_LEFT_70 +# frame: base_link +# position: [ 0.0, 0.7 ] +# yaw: 0.0 +# chassis_tolerance_position: 0.1 +# chassis_tolerance_angular: 0.2 +# common: +# timeout: 6. +# chassis_left60: &CHASSIS_LEFT_60 +# frame: base_link +# position: [ 0.0, 0.6 ] +# yaw: 0.0 +# chassis_tolerance_position: 0.1 +# chassis_tolerance_angular: 0.2 +# common: +# timeout: 6. +# chassis_left50: &CHASSIS_LEFT_50 +# frame: base_link +# position: [ 0.0, 0.5 ] +# yaw: 0.0 +# chassis_tolerance_position: 0.1 +# chassis_tolerance_angular: 0.2 +# common: +# timeout: 6. +# chassis_back_60: &CHASSIS_BACK_60 +# frame: base_link +# position: [ -0.6, 0.0 ] +# yaw: 0.0 +# chassis_tolerance_position: 0.1 +# chassis_tolerance_angular: 0.2 +# common: +# timeout: 6. +# chassis_turn_left: &CHASSIS_TURN_LEFT +# frame: base_link +# position: [ 0.0, 0.0 ] +# yaw: 1.57 +# chassis_tolerance_position: 0.1 +# chassis_tolerance_angular: 0.2 +# common: +# timeout: 6. +# chassis_turn_right: &CHASSIS_TURN_RIGHT +# frame: base_link +# position: [ 0.0, 0.0 ] +# yaw: -1.07 +# chassis_tolerance_position: 0.1 +# chassis_tolerance_angular: 0.2 +# common: +# timeout: 6. + chassis_turn_back: &CHASSIS_TURN_BACK frame: base_link - position: [ -0.4, 0. ] - yaw: 0.0 + position: [ 0.0, 0.0 ] + yaw: 3.14 chassis_tolerance_position: 0.1 chassis_tolerance_angular: 0.2 common: timeout: 2. - ore_rotator: - ready_pos: &READY_POS - 0.001 - exchange_pos: &EXCHANGE_POS - 1.6 - - ore_lifter: - bin_up_pos: &BIN_UP_POS - 0.22 - small_up_pos: &BIN_SMALL_UP_POS - 0.01 - bin_mid_pos: &BIN_MID_POS - 0.1 - bin_down_pos: &BIN_DOWN_POS - 0.001 - bin_get_up: &BIN_GET_UP - 0.033 - bin_get_down: &BIN_GET_DOWN - 0.225 - - extend_arm: - close: &EX_CLOSE - 0.0 - open: &EX_OPEN - 2.6 - steps_list: - TEST1: - - step: "gimbal test1" - gimbal: - <<: *GIMBAL_TEST - TEST2: - - step: "gimbal test2" - gimbal: - <<: *GIMBAL_TEST2 - - EXCHANGE_POS: - - step: "gimbal look front" - gimbal: - <<: *FRONT_POS - - step: "move arm" - arm: - joints: [*JOINT1_EXCHANGE_POS, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION] + GIMBAL_BACK: + - step: "gimbal move to back pos" + arms: + <<: *GIMBAL_BACK common: - <<: *NORMALLY + <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE - - step: "gimbal lifter tallest" - gimbal_lifter: - target: *TALLEST_POS - EXCHANGE_R: - - step: "gimbal look front" - gimbal: - <<: *FRONT_POS - - step: "move arm to exchange pos" - arm: - joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_BIN_GET_LEFT ] - common: - <<: *NORMALLY + + GIMBAL_MID: + - step: "gimbal move to mid pos" + arms: + <<: *GIMBAL_MID + common: + <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE - - step: "gimbal lifter tallest" - gimbal_lifter: - target: *TALLEST_POS - EXCHANGE_L: - - step: "gimbal look front" - gimbal: - <<: *FRONT_POS - - step: "move arm to exchange pos" - arm: - joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_BIN_GET_RIGHT ] - common: - <<: *NORMALLY + + GIMBAL_FRONT: + - step: "gimbal move to front pos" + arms: + <<: *GIMBAL_FRONT + common: + <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE - - step: "gimbal lifter tallest" - gimbal_lifter: - target: *TALLEST_POS - LEFT_EXCHANGE: - - step: "gimbal look front" - gimbal: - <<: *FRONT_POS - - step: "move arm to exchange pos" - arm: - joints: [*JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_L90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION] - common: - <<: *NORMALLY + + GIMBAL_PACK_UP: + - step: "gimbal pack up" + arms: + <<: *GIMBAL_PACK_UP + common: + <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE - - step: "gimbal lifter tallest" - gimbal_lifter: - target: *TALLEST_POS - RIGHT_EXCHANGE: - - step: "gimbal look front" - gimbal: - <<: *FRONT_POS - - step: "move arm to exchange pos" - arm: - joints: [*JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION] - common: - <<: *NORMALLY + + GIMBAL_LOOK_DOWN: + - step: "gimbal look down" + arms: + <<: *GIMBAL_LOOK_DOWN + common: + <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE - - step: "gimbal lifter tallest" - gimbal_lifter: - target: *TALLEST_POS - ############ MID ############# - - ############################ HOME ########################## - HOME: - - step: "close gripper" - gripper: - <<: *CLOSE_GRIPPER - - step: "pre_home" - arm: - <<: *PRE_HOME - - step: "gimbal look front" - gimbal: - <<: *FRONT_POS - - step: "gimbal lifter down" - gimbal_lifter: - target: *BUTTON_POS - - step: "close ex arm" - extend_arm: - front: *EX_CLOSE - back: *EX_CLOSE - - step: "ORE_ROTATOR_READY" - ore_rotator: - target: *READY_POS - - step: "ORE_LIFTER_UP" - ore_lifter: - target: *BIN_DOWN_POS - - step: "home" - arm: - <<: *HOME - - OPEN_GRIPPER: - - step: "open gripper" - gripper: - <<: *OPEN_GRIPPER - CLOSE_GRIPPER: - - step: "close gripper" - gripper: - <<: *CLOSE_GRIPPER + _GIMBAL_BACK: + - step: "gimbal move to back pos" + arms: + <<: *GIMBAL_BACK + common: + <<: *QUICKLY + tolerance: + <<: *NORMAL_TOLERANCE - ################### SMALL_ISLAND_FROM_UP ################ + _GIMBAL_MID: + - step: "gimbal move to mid pos" + arms: + <<: *GIMBAL_MID + common: + <<: *QUICKLY + tolerance: + <<: *NORMAL_TOLERANCE - ##### TAKE ONE USE ONE ##### + _GIMBAL_FRONT: + - step: "gimbal move to front pos" + arms: + <<: *GIMBAL_FRONT + common: + <<: *QUICKLY + tolerance: + <<: *NORMAL_TOLERANCE - TAKE_ONE_USE_ONE: - - step: "GIMBAL_READY" - gimbal: - <<: *FRONT_POS - - step: "ORE_ROTATOR_READY" - ore_rotator: - target: *READY_POS - - step: "ORE_LIFTER_READY" - ore_lifter: - target: *BIN_DOWN_POS - - step: "ready to get ore" - arm: - joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_TAKE_ONE, *JOINT4_TAKE_ONE, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_TAKE_ONE ] + _GIMBAL_PACK_UP: + - step: "gimbal pack up" + arms: + <<: *GIMBAL_PACK_UP common: <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE - - step: "OPEN_GRIPPER" - gripper: - <<: *OPEN_GRIPPER - - step: "gimbal lifter ready" - gimbal_lifter: - target: *LIFTER_SMALL_ISLAND - TAKE_ONE_USE_ONE0: - - step: "GET_STONE" - arm: - joints: [ *JOINT1_SMALL_DOWN, *JOINT2_SMALL_READY, *JOINT3_TAKE_ONE, *JOINT4_TAKE_ONE, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_TAKE_ONE ] + + _GIMBAL_LOOK_DOWN: + - step: "gimbal look down" + arms: + <<: *GIMBAL_LOOK_DOWN common: - <<: *NORMALLY + <<: *QUICKLY tolerance: <<: *NORMAL_TOLERANCE - - step: "JOINT1_UP" - arm: - joints: [ *JOINT1_SMALL_READY, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + + + GET_UP: + - step: "arms ready to get up" + arms: + groups: + - left_arm: [ 1.2, 0.868, -1.57, 0.163, 1.655, 0.0, 0.0 ] + - right_arm: [ 1.2, 0.868, -1.57, 0.163, 1.655, 0.0, 0.0 ] +# - left_arm: [ 1.2, 1.4, -1.564, 1.0, 1.567, 0.0, 0.0 ] +# - right_arm: [ 1.2, 1.4, -1.564, 1.0, 1.567, 0.0, 0.0] + - gimbal_arm: [ "KEEP", -0.048, 0.647, 1.184 ] common: - <<: *NORMALLY + <<: *MAXIMUM tolerance: <<: *BIGGER_TOLERANCE - - step: "CHASSIS_BACK" - chassis: - <<: *CHASSIS_BACK - - step: "CHASSIS_TURN" - chassis: - <<: *CHASSIS_TURN - - step: "gimbal lifter ready" - gimbal_lifter: - target: *TALLEST_POS - - step: "ROTATE_STONE" - arm: - joints: [ *JOINT1_SMALL_DOWN, *JOINT2_SMALL_READY, *JOINT3_TAKE_ONE, *JOINT4_TAKE_ONE, *JOINT5_SMALL_READY, *JOINT6_MID_POSITION, *JOINT7_TAKE_ONE ] + - step: "arms getting up" + arms: + groups: + - gimbal_arm: [ 0.175, -0.048, 0.647, 1.184 ] common: - <<: *NORMALLY + <<: *SLOWLY tolerance: <<: *BIGGER_TOLERANCE - - step: "move arm to exchange pos" - arm: - joints: [ *JOINT1_EXCHANGE_POS, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + - step: "arms ready" + arms: + groups: +# - left_arm: [ 0.627, 1.46, -1.587, 1.725, 1.411, 0.007, -0.4 ] +# - right_arm: [ 0.674, 1.46, -1.587, 1.725, 1.411, 0.007, -0.47 ] +# - left_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] +# - right_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] + - gimbal_arm: [ "KEEP", -1.177, -0.667, 2.085 ] common: - <<: *NORMALLY + <<: *MAXIMUM tolerance: - <<: *NORMAL_TOLERANCE - - - + <<: *BIGGER_TOLERANCE - ########ONE_STONE_ONCE######### - 0_TAKE_ONE_STONE_SMALL_ISLAND: - - step: "GIMBAL_READY" - gimbal: - <<: *FRONT_POS - - step: "ORE_ROTATOR_READY" - ore_rotator: - target: *READY_POS - - step: "ORE_LIFTER_READY" - ore_lifter: - target: *BIN_DOWN_POS - - step: "ARM_READY" - arm: - joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_MID, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] + SPECIAL_GET_UP: + - step: "right arm first" + arms: + groups: + # - left_arm: [ 1.458, 1.4, 0.161, 0.202, 2.13, -0.489, -1.368 ] + - right_arm: [ 1.2, 1.4, -1.564, 1.0, 1.567, 0.0, 0.0 ] common: <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "OPEN_GRIPPER" - gripper: - <<: *OPEN_GRIPPER - - step: "gimbal lifter ready" - gimbal_lifter: - target: *LIFTER_SMALL_ISLAND - 0_TAKE_ONE_STONE_SMALL_ISLAND0: - - step: "GET_STONE" - arm: - joints: [ *JOINT1_SMALL_DOWN, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_MID, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] + <<: *BIGGER_TOLERANCE + - step: "left arm last" + arms: + groups: + - left_arm: [ 1.2, 1.4, -1.564, 1.0, 1.567, 0.0, 0.0 ] + # - right_arm: [ 1.467, 1.495, -0.044, 0.077, -1.414, 1.009, 1.071 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "JOINT1_UP" - arm: - joints: [ *JOINT1_SMALL_READY, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + <<: *BIGGER_TOLERANCE + - step: "arms ready to get up" + arms: + groups: + - gimbal_arm: [ "KEEP", -0.048, 0.647, 1.184 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "ARM_UP_STONE" - arm: - joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_MID, *JOINT4_SMALL_READY, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] + <<: *BIGGER_TOLERANCE + - step: "arms getting up" + arms: + groups: + - gimbal_arm: [ 0.175, -0.048, 0.647, 1.184 ] common: - <<: *NORMALLY + <<: *SLOWLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "READY_STORE" - arm: - joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] + <<: *BIGGER_TOLERANCE + - step: "arms ready" + arms: + groups: + - left_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] + - right_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] + - gimbal_arm: [ "KEEP", -1.177, -0.667, 2.085 ] common: <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "STORE" - arm: - joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] + <<: *BIGGER_TOLERANCE + + + SET_DOWN: + - step: "arms ready to set down" + arms: + groups: + - left_arm: [ 1.2, 0.868, -1.57, 0.163, 1.655, 0.0, 0.0 ] + - right_arm: [ 1.2, 0.868, -1.57, 0.163, 1.655, 0.0, 0.0 ] +# - left_arm: [ 1.47, 1.567, -1.564, 1.58, 1.567, 0.0, 0.0 ] +# - right_arm: [ 1.47, 1.567, -1.564, 1.58, 1.567, 0.0, 0.0] + - gimbal_arm: [ "KEEP", -0.048, 0.647, 1.184 ] common: - <<: *NORMALLY + <<: *MAXIMUM tolerance: <<: *BIGGER_TOLERANCE - - step: "CLOSE_GRIPPER" - gripper: - <<: *CLOSE_GRIPPER - - step: "MOVE_ARM_OUT" - arm: - joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] - common: - <<: *NORMALLY - tolerance: - <<: *BIGGER_TOLERANCE - - step: "CHASSIS_BACK" - chassis: - <<: *CHASSIS_BACK - - step: "CHASSIS_TURN" - chassis: - <<: *CHASSIS_TURN - - step: "pre_home" - arm: - <<: *PRE_HOME - - step: "home" - arm: - <<: *HOME - - step: "ORE_LIFTER_DOWN" - ore_lifter: - target: *BIN_DOWN_POS - - step: "ADD_STONE" - stone_num: - change: "SILVER" - 1_TAKE_ONE_STONE_SMALL_ISLAND: - - step: "GIMBAL_READY" - gimbal: - <<: *FRONT_POS - - step: "ORE_ROTATOR_READY" - ore_rotator: - target: *READY_POS - - step: "ORE_LIFTER_READY" - ore_lifter: - target: *BIN_DOWN_POS - - step: "ARM_READY" - arm: - joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_MID, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] + - step: "arms setting down" + arms: + groups: + - gimbal_arm: [ -1.4, -0.048, 0.647, 1.184 ] common: - <<: *QUICKLY + <<: *SLOWLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "OPEN_GRIPPER" - gripper: - <<: *OPEN_GRIPPER - - step: "gimbal lifter ready" - gimbal_lifter: - target: *LIFTER_SMALL_ISLAND - 1_TAKE_ONE_STONE_SMALL_ISLAND0: - - step: "GET_STONE" - arm: - joints: [ *JOINT1_SMALL_DOWN, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_MID, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] + <<: *BIGGER_TOLERANCE + - step: "arms put down" + arms: + groups: +# - left_arm: [ 1.458, 1.4, 0.161, 0.202, 2.13, -0.489, -1.368 ] +# - right_arm: [ 1.467, 1.495, -0.044, 0.077, -1.414, 1.009, 1.071 ] + - left_arm: [ 1.4, 1.46, -1.587, 1.725, 1.411, 0.007, -1.4 ] + - right_arm: [ 1.4, 1.46, -1.587, 1.725, 1.411, 0.007, -1.4 ] + - gimbal_arm: [ "KEEP", 1.484, -0.478, 0.786 ] common: - <<: *NORMALLY + <<: *MAXIMUM tolerance: - <<: *NORMAL_TOLERANCE - - step: "JOINT1_UP" - arm: - joints: [ *JOINT1_SMALL_READY, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + <<: *BIGGER_TOLERANCE + SET_DOWN0: + - step: "arms ready" + arms: + groups: + - left_arm: [ 1.2, 1.4, -1.564, 1.0, 1.567, 0.0, 0.0 ] +# - right_arm: [ 1.2, 1.4, -1.564, 1.0, 1.567, 0.0, 0.0 ] common: - <<: *NORMALLY + <<: *MAXIMUM tolerance: - <<: *NORMAL_TOLERANCE - - step: "ARM_UP_STONE" - arm: - joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_MID, *JOINT4_SMALL_READY, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] + <<: *BIGGER_TOLERANCE + - step: "left arm first" + arms: + groups: + - left_arm: [ 1.47, 1.533, 0.161, 0.202, 2.13, -0.489, -1.208 ] +# - right_arm: [ 1.467, 1.495, -0.044, 0.077, -1.414, 1.009, 1.071 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "READY_STORE" - arm: - joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] + <<: *BIGGER_TOLERANCE +# - step: "right arm last" +# arms: +# groups: +## - left_arm: [ 1.458, 1.4, 0.161, 0.202, 2.13, -0.489, -1.368 ] +# - right_arm: [ 1.357, 1.495, -0.044, 0.077, -1.414, 1.009, 1.071 ] +# common: +# <<: *QUICKLY +# tolerance: +# <<: *BIGGER_TOLERANCE + + SPECIAL_SET_DOWN: + - step: "arms ready to set down" + arms: + groups: + - left_arm: [ 1.47, 1.567, -1.564, 1.58, 1.567, 0.0, 0.0 ] + - right_arm: [ 1.47, 1.567, -1.564, 1.58, 1.567, 0.0, 0.0 ] + - gimbal_arm: [ "KEEP", -0.048, 0.647, 1.184 ] common: <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "STORE" - arm: - joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] + <<: *BIGGER_TOLERANCE + - step: "arms setting down" + arms: + groups: + - gimbal_arm: [ -1.4, -0.048, 0.647, 1.184 ] common: - <<: *NORMALLY + <<: *SLOWLY tolerance: <<: *BIGGER_TOLERANCE - - step: "CLOSE_GRIPPER" - gripper: - <<: *CLOSE_GRIPPER - - step: "MOVE_ARM_OUT" - arm: - joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] - common: - <<: *NORMALLY - tolerance: - <<: *BIGGER_TOLERANCE - - step: "CHASSIS_BACK" - chassis: - <<: *CHASSIS_BACK - - step: "CHASSIS_TURN" - chassis: - <<: *CHASSIS_TURN - - step: "pre_home" - arm: - <<: *PRE_HOME - - step: "home" - arm: - <<: *HOME - - step: "ORE_LIFTER_DOWN" - ore_lifter: - target: *BIN_DOWN_POS - - step: "ADD_STONE" - stone_num: - change: "SILVER" - - ########################## TWO_STONE_ONCE ######################## - ######FIRST_STONE######## - TWO_STONE_SMALL_ISLAND: - - step: "GIMBAL_READY" - gimbal: - <<: *FRONT_POS - - step: "ORE_ROTATOR_READY" - ore_rotator: - target: *READY_POS - - step: "ORE_LIFTER_READY" - ore_lifter: - target: *BIN_DOWN_POS - - step: "OPEN_GRIPPER" - gripper: - <<: *OPEN_GRIPPER - - step: "gimbal lifter ready" - gimbal_lifter: - target: *LIFTER_SMALL_ISLAND - - step: "ARM_READY" - arm: - joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_LEFT, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] + - step: "arms put down" + arms: + groups: +# - left_arm: [ 1.4, 1.46, -1.587, 1.725, 1.411, 0.007, -1.4 ] + - right_arm: [ 1.4, 1.46, -1.587, 1.725, 1.411, 0.007, -1.4 ] + - gimbal_arm: [ "KEEP", 1.484, -0.478, 0.786 ] common: <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - TWO_STONE_SMALL_ISLAND0: - - step: "GET_STONE" - arm: - joints: [ *JOINT1_SMALL_DOWN, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_LEFT, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] + <<: *BIGGER_TOLERANCE + - step: "left arm ready" + arms: + groups: + - left_arm: [ 1.2, 1.4, -1.564, 1.0, 1.567, 0.0, 0.0 ] +# - right_arm: [ 1.2, 1.4, -1.564, 1.0, 1.567, 0.0, 0.0 ] common: <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "JOINT1_UP" - arm: - joints: [ *JOINT1_SMALL_READY, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + <<: *BIGGER_TOLERANCE + - step: "left arm first" + arms: + groups: + - left_arm: [ 1.487, 1.514, 0.161, 0.202, 2.13, -0.489, -1.261 ] + # - right_arm: [ 1.467, 1.495, -0.044, 0.077, -1.414, 1.009, 1.071 ] common: <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "ARM_UP_STONE" - arm: - joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_LEFT, *JOINT4_SMALL_READY, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] + <<: *BIGGER_TOLERANCE +# - step: "right arm last" +# arms: +# groups: +# # - left_arm: [ 1.458, 1.4, 0.161, 0.202, 2.13, -0.489, -1.368 ] +# - right_arm: [ 1.357, 1.495, -0.044, 0.077, -1.414, 1.009, 1.071 ] +# common: +# <<: *QUICKLY +# tolerance: +# <<: *BIGGER_TOLERANCE + + + GRIPPER_OPEN: + - step: "grippers open" + left_gripper: + <<: *OPEN_GRIPPER + right_gripper: + <<: *OPEN_GRIPPER + LEFT_GRIPPER_OPEN: + - step: "left gripper open" + left_gripper: + <<: *OPEN_GRIPPER + RIGHT_GRIPPER_OPEN: + - step: "right gripper open" + right_gripper: + <<: *OPEN_GRIPPER + + GRIPPER_CLOSE: + - step: "grippers close" + left_gripper: + <<: *CLOSE_GRIPPER + right_gripper: + <<: *CLOSE_GRIPPER + LEFT_GRIPPER_CLOSE: + - step: "left gripper close" + left_gripper: + <<: *CLOSE_GRIPPER + RIGHT_GRIPPER_CLOSE: + - step: "right gripper close" + right_gripper: + <<: *CLOSE_GRIPPER + + LEFT_GET_TOP_UNIT: + - step: "left gripper open" + left_gripper: + <<: *OPEN_GRIPPER + - step: "left ready to get top unit" + arms: + groups: + - left_arm: [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] common: <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "READY_STORE" - arm: - joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] + <<: *BIGGER_TOLERANCE + - step: "left gripper close" + left_gripper: + <<: *CLOSE_GRIPPER + - step: "left get up unit" + arms: + groups: + - left_arm: [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] common: <<: *QUICKLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "STORE" - arm: - joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] - common: - <<: *NORMALLY tolerance: <<: *BIGGER_TOLERANCE - - step: "ADD_STONE" - stone_num: - change: "SILVER" - - step: "CLOSE_GRIPPER" - gripper: - <<: *CLOSE_GRIPPER - - step: "MOVE_ARM_OUT" - arm: - joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + - step: "left back to ready pos" + arms: + groups: + - left_arm: [ 1.022, 1.567, -1.564, 1.0, 1.567, 0.0, 0.0 ] common: <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "ORE_LIFTER_DOWN" - ore_lifter: - target: *BIN_DOWN_POS - + <<: *BIGGER_TOLERANCE - #####SECOND_STONE######## - - step: "GIMBAL_READY" - gimbal: - <<: *FRONT_POS - - step: "ORE_ROTATOR_READY" - ore_rotator: - target: *READY_POS - - step: "ORE_LIFTER_READY" - ore_lifter: - target: *BIN_DOWN_POS - - step: "ARM_READY" - arm: - joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_MID, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] + LEFT_GET_BOTTOM_UNIT: + - step: "arms back to ready pos" + arms: + groups: + - left_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] + - right_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] common: <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "OPEN_GRIPPER" - gripper: + <<: *BIGGER_TOLERANCE + - step: "left gripper open" + left_gripper: <<: *OPEN_GRIPPER - - step: "GET_STONE" - arm: - joints: [ *JOINT1_SMALL_DOWN, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_MID, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] + - step: "left ready to get bottom unit" + arms: + groups: + - left_arm: [ 0.312, 1.644, -1.537, 1.018, -1.46, 0.059, -0.322 ] common: <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "JOINT1_UP" - arm: - joints: [ *JOINT1_SMALL_READY, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + <<: *BIGGER_TOLERANCE + LEFT_GET_BOTTOM_UNIT0: + - step: "left gripper close" + left_gripper: + <<: *CLOSE_GRIPPER + - step: "left pull down unit" + arms: + groups: + - left_arm: [ 0.557, 1.658, -1.537, 0.962, -1.411, 0.093, -0.104 ] common: <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "ARM_UP_STONE" - arm: - joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_MID, *JOINT4_SMALL_READY, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] + <<: *BIGGER_TOLERANCE + - step: "left back to ready pos" + arms: + groups: + - left_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] common: <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "READY_STORE" - arm: - joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] + <<: *BIGGER_TOLERANCE + + LEFT_STORE_MID: + - step: "right arm back" + arms: + groups: + #- left_arm: [ 0.812, 1.4, -1.147, 1.276, -0.548, 0.703, 1.631 ] + - right_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] common: <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "STORE" - arm: - joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] + <<: *BIGGER_TOLERANCE + - step: "left ready to store unit mid" + arms: + groups: + - left_arm: [ 0.812, 1.4, -1.147, 1.276, -0.548, 0.703, 1.631 ] + #- right_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: <<: *BIGGER_TOLERANCE - - step: "ADD_STONE" - stone_num: - change: "SILVER" - - step: "CLOSE_GRIPPER" - gripper: - <<: *CLOSE_GRIPPER - - step: "MOVE_ARM_OUT" - arm: - joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + LEFT_STORE_MID0: + - step: "left store unit mid" + arms: + groups: + - left_arm: [ 1.272, 1.442, -1.273, 1.375, -0.431, 1.019, 1.71 ] common: <<: *QUICKLY tolerance: <<: *BIGGER_TOLERANCE - - step: "CHASSIS_BACK" - chassis: - <<: *CHASSIS_BACK - - step: "CHASSIS_TURN" - chassis: - <<: *CHASSIS_TURN - - step: "pre_home" - arm: - <<: *PRE_HOME - - step: "home" - arm: - <<: *HOME - - - ######THREE_STONE_ONCE(yi jian san lian)######### - ######FIRST_STONE######## - THREE_STONE_SMALL_ISLAND: - - step: "GIMBAL_READY" - gimbal: - <<: *FRONT_POS - - step: "ORE_ROTATOR_READY" - ore_rotator: - target: *READY_POS - - step: "ORE_LIFTER_READY" - ore_lifter: - target: *BIN_MID_POS - - step: "ARM_READY" - arm: - joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_LEFT, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] - common: - <<: *NORMALLY - tolerance: - <<: *NORMAL_TOLERANCE - - step: "OPEN_GRIPPER" - gripper: + - step: "left gripper open" + left_gripper: <<: *OPEN_GRIPPER - THREE_STONE_SMALL_ISLAND0: - - step: "GET_STONE" - arm: - joints: [ *JOINT1_SMALL_DOWN, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_LEFT, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] + - step: "left leave unit" + arms: + groups: + - left_arm: [ 1.356, 0.636, -1.329, 1.503, -0.8, 1.018, 1.714 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "JOINT1_UP" - arm: - joints: [ *JOINT1_SMALL_READY, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + <<: *BIGGER_TOLERANCE + - step: "left back to ready pos" + arms: + groups: + - left_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "ARM_UP_STONE" - arm: - joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_LEFT, *JOINT4_SMALL_READY, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] + <<: *BIGGER_TOLERANCE + + LEFT_GET_STORED_MID: + - step: "left gripper open" + left_gripper: + <<: *OPEN_GRIPPER + - step: "left ready to get stored unit mid" + arms: + groups: + - left_arm: [ 1.356, 0.636, -1.329, 1.503, -0.8, 1.018, 1.714 ] + - right_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "JOINT6_BACK" - arm: - joints: [ "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", *JOINT6_SMALL_BACK, "KEEP" ] + <<: *BIGGER_TOLERANCE + LEFT_GET_STORED_MID0: + - step: "left get stored unit mid" + arms: + groups: + - left_arm: [ 1.272, 1.442, -1.273, 1.375, -0.431, 1.019, 1.71 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "TURN_STONE" - arm: - joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] + <<: *BIGGER_TOLERANCE + - step: "left gripper close" + left_gripper: + <<: *CLOSE_GRIPPER + - step: "left get up unit mid" + arms: + groups: + - left_arm: [ 0.791, 1.44, -1.218, 1.264, -0.572, 0.658, 1.634 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "READY_STORE" - arm: - joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] + <<: *BIGGER_TOLERANCE + - step: "left back to ready pos" + arms: + groups: + - left_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "STORE" - arm: - joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] + <<: *BIGGER_TOLERANCE + + LEFT_STORE_SIDE: + - step: "left avoid side" + arms: + groups: + - left_arm: [ 0.958, 1.283, -1.463, 1.425, -0.693, 1.037, 0.79 ] common: - <<: *SLOWLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "CLOSE_GRIPPER" - gripper: - <<: *CLOSE_GRIPPER - - step: "MOVE_ARM_OUT" - arm: - joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + <<: *BIGGER_TOLERANCE + - step: "left ready to store unit side" + arms: + groups: + - left_arm: [ 0.827, 0.921, -1.063, 1.782, -0.878, 0.628, 1.395 ] common: - <<: *SLOWLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "ORE_LIFTER_DOWN" - ore_lifter: - target: *BIN_DOWN_POS - - step: "ADD_STONE" - stone_num: - change: "SILVER" - - #####SECOND_STONE######## -# THREE_STONE_SMALL_ISLAND00: - - step: "GIMBAL_READY" - gimbal: - <<: *FRONT_POS - - step: "ORE_ROTATOR_READY" - ore_rotator: - target: *READY_POS - - step: "ORE_LIFTER_READY" - ore_lifter: - target: *BIN_DOWN_POS - - step: "ARM_READY" - arm: - joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_MID, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] - common: - <<: *NORMALLY + <<: *BIGGER_TOLERANCE + - step: "left store unit side" + arms: + groups: + - left_arm: [ 1.375, 1.165, -1.088, 1.736, -0.861, 1.009, 1.744 ] + common: + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "OPEN_GRIPPER" - gripper: + <<: *BIGGER_TOLERANCE + - step: "left gripper open" + left_gripper: <<: *OPEN_GRIPPER -# THREE_STONE_SMALL_ISLAND000: - - step: "GET_STONE" - arm: - joints: [ *JOINT1_SMALL_DOWN, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_MID, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] + - step: "left leave unit" + arms: + groups: + - left_arm: [ 1.77, 0.686, -1.57, 1.689, -1.036, 1.011, 1.724 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "JOINT1_UP" - arm: - joints: [ *JOINT1_SMALL_READY, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + <<: *BIGGER_TOLERANCE + - step: "left back to ready pos" + arms: + groups: + - left_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "ARM_UP_STONE" - arm: - joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_MID, *JOINT4_SMALL_READY, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] + <<: *BIGGER_TOLERANCE + + LEFT_GET_STORED_SIDE: + - step: "left gripper open" + left_gripper: + <<: *OPEN_GRIPPER + - step: "left ready to get stored unit side" + arms: + groups: + - left_arm: [ 1.77, 0.686, -1.57, 1.689, -1.036, 1.011, 1.724 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "JOINT6_BACK" - arm: - joints: [ "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", *JOINT6_SMALL_BACK, "KEEP" ] + <<: *BIGGER_TOLERANCE + - step: "left get stored unit side" + arms: + groups: + - left_arm: [ 1.456, 1.132, -1.092, 1.76, -0.829, 1.01, 1.712 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "TURN_STONE" - arm: - joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] + <<: *BIGGER_TOLERANCE + - step: "left gripper close" + left_gripper: + <<: *CLOSE_GRIPPER + - step: "left take out unit side" + arms: + groups: + - left_arm: [ 0.827, 0.921, -1.063, 1.782, -0.878, 0.628, 1.395 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "READY_STORE" - arm: - joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] + <<: *BIGGER_TOLERANCE + - step: "left avoid mid" + arms: + groups: + - left_arm: [ 0.958, 1.283, -1.463, 1.425, -0.693, 1.037, 0.79 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "STORE" - arm: - joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] + <<: *BIGGER_TOLERANCE + - step: "left back to ready pos" + arms: + groups: + - left_arm: [ 1.2, 0.868, -1.57, 0.163, 1.655, 0.0, 0.0 ] +# - left_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] common: - <<: *SLOWLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "CLOSE_GRIPPER" - gripper: + <<: *BIGGER_TOLERANCE + + RIGHT_GET_TOP_UNIT: + - step: "right gripper open" + right_gripper: + <<: *OPEN_GRIPPER + - step: "right ready to get top unit" + arms: + groups: + - right_arm: [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] + common: + <<: *QUICKLY + tolerance: + <<: *BIGGER_TOLERANCE + - step: "right gripper close" + right_gripper: <<: *CLOSE_GRIPPER - - step: "MOVE_ARM_OUT" - arm: - joints: [ *JOINT1_SMALL_READY_STORE, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + - step: "right get up unit" + arms: + groups: + - right_arm: [ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 ] common: - <<: *SLOWLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "ADD_STONE" - stone_num: - change: "SILVER" - #######THIRD_STONE######### -# THREE_STONE_SMALL_ISLAND0000: - - step: "GIMBAL_READY" - gimbal: - <<: *FRONT_POS - - step: "ORE_ROTATOR_READY" - ore_rotator: - target: *READY_POS - - step: "ORE_LIFTER_READY" - ore_lifter: - target: *BIN_DOWN_POS - - step: "ARM_READY" - arm: - joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_RIGHT, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] - common: - <<: *NORMALLY + <<: *BIGGER_TOLERANCE + - step: "right back to ready pos" + arms: + groups: + - right_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] + common: + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "OPEN_GRIPPER" - gripper: + <<: *BIGGER_TOLERANCE + + RIGHT_GET_BOTTOM_UNIT: + - step: "right gripper open" + right_gripper: <<: *OPEN_GRIPPER -# THREE_STONE_SMALL_ISLAND00000: - - step: "GET_STONE" - arm: - joints: [ *JOINT1_SMALL_DOWN, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_RIGHT, *JOINT4_SMALL_READY, *JOINT5_SMALL_READY, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] + - step: "right ready to get bottom unit" + arms: + groups: + - right_arm: [ 0.322, 1.644, -1.537, 1.018, -1.46, 0.059, -0.37 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "JOINT1_UP" - arm: - joints: [ *JOINT1_SMALL_READY, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + <<: *BIGGER_TOLERANCE + RIGHT_GET_BOTTOM_UNIT0: + - step: "right gripper close" + right_gripper: + <<: *CLOSE_GRIPPER + - step: "right pull down unit" + arms: + groups: + - right_arm: [ 0.545, 1.658, -1.537, 0.962, -1.411, 0.093, -0.104 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "ARM_UP_STONE" - arm: - joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_READY_RIGHT, *JOINT4_SMALL_READY, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] + <<: *BIGGER_TOLERANCE + - step: "right back to ready pos" + arms: + groups: + - right_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "JOINT6_BACK" - arm: - joints: [ "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", *JOINT6_SMALL_BACK, "KEEP" ] + <<: *BIGGER_TOLERANCE + + RIGHT_STORE_MID: + - step: "left arm back" + arms: + groups: + #- right_arm: [ 0.625, 1.227, -0.813, 1.166, -0.839, 0.942, 1.414 ] + - left_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "TURN_STONE" - arm: - joints: [ *JOINT1_SMALL_READY, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY, *JOINT7_SMALL_READY ] + <<: *BIGGER_TOLERANCE + - step: "right ready to store unit mid" + arms: + groups: + - right_arm: [ 0.625, 1.227, -0.813, 1.166, -0.839, 0.942, 1.414 ] + #- left_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "READY_STORE(THIRD_STONE)" - arm: - joints: [ *JOINT1_SMALL_READY_STORE_THIRD, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_UP_STONE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] + <<: *BIGGER_TOLERANCE + RIGHT_STORE_MID0: + - step: "right store unit mid" + arms: + groups: + - right_arm: [ 1.267, 1.398, -1.2, 1.308, -0.518, 1.051, 1.593 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "STORE" - arm: - joints: [ *JOINT1_SMALL_READY_STORE_THIRD, *JOINT2_SMALL_STORE, *JOINT3_SMALL_TURN, *JOINT4_SMALL_TURN, *JOINT5_SMALL_STORE, *JOINT6_SMALL_READY_STORE, *JOINT7_SMALL_READY_STORE ] + <<: *BIGGER_TOLERANCE + - step: "right gripper open" + right_gripper: + <<: *OPEN_GRIPPER + - step: "right leave unit" + arms: + groups: + - right_arm: [ 1.09, 0.67, -1.101, 1.28, -0.744, 1.03, 1.595 ] common: - <<: *SLOWLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE -# - step: "CLOSE_GRIPPER" -# gripper: -# <<: *CLOSE_GRIPPER -# - step: "MOVE_ARM_OUT" -# arm: -# joints: [ *JOINT1_SMALL_READY_STORE_THIRD, *JOINT2_SMALL_READY, *JOINT3_SMALL_TURN, *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] -# common: -# <<: *SLOWLY -# tolerance: -# <<: *NORMAL_TOLERANCE - - step: "ADD_STONE" - stone_num: - change: "SILVER" - - ######GET STONE FROM BIN######### - - GET_DOWN_STONE_BIN: - - step: "GIMBAL_READY" - gimbal: - <<: *FRONT_POS - - step: "ORE_ROTATOR_READY" - ore_rotator: - target: *EXCHANGE_POS - - step: "ORE_LIFTER_UP" - ore_lifter: - target: *BIN_UP_POS - - step: "JOINT2 move out" - arm: - joints: [ "KEEP", *JOINT2_AVOID_COLLISION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + <<: *BIGGER_TOLERANCE + - step: "right back to ready pos" + arms: + groups: + - right_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] common: - <<: *SLOWLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "READY_TO_GET" - arm: - joints: [ *JOINT1_BIN_GET_STONE, *JOINT2_BIN_GET_STONE, *JOINT3_BIN_GET_LV4_STONE, *JOINT4_R_POSITION, *JOINT5_BIN_GET_STONE, *JOINT6_BIN_GET_STONE, *JOINT7_MID_POSITION ] + <<: *BIGGER_TOLERANCE + + RIGHT_GET_STORED_MID: + - step: "right gripper open" + right_gripper: + <<: *OPEN_GRIPPER + - step: "right ready to get stored unit mid" + arms: + groups: + - right_arm: [ 1.09, 0.67, -1.101, 1.28, -0.744, 1.03, 1.595 ] + - left_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] common: <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "OPEN_GRIPPER" - gripper: - <<: *OPEN_GRIPPER - - step: "JOINT2_BACK_GET_STONE" - arm: - joints: [ "KEEP", *JOINT2_BACK_BIN_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + <<: *BIGGER_TOLERANCE + RIGHT_GET_STORED_MID0: + - step: "right get stored unit mid" + arms: + groups: + - right_arm: [ 1.267, 1.398, -1.2, 1.308, -0.518, 1.051, 1.593 ] common: - <<: *SLOWLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "JOINT1_UP" - arm: - joints: [ *JOINT1_UP_POS, "KEEP", "KEEP", "KEEP", *JOINT5_MID_POSITION, "KEEP", "KEEP" ] + <<: *BIGGER_TOLERANCE + - step: "right gripper close" + right_gripper: + <<: *CLOSE_GRIPPER + - step: "right get up unit mid" + arms: + groups: + - right_arm: [ 0.625, 1.227, -0.813, 1.166, -0.839, 0.942, 1.414 ] common: <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "exchange pos" - arm: - joints: [ *JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + <<: *BIGGER_TOLERANCE + - step: "right back to ready pos" + arms: + groups: + - right_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "remove stone" - stone_num: - change: "-1" - - step: "ORE_LIFTER_DOWN" - ore_lifter: - target: *BIN_DOWN_POS - - step: "gimbal lifter tallest" - gimbal_lifter: - target: *TALLEST_POS - + <<: *BIGGER_TOLERANCE - GET_UP_STONE_BIN: - - step: "GIMBAL_READY" - gimbal: - <<: *FRONT_POS - - step: "ORE_ROTATOR_READY" - ore_rotator: - target: *EXCHANGE_POS - - step: "roe lifter ready" - ore_lifter: - target: *BIN_SMALL_UP_POS - - step: "JOINT2 move out" - arm: - joints: [ "KEEP", *JOINT2_AVOID_COLLISION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + RIGHT_STORE_SIDE: + - step: "right avoid mid" + arms: + groups: + - right_arm: [ 0.958, 1.283, -1.463, 1.425, -0.693, 1.037, 0.79 ] common: - <<: *SLOWLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "READY_TO_GET" - arm: - joints: [ *JOINT1_BIN_GET_STONE, *JOINT2_BIN_GET_STONE, *JOINT3_BIN_GET_LV4_STONE, *JOINT4_R_POSITION, *JOINT5_BIN_GET_STONE, *JOINT6_BIN_GET_STONE, *JOINT7_MID_POSITION ] + <<: *BIGGER_TOLERANCE + - step: "right ready to store unit side" + arms: + groups: + - right_arm: [ 0.575, 0.632, -0.626, 1.662, -0.943, 1.001, 0.962 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "OPEN_GRIPPER" - gripper: + <<: *BIGGER_TOLERANCE + - step: "right store unit side" + arms: + groups: + - right_arm: [ 1.422, 1.05, -1.164, 1.725, -0.734, 1.04, 1.475 ] + common: + <<: *QUICKLY + tolerance: + <<: *BIGGER_TOLERANCE + - step: "right gripper open" + right_gripper: <<: *OPEN_GRIPPER - - step: "JOINT2_BACK_GET_STONE" - arm: - joints: [ "KEEP", *JOINT2_BACK_BIN_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + - step: "right leave unit" + arms: + groups: + - right_arm: [ 1.766, 0.451, -1.599, 1.701, -1.126, 1.061, 1.591 ] common: - <<: *SLOWLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "JOINT1_UP" - arm: - joints: [ *JOINT1_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + <<: *BIGGER_TOLERANCE + - step: "right back to ready pos" + arms: + groups: + - right_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] common: <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "JOINT5_UP" - arm: - joints: [ "KEEP", "KEEP", "KEEP", "KEEP", *JOINT5_MID_POSITION, "KEEP", "KEEP" ] + <<: *BIGGER_TOLERANCE + + RIGHT_GET_STORED_SIDE: + - step: "right gripper open" + right_gripper: + <<: *OPEN_GRIPPER + - step: "right ready to get stored unit side" + arms: + groups: + - right_arm: [ 1.785, 0.452, -1.579, 1.681, -0.92, 0.982, 1.592 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "exchange pos" - arm: - joints: [ *JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + <<: *BIGGER_TOLERANCE + - step: "right get stored unit side" + arms: + groups: + - right_arm: [ 1.422, 1.05, -1.164, 1.725, -0.734, 1.04, 1.475 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "remove stone" - stone_num: - change: "-1" - - step: "ORE_LIFTER_DOWN" - ore_lifter: - target: *BIN_DOWN_POS - - step: "gimbal lifter tallest" - gimbal_lifter: - target: *TALLEST_POS - - - GET_DOWN_STONE_LEFT: - - step: "GIMBAL_READY" - gimbal: - <<: *FRONT_POS - - step: "ORE_ROTATOR_READY" - ore_rotator: - target: *EXCHANGE_POS - - step: "ORE_LIFTER_UP" - ore_lifter: - target: *BIN_GET_DOWN - - step: "JOINT2 move out" - arm: - joints: [ "KEEP", *JOINT2_AVOID_COLLISION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + <<: *BIGGER_TOLERANCE + - step: "right gripper close" + right_gripper: + <<: *CLOSE_GRIPPER + - step: "right take out unit side" + arms: + groups: + - right_arm: [ 0.575, 0.632, -0.626, 1.662, -0.943, 1.001, 0.962 ] common: - <<: *SLOWLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "READY_TO_GET" - arm: - joints: [ *JOINT1_BIN_GET_STONE_LEFT, *JOINT2_BIN_GET_STONE, *JOINT3_BIN_GET_STONE, *JOINT4_R_POSITION, *JOINT5_BIN_GET_STONE, *JOINT6_BIN_GET_STONE, *JOINT7_BIN_GET_LEFT ] + <<: *BIGGER_TOLERANCE + - step: "right avoid mid" + arms: + groups: + - right_arm: [ 0.958, 1.283, -1.463, 1.425, -0.693, 1.037, 0.79 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: <<: *BIGGER_TOLERANCE - - step: "OPEN_GRIPPER" - gripper: + - step: "right back to ready pos" + arms: + groups: + - right_arm: [ 1.2, 0.868, -1.57, 0.163, 1.655, 0.0, 0.0 ] +# - right_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] + common: + <<: *QUICKLY + tolerance: + <<: *BIGGER_TOLERANCE + + GET_STORED_SIDE_UNITS: + - step: "grippers open" + left_gripper: + <<: *OPEN_GRIPPER + right_gripper: <<: *OPEN_GRIPPER - - step: "JOINT2_BACK_GET_STONE" - arm: - joints: [ "KEEP", *JOINT2_BACK_BIN_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + - step: "left ready to get stored side units" + arms: + groups: + - left_arm: [ 1.77, 0.686, -1.57, 1.689, -1.036, 1.011, 1.724 ] +# - right_arm: [ 1.716, 0.651, -1.54, 1.671, -1.077, 1.055, 1.647 ] common: - <<: *SLOWLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "JOINT1_UP" - arm: - joints: [ *JOINT1_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + <<: *BIGGER_TOLERANCE + - step: "left get stored side units and right ready to get side unit" + arms: + groups: + - left_arm: [ 1.456, 1.132, -1.092, 1.76, -0.829, 1.01, 1.712 ] + - right_arm: [ 1.785, 0.452, -1.579, 1.681, -0.92, 0.982, 1.592 ] +# - right_arm: [ 1.422, 1.05, -1.164, 1.772, -0.734, 1.04, 1.475 ] common: <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "JOINT5_UP" - arm: - joints: [ "KEEP", "KEEP", "KEEP", "KEEP", *JOINT5_MID_POSITION, "KEEP", "KEEP" ] + <<: *BIGGER_TOLERANCE + - step: "left gripper close" + left_gripper: + <<: *CLOSE_GRIPPER +# right_gripper: +# <<: *CLOSE_GRIPPER + - step: "left take out stored side unit and right get stored side" + arms: + groups: + - left_arm: [ 0.827, 0.921, -1.063, 1.782, -0.878, 0.628, 1.395 ] + - right_arm: [ 1.422, 1.05, -1.164, 1.725, -0.734, 1.04, 1.475 ] +# - right_arm: [ 1.716, 0.651, -1.54, 1.671, -1.077, 1.055, 1.647 ] +# - right_arm: [ 0.529, 0.615, -0.552, 1.742, -0.905, 1.052, 0.948 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "remove stone" - stone_num: - change: "-1" - - step: "exchange pos" - arm: - joints: [*JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_L90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION] + <<: *BIGGER_TOLERANCE + - step: "left avoid mid and right gripper close" + arms: + groups: + - left_arm: [ 0.958, 1.283, -1.463, 1.425, -0.693, 1.037, 0.79 ] +# - right_arm: [ 1.422, 1.05, -1.164, 1.772, -0.734, 1.04, 1.475 ] +# - right_arm: [ 0.958, 1.283, -1.463, 1.425, -0.693, 1.037, 0.79 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "ORE_LIFTER_DOWN" - ore_lifter: - target: *BIN_DOWN_POS - - step: "gimbal lifter tallest" - gimbal_lifter: - target: *TALLEST_POS - + <<: *BIGGER_TOLERANCE + right_gripper: + <<: *CLOSE_GRIPPER + - step: "left back to ready pos and right take out stored side" + arms: + groups: + - left_arm: [ 1.2, 0.868, -1.57, 0.163, 1.655, 0.0, 0.0 ] +# - left_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] + - right_arm: [ 0.575, 0.632, -0.626, 1.662, -0.943, 1.001, 0.962 ] +# - right_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] + common: + <<: *QUICKLY + tolerance: + <<: *BIGGER_TOLERANCE +# - step: "right avoid mid" + - step: "right back to ready pos" + arms: + groups: + - right_arm: [ 1.2, 0.868, -1.57, 0.163, 1.655, 0.0, 0.0 ] +# - right_arm: [ 0.958, 1.283, -1.463, 1.425, -0.693, 1.037, 0.79 ] + common: + <<: *QUICKLY + tolerance: + <<: *BIGGER_TOLERANCE +# - step: "right back to ready pos" +# arms: +# groups: +# - right_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] +# common: +# <<: *QUICKLY +# tolerance: +# <<: *BIGGER_TOLERANCE - GET_DOWN_STONE_RIGHT: - - step: "GIMBAL_READY" - gimbal: - <<: *FRONT_POS - - step: "ORE_ROTATOR_READY" - ore_rotator: - target: *EXCHANGE_POS - - step: "ORE_LIFTER_UP" - ore_lifter: - target: *BIN_GET_DOWN - - step: "JOINT2 move out" - arm: - joints: [ "KEEP", *JOINT2_AVOID_COLLISION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + STORE_UNITS_SIDE: + - step: "right avoid mid" + arms: + groups: + - right_arm: [ 0.958, 1.283, -1.463, 1.425, -0.693, 1.037, 0.79 ] common: - <<: *SLOWLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "READY_TO_GET" - arm: - joints: [ *JOINT1_BIN_GET_STONE_RIGHT, *JOINT2_BIN_GET_STONE, *JOINT3_BIN_GET_STONE, *JOINT4_R_POSITION, *JOINT5_BIN_GET_STONE, *JOINT6_BIN_GET_STONE, *JOINT7_BIN_GET_RIGHT ] + <<: *BIGGER_TOLERANCE + - step: "left avoid mid and right ready to store side" + arms: + groups: + - left_arm: [ 0.958, 1.283, -1.463, 1.425, -0.693, 1.037, 0.79 ] + - right_arm: [ 0.575, 0.632, -0.626, 1.662, -0.943, 1.001, 0.962 ] + # - right_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: <<: *BIGGER_TOLERANCE - - step: "OPEN_GRIPPER" - gripper: - <<: *OPEN_GRIPPER - - step: "JOINT2_BACK_GET_STONE" - arm: - joints: [ "KEEP", *JOINT2_BACK_BIN_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + - step: "left ready to store side unit and right store side" + arms: + groups: + - left_arm: [ 0.827, 0.921, -1.063, 1.782, -0.878, 0.628, 1.395 ] + - right_arm: [ 1.422, 1.05, -1.164, 1.725, -0.734, 1.04, 1.475 ] + # - right_arm: [ 1.716, 0.651, -1.54, 1.671, -1.077, 1.055, 1.647 ] + # - right_arm: [ 0.529, 0.615, -0.552, 1.742, -0.905, 1.052, 0.948 ] common: - <<: *SLOWLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "JOINT1_UP" - arm: - joints: [ *JOINT1_UP_POS, "KEEP", "KEEP", "KEEP", *JOINT5_MID_POSITION, "KEEP", "KEEP" ] + <<: *BIGGER_TOLERANCE + - step: "right gripper open" + right_gripper: + <<: *OPEN_GRIPPER + - step: "left store unit side and right leave unit" + arms: + groups: + - left_arm: [ 1.375, 1.165, -1.088, 1.736, -0.861, 1.009, 1.744 ] + - right_arm: [ 1.771, 0.651, -1.567, 1.645, -1.01, 0.998, 1.583 ] + # - right_arm: [ 1.422, 1.05, -1.164, 1.772, -0.734, 1.04, 1.475 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "exchange pos" - arm: - joints: [*JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION] + <<: *BIGGER_TOLERANCE + - step: "left gripper open and right back to ready pos" + left_gripper: + <<: *OPEN_GRIPPER + arms: + groups: + - right_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "remove stone" - stone_num: - change: "-1" - - step: "ORE_LIFTER_DOWN" - ore_lifter: - target: *BIN_DOWN_POS - - step: "gimbal lifter tallest" - gimbal_lifter: - target: *TALLEST_POS - - - GET_UP_STONE_LEFT: - - step: "GIMBAL_READY" - gimbal: - <<: *FRONT_POS - - step: "ORE_ROTATOR_READY" - ore_rotator: - target: *EXCHANGE_POS - - step: "ORE_LIFTER_READY" - ore_lifter: - target: *BIN_GET_UP - - step: "JOINT2 move out" - arm: - joints: [ "KEEP", *JOINT2_AVOID_COLLISION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + <<: *BIGGER_TOLERANCE + - step: "left leave unit" + arms: + groups: + - left_arm: [ 1.77, 0.686, -1.57, 1.689, -1.036, 1.011, 1.724 ] common: - <<: *SLOWLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "READY_TO_GET" - arm: - joints: [ *JOINT1_BIN_GET_STONE_LEFT, *JOINT2_BIN_GET_STONE, *JOINT3_BIN_GET_STONE, *JOINT4_R_POSITION, *JOINT5_BIN_GET_STONE, *JOINT6_BIN_GET_STONE, *JOINT7_BIN_GET_LEFT ] + <<: *BIGGER_TOLERANCE + - step: "left back to ready pos" + arms: + groups: + - left_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: <<: *BIGGER_TOLERANCE - - step: "OPEN_GRIPPER" - gripper: + + GET_DOWN_UNITS: + - step: "grippers open" + left_gripper: <<: *OPEN_GRIPPER - - step: "JOINT2_BACK_GET_STONE" - arm: - joints: [ "KEEP", *JOINT2_BACK_BIN_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + right_gripper: + <<: *OPEN_GRIPPER + - step: "arms ready to get down two units" + arms: + groups: + - left_arm: [ 0.461, 1.688, -1.515, 1.665, -0.466, -0.525, -0.464 ] + - right_arm: [ 0.428, 1.655, -1.493, 1.654, -0.645, -0.57, -0.535 ] common: - <<: *SLOWLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "JOINT1_UP" - arm: - joints: [ *JOINT1_UP_POS, "KEEP", "KEEP", "KEEP", *JOINT5_MID_POSITION, "KEEP", "KEEP" ] + <<: *BIGGER_TOLERANCE + GET_DOWN_UNITS0: + - step: "grippers close" + left_gripper: + <<: *CLOSE_GRIPPER + right_gripper: + <<: *CLOSE_GRIPPER + - step: "arms pull down units" + arms: + groups: + - left_arm: [ 0.81, 1.101, -1.57, 1.602, -0.775, -0.262, -0.096 ] + - right_arm: [ 0.81, 1.111, -1.57, 1.602, -0.775, -0.262, -0.096 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "exchange pos" - arm: - joints: [*JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_L90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION] + <<: *BIGGER_TOLERANCE + - step: "arms avoid mid" + arms: + groups: + - left_arm: [ 0.958, 1.283, -1.463, 1.425, -0.693, 1.037, 0.79 ] + - right_arm: [ 0.958, 1.283, -1.463, 1.425, -0.693, 1.037, 0.79 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "remove stone" - stone_num: - change: "-1" - - step: "ORE_LIFTER_DOWN" - ore_lifter: - target: *BIN_DOWN_POS - - step: "gimbal lifter tallest" - gimbal_lifter: - target: *TALLEST_POS - + <<: *BIGGER_TOLERANCE +# GET_DOWN_UNITS00: +# store - GET_UP_STONE_RIGHT: - - step: "GIMBAL_READY" - gimbal: - <<: *FRONT_POS - - step: "ORE_ROTATOR_READY" - ore_rotator: - target: *EXCHANGE_POS - - step: "ORE_LIFTER_READY" - ore_lifter: - target: *BIN_GET_UP - - step: "JOINT2 move out" - arm: - joints: [ "KEEP", *JOINT2_AVOID_COLLISION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + GET_UP_UNITS: + - step: "arms back to ready pos" + arms: + groups: + - left_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] + - right_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] common: - <<: *SLOWLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "READY_TO_GET" - arm: - joints: [ *JOINT1_BIN_GET_STONE_RIGHT, *JOINT2_BIN_GET_STONE, *JOINT3_BIN_GET_STONE, *JOINT4_R_POSITION, *JOINT5_BIN_GET_STONE, *JOINT6_BIN_GET_STONE, *JOINT7_BIN_GET_RIGHT ] + <<: *BIGGER_TOLERANCE + - step: "arms avoid mid" + arms: + groups: + - left_arm: [ 1.192, 0.669, -1.568, 1.594, -0.541, 0.695, 0.041 ] + - right_arm: [ 1.192, 0.669, -1.568, 1.594, -0.541, 0.695, 0.041 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: <<: *BIGGER_TOLERANCE - - step: "OPEN_GRIPPER" - gripper: - <<: *OPEN_GRIPPER - - step: "JOINT2_BACK_GET_STONE" - arm: - joints: [ "KEEP", *JOINT2_BACK_BIN_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + - step: "arms get up two units" + arms: + groups: + - left_arm: [ 0.0, 1.464, -1.549, 1.209, 0.339, -0.353, 0.512 ] + - right_arm: [ -0.02, 1.474, -1.424, 1.017, 0.168, -0.074, 0.293 ] common: - <<: *SLOWLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "JOINT1_UP" - arm: - joints: [ *JOINT1_UP_POS, "KEEP", "KEEP", "KEEP", *JOINT5_MID_POSITION, "KEEP", "KEEP" ] + <<: *BIGGER_TOLERANCE + GET_UP_UNITS0: + - step: "grippers close" + left_gripper: + <<: *CLOSE_GRIPPER + right_gripper: + <<: *CLOSE_GRIPPER + - step: "arms pull up units" + arms: + groups: + # old +# - left_arm: [ -0.426, 1.258, -1.436, 0.459, 0.158, 0.135, 0.458 ] +# - right_arm: [ -0.426, 1.258, -1.436, 0.459, 0.158, 0.135, 0.458 ] + # old + - left_arm: [ -0.37, 1.121, -1.394, 1.013, 0.177, -0.584, 0.685 ] + - right_arm: [ -0.24, 1.184, -1.396, 0.876, 0.051, -0.073, 0.449 ] common: <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "exchange pos" - arm: - joints: [*JOINT1_DOWN_POSITION, *JOINT2_EXCHANGE_POSITION, *JOINT3_MID_POSITION, *JOINT4_MID_POSITION, *JOINT5_R90_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION] + <<: *BIGGER_TOLERANCE + - step: "arms avoid mid" + arms: + groups: + - left_arm: [ 1.192, 0.669, -1.568, 1.594, -0.541, 0.695, 0.041 ] + - right_arm: [ 1.192, 0.669, -1.568, 1.594, -0.541, 0.695, 0.041 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "remove stone" - stone_num: - change: "-1" - - step: "ORE_LIFTER_DOWN" - ore_lifter: - target: *BIN_DOWN_POS - - step: "gimbal lifter tallest" - gimbal_lifter: - target: *TALLEST_POS - - - #################BIG_ISLAND############# - ######MID_STONE####### - MID_BIG_ISLAND: - - step: "gimbal ready" - gimbal: - <<: *BIG_ISLAND_POS - - step: "open ex arm" - extend_arm: - front: *EX_OPEN - back: *EX_OPEN - - step: "MID_ARM_READY" - arm: - joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_BIG_READY, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY , *JOINT7_BIG_POS] - common: - <<: *QUICKLY + <<: *BIGGER_TOLERANCE + - step: "arms back to ready pos" + arms: + groups: + - left_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] + - right_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] + common: + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "ORE_ROTATOR_READY" - ore_rotator: - target: *EXCHANGE_POS - - step: "gimbal lifter" - gimbal_lifter: - target: *LIFTER_BIG_ISLAND - - step: "OPEN_GRIPPER" - gripper: + <<: *BIGGER_TOLERANCE + + GET_FIVE_UNITS: + - step: "grippers open" + left_gripper: + <<: *OPEN_GRIPPER + right_gripper: <<: *OPEN_GRIPPER - MID_BIG_ISLAND0: - - step: "GET_MID_STONE" - arm: - joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_GET_BIG_MID, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ] + - step: "arms ready to get down two units" + arms: + groups: + - left_arm: [ 0.461, 1.688, -1.515, 1.665, -0.466, -0.525, -0.464 ] + - right_arm: [ 0.478, 1.655, -1.493, 1.654, -0.645, -0.57, -0.535 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: <<: *BIGGER_TOLERANCE - MID_BIG_ISLAND00: - - step: "ADJUST_STONE" - arm: - joints: [ *JOINT1_BIG_ADJUST_MID, *JOINT2_BIG_READY, *JOINT3_GET_BIG_MID, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_ADJUST_MID, *JOINT7_BIG_POS ] + GET_FIVE_UNITS0: + - step: "grippers close" + left_gripper: + <<: *CLOSE_GRIPPER + right_gripper: + <<: *CLOSE_GRIPPER + - step: "arms pull down units" + arms: + groups: + - left_arm: [ 0.81, 1.101, -1.57, 1.602, -0.775, -0.262, -0.096 ] + - right_arm: [ 0.81, 1.111, -1.57, 1.602, -0.775, -0.262, -0.096 ] common: - <<: *SLOWLY + <<: *QUICKLY tolerance: <<: *BIGGER_TOLERANCE - - step: "PULL_OUT_STONE" - arm: - joints: [ "KEEP", "KEEP", *JOINT3_BIG_PULL_OUT, "KEEP", "KEEP", "KEEP", "KEEP" ] + - step: "arms avoid mid" + arms: + groups: + - left_arm: [ 0.958, 1.283, -1.463, 1.425, -0.693, 1.037, 0.79 ] + - right_arm: [ 0.958, 1.283, -1.463, 1.425, -0.693, 1.037, 0.79 ] common: - <<: *SLOWLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "gimbal front" - gimbal: - <<: *FRONT_POS - MID_BIG_ISLAND000: - - step: "JOINT1_UP" - arm: - joints: [ *JOINT1_BIG_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + <<: *BIGGER_TOLERANCE + - step: "left avoid mid and right ready to store side" + arms: + groups: + - left_arm: [ 0.958, 1.283, -1.463, 1.425, -0.693, 1.037, 0.79 ] + - right_arm: [ 0.575, 0.632, -0.626, 1.662, -0.943, 1.001, 0.962 ] + # - right_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "ARM_UP_STONE" - arm: - joints: [ *JOINT1_BIG_UP_POS, *JOINT2_BIG_ARM_UP, *JOINT3_BIG_ARM_UP, *JOINT4_BIG_STORE, *JOINT5_BIG_READY, *JOINT6_BIG_ARM_UP, *JOINT7_BIG_POS ] + <<: *BIGGER_TOLERANCE + - step: "left ready to store side unit and right store side" + arms: + groups: + - left_arm: [ 0.827, 0.921, -1.063, 1.782, -0.878, 0.628, 1.395 ] + - right_arm: [ 1.422, 1.05, -1.164, 1.725, -0.734, 1.04, 1.475 ] + # - right_arm: [ 1.716, 0.651, -1.54, 1.671, -1.077, 1.055, 1.647 ] + # - right_arm: [ 0.529, 0.615, -0.552, 1.742, -0.905, 1.052, 0.948 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "READY_TO_STORE" - arm: - joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_BIG_STORE, *JOINT5_BIG_READY_STORE, *JOINT6_BIG_READY_STORE, *JOINT7_BIG_POS ] + <<: *BIGGER_TOLERANCE + - step: "right gripper open" + right_gripper: + <<: *OPEN_GRIPPER + - step: "left store unit side and right leave unit" + arms: + groups: + - left_arm: [ 1.375, 1.165, -1.088, 1.736, -0.861, 1.009, 1.744 ] + - right_arm: [ 1.771, 0.651, -1.567, 1.645, -1.01, 0.998, 1.583 ] + # - right_arm: [ 1.422, 1.05, -1.164, 1.772, -0.734, 1.04, 1.475 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "STORE_STONE" - arm: - joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_BIG_STORE, *JOINT5_BIG_STORE, *JOINT6_BIG_READY_STORE, *JOINT7_BIG_POS ] + <<: *BIGGER_TOLERANCE + - step: "left gripper open and right back to ready pos" + left_gripper: + <<: *OPEN_GRIPPER + arms: + groups: + - right_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "CLOSE_GRIPPER" - gripper: - <<: *CLOSE_GRIPPER - - step: "add stone" - stone_num: - change: "GOLD" - - step: "move arm out" - arm: - joints: [ *JOINT1_DOWN_POSITION, *JOINT2_BACK_POSITION, "KEEP", *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] + <<: *BIGGER_TOLERANCE + - step: "left leave unit" + arms: + groups: + - left_arm: [ 1.77, 0.686, -1.57, 1.689, -1.036, 1.011, 1.724 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "ORE_LIFTER_DOWN" - ore_lifter: - target: *BIN_DOWN_POS - - step: "ORE_ROTATOR_RESET" - ore_rotator: - target: *READY_POS - - step: "EX_ARM_RESET" - extend_arm: - front: *EX_CLOSE - back: *EX_CLOSE - - step: "pre_home" - arm: - <<: *PRE_HOME - - step: "home" - arm: - <<: *HOME - - step: "gimbal front" - gimbal: - <<: *FRONT_POS - - ########SIDE_STONE######### - SIDE_BIG_ISLAND: - - step: "gimbal ready" - gimbal: - <<: *BIG_ISLAND_POS - - step: "open ex arm" - extend_arm: - front: *EX_OPEN - back: *EX_CLOSE - - step: "MID_ARM_READY" - arm: - joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_BIG_READY, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ] + <<: *BIGGER_TOLERANCE +# - step: "left back to ready pos" +# arms: +# groups: +# - left_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] +# common: +# <<: *QUICKLY +# tolerance: +# <<: *BIGGER_TOLERANCE + # get bottom unit + # stay +# - step: "left arm ready to get bottom unit" +# arms: +# groups: +# - left_arm: [ 0.656, 1.365, -1.492, 1.724, 0.927, 0.276, 0.865 ] +# common: +# <<: *QUICKLY +# tolerance: +# <<: *BIGGER_TOLERANCE +# GET_FIVE_UNITS00: +# - step: "left arm get bottom unit" +# arms: +# groups: +# - left_arm: [ 0.458, 1.594, -1.478, 1.307, 1.2, 0.373, 0.714 ] +# common: +# <<: *QUICKLY +# tolerance: +# <<: *BIGGER_TOLERANCE +# - step: "left gripper close" +# left_gripper: +# <<: *CLOSE_GRIPPER +# - step: "left arm pull down bottom unit" +# arms: +# groups: +# - left_arm: [ 0.811, 1.539, -1.471, 1.25, 1.434, 0.446, 0.303 ] +# common: +# <<: *QUICKLY +# tolerance: +# <<: *BIGGER_TOLERANCE +# - step: "left arm get back" +# arms: +# groups: +# - left_arm: [ 1.026, 1.334, -1.57, 1.248, 2.881, -0.048, -0.987 ] +# common: +# <<: *QUICKLY +# tolerance: +# <<: *BIGGER_TOLERANCE +# - step: "left arm ready to store mid" +# arms: +# groups: +# - left_arm: [ 0.762, 1.392, -1.174, 1.278, 2.549, -0.726, -1.441 ] +# common: +# <<: *QUICKLY +# tolerance: +# <<: *BIGGER_TOLERANCE +# - step: "left arm store mid" +# arms: +# groups: +# - left_arm: [ 1.094, 1.243, -0.96, 1.29, 2.587, -1.18, -1.452 ] +# common: +# <<: *QUICKLY +# tolerance: +# <<: *BIGGER_TOLERANCE +# - step: "left gripper open" +# left_gripper: +# <<: *OPEN_GRIPPER +# - step: "left arm leave mid unit" +# arms: +# groups: +# - left_arm: [ 1.173, 0.52, -1.043, 1.433, 2.264, -1.063, -1.416 ] +# common: +# <<: *QUICKLY +# tolerance: +# <<: *BIGGER_TOLERANCE + # stay + - step: "left back to ready pos" + arms: + groups: + - left_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] common: <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "ORE_ROTATOR_READY" - ore_rotator: - target: *EXCHANGE_POS - - step: "gimbal lifter" - gimbal_lifter: - target: *LIFTER_BIG_ISLAND - - step: "OPEN_GRIPPER" - gripper: - <<: *OPEN_GRIPPER - - SIDE_BIG_ISLAND0: - - step: "GET_MID_STONE" - arm: - joints: [ *JOINT1_BIG_READY, *JOINT2_BIG_READY, *JOINT3_GET_BIG_SIDE, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_READY, *JOINT7_BIG_POS ] + <<: *BIGGER_TOLERANCE + - step: "arms avoid mid" + arms: + groups: + - left_arm: [ 1.192, 0.669, -1.568, 1.594, -0.541, 0.695, 0.041 ] + - right_arm: [ 1.192, 0.669, -1.568, 1.594, -0.541, 0.695, 0.041 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: <<: *BIGGER_TOLERANCE - - step: "ADJUST_STONE" - arm: - joints: [ *JOINT1_BIG_ADJUST_MID, *JOINT2_BIG_READY, *JOINT3_GET_BIG_SIDE, *JOINT4_R_POSITION, *JOINT5_BIG_READY, *JOINT6_BIG_ADJUST_MID, *JOINT7_BIG_POS ] + - step: "arms ready to get up two units" + arms: + groups: + - left_arm: [ 0.111, 1.36, -1.493, 1.786, 0.386, -0.816, 0.75 ] + - right_arm: [ 0.145, 1.516, -1.495, 1.722, 0.442, -0.729, 0.548 ] common: - <<: *SLOWLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "PULL_OUT_STONE" - arm: - joints: [ "KEEP", "KEEP", *JOINT3_BIG_PULL_OUT, "KEEP", "KEEP", "KEEP", "KEEP" ] + <<: *BIGGER_TOLERANCE +# STOP: +# GET_FIVE_UNITS000: + GET_FIVE_UNITS00: + - step: "arms get up two units" + arms: + groups: + - left_arm: [ -0.038, 1.464, -1.549, 1.209, 0.339, -0.353, 0.512 ] + - right_arm: [ -0.041, 1.481, -1.409, 1.002, 0.16, -0.093, 0.293 ] common: - <<: *SLOWLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "gimbal front" - gimbal: - <<: *FRONT_POS - SIDE_BIG_ISLAND00: - - step: "JOINT1_UP" - arm: - joints: [ *JOINT1_BIG_UP_POS, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + <<: *BIGGER_TOLERANCE + ### + - step: "grippers close" + left_gripper: + <<: *CLOSE_GRIPPER + right_gripper: + <<: *CLOSE_GRIPPER + - step: "arms pull up units" + arms: + groups: + # old +# - left_arm: [ -0.426, 1.258, -1.436, 0.459, 0.158, 0.135, 0.458 ] +# - right_arm: [ -0.426, 1.258, -1.436, 0.459, 0.158, 0.135, 0.458 ] + # old + - left_arm: [ -0.37, 1.121, -1.394, 1.013, 0.177, -0.584, 0.685 ] + - right_arm: [ -0.24, 1.184, -1.396, 0.876, 0.051, -0.073, 0.449 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "ARM_UP_STONE" - arm: - joints: [ *JOINT1_BIG_UP_POS, *JOINT2_BIG_ARM_UP, *JOINT3_BIG_ARM_UP, *JOINT4_BIG_STORE, *JOINT5_BIG_READY, *JOINT6_BIG_ARM_UP, *JOINT7_BIG_POS ] + <<: *BIGGER_TOLERANCE + - step: "arms avoid mid" + arms: + groups: + - left_arm: [ 1.192, 0.669, -1.568, 1.594, -0.541, 0.695, 0.041 ] + - right_arm: [ 1.192, 0.669, -1.568, 1.594, -0.541, 0.695, 0.041 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "READY_TO_STORE" - arm: - joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_BIG_STORE, *JOINT5_BIG_READY_STORE, *JOINT6_BIG_READY_STORE, *JOINT7_BIG_POS ] + <<: *BIGGER_TOLERANCE + - step: "arms back to ready pos" + arms: + groups: + - left_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] + - right_arm: [ 1.488, 1.4, -1.564, 1.58, 1.567, 0.0, 0.0 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "add stone" - stone_num: - change: "GOLD" - - step: "STORE_STONE" - arm: - joints: [ *JOINT1_BIG_READY_STORE, *JOINT2_BIG_READY_STORE, *JOINT3_BIG_ARM_UP, *JOINT4_BIG_STORE, *JOINT5_BIG_STORE, *JOINT6_BIG_READY_STORE, *JOINT7_BIG_POS ] + <<: *BIGGER_TOLERANCE + GET_FIVE_UNITS000: +# special set down + - step: "arms ready to set down" + arms: + groups: + - left_arm: [ 1.47, 1.567, -1.564, 1.58, 1.567, 0.0, 0.0 ] + - right_arm: [ 1.47, 1.567, -1.564, 1.58, 1.567, 0.0, 0.0 ] + - gimbal_arm: [ "KEEP", -0.048, 0.647, 1.184 ] common: - <<: *NORMALLY + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - - step: "CLOSE_GRIPPER" - gripper: - <<: *CLOSE_GRIPPER - - step: "move arm out" - arm: - joints: [ *JOINT1_DOWN_POSITION, *JOINT2_BACK_POSITION, "KEEP", *JOINT4_R_POSITION, *JOINT5_MID_POSITION, *JOINT6_UP_POSITION, *JOINT7_MID_POSITION ] - common: - <<: *NORMALLY - tolerance: - <<: *BIGGER_TOLERANCE - - step: "gimbal front" - gimbal: - <<: *FRONT_POS - - step: "pre_home" - arm: - <<: *PRE_HOME - - step: "home" - arm: - <<: *HOME - - step: "ORE_LIFTER_DOWN" - ore_lifter: - target: *BIN_DOWN_POS - - step: "ORE_ROTATOR_RESET" - ore_rotator: - target: *READY_POS - - step: "EX_ARM_RESET" - extend_arm: - front: *EX_CLOSE - back: *EX_CLOSE - - - - - ################## STORE ################# - - - - ##########GIMBAL_LIFTER########## - ZERO_STONE_GIMBAL_LIFTER: - - step: "gimbal lifter zero stone pos" - gimbal_lifter: - target: *BUTTON_POS - ONE_STONE_GIMBAL_LIFTER: - - step: "gimbal lifter one stone pos" - gimbal_lifter: - target: *MID_POS - TWO_STONE_GIMBAL_LIFTER: - - step: "gimbal lifter zero stone pos" - gimbal_lifter: - target: *TWO_STONE_POS - TALLEST_GIMBAL_LIFTER: - - step: "gimbal lifter tallest pos" - gimbal_lifter: - target: *TALLEST_POS - - ##########ORE_LIFTER############### - ORE_LIFTER_DOWN: - - step: "ore lifter down" - ore_lifter: - target: *BIN_DOWN_POS - ORE_LIFTER_MID: - - step: "ore lifter mid" - ore_lifter: - target: *BIN_MID_POS - ORE_LIFTER_UP: - - step: "ore lifter up" - ore_lifter: - target: *BIN_UP_POS - - ################ GIMBAL LIFTER ############### - - GIMBAL_DOWN: - - step: "gimbal lowest" - gimbal_lifter: - target: *BUTTON_POS - - GIMBAL_MID: - - step: "gimbal mid" - gimbal_lifter: - target: *LIFTER_BIG_ISLAND - - GIMBAL_TALL: - - step: "gimbal tall" - gimbal_lifter: - target: *TALLEST_POS - - GIMBAL_TEST1: - - step: "gimbal 1" - gimbal_lifter: - target: *GL_TEST1 - GIMBAL_TEST2: - - step: "gimbal 2" - gimbal_lifter: - target: *GL_TEST2 - GIMBAL_TEST3: - - step: "gimbal 3" - gimbal_lifter: - target: *GL_TEST3 - - ############## ORE ROTATOR ############### - ORE_ROTATOR_INIT: - - step: "ore rotator init pos" - ore_rotator: - target: *READY_POS - - ORE_ROTATOR_EXCHANGE: - - step: "ore rotator exchange pos" - ore_rotator: - target: *EXCHANGE_POS - - ########### GIMBAL ########## - - GIMBAL_FRONT: - - step: "gimbal look front" - gimbal: - <<: *FRONT_POS - GIMBAL_LEFT: - - step: "gimbal look left" - gimbal: - <<: *LEFT_POS - GIMBAL_RIGHT: - - step: "gimbal look right" - gimbal: - <<: *RIGHT_POS - - - #################################################### TEST ########################################################## - ARM_BACK_TEST: - - step: "arm back test" - arm: - joints: [ "KEEP", *JOINT2_BACK_POSITION, *JOINT3_HOME_POS, "KEEP", "KEEP", "KEEP", "KEEP" ] - record_arm2base: true + <<: *BIGGER_TOLERANCE + - step: "arms setting down" + arms: + groups: + - gimbal_arm: [ -1.4, -0.048, 0.647, 1.184 ] common: - <<: *NORMALLY + <<: *SLOWLY tolerance: - <<: *NORMAL_TOLERANCE - ARM_BACK_TEST1: - - step: "test" - chassis_target: - frame: base_link - position: [ 0., 0. ] - yaw: 0. - offset: [ 0., 0. ] - yaw_scale: 0.9 - target_frame: arm - common: - timeout: 2. - - -# - step: "test2" -# chassis: -# frame: base_link -# position: [ 0., 0. ] -# yaw: -1.40 -# chassis_tolerance_position: 0.1 -# chassis_tolerance_angular: 0.2 -# common: -# timeout: 2. - - - CHASSIS_TARGET_TEST1: - - step: "test" - chassis_target: - frame: base_link - offset: [ 0., 0. ] - yaw_scale: 0.9 - target_frame: exchanger - common: - timeout: 2. - - ########## EXCHANGE ############ - EXCHANGE_DOWN: - - step: "test new exchange" - arm: - frame: exchanger - is_refer_planning_frame: true - xyz: [ -0.3, 0.2, -0.2 ] - rpy: [ 0., -1.5707963, 0. ] - drift_dimensions: [ true, true, true, true, true, true ] - tolerance_position: 0.01 - tolerance_orientation: 0.001 - spacial_shape: SPHERE - radius: 0.3 - point_resolution: 0.5 - max_planning_times: 1 - common: - <<: *NORMALLY - - EXCHANGE_STRAIGHT: - - step: "test new exchange" - arm: - frame: exchanger - is_refer_planning_frame: true - xyz: [ -0.3, 0., 0. ] - rpy: [ 0., 3.1415926, 0. ] - drift_dimensions: [ true, true, true, true, true, true ] - tolerance_position: 0.01 - tolerance_orientation: 0.001 - spacial_shape: SPHERE - radius: 0.5 - point_resolution: 0.5 - max_planning_times: 1 - common: - <<: *NORMALLY - - AUTO_TEST: - - step: "chassis approach" - chassis_target: - frame: base_link - offset: [ 0., 0. ] - yaw_scale: 0.9 - target_frame: exchanger - common: - timeout: 2. - - step: "auto sphere exchange" - arm: - frame: exchanger - is_refer_planning_frame: true - xyz: [ -0.3, 0., 0. ] - rpy: [ 0., 3.1415926, 0. ] - drift_dimensions: [ true, true, true, true, true, true ] - tolerance_position: 0.01 - tolerance_orientation: 0.001 - spacial_shape: SPHERE - radius: 0.5 - point_resolution: 0.5 - max_planning_times: 1 - common: - <<: *NORMALLY - - step: "arm back" - arm: - joints: [ "KEEP", *JOINT2_BACK_POSITION, *JOINT3_HOME_POS, "KEEP", "KEEP", "KEEP", "KEEP" ] - record_arm2base: true - common: - <<: *NORMALLY + <<: *BIGGER_TOLERANCE + - step: "arms put down" + arms: + groups: + # - left_arm: [ 1.4, 1.46, -1.587, 1.725, 1.411, 0.007, -1.4 ] + - right_arm: [ 1.4, 1.46, -1.587, 1.725, 1.411, 0.007, -1.4 ] + - gimbal_arm: [ "KEEP", 1.484, -0.478, 0.786 ] + common: + <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE - AUTO_TEST0: - - step: "chassis compensation" - chassis_target: - frame: base_link - position: [ 0., 0. ] - yaw: 0. - offset: [ 0., 0. ] - yaw_scale: 0.9 - target_frame: arm - common: - timeout: 2. - - JOINT2_TEST_F: - - step: "joint2 front" - arm: - joints: ["KEEP",*JOINT2_FURTHEST_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + <<: *BIGGER_TOLERANCE + - step: "left arm ready" + arms: + groups: + - left_arm: [ 1.2, 1.4, -1.564, 1.0, 1.567, 0.0, 0.0 ] + # - right_arm: [ 1.2, 1.4, -1.564, 1.0, 1.567, 0.0, 0.0 ] common: <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE + <<: *BIGGER_TOLERANCE + - step: "left arm first" + arms: + groups: + - left_arm: [ 1.47, 1.533, 0.161, 0.202, 2.13, -0.489, -1.208 ] + # - right_arm: [ 1.467, 1.495, -0.044, 0.077, -1.414, 1.009, 1.071 ] + common: + <<: *QUICKLY + tolerance: + <<: *BIGGER_TOLERANCE - JOINT2_TEST_B: - - step: "joint2 front" - arm: - joints: [ "KEEP",*JOINT2_BACK_POSITION, "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + CHASSIS_DEBUG: + - step: "run empty motion to debug" + arms: + groups: + - left_arm: [ "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + - right_arm: [ "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP", "KEEP" ] + - gimbal_arm: [ "KEEP", "KEEP", "KEEP", "KEEP" ] common: <<: *QUICKLY tolerance: - <<: *NORMAL_TOLERANCE + <<: *BIGGER_TOLERANCE diff --git a/engineer_middleware/include/engineer_middleware/chassis_interface.h b/engineer_middleware/include/engineer_middleware/chassis_interface.h index 95e437f..610f09f 100644 --- a/engineer_middleware/include/engineer_middleware/chassis_interface.h +++ b/engineer_middleware/include/engineer_middleware/chassis_interface.h @@ -62,6 +62,7 @@ class ChassisInterface pid_yaw_.init(ros::NodeHandle(nh_pid_w, "pid")); vel_pub_ = nh.advertise("/cmd_vel", 1); nh_base_motion.getParam("yaw_start_threshold", yaw_start_threshold_); + nh_base_motion.getParam("linear_start_threshold", linear_start_threshold_); nh_base_motion.getParam("max_vel", max_vel_); } @@ -136,10 +137,11 @@ class ChassisInterface quatToRPY(goal_.pose.orientation, roll, pitch, yaw_goal); error_yaw_ = angles::shortest_angular_distance(yaw_current, yaw_goal); geometry_msgs::Twist cmd_vel{}; - cmd_vel.linear.x = - abs(pid_x_.computeCommand(error.x, period)) <= max_vel_ ? pid_x_.computeCommand(error.x, period) : 0; - cmd_vel.linear.y = - abs(pid_y_.computeCommand(error.y, period)) <= max_vel_ ? pid_y_.computeCommand(error.y, period) : 0; + const double cmd_x = pid_x_.computeCommand(error.x, period), cmd_y = pid_y_.computeCommand(error.y, period); + cmd_vel.linear.x = abs(cmd_x) <= max_vel_ ? cmd_x : max_vel_ * (cmd_x > 0 ? 1 : -1); + cmd_vel.linear.y = abs(cmd_y) <= max_vel_ ? cmd_y : max_vel_ * (cmd_y > 0 ? 1 : -1); + cmd_vel.linear.x = abs(cmd_x) >= linear_start_threshold_ ? cmd_vel.linear.x : 0; + cmd_vel.linear.y = abs(cmd_y) >= linear_start_threshold_ ? cmd_vel.linear.y : 0; cmd_vel.angular.z = abs(pid_yaw_.computeCommand(error_yaw_, period)) >= yaw_start_threshold_ ? pid_yaw_.computeCommand(error_yaw_, period) : 0; @@ -154,6 +156,6 @@ class ChassisInterface geometry_msgs::PoseStamped goal_{}; ros::Publisher vel_pub_; double error_pos_{}, error_yaw_{}; - double yaw_start_threshold_{}, max_vel_{}; + double yaw_start_threshold_{}, linear_start_threshold_{}, max_vel_{}; }; } // namespace engineer_middleware diff --git a/engineer_middleware/include/engineer_middleware/middleware.h b/engineer_middleware/include/engineer_middleware/middleware.h index 199935f..5a12448 100644 --- a/engineer_middleware/include/engineer_middleware/middleware.h +++ b/engineer_middleware/include/engineer_middleware/middleware.h @@ -51,6 +51,8 @@ #include #include #include +#include +#include namespace engineer_middleware { @@ -61,20 +63,22 @@ class Middleware void executeCB(const actionlib::SimpleActionServer::GoalConstPtr& goal) { std::string name; + bool success = false; name = goal->step_queue_name; is_middleware_control_ = true; ROS_INFO("Start step queue id %s", name.c_str()); auto step_queue = step_queues_.find(name); if (step_queue != step_queues_.end()) - step_queue->second.run(as_); + success = step_queue->second.run(as_); ROS_INFO("Finish step queue id %s", name.c_str()); + if (!success) + as_.setAborted(); is_middleware_control_ = false; } void run(ros::Duration period) { - // TODO chassis run crazily in motion - // if (is_middleware_control_) - // chassis_interface_.run(period); + if (is_middleware_control_) + chassis_interface_.run(period); } private: @@ -82,14 +86,15 @@ class Middleware actionlib::SimpleActionServer as_; moveit::planning_interface::MoveGroupInterface arm_group_; ChassisInterface chassis_interface_; - ros::Publisher hand_pub_, end_effector_pub_, gimbal_pub_, gpio_pub_, reversal_pub_, planning_result_pub_, - stone_num_pub_, point_cloud_pub_, ore_rotate_pub_, ore_lift_pub_, gimbal_lift_pub_, extend_arm_f_pub_, - extend_arm_b_pub_, silver_lifter_pub_, silver_pusher_pub_, silver_rotator_pub_, gold_pusher_pub_, - gold_lifter_pub_, middle_pitch_pub_; + actionlib::SimpleActionClient teaching_client_; + ros::Publisher hand_pub_, end_effector_pub_, gimbal_pub_, gpio_pub_, reversal_pub_, + planning_result_pub_, point_cloud_pub_, left_gripper_pub_, right_gripper_pub_, lifter_pub_; std::unordered_map step_queues_; tf2_ros::Buffer tf_; tf2_ros::TransformListener tf_listener_; bool is_middleware_control_; + robot_model_loader::RobotModelLoader rml_; + robot_model::RobotModelConstPtr robot_model_; }; } // namespace engineer_middleware diff --git a/engineer_middleware/include/engineer_middleware/motion.h b/engineer_middleware/include/engineer_middleware/motion.h index eec089f..b0611e8 100644 --- a/engineer_middleware/include/engineer_middleware/motion.h +++ b/engineer_middleware/include/engineer_middleware/motion.h @@ -49,6 +49,11 @@ #include #include #include +#include +#include +#include +#include +#include namespace engineer_middleware { @@ -85,8 +90,8 @@ class MoveitMotionBase : public MotionBase(motion, interface) { - speed_ = xmlRpcGetDouble(motion["common"], "speed", 0.1); - accel_ = xmlRpcGetDouble(motion["common"], "accel", 0.1); + speed_ = xmlRpcGetDouble(motion["common"], "speed", 0.3); + accel_ = xmlRpcGetDouble(motion["common"], "accel", 0.3); } bool move() override { @@ -134,98 +139,111 @@ class EndEffectorMotion : public MoveitMotionBase : MoveitMotionBase(motion, interface), tf_(tf), has_pos_(false), has_ori_(false), is_cartesian_(false) { target_.pose.orientation.w = 1.; - tolerance_position_ = xmlRpcGetDouble(motion, "tolerance_position", 0.01); - tolerance_orientation_ = xmlRpcGetDouble(motion, "tolerance_orientation", 0.1); - ROS_ASSERT(motion.hasMember("frame")); - target_.header.frame_id = std::string(motion["frame"]); - if (motion.hasMember("xyz")) - { - ROS_ASSERT(motion["xyz"].getType() == XmlRpc::XmlRpcValue::TypeArray); - target_.pose.position.x = xmlRpcGetDouble(motion["xyz"], 0); - target_.pose.position.y = xmlRpcGetDouble(motion["xyz"], 1); - target_.pose.position.z = xmlRpcGetDouble(motion["xyz"], 2); - has_pos_ = true; - } - if (motion.hasMember("rpy")) - { - ROS_ASSERT(motion["rpy"].getType() == XmlRpc::XmlRpcValue::TypeArray); - tf2::Quaternion quat_tf; - quat_tf.setRPY(motion["rpy"][0], motion["rpy"][1], motion["rpy"][2]); - geometry_msgs::Quaternion quat_msg = tf2::toMsg(quat_tf); - target_.pose.orientation = quat_msg; - has_ori_ = true; + if (motion.hasMember("end_effector")) { + tolerance_position_ = xmlRpcGetDouble(motion["end_effector"], "tolerance_position", 0.01); + tolerance_orientation_ = xmlRpcGetDouble(motion["end_effector"], "tolerance_orientation", 0.1); + ROS_ASSERT(motion["end_effector"].hasMember("frame")); + target_.header.frame_id = std::string(motion["end_effector"]["frame"]); + if (motion["end_effector"].hasMember("xyz")) + { + ROS_ASSERT(motion["end_effector"]["xyz"].getType() == XmlRpc::XmlRpcValue::TypeArray); + target_.pose.position.x = xmlRpcGetDouble(motion["end_effector"]["xyz"], 0); + target_.pose.position.y = xmlRpcGetDouble(motion["end_effector"]["xyz"], 1); + target_.pose.position.z = xmlRpcGetDouble(motion["end_effector"]["xyz"], 2); + has_pos_ = true; + } + if (motion["end_effector"].hasMember("rpy")) + { + ROS_ASSERT(motion["end_effector"]["rpy"].getType() == XmlRpc::XmlRpcValue::TypeArray); + tf2::Quaternion quat_tf; + quat_tf.setRPY(motion["end_effector"]["rpy"][0], motion["end_effector"]["rpy"][1], motion["end_effector"]["rpy"][2]); + geometry_msgs::Quaternion quat_msg = tf2::toMsg(quat_tf); + target_.pose.orientation = quat_msg; + has_ori_ = true; + } + ROS_ASSERT(has_pos_ || has_ori_); + if (motion["end_effector"].hasMember("cartesian")) + is_cartesian_ = motion["end_effector"]["cartesian"]; } - ROS_ASSERT(has_pos_ || has_ori_); - if (motion.hasMember("cartesian")) - is_cartesian_ = motion["cartesian"]; + } bool move() override { - MoveitMotionBase::move(); - geometry_msgs::PoseStamped final_target; - if (!target_.header.frame_id.empty()) + if (!MoveitMotionBase::move()) + return false; + has_final_target_ = false; + interface_.setStartStateToCurrentState(); + try { - try - { - tf2::doTransform(target_.pose, final_target.pose, - tf_.lookupTransform(interface_.getPlanningFrame(), target_.header.frame_id, ros::Time(0))); - final_target.header.frame_id = interface_.getPlanningFrame(); - } - catch (tf2::TransformException& ex) - { - ROS_WARN("%s", ex.what()); - return false; - } + tf2::doTransform(target_.pose, final_target_.pose, + tf_.lookupTransform(interface_.getPlanningFrame(), target_.header.frame_id, ros::Time(0))); + final_target_.header.frame_id = interface_.getPlanningFrame(); } + catch (tf2::TransformException& ex) + { + ROS_WARN("%s", ex.what()); + return false; + } + has_final_target_ = true; if (is_cartesian_) { moveit_msgs::RobotTrajectory trajectory; std::vector waypoints; - waypoints.push_back(target_.pose); - if (interface_.computeCartesianPath(waypoints, 0.01, 0.0, trajectory) != 1) + waypoints.push_back(final_target_.pose); + double fraction = interface_.computeCartesianPath(waypoints, 0.01, trajectory); + if (fraction < 0.9999) { ROS_INFO_STREAM("Collisions will occur in the" - << interface_.computeCartesianPath(waypoints, 0.01, 0.0, trajectory) << "of the trajectory"); + << fraction << "of the trajectory"); return false; } - return interface_.asyncExecute(trajectory) == moveit::planning_interface::MoveItErrorCode::SUCCESS; - } - else - { - if (has_pos_ && has_ori_) - interface_.setPoseTarget(final_target); - else if (has_pos_ && !has_ori_) - interface_.setPositionTarget(final_target.pose.position.x, final_target.pose.position.y, - final_target.pose.position.z); - else if (!has_pos_ && has_ori_) - interface_.setOrientationTarget(final_target.pose.orientation.x, final_target.pose.orientation.y, - final_target.pose.orientation.z, final_target.pose.orientation.w); - moveit::planning_interface::MoveGroupInterface::Plan plan; - msg_.data = interface_.plan(plan).val; - return interface_.asyncExecute(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS; + return interface_.asyncExecute(trajectory) == moveit::core::MoveItErrorCode::SUCCESS; } + + if (has_pos_ && has_ori_) + interface_.setPoseTarget(final_target_); + else if (has_pos_ && !has_ori_) + interface_.setPositionTarget(final_target_.pose.position.x, final_target_.pose.position.y, + final_target_.pose.position.z); + else if (!has_pos_ && has_ori_) + interface_.setOrientationTarget(final_target_.pose.orientation.x, final_target_.pose.orientation.y, + final_target_.pose.orientation.z, final_target_.pose.orientation.w); + moveit::planning_interface::MoveGroupInterface::Plan plan; + msg_.data = interface_.plan(plan).val; + return interface_.asyncExecute(plan) == moveit::core::MoveItErrorCode::SUCCESS; } protected: bool isReachGoal() override { - geometry_msgs::Pose pose = interface_.getCurrentPose().pose; + if (!has_final_target_) + return false; + + geometry_msgs::Pose current_pose = interface_.getCurrentPose().pose; + double roll_current, pitch_current, yaw_current, roll_goal, pitch_goal, yaw_goal; - quatToRPY(pose.orientation, roll_current, pitch_current, yaw_current); - quatToRPY(target_.pose.orientation, roll_goal, pitch_goal, yaw_goal); - // TODO: Add orientation error check - return (std::pow(pose.position.x - target_.pose.position.x, 2) + - std::pow(pose.position.y - target_.pose.position.y, 2) + - std::pow(pose.position.z - target_.pose.position.z, 2) < - tolerance_position_ && + quatToRPY(current_pose.orientation, roll_current, pitch_current, yaw_current); + quatToRPY(final_target_.pose.orientation, roll_goal, pitch_goal, yaw_goal); + + double dx = current_pose.position.x - final_target_.pose.position.x; + double dy = current_pose.position.y - final_target_.pose.position.y; + double dz = current_pose.position.z - final_target_.pose.position.z; + double pos_err = dx * dx + dy * dy + dz * dz; + + // ROS_INFO("Position error: %f, Orientation error: %f", std::sqrt(pos_err), + // std::max({ std::abs(angles::shortest_angular_distance(yaw_current, yaw_goal)), + // std::abs(angles::shortest_angular_distance(pitch_current, pitch_goal)), + // std::abs(angles::shortest_angular_distance(roll_current, roll_goal)) })); + return (pos_err < (tolerance_position_ * tolerance_position_) && std::abs(angles::shortest_angular_distance(yaw_current, yaw_goal)) < tolerance_orientation_ && std::abs(angles::shortest_angular_distance(pitch_current, pitch_goal)) < tolerance_orientation_ && - std::abs(angles::shortest_angular_distance(yaw_current, yaw_goal)) < tolerance_orientation_); + std::abs(angles::shortest_angular_distance(roll_current, roll_goal)) < tolerance_orientation_); } tf2_ros::Buffer& tf_; bool has_pos_, has_ori_, is_cartesian_; - geometry_msgs::PoseStamped target_; + geometry_msgs::PoseStamped target_, final_target_; + bool has_final_target_{false}; double tolerance_position_, tolerance_orientation_; }; @@ -350,7 +368,7 @@ class SpaceEeMotion : public EndEffectorMotion moveit::planning_interface::MoveGroupInterface::Plan plan; msg_.data = interface_.plan(plan).val; if (msg_.data == 1) - return interface_.asyncExecute(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS; + return interface_.asyncExecute(plan) == moveit::core::MoveItErrorCode::SUCCESS; } return false; } @@ -447,7 +465,7 @@ class JointMotion : public MoveitMotionBase interface_.setJointValueTarget(final_target_); moveit::planning_interface::MoveGroupInterface::Plan plan; msg_.data = interface_.plan(plan).val; - return (interface_.asyncExecute(plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS); + return (interface_.asyncExecute(plan) == moveit::core::MoveItErrorCode::SUCCESS); } private: @@ -460,8 +478,122 @@ class JointMotion : public MoveitMotionBase { error = std::abs(final_target_[i] - current[i]); joint_reached = (error < tolerance_joints_[i]); - // if (!joint_reached) - // ROS_INFO_STREAM("Joint" << i + 1 << " didn't reach configured tolerance range,error: " << error); + // if (!joint_reached) + // ROS_INFO_STREAM("Joint" << i + 1 << " didn't reach configured tolerance range,error: " << error); + flag &= joint_reached; + } + return flag; + } + std::vector target_, final_target_, tolerance_joints_; + bool record_arm2base_{ false }; + tf2_ros::Buffer& tf_buffer_; +}; + +class ArmsMotion : public MoveitMotionBase +{ +public: + ArmsMotion(XmlRpc::XmlRpcValue& motion, moveit::planning_interface::MoveGroupInterface& interface, + tf2_ros::Buffer& tf_buffer, robot_model::RobotModelConstPtr robot_model) + : MoveitMotionBase(motion, interface), tf_buffer_(tf_buffer) + { + if (motion.hasMember("groups")) + { + ROS_ASSERT(motion["groups"].getType() == XmlRpc::XmlRpcValue::TypeArray); + // test + auto names = interface_.getVariableNames(); + target_.resize(names.size(), NAN); + std::vector sub_names; + for (int i = 0; i < motion["groups"].size(); ++i) + { + auto group = motion["groups"][i]; + auto subgroup_name = group.begin()->first; + const robot_model::JointModelGroup* jmg = robot_model->getJointModelGroup(subgroup_name); + if (jmg) + sub_names = jmg->getVariableNames(); + // ROS_INFO("group num sides: %lu, sub names size: %lu", static_cast(group[subgroup_name].size()), sub_names.size()); + ROS_ASSERT(static_cast(group[subgroup_name].size()) == sub_names.size()); + for (size_t j = 0; j < sub_names.size(); ++j) + { + double val = NAN; + if (group[subgroup_name][j].getType() == XmlRpc::XmlRpcValue::TypeDouble) + val = group[subgroup_name][j]; + else if (group[subgroup_name][j] == "KEEP") + val = NAN; + else + ROS_ERROR("ERROR TYPE OR STRING!!!"); + + auto it = std::find(names.begin(), names.end(), sub_names[j]); + if(it == names.end()) + { + ROS_ERROR("Joint name %s not found in %s", sub_names[j].c_str(), subgroup_name.c_str()); + } + size_t idx = std::distance(names.begin(), it); + target_[idx] = val; + } + } + } + if (motion.hasMember("tolerance")) + { + ROS_ASSERT(motion["tolerance"]["tolerance_joints"].getType() == XmlRpc::XmlRpcValue::TypeArray); + for (int i = 0; i < motion["tolerance"]["tolerance_joints"].size(); ++i) + tolerance_joints_.push_back(xmlRpcGetDouble(motion["tolerance"]["tolerance_joints"], i)); + } + if (motion.hasMember("record_arm2base")) + record_arm2base_ = bool(motion["record_arm2base"]); + } + static geometry_msgs::TransformStamped arm2base; + bool move() override + { + if (record_arm2base_) + { + try + { + arm2base.header.frame_id = "base_link"; + arm2base.header.stamp = ros::Time::now(); + arm2base.child_frame_id = "chassis_target"; + + arm2base = tf_buffer_.lookupTransform("base_link", "gimbal_link3", ros::Time(0)); + ROS_INFO_STREAM("X is: " << arm2base.transform.translation.x << "Y is: " << arm2base.transform.translation.y); + } + catch (tf2::TransformException& ex) + { + ROS_WARN("%s", ex.what()); + return false; + } + } + final_target_.clear(); + if (target_.empty()) + return false; + MoveitMotionBase::move(); + for (int i = 0; i < (int)target_.size(); i++) + { + if (!isfinite(target_[i])) + { + final_target_.push_back(interface_.getCurrentJointValues()[i]); + } + else + { + final_target_.push_back(target_[i]); + } + } + interface_.setJointValueTarget(final_target_); + moveit::planning_interface::MoveGroupInterface::Plan plan; + msg_.data = interface_.plan(plan).val; + return (interface_.asyncExecute(plan) == moveit::core::MoveItErrorCode::SUCCESS); + } + +private: + bool isReachGoal() override + { + std::vector current = interface_.getCurrentJointValues(); + double error = 0.; + bool flag = 1, joint_reached = 0; + for (int i = 0; i < (int)final_target_.size(); ++i) + { + error = std::abs(final_target_[i] - current[i]); + joint_reached = (error < tolerance_joints_[i]); + if (!joint_reached) + ROS_INFO_STREAM("Joint[" << i << "] didn't reach configured tolerance range,error: " << error); flag &= joint_reached; } return flag; @@ -529,8 +661,8 @@ class GpioMotion : public PublishMotion : PublishMotion(motion, interface) { delay_ = xmlRpcGetDouble(motion, "delay", 0.01); - msg_.gpio_state.assign(6, false); - msg_.gpio_name.assign(6, "no_registered"); + msg_.gpio_state.assign(3, false); + msg_.gpio_name.assign(3, "no_registered"); pin_ = motion["pin"]; state_ = motion["state"]; switch (pin_) @@ -539,27 +671,10 @@ class GpioMotion : public PublishMotion msg_.gpio_name[0] = "main_gripper"; break; case 1: - msg_.gpio_name[1] = "silver_gripper1"; + msg_.gpio_name[1] = "small_gripper"; break; case 2: - msg_.gpio_name[2] = "silver_gripper2"; - break; - case 3: - msg_.gpio_name[3] = "silver_gripper3"; - break; - case 4: - msg_.gpio_name[4] = "gold_gripper"; - break; - case 5: - msg_.gpio_name[5] = "silver_pump"; - break; - case 6: - msg_.gpio_name[6] = "unused"; - ROS_WARN_STREAM("GPIO port 7 is unused now!"); - break; - case 7: - msg_.gpio_name[7] = "unused"; - ROS_WARN_STREAM("GPIO port 7 is unused now!"); + msg_.gpio_name[2] = "transfer_gripper"; break; } } @@ -884,4 +999,193 @@ class ChassisTargetMotion : public ChassisMotion tf2_ros::Buffer& tf_buffer_; }; +class TargetPoseMotion : public MoveitMotionBase +{ +public: + TargetPoseMotion( XmlRpc::XmlRpcValue& motion, moveit::planning_interface::MoveGroupInterface& interface, tf2_ros::Buffer& tf ) + : MoveitMotionBase( motion, interface ), tf_buffer_( tf ), is_cartesian( false ) + { + tolerance_position_ = xmlRpcGetDouble( motion, "tolerance_position", 0.01 ); + tolerance_orientation_ = xmlRpcGetDouble( motion, "tolerance_orientation", 0.03 ); + is_cartesian = motion["is_cartesian"]; + if (motion.hasMember("target_pose")) + { + XmlRpc::XmlRpcValue& pose = motion["target_pose"]; + ROS_ASSERT(pose.getType() == XmlRpc::XmlRpcValue::TypeStruct); + ROS_ASSERT( pose.hasMember("frame") ); + if ( pose.hasMember("frame") ) + { + target_pose_.header.frame_id = std::string( pose["frame"] ); + if (pose.hasMember("xyz")) + { + target_pose_.pose.position.x = xmlRpcGetDouble( pose["xyz"], 0 ); + target_pose_.pose.position.y = xmlRpcGetDouble( pose["xyz"], 1 ); + target_pose_.pose.position.z = xmlRpcGetDouble( pose["xyz"], 2 ); + } + else + target_pose_.pose.position.x = target_pose_.pose.position.y = target_pose_.pose.position.z = 0.0; + tf2::Quaternion quat_target; + if (pose.hasMember("rpy")) + { + quat_target.setRPY( xmlRpcGetDouble( pose["rpy"], 0 ), + xmlRpcGetDouble( pose["rpy"], 1 ), + xmlRpcGetDouble( pose["rpy"], 2 )); + } + else + quat_target.setRPY( 0.0,0.0,0.0 ); + target_pose_.pose.orientation = toMsg( quat_target ); + } + } + } + + bool move() override + { + MoveitMotionBase::move(); + if ( !target_pose_.header.frame_id.empty() ) + { + try + { + tf2::doTransform( target_pose_.pose, plan_target_pose_.pose, + tf_buffer_.lookupTransform(interface_.getPlanningFrame(), + target_pose_.header.frame_id, ros::Time(0)) ); + plan_target_pose_.header.frame_id = interface_.getPlanningFrame(); + static tf2_ros::StaticTransformBroadcaster static_broadcaster; + geometry_msgs::TransformStamped target_frame_transformStamped; + target_frame_transformStamped.header = plan_target_pose_.header; + target_frame_transformStamped.child_frame_id = "plan_target_frame"; + target_frame_transformStamped.transform.translation.x = plan_target_pose_.pose.position.x; + target_frame_transformStamped.transform.translation.y = plan_target_pose_.pose.position.y; + target_frame_transformStamped.transform.translation.z = plan_target_pose_.pose.position.z; + target_frame_transformStamped.transform.rotation = plan_target_pose_.pose.orientation; + static_broadcaster.sendTransform(target_frame_transformStamped); + } + catch ( tf2::TransformException& ex ) + { + ROS_WARN( "%s", ex.what() ); + return false; + } + } + else + { + ROS_ERROR("No frame specified for target pose"); + return false; + } + + std::vector target; + target.push_back( plan_target_pose_ ); + interface_.setPoseTargets( target ); + if ( is_cartesian ) + interface_.setPlanningPipelineId("cartesian_interpolation"); + else + interface_.setPlanningPipelineId("linear_interpolation"); + moveit::planning_interface::MoveGroupInterface::Plan plan; + msg_.data = interface_.plan( plan ).val; + return interface_.asyncExecute( plan ) == moveit::core::MoveItErrorCode::SUCCESS; + } + +protected: + bool isReachGoal() override + { + geometry_msgs::Pose pose = interface_.getCurrentPose().pose; + double roll_current, pitch_current, yaw_current, roll_goal, pitch_goal, yaw_goal; + quatToRPY(pose.orientation, roll_current, pitch_current, yaw_current); + quatToRPY(plan_target_pose_.pose.orientation, roll_goal, pitch_goal, yaw_goal); + + return std::pow( pose.position.x - plan_target_pose_.pose.position.x, 2 ) + + std::pow( pose.position.y - plan_target_pose_.pose.position.y, 2 ) + + std::pow( pose.position.z - plan_target_pose_.pose.position.z, 2 ) < std::pow( tolerance_position_, 2 ) && + std::abs(angles::shortest_angular_distance(yaw_current, yaw_goal)) < tolerance_orientation_ && + std::abs(angles::shortest_angular_distance(pitch_current, pitch_goal)) < tolerance_orientation_ && + std::abs(angles::shortest_angular_distance(roll_current, roll_goal)) < tolerance_orientation_; + } + tf2_ros::Buffer& tf_buffer_; + geometry_msgs::PoseStamped target_pose_, plan_target_pose_; + double tolerance_position_, tolerance_orientation_; + bool is_cartesian; +}; + +class TrajectoryPlaybackMotion : public MotionBase> +{ +public: + TrajectoryPlaybackMotion( XmlRpc::XmlRpcValue& motion, actionlib::SimpleActionClient& teaching_client ) + : MotionBase>( motion, teaching_client ) + { + ROS_ASSERT( motion.hasMember("filename") ); + filename_ = static_cast(motion["filename"]); + speed_ = xmlRpcGetDouble( motion, "speed", 1.0 ); + sync_time_ = xmlRpcGetDouble( motion, "sync_time", 0.1 ); + interface_.waitForServer(); + } + bool move() override + { + if (interface_.isServerConnected()) + { + is_finish_ = false; + rm_msgs::EngineerTrajectoryTeachingGoal goal; + goal.command = goal.PLAYBACK; + goal.filename = filename_; + goal.speed = speed_; + goal.sync_time = sync_time_; + interface_.sendGoal(goal, boost::bind(&TrajectoryPlaybackMotion::doneCb, this, _1, _2)); + return true; + } + return false; + } + void doneCb(const actionlib::SimpleClientGoalState& state, const rm_msgs::EngineerTrajectoryTeachingResultConstPtr& result) + { + is_finish_ = true; + } + bool isFinish() override + { + return is_finish_; + } + void stop() override + { + interface_.cancelAllGoals(); + } + +protected: + bool is_finish_{ false }; + std::string filename_; + double speed_, sync_time_; +}; + +class GripperMotion : public PublishMotion +{ +public: + GripperMotion(XmlRpc::XmlRpcValue& motion, ros::Publisher& interface) + : PublishMotion(motion, interface) + { + ROS_ASSERT(motion.hasMember("command")); + ROS_ASSERT(motion["command"].getType() == XmlRpc::XmlRpcValue::TypeBoolean); + command_ = motion["command"]; + } + bool move() override + { + msg_.data = command_; + return PublishMotion::move(); + } + +private: + bool command_; +}; + +class LifterMotion : public PublishMotion +{ +public: + LifterMotion(XmlRpc::XmlRpcValue& motion, ros::Publisher& interface) + : PublishMotion(motion, interface) + { + ROS_ASSERT(motion.hasMember("state")); + ROS_ASSERT(motion["state"].getType() == XmlRpc::XmlRpcValue::TypeInt); + command_ = motion["state"]; + } + bool move() override + { + msg_.data = command_; + return PublishMotion::move(); + } +private: + int command_; +}; }; // namespace engineer_middleware diff --git a/engineer_middleware/include/engineer_middleware/step.h b/engineer_middleware/include/engineer_middleware/step.h index 563c4e4..c8de702 100644 --- a/engineer_middleware/include/engineer_middleware/step.h +++ b/engineer_middleware/include/engineer_middleware/step.h @@ -47,13 +47,12 @@ class Step { public: Step(const XmlRpc::XmlRpcValue& step, const XmlRpc::XmlRpcValue& scenes, tf2_ros::Buffer& tf, - moveit::planning_interface::MoveGroupInterface& arm_group, ChassisInterface& chassis_interface, - ros::Publisher& hand_pub, ros::Publisher& end_effector_pub, ros::Publisher& gimbal_pub, ros::Publisher& gpio_pub, - ros::Publisher& reversal_pub, ros::Publisher& stone_num_pub, ros::Publisher& planning_result_pub, - ros::Publisher& point_cloud_pub, ros::Publisher& ore_rotate_pub, ros::Publisher& ore_lift_pub, - ros::Publisher& gimbal_lift_pub, ros::Publisher& extend_arm_f_pub, ros::Publisher& extend_arm_b_pub, - ros::Publisher& silver_lifter_pub, ros::Publisher& silver_pusher_pub, ros::Publisher& silver_rotator_pub, - ros::Publisher& gold_pusher_pub, ros::Publisher& gold_lifter_pub, ros::Publisher& middle_pitch_pub) + moveit::planning_interface::MoveGroupInterface& arm_group, + ChassisInterface& chassis_interface, actionlib::SimpleActionClient& teaching_client, + ros::Publisher& hand_pub,ros::Publisher& end_effector_pub,ros::Publisher& gimbal_pub, ros::Publisher& gpio_pub, + ros::Publisher& reversal_pub,ros::Publisher& planning_result_pub, ros::Publisher& point_cloud_pub, ros::Publisher& left_gripper_pub, ros::Publisher& right_gripper_pub, + ros::Publisher& lifter_pub_, + robot_model::RobotModelConstPtr robot_model) : planning_result_pub_(planning_result_pub), point_cloud_pub_(point_cloud_pub), arm_group_(arm_group) { ROS_ASSERT(step.hasMember("step")); @@ -62,58 +61,51 @@ class Step { if (step["arm"].hasMember("joints")) arm_motion_ = new JointMotion(step["arm"], arm_group, tf); - else if (step["arm"].hasMember("spacial_shape")) - arm_motion_ = new SpaceEeMotion(step["arm"], arm_group, tf); - else + else if (step["arm"].hasMember("target_pose")) + arm_motion_ = new TargetPoseMotion(step["arm"], arm_group, tf); + // else if (step["arm"].hasMember("spacial_shape")) + // arm_motion_ = new SpaceEeMotion(step["arm"], arm_group, tf); + else if (step["arm"].hasMember("end_effector")) arm_motion_ = new EndEffectorMotion(step["arm"], arm_group, tf); } + if (step.hasMember("arms")) + { + arms_motion_ = new ArmsMotion(step["arms"], arm_group, tf, robot_model); + // if (step["arms"].hasMember("joints")) + // arm_motion_ = new JointMotion(step["arms"], arm_group, tf); + // else if (step["arms"].hasMember("target_poses")) + // arm_motion_ = new TargetPoseMotion(step["arms"], arm_group, tf); + // else if (step["arm"].hasMember("spacial_shape")) + // arm_motion_ = new SpaceEeMotion(step["arm"], arm_group, tf); + // else if (step["arms"].hasMember("end_effectors")) + // arm_motion_ = new EndEffectorMotion(step["arms"], arm_group, tf); + } if (step.hasMember("chassis")) chassis_motion_ = new ChassisMotion(step["chassis"], chassis_interface); if (step.hasMember("hand")) hand_motion_ = new HandMotion(step["hand"], hand_pub); if (step.hasMember("end_effector")) end_effector_motion_ = new JointPositionMotion(step["end_effector"], end_effector_pub, tf); - if (step.hasMember("stone_num")) - stone_num_motion_ = new StoneNumMotion(step["stone_num"], stone_num_pub); if (step.hasMember("gimbal")) gimbal_motion_ = new GimbalMotion(step["gimbal"], gimbal_pub); - if (step.hasMember("gripper")) - gpio_motion_ = new GpioMotion(step["gripper"], gpio_pub); if (step.hasMember("reversal")) reversal_motion_ = new ReversalMotion(step["reversal"], reversal_pub); if (step.hasMember("scene_name")) { - for (XmlRpc::XmlRpcValue::ValueStruct::const_iterator it = scenes.begin(); it != scenes.end(); ++it) - if (step["scene_name"] == it->first) - planning_scene_ = new PlanningScene(it->second, arm_group); - } - if (step.hasMember("ore_rotator")) - ore_rotate_motion_ = new JointPointMotion(step["ore_rotator"], ore_rotate_pub); - if (step.hasMember("ore_lifter")) - ore_lift_motion_ = new JointPointMotion(step["ore_lifter"], ore_lift_pub); - if (step.hasMember("gimbal_lifter")) - gimbal_lift_motion_ = new JointPointMotion(step["gimbal_lifter"], gimbal_lift_pub); - if (step.hasMember("extend_arm")) - { - if (step["extend_arm"].hasMember("front")) - extend_arm_front_motion_ = new ExtendMotion(step["extend_arm"], extend_arm_f_pub, true); - if (step["extend_arm"].hasMember("back")) - extend_arm_back_motion_ = new ExtendMotion(step["extend_arm"], extend_arm_b_pub, false); + for (const auto & scene : scenes) + if (step["scene_name"] == scene.first) + planning_scene_ = new PlanningScene(scene.second, arm_group); } if (step.hasMember("chassis_target")) chassis_target_motion_ = new ChassisTargetMotion(step["chassis_target"], chassis_interface, tf); - if (step.hasMember("silver_lifter")) - silver_lifter_motion_ = new JointPointMotion(step["silver_lifter"], silver_lifter_pub); - if (step.hasMember("silver_pusher")) - silver_pusher_motion_ = new JointPointMotion(step["silver_pusher"], silver_pusher_pub); - if (step.hasMember("silver_rotator")) - silver_rotator_motion_ = new JointPointMotion(step["silver_rotator"], silver_rotator_pub); - if (step.hasMember("gold_pusher")) - gold_pusher_motion_ = new JointPointMotion(step["gold_pusher"], gold_pusher_pub); - if (step.hasMember("gold_lifter")) - gold_lifter_motion_ = new JointPointMotion(step["gold_lifter"], gold_lifter_pub); - if (step.hasMember("middle_pitch")) - middle_pitch_motion_ = new JointPointMotion(step["middle_pitch"], middle_pitch_pub); + if (step.hasMember("trajectory_playback")) + trajectory_playback_motion_ = new TrajectoryPlaybackMotion(step["trajectory_playback"], teaching_client); + if (step.hasMember("left_gripper")) + left_gripper_motion_ = new GripperMotion(step["left_gripper"], left_gripper_pub); + if (step.hasMember("right_gripper")) + right_gripper_motion_ = new GripperMotion(step["right_gripper"], right_gripper_pub); + if (step.hasMember("lifter")) + lifter_motion_ = new LifterMotion(step["lifter"], lifter_pub_); } bool move() { @@ -129,12 +121,12 @@ class Step std_msgs::Int32 msg = arm_motion_->getPlanningResult(); planning_result_pub_.publish(msg); } + if (arms_motion_) + success &= arms_motion_->move(); if (hand_motion_) success &= hand_motion_->move(); if (end_effector_motion_) success &= end_effector_motion_->move(); - if (stone_num_motion_) - success &= stone_num_motion_->move(); if (chassis_motion_) success &= chassis_motion_->move(); if (gimbal_motion_) @@ -145,42 +137,30 @@ class Step success &= reversal_motion_->move(); if (planning_scene_) planning_scene_->add(); - if (ore_lift_motion_) - success &= ore_lift_motion_->move(); - if (ore_rotate_motion_) - success &= ore_rotate_motion_->move(); - if (gimbal_lift_motion_) - success &= gimbal_lift_motion_->move(); - if (extend_arm_back_motion_) - success &= extend_arm_back_motion_->move(); - if (extend_arm_front_motion_) - success &= extend_arm_front_motion_->move(); - if (chassis_target_motion_) - success &= chassis_target_motion_->move(); - if (silver_lifter_motion_) - success &= silver_lifter_motion_->move(); - if (silver_pusher_motion_) - success &= silver_pusher_motion_->move(); - if (silver_rotator_motion_) - success &= silver_rotator_motion_->move(); - if (gold_pusher_motion_) - success &= gold_pusher_motion_->move(); - if (gold_lifter_motion_) - success &= gold_lifter_motion_->move(); - if (middle_pitch_motion_) - success &= middle_pitch_motion_->move(); + if (trajectory_playback_motion_) + success &= trajectory_playback_motion_->move(); + if (left_gripper_motion_) + success &= left_gripper_motion_->move(); + if (right_gripper_motion_) + success &= right_gripper_motion_->move(); + if (lifter_motion_) + success &= lifter_motion_->move(); return success; } void stop() { if (arm_motion_) arm_motion_->stop(); + if (arms_motion_) + arms_motion_->stop(); if (hand_motion_) hand_motion_->stop(); if (chassis_motion_) chassis_motion_->stop(); if (chassis_target_motion_) chassis_target_motion_->stop(); + if (trajectory_playback_motion_) + trajectory_playback_motion_->stop(); } void deleteScene() @@ -198,6 +178,8 @@ class Step bool success = true; if (arm_motion_) success &= arm_motion_->isFinish(); + if (arms_motion_) + success &= arms_motion_->isFinish(); if (hand_motion_) success &= hand_motion_->isFinish(); if (end_effector_motion_) @@ -210,20 +192,16 @@ class Step success &= reversal_motion_->isFinish(); if (chassis_target_motion_) success &= chassis_target_motion_->isFinish(); - if (silver_lifter_motion_) - success &= silver_lifter_motion_->isFinish(); - if (silver_pusher_motion_) - success &= silver_pusher_motion_->isFinish(); - if (silver_rotator_motion_) - success &= silver_rotator_motion_->isFinish(); if (gpio_motion_) success &= gpio_motion_->isFinish(); - if (gold_pusher_motion_) - success &= gold_pusher_motion_->isFinish(); - if (gold_lifter_motion_) - success &= gold_lifter_motion_->isFinish(); - if (middle_pitch_motion_) - success &= middle_pitch_motion_->isFinish(); + if (trajectory_playback_motion_) + success &= trajectory_playback_motion_->isFinish(); + if (left_gripper_motion_) + success &= left_gripper_motion_->isFinish(); + if (right_gripper_motion_) + success &= right_gripper_motion_->isFinish(); + if (lifter_motion_) + success &= lifter_motion_->isFinish(); return success; } bool checkTimeout(ros::Duration period) @@ -231,6 +209,8 @@ class Step bool success = true; if (arm_motion_) success &= arm_motion_->checkTimeout(period); + if (arms_motion_) + success &= arms_motion_->checkTimeout(period); if (hand_motion_) success &= hand_motion_->checkTimeout(period); if (end_effector_motion_) @@ -254,13 +234,9 @@ class Step ros::Publisher planning_result_pub_; ros::Publisher point_cloud_pub_; MoveitMotionBase* arm_motion_{}; + MoveitMotionBase* arms_motion_{}; HandMotion* hand_motion_{}; JointPositionMotion* end_effector_motion_{}; - JointPointMotion *ore_rotate_motion_{}, *ore_lift_motion_{}, *gimbal_lift_motion_{}; - JointPointMotion *silver_lifter_motion_{}, *silver_pusher_motion_{}, *silver_rotator_motion_{}, - *gold_pusher_motion_{}, *gold_lifter_motion_{}, *middle_pitch_motion_{}; - ExtendMotion *extend_arm_front_motion_{}, *extend_arm_back_motion_{}; - StoneNumMotion* stone_num_motion_{}; ChassisMotion* chassis_motion_{}; GimbalMotion* gimbal_motion_{}; GpioMotion* gpio_motion_{}; @@ -269,6 +245,10 @@ class Step moveit::planning_interface::PlanningSceneInterface planning_scene_interface_; moveit::planning_interface::MoveGroupInterface& arm_group_; ChassisTargetMotion* chassis_target_motion_{}; + TrajectoryPlaybackMotion* trajectory_playback_motion_{}; + GripperMotion* left_gripper_motion_{}; + GripperMotion* right_gripper_motion_{}; + LifterMotion* lifter_motion_{}; }; } // namespace engineer_middleware diff --git a/engineer_middleware/include/engineer_middleware/step_queue.h b/engineer_middleware/include/engineer_middleware/step_queue.h index 32424d7..d6fbb55 100644 --- a/engineer_middleware/include/engineer_middleware/step_queue.h +++ b/engineer_middleware/include/engineer_middleware/step_queue.h @@ -52,22 +52,20 @@ class StepQueue { public: StepQueue(const XmlRpc::XmlRpcValue& steps, const XmlRpc::XmlRpcValue& scenes, tf2_ros::Buffer& tf, - moveit::planning_interface::MoveGroupInterface& arm_group, ChassisInterface& chassis_interface, - ros::Publisher& hand_pub, ros::Publisher& end_effector_pub, ros::Publisher& stone_num_pub, + moveit::planning_interface::MoveGroupInterface& arm_group, + ChassisInterface& chassis_interface, actionlib::SimpleActionClient& teaching_client, + ros::Publisher& hand_pub, ros::Publisher& end_effector_pub, ros::Publisher& gimbal_pub, ros::Publisher& gpio_pub, ros::Publisher& reversal_pub, - ros::Publisher& planning_result_pub, ros::Publisher& point_cloud_pub, ros::Publisher& ore_rotate_pub, - ros::Publisher& ore_lift_pub, ros::Publisher& gimbal_lift_pub, ros::Publisher& extend_arm_f_pub, - ros::Publisher& extend_arm_b_pub, ros::Publisher& silver_lifter_pub, ros::Publisher& silver_pusher_pub, - ros::Publisher& silver_rotator_pub, ros::Publisher& gold_pusher_pub, ros::Publisher& gold_lifter_pub, - ros::Publisher& middle_pitch_pub) + ros::Publisher& planning_result_pub, ros::Publisher& point_cloud_pub, ros::Publisher& left_gripper_pub, ros::Publisher& right_gripper_pub, + ros::Publisher& lifter_pub, + robot_model::RobotModelConstPtr robot_model) : chassis_interface_(chassis_interface) { ROS_ASSERT(steps.getType() == XmlRpc::XmlRpcValue::TypeArray); for (int i = 0; i < steps.size(); ++i) - queue_.emplace_back(steps[i], scenes, tf, arm_group, chassis_interface, hand_pub, end_effector_pub, stone_num_pub, - gimbal_pub, gpio_pub, reversal_pub, planning_result_pub, point_cloud_pub, ore_rotate_pub, - ore_lift_pub, gimbal_lift_pub, extend_arm_f_pub, extend_arm_b_pub, silver_lifter_pub, - silver_pusher_pub, silver_rotator_pub, gold_pusher_pub, gold_lifter_pub, middle_pitch_pub); + queue_.emplace_back(steps[i], scenes, tf, arm_group, chassis_interface, teaching_client, hand_pub, end_effector_pub, + gimbal_pub, gpio_pub, reversal_pub, planning_result_pub, point_cloud_pub, left_gripper_pub, + right_gripper_pub, lifter_pub, robot_model); } bool run(actionlib::SimpleActionServer& as) { @@ -91,6 +89,7 @@ class StepQueue if (!queue_[i].checkTimeout(ros::Time::now() - start)) { queue_[i].stop(); + as.setAborted(); return false; } if (as.isPreemptRequested() || !ros::ok()) diff --git a/engineer_middleware/launch/load.launch b/engineer_middleware/launch/load.launch index 816a8c9..9ece14f 100644 --- a/engineer_middleware/launch/load.launch +++ b/engineer_middleware/launch/load.launch @@ -7,5 +7,5 @@ + clear_params="true" output="screen"/> diff --git a/engineer_middleware/launch/sim.launch b/engineer_middleware/launch/sim.launch index c8057e2..7017b03 100644 --- a/engineer_middleware/launch/sim.launch +++ b/engineer_middleware/launch/sim.launch @@ -1,11 +1,13 @@ - - - - - + + + + + + + diff --git a/engineer_middleware/mesh/units_studio.stl b/engineer_middleware/mesh/units_studio.stl new file mode 100644 index 0000000..ddcff9f Binary files /dev/null and b/engineer_middleware/mesh/units_studio.stl differ diff --git a/engineer_middleware/package.xml b/engineer_middleware/package.xml index 8fa2577..0d5ac75 100644 --- a/engineer_middleware/package.xml +++ b/engineer_middleware/package.xml @@ -20,5 +20,6 @@ control_toolbox moveit_core angles + yaml-cpp moveit_ros_planning_interface diff --git a/engineer_middleware/src/middleware.cpp b/engineer_middleware/src/middleware.cpp index 34b0105..acd2907 100644 --- a/engineer_middleware/src/middleware.cpp +++ b/engineer_middleware/src/middleware.cpp @@ -37,33 +37,26 @@ #include "engineer_middleware/middleware.h" #include +#include "geometry_msgs/TransformStamped.h" namespace engineer_middleware { Middleware::Middleware(ros::NodeHandle& nh) : nh_(nh) , as_( nh_, "move_steps", [this](auto&& PH1) { executeCB(std::forward(PH1)); }, false) - , arm_group_(moveit::planning_interface::MoveGroupInterface("engineer_arm")) + , arm_group_(moveit::planning_interface::MoveGroupInterface("engineer_arms")) , chassis_interface_(nh, tf_) + , teaching_client_("/engineer_trajectory_teaching/trajectory_server", true) , hand_pub_(nh.advertise("/controllers/hand_controller/command", 10)) , end_effector_pub_(nh.advertise("/controllers/joint7_controller/command", 10)) , gimbal_pub_(nh.advertise("/controllers/gimbal_controller/command", 10)) , gpio_pub_(nh.advertise("/controllers/gpio_controller/command", 10)) , reversal_pub_(nh.advertise("/controllers/multi_dof_controller/command", 10)) , planning_result_pub_(nh.advertise("/planning_result", 10)) - , stone_num_pub_(nh.advertise("/stone_num", 10)) , point_cloud_pub_(nh.advertise("/cloud", 100)) - , ore_rotate_pub_(nh.advertise("/controllers/ore_bin_rotate_controller/command", 10)) - , ore_lift_pub_(nh.advertise("/controllers/ore_bin_lifter_controller/command", 10)) - , gimbal_lift_pub_(nh.advertise("/controllers/gimbal_lifter_controller/command", 10)) - , extend_arm_f_pub_(nh.advertise("/controllers/extend_arm_front_controller/command", 10)) - , extend_arm_b_pub_(nh.advertise("/controllers/extend_arm_back_controller/command", 10)) - , silver_lifter_pub_(nh.advertise("/controllers/silver_lifter_controller/command", 10)) - , silver_pusher_pub_(nh.advertise("/controllers/silver_pusher_controller/command", 10)) - , silver_rotator_pub_(nh.advertise("/controllers/silver_rotator_controller/command", 10)) - , gold_pusher_pub_(nh.advertise("/controllers/gold_pusher_controller/command", 10)) - , gold_lifter_pub_(nh.advertise("/controllers/gold_lifter_controller/command", 10)) - , middle_pitch_pub_(nh.advertise("/controllers/middle_pitch_controller/command", 10)) + , left_gripper_pub_(nh.advertise("/controllers/left_gripper_controller/command", 10)) + , right_gripper_pub_(nh.advertise("/controllers/right_gripper_controller/command", 10)) + , lifter_pub_(nh.advertise("/engineer_lifter_cmd", 10)) , tf_listener_(tf_) , is_middleware_control_(false) { @@ -75,14 +68,17 @@ Middleware::Middleware(ros::NodeHandle& nh) nh.getParam("scenes_list", scenes_list); ROS_ASSERT(steps_list.getType() == XmlRpc::XmlRpcValue::Type::TypeStruct); ROS_ASSERT(scenes_list.getType() == XmlRpc::XmlRpcValue::Type::TypeStruct); + rml_ = robot_model_loader::RobotModelLoader("robot_description"); + robot_model_ = rml_.getModel(); + if (!robot_model_) + ROS_ERROR("cannot get robot model."); for (XmlRpc::XmlRpcValue::ValueStruct::const_iterator it = steps_list.begin(); it != steps_list.end(); ++it) { step_queues_.insert(std::make_pair( - it->first, StepQueue(it->second, scenes_list, tf_, arm_group_, chassis_interface_, hand_pub_, - end_effector_pub_, gimbal_pub_, gpio_pub_, reversal_pub_, stone_num_pub_, - planning_result_pub_, point_cloud_pub_, ore_rotate_pub_, ore_lift_pub_, gimbal_lift_pub_, - extend_arm_f_pub_, extend_arm_b_pub_, silver_lifter_pub_, silver_pusher_pub_, - silver_rotator_pub_, gold_pusher_pub_, gold_lifter_pub_, middle_pitch_pub_))); + it->first, StepQueue(it->second, scenes_list, tf_, arm_group_, chassis_interface_, + teaching_client_, hand_pub_, end_effector_pub_, gimbal_pub_, gpio_pub_, + reversal_pub_, planning_result_pub_, point_cloud_pub_, left_gripper_pub_, + right_gripper_pub_, lifter_pub_, robot_model_))); } } else @@ -90,5 +86,6 @@ Middleware::Middleware(ros::NodeHandle& nh) as_.start(); } geometry_msgs::TransformStamped engineer_middleware::JointMotion::arm2base; +geometry_msgs::TransformStamped engineer_middleware::ArmsMotion::arm2base; } // namespace engineer_middleware diff --git a/engineer_planning_adapters/CMakeLists.txt b/engineer_planning_adapters/CMakeLists.txt new file mode 100644 index 0000000..113db18 --- /dev/null +++ b/engineer_planning_adapters/CMakeLists.txt @@ -0,0 +1,46 @@ +cmake_minimum_required(VERSION 3.10) +project(engineer_planning_adapters) + +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +add_definitions(-Wall -Werror) + +find_package(catkin REQUIRED COMPONENTS + roscpp + moveit_core + moveit_ros_planning + moveit_ros_planning_interface + pluginlib +) + +catkin_package( + INCLUDE_DIRS + include + CATKIN_DEPENDS + roscpp + moveit_core + moveit_ros_planning + pluginlib +) + +include_directories(include ${catkin_INCLUDE_DIRS}) + +add_library(${PROJECT_NAME} + src/special_joint_constraint_adapter.cpp +) +target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) + +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +) + +install(FILES planning_adapter_plugins.xml + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) \ No newline at end of file diff --git a/engineer_planning_adapters/include/engineer_planning_adapters/special_joint_constraint_adapter.h b/engineer_planning_adapters/include/engineer_planning_adapters/special_joint_constraint_adapter.h new file mode 100644 index 0000000..c82e407 --- /dev/null +++ b/engineer_planning_adapters/include/engineer_planning_adapters/special_joint_constraint_adapter.h @@ -0,0 +1,31 @@ +// +// Created by ch on 2026/5/6. +// + +#pragma once + +#include +#include +#include + +namespace engineer_planning_adapters +{ +class SpecialJointConstraintAdapter : public planning_request_adapter::PlanningRequestAdapter +{ +public: + std::string getDescription() const override; + void initialize(const ros::NodeHandle& nh) override; + + bool adaptAndPlan(const PlannerFn& planner, + const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res, + std::vector& added_path_index) const override; + +private: + bool enabled_{true}; + std::string joint_a_; + std::string joint_b_; + double constant_{0.0}; +}; +} // namespace engineer_planning_adapters \ No newline at end of file diff --git a/engineer_planning_adapters/package.xml b/engineer_planning_adapters/package.xml new file mode 100644 index 0000000..dc381cc --- /dev/null +++ b/engineer_planning_adapters/package.xml @@ -0,0 +1,23 @@ + + + engineer_planning_adapters + 0.0.0 + Custom MoveIt request adapters + ch + BSD-3-Clause + + catkin + roscpp + moveit_core + moveit_ros_planning + pluginlib + + roscpp + moveit_core + moveit_ros_planning + pluginlib + + + + + diff --git a/engineer_planning_adapters/planning_adapter_plugins.xml b/engineer_planning_adapters/planning_adapter_plugins.xml new file mode 100644 index 0000000..f0029e7 --- /dev/null +++ b/engineer_planning_adapters/planning_adapter_plugins.xml @@ -0,0 +1,5 @@ + + + \ No newline at end of file diff --git a/engineer_planning_adapters/src/special_joint_constraint_adapter.cpp b/engineer_planning_adapters/src/special_joint_constraint_adapter.cpp new file mode 100644 index 0000000..3faafed --- /dev/null +++ b/engineer_planning_adapters/src/special_joint_constraint_adapter.cpp @@ -0,0 +1,86 @@ +// +// Created by ch on 2026/5/6. +// + +#include + +#include +#include +#include +#include + +namespace engineer_planning_adapters +{ +std::string SpecialJointConstraintAdapter::getDescription() const +{ + return "SpecialJointConstraintAdapter"; +} + +void SpecialJointConstraintAdapter::initialize(const ros::NodeHandle& nh) +{ + ros::NodeHandle pnh(nh, "special_joint_constraint"); + pnh.param("enabled", enabled_, false); + pnh.param("joint_a", joint_a_, std::string()); + pnh.param("joint_b", joint_b_, std::string()); + pnh.param("constant", constant_, 0.0); +} + +bool SpecialJointConstraintAdapter::adaptAndPlan( + const PlannerFn& planner, + const planning_scene::PlanningSceneConstPtr& planning_scene, + const planning_interface::MotionPlanRequest& req, + planning_interface::MotionPlanResponse& res, + std::vector& added_path_index) const +{ + if (!enabled_) + return planner(planning_scene, req, res); + + if (joint_a_.empty() || joint_b_.empty()) + { + ROS_ERROR_STREAM("SpecialJointConstraintAdapter: joint_a/joint_b not set"); + return false; + } + + planning_scene::PlanningScenePtr scene = planning_scene->diff(); + const robot_model::JointModel* jm_a = scene->getRobotModel()->getJointModel(joint_a_); + const robot_model::JointModel* jm_b = scene->getRobotModel()->getJointModel(joint_b_); + + if (!jm_a || !jm_b) + { + ROS_ERROR_STREAM("SpecialJointConstraintAdapter: unknown joint(s): " + << joint_a_ << ", " << joint_b_); + return false; + } + + if (jm_a->getVariableCount() != 1 || jm_b->getVariableCount() != 1) + { + ROS_ERROR_STREAM("SpecialJointConstraintAdapter: only 1-DOF joints supported"); + return false; + } + + const std::string var_a = jm_a->getVariableNames()[0]; + const std::string var_b = jm_b->getVariableNames()[0]; + const double constant = constant_; + + scene->setStateFeasibilityPredicate( + [var_a, var_b, constant](const robot_state::RobotState& state, bool verbose) -> bool + { + const double a = state.getVariablePosition(var_a); + const double b = state.getVariablePosition(var_b); + if (a >= constant - b) + return true; + + if (verbose) + { + ROS_WARN_STREAM_THROTTLE(1.0, "Joint relation violated: " << var_a + << " < " << constant << " - " << var_b); + } + return false; + }); + + return planner(scene, req, res); +} +} // namespace engineer_planning_adapters + +PLUGINLIB_EXPORT_CLASS(engineer_planning_adapters::SpecialJointConstraintAdapter, + planning_request_adapter::PlanningRequestAdapter) \ No newline at end of file diff --git a/engineer_trajectory_teaching/CMakeLists.txt b/engineer_trajectory_teaching/CMakeLists.txt new file mode 100644 index 0000000..7606e25 --- /dev/null +++ b/engineer_trajectory_teaching/CMakeLists.txt @@ -0,0 +1,121 @@ +cmake_minimum_required(VERSION 3.0.2) +project(engineer_trajectory_teaching) + +## Use C++14 +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +## By adding -Wall and -Werror, the compiler does not ignore warnings anymore, +## enforcing cleaner code. +add_definitions(-Wall -Werror -Wno-address-of-packed-member) + +## Find catkin macros and libraries +find_package(yaml-cpp REQUIRED) + +find_package(catkin REQUIRED + COMPONENTS + roscpp + rm_msgs + std_msgs + sensor_msgs + trajectory_msgs + control_msgs + actionlib +) + +generate_messages( + DEPENDENCIES + std_msgs + sensor_msgs + trajectory_msgs + control_msgs +) +## Find system libraries +#find_package(Eigen3 REQUIRED) + +################################### +## catkin specific configuration ## +################################### +## The catkin_package macro generates cmake config files for your package +## Declare things to be passed to dependent projects +## INCLUDE_DIRS: uncomment this if your package contains header files +## LIBRARIES: libraries you create in this project that dependent projects also need +## CATKIN_DEPENDS: catkin_packages dependent projects also need +## DEPENDS: system dependencies of this project that dependent projects also need +catkin_package( + INCLUDE_DIRS + include + LIBRARIES + CATKIN_DEPENDS + roscpp + rm_msgs + std_msgs + sensor_msgs + trajectory_msgs + control_msgs + DEPENDS +) + +########### +## Build ## +########### + +## Specify additional locations of header files +## Your package locations should be listed before other locations +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +## Declare cpp executables +FILE(GLOB ALL_SOURCES "src/*.cpp" ) +add_executable(${PROJECT_NAME} ${ALL_SOURCES}) + +## Add dependencies to exported targets, like ROS msgs or srvs +add_dependencies(${PROJECT_NAME} + ${catkin_EXPORTED_TARGETS} +) + +## Specify libraries to link executable targets against +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} + yaml-cpp +) + +############# +## Install ## +############# + +# Mark executables and/or libraries for installation +install( + TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +# Mark cpp header files for installation +install( + DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + FILES_MATCHING PATTERN "*.h" +) + +## Mark other files for installation +install( + DIRECTORY launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +############# +## Testing ## +############# + +#if (${CATKIN_ENABLE_TESTING}) +# set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -pthread") +# ## Add gtest based cpp test target and link libraries +# catkin_add_gtest(${PROJECT_NAME}-test +# test/test_ros_package_template.cpp +# test/AlgorithmTest.cpp) +# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}_core) +#endif () diff --git a/engineer_trajectory_teaching/config/config.yaml b/engineer_trajectory_teaching/config/config.yaml new file mode 100644 index 0000000..8cc601d --- /dev/null +++ b/engineer_trajectory_teaching/config/config.yaml @@ -0,0 +1,3 @@ +record: + record_joint_names: [ "joint1","joint2","joint3","joint4","joint5","joint6" ] + record_interval_threshold: 5.0 diff --git a/engineer_trajectory_teaching/include/engineer_trajectory_teaching/engineer_trajectory_teaching.h b/engineer_trajectory_teaching/include/engineer_trajectory_teaching/engineer_trajectory_teaching.h new file mode 100644 index 0000000..b21e690 --- /dev/null +++ b/engineer_trajectory_teaching/include/engineer_trajectory_teaching/engineer_trajectory_teaching.h @@ -0,0 +1,63 @@ +// +// Created by ch on 25-7-6. +// + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace engineer_trajectory_teaching +{ +class EngineerTrajectoryTeaching +{ +public: + explicit EngineerTrajectoryTeaching(ros::NodeHandle& nh); + void executeCB(const actionlib::SimpleActionServer::GoalConstPtr& goal); + +private: + void startRecord(); + void record(); + void stopRecord(const std::string& file_path); + void jointStateCallback(const sensor_msgs::JointState::ConstPtr& data); + void controllerStateCallback(const control_msgs::JointTrajectoryControllerStateConstPtr& msg); + void recordToYAML(const std::string& file_path); + void playback(const YAML::Node& point, ros::Duration& duration); + void stopPlayback(); + + struct TrajectoryPoint + { + double timestamp; + std::vector joint_position; + }; + struct Trajectory + { + std::vector record_joint_names; + std::vector trajectory_points; + } recorded_trajectory_; + + ros::NodeHandle nh_; + actionlib::SimpleActionServer as_; + rm_msgs::EngineerTrajectoryTeachingFeedback feedback_; + rm_msgs::EngineerTrajectoryTeachingResult result_; + control_msgs::JointTrajectoryControllerState controller_state_; + trajectory_msgs::JointTrajectory trajectory_cmd_; + ros::Subscriber joint_state_sub_, controller_state_sub_; + ros::Publisher trajectory_cmd_pub_, small_joint1_cmd_pub_, small_joint2_cmd_pub_, + small_joint3_cmd_pub_, gpio_cmd_pub_, gimbal_cmd_pub_, middle_pitch_cmd_pub_; + std::map joint_state_; + ros::Time start_record_time_; + ros::Duration last_time_from_start_, time_from_start_; + std::string file_path_; + double record_interval_threshold_ = 0.1, sync_time_ = 0.1,speed_ = 1.0; +}; +} diff --git a/engineer_trajectory_teaching/launch/load.launch b/engineer_trajectory_teaching/launch/load.launch new file mode 100644 index 0000000..b36712d --- /dev/null +++ b/engineer_trajectory_teaching/launch/load.launch @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/engineer_trajectory_teaching/package.xml b/engineer_trajectory_teaching/package.xml new file mode 100644 index 0000000..809aa0a --- /dev/null +++ b/engineer_trajectory_teaching/package.xml @@ -0,0 +1,66 @@ + + + engineer_trajectory_teaching + 0.0.0 + The engineer_trajectory_teaching package + + + + + ch + + + + + + TODO + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + catkin + roscpp + yaml-cpp + rm_msgs + sensor_msgs + std_msgs + trajectory_msgs + control_msgs + actionlib + + + + + + + diff --git a/engineer_trajectory_teaching/src/engineer_trajectory_teaching.cpp b/engineer_trajectory_teaching/src/engineer_trajectory_teaching.cpp new file mode 100644 index 0000000..4da53c0 --- /dev/null +++ b/engineer_trajectory_teaching/src/engineer_trajectory_teaching.cpp @@ -0,0 +1,285 @@ +// +// Created by ch on 25-7-6. +// +#include "engineer_trajectory_teaching/engineer_trajectory_teaching.h" + +namespace engineer_trajectory_teaching +{ + EngineerTrajectoryTeaching::EngineerTrajectoryTeaching(ros::NodeHandle &nh) + : nh_(nh), + as_(nh_, "trajectory_server", [this](auto&& PH1) { executeCB(std::forward(PH1)); }, false) + { + ROS_INFO_STREAM(nh_.getNamespace().c_str()); + ROS_ASSERT(nh_.getParam("record/record_joint_names", recorded_trajectory_.record_joint_names)); + nh_.param("record/record_interval_threshold", record_interval_threshold_, 0.1); + joint_state_sub_ = nh_.subscribe("/joint_states", 10, &EngineerTrajectoryTeaching::jointStateCallback, this); + controller_state_sub_ = nh_.subscribe("/controllers/arm_trajectory_controller/state", 10, &EngineerTrajectoryTeaching::controllerStateCallback, this); + trajectory_cmd_pub_ = nh_.advertise("/controllers/arm_trajectory_controller/command", 10); + small_joint1_cmd_pub_ = nh_.advertise("/controllers/small_joint1_controller/command", 10); + small_joint2_cmd_pub_ = nh_.advertise("/controllers/small_joint2_controller/command", 10); + small_joint3_cmd_pub_ = nh_.advertise("/controllers/small_joint3_controller/command", 10); + gpio_cmd_pub_ = nh_.advertise("/controllers/gpio_controller/command", 10); + gimbal_cmd_pub_ = nh_.advertise("/controllers/gimbal_controller/command", 10); + middle_pitch_cmd_pub_ = nh_.advertise("/controllers/middle_pitch_controller/command", 10); + as_.start(); + } + + void EngineerTrajectoryTeaching::executeCB(const actionlib::SimpleActionServer::GoalConstPtr &goal) + { + result_.succeed = false; + if (goal->filename.empty()) + { + ROS_ERROR_STREAM("The file path is empty!"); + as_.setAborted(result_); + return; + } + file_path_ = goal->filename; + if (goal->command == rm_msgs::EngineerTrajectoryTeachingGoal_>::RECORD) + { + startRecord(); + while (as_.isActive()) + { + record(); + if (as_.isPreemptRequested() || !ros::ok()) + { + stopRecord(file_path_); + break; + } + } + result_.succeed = true; + as_.setSucceeded(result_); + } + if (goal->command == rm_msgs::EngineerTrajectoryTeachingGoal_>::PLAYBACK) + { + if (goal->sync_time <= 0.0 || goal->speed <= 0.0) + { + ROS_ERROR_STREAM("THe sync_time or speed is invalid!"); + as_.setAborted(result_); + return; + } + sync_time_ = goal->sync_time; + speed_ = goal->speed; + try + { + YAML::Node config = YAML::LoadFile(file_path_); + ros::Time last_time = ros::Time::now(); + + ros::Duration duration = ros::Duration(0.0); + auto joint_names = config["joint_names"].as>(); + trajectory_cmd_.joint_names = joint_names; + auto points = config["points"]; + auto point = points.begin(); + auto last_time_stamp = (*point)["timestamp"].as(); + while (as_.isActive() && point != points.end()) + { + if (ros::Time::now() - last_time >= duration) + { + duration = ros::Duration(((*point)["timestamp"].as() - last_time_stamp) / speed_); + if (point == points.begin()) + duration += ros::Duration(sync_time_); + playback(*point, duration); + last_time_stamp = (*point)["timestamp"].as(); + ++point; + last_time = ros::Time::now(); + } + if (as_.isPreemptRequested() || !ros::ok()) + { + stopPlayback(); + break; + } + } + if (point == points.end()) + duration.sleep(); + result_.succeed = true; + as_.setSucceeded(result_); + } + catch (std::exception &e) + { + stopPlayback(); + ROS_WARN_STREAM("YAML parsing error: " << e.what()); + } + } + } + + void EngineerTrajectoryTeaching::jointStateCallback(const sensor_msgs::JointStateConstPtr &msg) + { + joint_state_.clear(); + for (size_t i = 0; i < msg->name.size(); i++) + { + joint_state_.insert(std::map::value_type(msg->name[i], msg->position[i])); + } + } + + void EngineerTrajectoryTeaching::controllerStateCallback(const control_msgs::JointTrajectoryControllerStateConstPtr &msg) + { + controller_state_ = *msg; + } + + + void EngineerTrajectoryTeaching::record() + { + time_from_start_ = ros::Time::now() - start_record_time_; + if ((time_from_start_ - last_time_from_start_).toSec() < record_interval_threshold_ ) + return; + TrajectoryPoint point; + point.timestamp = time_from_start_.toSec(); + for (const auto& record_joint_name : recorded_trajectory_.record_joint_names) + { + try + { + point.joint_position.push_back(joint_state_.at(record_joint_name)); + } + catch (std::exception &e) + { + ROS_ERROR_STREAM("The joint named: " << record_joint_name << " was not found!"); + return; + } + } + recorded_trajectory_.trajectory_points.push_back(point); + feedback_.mode = "RECORD"; + feedback_.state = rm_msgs::EngineerTrajectoryTeachingFeedback::START; + feedback_.text = "Recording..."; + feedback_.time_from_start = time_from_start_.toSec(); + as_.publishFeedback(feedback_); + last_time_from_start_ = time_from_start_; + } + + void EngineerTrajectoryTeaching::startRecord() + { + recorded_trajectory_.trajectory_points.clear(); + start_record_time_ = ros::Time::now(); + last_time_from_start_ = ros::Duration(-500.0); + ROS_INFO_STREAM("Start recording ..."); + } + + void EngineerTrajectoryTeaching::stopRecord(const std::string& file_path) + { + recordToYAML(file_path); + ROS_INFO_STREAM("Saved trajectory to: " << file_path); + } + + void EngineerTrajectoryTeaching::recordToYAML(const std::string& file_path) + { + try + { + std::ofstream f_out(file_path); + YAML::Emitter emitter(f_out); + emitter << YAML::BeginMap; + emitter << YAML::Key << "joint_names" << YAML::Value << YAML::Flow << recorded_trajectory_.record_joint_names; + emitter << YAML::Key << "points" << YAML::Value << YAML::BeginSeq; + for (const auto& point : recorded_trajectory_.trajectory_points) + { + emitter << YAML::BeginMap; + emitter << YAML::Key << "timestamp" << YAML::Value << point.timestamp; + emitter << YAML::Key << "positions" << YAML::Value << YAML::Flow << point.joint_position; + emitter << YAML::EndMap; + } + emitter << YAML::EndSeq; + emitter << YAML::EndMap; + f_out.close(); + ROS_INFO("Save successfully"); + } + catch (std::exception &e) + { + ROS_ERROR_STREAM("YAML parsing error: " << e.what()); + } + } + + void EngineerTrajectoryTeaching::playback(const YAML::Node &point, ros::Duration& duration) + { + if (point["positions"].IsDefined()) + { + trajectory_cmd_.points.clear(); + trajectory_cmd_.header.stamp = ros::Time(0); + trajectory_msgs::JointTrajectoryPoint trajectory_point; + const auto positions = point["positions"].as>(); + trajectory_point.positions = positions; + if (point["velocities"].IsDefined()) + { + const auto velocities = point["velocities"].as>(); + trajectory_point.velocities = velocities; + } + if (point["accelerations"].IsDefined()) + { + const auto accelerations = point["accelerations"].as>(); + trajectory_point.accelerations = accelerations; + } + trajectory_point.time_from_start = duration; + trajectory_cmd_.points.push_back(trajectory_point); + trajectory_cmd_pub_.publish(trajectory_cmd_); + } + if (point["small_joint1"].IsDefined()) + { + std_msgs::Float64 small_joint1_cmd; + small_joint1_cmd.data = point["small_joint1"].as(); + small_joint1_cmd_pub_.publish(small_joint1_cmd); + } + if (point["small_joint2"].IsDefined()) + { + std_msgs::Float64 small_joint2_cmd; + small_joint2_cmd.data = point["small_joint2"].as(); + small_joint2_cmd_pub_.publish(small_joint2_cmd); + } + if (point["small_joint3"].IsDefined()) + { + std_msgs::Float64 small_joint3_cmd; + small_joint3_cmd.data = point["small_joint3"].as(); + small_joint3_cmd_pub_.publish(small_joint3_cmd); + } + if (point["main_gripper"].IsDefined() || point["small_gripper"].IsDefined() || point["transfer_gripper"].IsDefined()) + { + rm_msgs::GpioData gpio_cmd; + if (point["main_gripper"].IsDefined()) + { + gpio_cmd.gpio_name.emplace_back("main_gripper"); + gpio_cmd.gpio_state.emplace_back(point["main_gripper"].as()); + } + if (point["small_gripper"].IsDefined()) + { + gpio_cmd.gpio_name.emplace_back("small_gripper"); + gpio_cmd.gpio_state.emplace_back(point["small_gripper"].as()); + } + if (point["transfer_gripper"].IsDefined()) + { + gpio_cmd.gpio_name.emplace_back("transfer_gripper"); + gpio_cmd.gpio_state.emplace_back(point["transfer_gripper"].as()); + } + gpio_cmd_pub_.publish(gpio_cmd); + } + if (point["gimbal"].IsDefined()) + { + rm_msgs::GimbalCmd gimbal_cmd; + gimbal_cmd.mode = rm_msgs::GimbalCmd::DIRECT; + gimbal_cmd.target_pos.header.frame_id = "base_link"; + gimbal_cmd.target_pos.point.x = point["gimbal"][0].as(); + gimbal_cmd.target_pos.point.y = point["gimbal"][1].as(); + gimbal_cmd.target_pos.point.z = point["gimbal"][2].as(); + gimbal_cmd_pub_.publish(gimbal_cmd); + } + if (point["middle_pitch"].IsDefined()) + { + std_msgs::Float64 middle_pitch_cmd; + middle_pitch_cmd.data = point["middle_pitch"].as(); + middle_pitch_cmd_pub_.publish(middle_pitch_cmd); + } + feedback_.mode = "PLAYBACK"; + feedback_.state = rm_msgs::EngineerTrajectoryTeachingFeedback::START; + feedback_.text = "Playback..."; + feedback_.time_from_start = point["timestamp"].as(); + as_.publishFeedback(feedback_); + } + + void EngineerTrajectoryTeaching::stopPlayback() + { + trajectory_cmd_.points.clear(); + trajectory_cmd_.header.stamp = ros::Time(0); + trajectory_cmd_.joint_names = controller_state_.joint_names; + trajectory_msgs::JointTrajectoryPoint current_point; + current_point.positions = controller_state_.actual.positions; + current_point.time_from_start = ros::Duration(0.1); + trajectory_cmd_.points.push_back(current_point); + trajectory_cmd_pub_.publish(trajectory_cmd_); + ROS_INFO_STREAM("Stop playback"); + } + +} \ No newline at end of file diff --git a/engineer_trajectory_teaching/src/main.cpp b/engineer_trajectory_teaching/src/main.cpp new file mode 100644 index 0000000..d3804f6 --- /dev/null +++ b/engineer_trajectory_teaching/src/main.cpp @@ -0,0 +1,21 @@ +// +// Created by ch on 25-7-7. +// +#include "engineer_trajectory_teaching/engineer_trajectory_teaching.h" + +using namespace engineer_trajectory_teaching; + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "engineer_trajectory_teaching"); + ros::NodeHandle nh("~"); + engineer_trajectory_teaching::EngineerTrajectoryTeaching engineer_trajectory_teaching(nh); + ros::Rate loop_rate(100); + ros::MultiThreadedSpinner spinner(3); + while (ros::ok()) + { + spinner.spin(); + loop_rate.sleep(); + } + return 0; +} diff --git a/engineer_trajectory_teaching/trajectory/big_change_small.yaml b/engineer_trajectory_teaching/trajectory/big_change_small.yaml new file mode 100644 index 0000000..0421a9d --- /dev/null +++ b/engineer_trajectory_teaching/trajectory/big_change_small.yaml @@ -0,0 +1,57 @@ +joint_names: [joint1, joint2, joint3, joint4, joint5, joint6] +points: + - timestamp: 0.000093433 + positions: [ 0.1, 1.12080810546875, -2.29154296875, -1.568243164062501, 0.08296142578125015, -1.59 ] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + small_joint3: -0.01 + small_joint1: 0.238 + small_joint2: -0.176 + middle_pitch: 0.071 + transfer_gripper: true + small_gripper: true + - timestamp: 0.2 + positions: [ 0.2, 1.12080810546875, -2.29154296875, -1.568243164062501, 0.08296142578125015, -1.59 ] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + - timestamp: 1.15 + positions: [ 0.2, -0.763, -2.096, -1.568243164062501, 0.35, -1.59 ] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + - timestamp: 1.7 + positions: [ 0.2, -0.763, -2.096, -1.568243164062501, 0.35, -1.59 ] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + main_gripper: false + - timestamp: 2.2 + positions: [ 0.4, -0.5, -2.096, -1.568243164062501, 0.35, -1.59 ] + - timestamp: 2.7 + positions: [0.55, -1.281, -2.189, -0.034, 1.575, -0.307] + - timestamp: 3.2 + positions: [0.55, -2.142, -2.189, -0.034, 1.66, -0.307] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + main_gripper: true + - timestamp: 3.5 + positions: [0.55, -2.142, -2.189, -0.034, 1.66, -0.307] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + - timestamp: 3.65 + positions: [0.47, -2.142, -2.189, -0.034, 1.66, -0.307] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + transfer_gripper: false + small_gripper: true + - timestamp: 4.0 + positions: [0.47, -2.142, -2.189, -0.034, 1.66, -0.307] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + - timestamp: 4.2 + positions: [0.55, -2.142, -2.189, -0.034, 1.66, -0.307] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + small_joint3: 1.61 + - timestamp: 4.7 + positions: [0.55, -1.281, -2.189, -0.034, 1.575, -0.307] + - timestamp: 5.7 + positions: [ 0.55, 0.871, -2.407, -0.064, 0.653, 0.01 ] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + small_joint1: 0.1805 + - timestamp: 6.0 + positions: [0.55, 0.871, -2.407, -0.064, 0.653, 0.01] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + middle_pitch: 1.057 + - timestamp: 6.4 + positions: [0.244, 0.871, -2.407, -0.064, 0.653, 0.01] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] \ No newline at end of file diff --git a/engineer_trajectory_teaching/trajectory/big_island1.yaml b/engineer_trajectory_teaching/trajectory/big_island1.yaml new file mode 100644 index 0000000..447dc54 --- /dev/null +++ b/engineer_trajectory_teaching/trajectory/big_island1.yaml @@ -0,0 +1,42 @@ +joint_names: [joint1, joint2, joint3, joint4, joint5, joint6] +points: + - timestamp: 0.300091673 + positions: [0.00804281809659451, 1.093, -2.359, -0.04538916015625039, 0.1205249023437503, 1.60922314453125] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + middle_pitch: 0.06 + - timestamp: 0.500091673 + positions: [0.003484281809659451, 1.093, -2.359, -0.04538916015625039, 0.1205249023437503, 1.60922314453125] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + small_joint3: 0.039 + main_gripper: true + small_gripper: true + transfer_gripper: false + - timestamp: 0.7 + small_joint1: 0.065 + small_joint2: -0.01 + - timestamp: 1.000091753 + positions: [0.003484281809659451, 1.093, -2.359, -1.125914062500001, 0.01550048828125022, -1.588] + - timestamp: 1.500091771 + positions: [0.003484281809659451, 0.689747314453125, -2.032906494140625, -1.414539550781251, 0.006301269531250153, -1.588] + small_joint2: -0.393 + - timestamp: 2.000091855 + positions: [0.003488954516601328, 0.3471875, -1.741845092773437, -1.53182958984375, -0.04582763671875084, -1.588] + - timestamp: 2.500092752 + positions: [0.003493627223543204, 0.3262066650390625, -1.729256591796875, -1.548694824218751, -0.05387695312499935, -1.588] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + small_joint3: -0.07 + - timestamp: 3.000092959 + positions: [0.04434411102172571, 0.3258251953125, -1.742989501953125, -1.565560058593749, -0.05349365234374942, -1.588] + small_joint1: 0.098 + middle_pitch: 0.0 + - timestamp: 3.500093207 + positions: [0.05834932343668896, 0.3258251953125, -1.742989501953125, -1.567859863281251, -0.01593017578124928, -1.588] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + - timestamp: 4.000093288 + positions: [0.05861886429921393, 0.7053875732421875, -2.026421508789062, -1.566709960937501, 0.009750976562499514, -1.588] + - timestamp: 4.500093391 + positions: [0.05837121083129446, 1.099064331054687, -2.281624755859375, -1.564793457031249, 0.06341308593750022, -1.588] + - timestamp: 5.000093433 + positions: [0.05835411828694595, 1.12080810546875, -2.29154296875, -1.568243164062501, 0.08296142578125015, -1.588] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + small_joint2: -0.001 \ No newline at end of file diff --git a/engineer_trajectory_teaching/trajectory/big_island2.yaml b/engineer_trajectory_teaching/trajectory/big_island2.yaml new file mode 100644 index 0000000..d1cdd0c --- /dev/null +++ b/engineer_trajectory_teaching/trajectory/big_island2.yaml @@ -0,0 +1,16 @@ +joint_names: [joint1, joint2, joint3, joint4, joint5, joint6] +points: + - timestamp: 0.000093433 + positions: [ 0.3640007556342396, 1.12080810546875, -2.29154296875, -1.568243164062501, 0.08296142578125015, 1.5528779296875 ] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + small_joint1: 0.24 + small_joint2: -0.176 + - timestamp: 1.000050034 + positions: [0.3640007556342396, 0.871, -2.407, -0.064, 0.653, 0.01] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + middle_pitch: 1.057 + small_joint3: 1.61 + - timestamp: 2.000050034 + positions: [0.244, 0.871, -2.407, -0.064, 0.653, 0.01] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + small_joint1: 0.1805 \ No newline at end of file diff --git a/engineer_trajectory_teaching/trajectory/big_island_left.yaml b/engineer_trajectory_teaching/trajectory/big_island_left.yaml new file mode 100644 index 0000000..20f28f5 --- /dev/null +++ b/engineer_trajectory_teaching/trajectory/big_island_left.yaml @@ -0,0 +1,27 @@ +joint_names: [joint1, joint2, joint3, joint4, joint5, joint6] +points: + - timestamp: 0.300091673 + positions: [0.00804281809659451, 1.093, -2.359, -0.04538916015625039, 0.1205249023437503, 1.60922314453125] + middle_pitch: 0.06 + - timestamp: 0.500091673 + positions: [0.003484281809659451, 1.093, -2.359, -0.04538916015625039, 0.1205249023437503, 1.60922314453125] + main_gripper: true + - timestamp: 1.000091753 + positions: [0.003484281809659451, 1.093, -2.359, -1.125914062500001, 0.01550048828125022, -1.588] + - timestamp: 1.500091771 + positions: [0.003484281809659451, 0.689747314453125, -2.032906494140625, -1.414539550781251, 0.006301269531250153, -1.588] + - timestamp: 2.000091855 + positions: [0.003488954516601328, 0.3471875, -1.741845092773437, -1.53182958984375, -0.04582763671875084, -1.588] + - timestamp: 2.500092752 + positions: [0.003493627223543204, 0.3262066650390625, -1.729256591796875, -1.548694824218751, -0.05387695312499935, -1.588] + - timestamp: 3.000092959 + positions: [0.04034411102172571, 0.3258251953125, -1.742989501953125, -1.565560058593749, -0.05349365234374942, -1.588] + middle_pitch: 0.0 + - timestamp: 3.500093207 + positions: [0.05534932343668896, 0.3258251953125, -1.742989501953125, -1.567859863281251, -0.01593017578124928, -1.588] + - timestamp: 4.000093288 + positions: [0.05361886429921393, 0.7053875732421875, -2.026421508789062, -1.566709960937501, 0.009750976562499514, -1.588] + - timestamp: 4.500093391 + positions: [0.05337121083129446, 1.099064331054687, -2.281624755859375, -1.564793457031249, 0.06341308593750022, -1.588] + - timestamp: 5.000093433 + positions: [0.05335411828694595, 1.12080810546875, -2.29154296875, -1.568243164062501, 0.08296142578125015, -1.588] \ No newline at end of file diff --git a/engineer_trajectory_teaching/trajectory/big_island_left2.yaml b/engineer_trajectory_teaching/trajectory/big_island_left2.yaml new file mode 100644 index 0000000..6f794d1 --- /dev/null +++ b/engineer_trajectory_teaching/trajectory/big_island_left2.yaml @@ -0,0 +1,9 @@ +joint_names: [joint1, joint2, joint3, joint4, joint5, joint6] +points: + - timestamp: 0.000093433 + positions: [ 0.3640007556342396, 1.12080810546875, -2.29154296875, -1.568243164062501, 0.08296142578125015, 1.5528779296875 ] + - timestamp: 1.000050034 + positions: [0.3640007556342396, 0.871, -2.407, -0.064, 0.653, 0.01] + middle_pitch: 1.057 + - timestamp: 2.000050034 + positions: [0.244, 0.871, -2.407, -0.064, 0.653, 0.01] \ No newline at end of file diff --git a/engineer_trajectory_teaching/trajectory/big_island_mid.yaml b/engineer_trajectory_teaching/trajectory/big_island_mid.yaml new file mode 100644 index 0000000..69df467 --- /dev/null +++ b/engineer_trajectory_teaching/trajectory/big_island_mid.yaml @@ -0,0 +1,20 @@ +joint_names: [joint1, joint2, joint3, joint4, joint5, joint6] +points: + - timestamp: 4.2378e-05 + positions: [0.004657131252070519, 1.096012573242187, -2.492577514648437, -0.0004916992187505853, -0.0009105078125003496, -0.001002441406249166] + main_gripper: true + middle_pitch: 0.071 + - timestamp: 0.5 + positions: [0.004657131252070519, 1.058628540039062, -2.173287353515625, -0.4478037109374994, -0.04192369140624985, -1.54] + - timestamp: 1.0 + positions: [0.004657131252070519, 0.7580303955078125, -2.021080932617187, -1.515296386718751, 0.1412940820312499, -1.54] + - timestamp: 1.5 + positions: [0.004657131252070519, 0.3506207275390625, -1.760155639648437, -1.511846679687499, 0.1029640039062499, -1.54] + - timestamp: 2.0 + positions: [0.004657131252070519, -0.08997680664062502, -1.364953002929687, -1.522962402343751, 0.1067970117187492, -1.54] + - timestamp: 2.5 + positions: [0.06931673663141094, -0.08997680664062502, -1.365715942382812, -1.504563964843751, 0.1071803124999992, -1.54] + - timestamp: 3.0 + positions: [0.06931673663141094, 0.4116558837890625, -1.80326171875, -1.5578427734375, 0.1439771874999994, -1.54] + - timestamp: 3.5 + positions: [0.06931673663141094, 1.022770385742187, -2.054268798828125, -1.5164462890625, 0.5084962304687494, -1.54] \ No newline at end of file diff --git a/engineer_trajectory_teaching/trajectory/big_island_mid2.yaml b/engineer_trajectory_teaching/trajectory/big_island_mid2.yaml new file mode 100644 index 0000000..dfa37ca --- /dev/null +++ b/engineer_trajectory_teaching/trajectory/big_island_mid2.yaml @@ -0,0 +1,9 @@ +joint_names: [joint1, joint2, joint3, joint4, joint5, joint6] +points: + - timestamp: 0.000093433 + positions: [ 0.3640007556342396, 1.022770385742187, -2.054268798828125, -1.5164462890625, 0.5084962304687494, -1.54] + - timestamp: 1.000050034 + positions: [0.3640007556342396, 0.871, -2.407, -0.064, 0.653, 0.01] + middle_pitch: 1.057 + - timestamp: 2.000050034 + positions: [0.244, 0.871, -2.407, -0.064, 0.653, 0.01] \ No newline at end of file diff --git a/engineer_trajectory_teaching/trajectory/big_island_right.yaml b/engineer_trajectory_teaching/trajectory/big_island_right.yaml new file mode 100644 index 0000000..56776cd --- /dev/null +++ b/engineer_trajectory_teaching/trajectory/big_island_right.yaml @@ -0,0 +1,16 @@ +joint_names: [joint1, joint2, joint3, joint4, joint5, joint6] +points: + - timestamp: 0.000050034 + positions: [ 0.01, -2.212, 1.862, -1.65, -0.565, -1.54 ] + middle_pitch: 0.06 + - timestamp: 1.0 + positions: [ 0.01, -1.466, 1.474, -1.65, -0.379, -1.54 ] + - timestamp: 1.5 + positions: [ 0.01, -1.466, 1.474, -1.65, -0.379, -1.54 ] + - timestamp: 1.7 + positions: [ 0.059, -1.466, 1.474, -1.65, -0.379, -1.54 ] + middle_pitch: 0.0 + - timestamp: 2.4 + positions: [ 0.059, -2.353, 1.802, -1.65, -0.932, -1.54 ] + - timestamp: 2.8 + positions: [ 0.059, -2.533, 1.806, -1.65, -1.052, -1.54 ] \ No newline at end of file diff --git a/engineer_trajectory_teaching/trajectory/big_island_right2.yaml b/engineer_trajectory_teaching/trajectory/big_island_right2.yaml new file mode 100644 index 0000000..56ac173 --- /dev/null +++ b/engineer_trajectory_teaching/trajectory/big_island_right2.yaml @@ -0,0 +1,9 @@ +joint_names: [joint1, joint2, joint3, joint4, joint5, joint6] +points: + - timestamp: 0.000093433 + positions: [ 0.3640007556342396, 1.12080810546875, -2.29154296875, -1.568243164062501, 0.08296142578125015, 1.5528779296875 ] + - timestamp: 1.000050034 + positions: [0.3640007556342396, 0.871, -2.407, -0.064, 0.653, 0.01] + middle_pitch: 1.057 + - timestamp: 2.000050034 + positions: [0.234, 0.871, -2.407, -0.064, 0.653, 0.01] \ No newline at end of file diff --git a/engineer_trajectory_teaching/trajectory/big_island_right_ready.yaml b/engineer_trajectory_teaching/trajectory/big_island_right_ready.yaml new file mode 100644 index 0000000..2eed9a4 --- /dev/null +++ b/engineer_trajectory_teaching/trajectory/big_island_right_ready.yaml @@ -0,0 +1,6 @@ +joint_names: [joint1, joint2, joint3, joint4, joint5, joint6] +points: + - timestamp: 0.000050034 + positions: [ 0.01, -2.212, 1.862, -1.65, -0.565, -1.54 ] + middle_pitch: 0.06 + main_gripper: true \ No newline at end of file diff --git a/engineer_trajectory_teaching/trajectory/get_second_stone.yaml b/engineer_trajectory_teaching/trajectory/get_second_stone.yaml new file mode 100644 index 0000000..063230c --- /dev/null +++ b/engineer_trajectory_teaching/trajectory/get_second_stone.yaml @@ -0,0 +1,23 @@ +joint_names: [joint1, joint2, joint3, joint4, joint5, joint6] +points: + - timestamp: 0.000050034 + positions: [ 0.3, -1.311, -2.272, -0.062, 1.648, -1.106 ] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + - timestamp: 0.2 + positions: [ 0.3, -1.311, -2.272, -0.062, 1.648, -1.106 ] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + main_gripper: true + - timestamp: 0.6 + positions: [ 0.139, -1.311, -2.272, -0.062, 1.648, -1.106 ] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + - timestamp: 1.0 + positions: [ 0.139, -1.311, -2.272, -0.062, 1.648, -1.106 ] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + - timestamp: 1.5 + positions: [ 0.4, -1.311, -2.272, -0.062, 1.648, -1.106 ] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + small_joint1: 0.001 + small_joint2: -0.001 + small_joint3: 0.001 + small_gripper: false + transfer_gripper: false \ No newline at end of file diff --git a/engineer_trajectory_teaching/trajectory/get_second_stone_side.yaml b/engineer_trajectory_teaching/trajectory/get_second_stone_side.yaml new file mode 100644 index 0000000..8e74e6e --- /dev/null +++ b/engineer_trajectory_teaching/trajectory/get_second_stone_side.yaml @@ -0,0 +1,30 @@ +joint_names: [joint1, joint2, joint3, joint4, joint5, joint6] +points: + - timestamp: 0.000050034 + positions: [ 0.26, 0.259, -2.569, -1.47, 0.738, -1.506 ] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + - timestamp: 0.2 + positions: [ 0.26, 0.259, -2.569, -1.47, 0.738, -1.506 ] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + main_gripper: true + small_joint2: -0.059 + - timestamp: 0.6 + positions: [ 0.055, 0.259, -2.569, -1.47, 0.738, -1.506 ] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + - timestamp: 0.8 + positions: [ 0.055, 0.259, -2.569, -1.47, 0.738, -1.506 ] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + - timestamp: 1.2 + positions: [ 0.055, -0.729, -2.538, -1.471, 0.0, -1.506 ] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + - timestamp: 1.6 + positions: [ 0.055, -0.729, -2.538, -1.471, 0.0, -1.506 ] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + small_joint1: 0.001 + small_joint2: -0.001 + small_joint3: 0.001 + small_gripper: false + transfer_gripper: false + - timestamp: 1.8 + positions: [ 0.3, -0.729, -2.538, -1.471, 0.0, -1.506 ] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] \ No newline at end of file diff --git a/engineer_trajectory_teaching/trajectory/get_transfer_stored.yaml b/engineer_trajectory_teaching/trajectory/get_transfer_stored.yaml new file mode 100644 index 0000000..14785e5 --- /dev/null +++ b/engineer_trajectory_teaching/trajectory/get_transfer_stored.yaml @@ -0,0 +1,26 @@ +joint_names: [joint1, joint2, joint3, joint4, joint5, joint6] +points: + - timestamp: 1.5 + positions: [0.55, -1.281, -2.189, -0.034, 1.575, -0.307] + middle_pitch: 0.071 + - timestamp: 2.0 + positions: [0.55, -2.142, -2.189, -0.034, 1.66, -0.307] + small_joint3: -0.01 + small_joint1: 0.238 + small_joint2: -0.176 + main_gripper: true + - timestamp: 2.3 + positions: [0.55, -2.142, -2.189, -0.034, 1.66, -0.307] + - timestamp: 2.45 + positions: [0.47, -2.142, -2.189, -0.034, 1.66, -0.307] + transfer_gripper: false + small_gripper: false + - timestamp: 2.8 + positions: [0.47, -2.142, -2.189, -0.034, 1.66, -0.307] + - timestamp: 3.0 + positions: [0.55, -2.142, -2.189, -0.034, 1.66, -0.307] + small_joint3: -0.01 + small_joint1: 0.01 + small_joint2: -0.01 + - timestamp: 3.5 + positions: [0.55, -1.281, -2.189, -0.034, 1.575, -0.307] \ No newline at end of file diff --git a/engineer_trajectory_teaching/trajectory/ground_stone1.yaml b/engineer_trajectory_teaching/trajectory/ground_stone1.yaml new file mode 100644 index 0000000..9f2e51e --- /dev/null +++ b/engineer_trajectory_teaching/trajectory/ground_stone1.yaml @@ -0,0 +1,10 @@ +joint_names: [joint1, joint2, joint3, joint4, joint5, joint6] +points: + - timestamp: 0.00004814 + positions: [0.09982151112444874, 1.058628540039062, -2.47007080078125, 0.009422851562500112, 0.4447973632812506, -0.01022265624999985] + - timestamp: 0.5 + positions: [0.003, 1.058628540039062, -2.47007080078125, 0.009422851562500112, 0.4447973632812506, -0.01022265624999985] + middle_pitch: 0.0 + - timestamp: 1.1 + positions: [0.003, -0.112, -2.401, 0.001, 1.128, 1.264] + main_gripper: true \ No newline at end of file diff --git a/engineer_trajectory_teaching/trajectory/ground_stone2.yaml b/engineer_trajectory_teaching/trajectory/ground_stone2.yaml new file mode 100644 index 0000000..aa546e1 --- /dev/null +++ b/engineer_trajectory_teaching/trajectory/ground_stone2.yaml @@ -0,0 +1,13 @@ +joint_names: [joint1, joint2, joint3, joint4, joint5, joint6] +points: + - timestamp: 0.00001 + positions: [0.003, -0.112, -2.401, 0.001, 1.128, 1.264] + main_gripper: true + middle_pitch: 0.585 + - timestamp: 0.9 + positions: [0.376, -0.112, -2.401, 0.001, 1.128, 1.264] + - timestamp: 1.4 + positions: [0.376, 0.871, -2.407, -0.064, 0.653, 0.645] + middle_pitch: 1.057 + - timestamp: 1.9 + positions: [0.234, 0.871, -2.407, -0.064, 0.653, 0.645] \ No newline at end of file diff --git a/engineer_trajectory_teaching/trajectory/small_island1.yaml b/engineer_trajectory_teaching/trajectory/small_island1.yaml new file mode 100644 index 0000000..94641d1 --- /dev/null +++ b/engineer_trajectory_teaching/trajectory/small_island1.yaml @@ -0,0 +1,40 @@ +joint_names: [joint1, joint2, joint3, joint4, joint5, joint6] +points: + - timestamp: 1.00004814 + positions: [0.06082151112444874, 1.058628540039062, -2.47007080078125, 0.009422851562500112, 0.4447973632812506, -0.01022265624999985] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + main_gripper: true + small_gripper: true + transfer_gripper: false + small_joint1: 0.24 + small_joint2: -0.389 + small_joint3: 1.61 + middle_pitch: 1.057 + - timestamp: 1.500048414 + positions: [0.1585558495207434, 0.5, -2.4, 0.007889648437500396, 0.639130859375, -0.2] + - timestamp: 2.2 + positions: [0.1837121461268273, 0.0, -2.1, -0.03120703124999946, 0.691, -0.2] + small_joint1: -0.015 + - timestamp: 2.900048747 + positions: [0.1831078093623446, -0.388, -1.95, -0.1691953125000005, 0.691, -0.2] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + - timestamp: 3.350049087 + positions: [0.152579457342083, -0.388, -1.95, -0.041, 0.691, -0.2] + - timestamp: 3.400049244 + positions: [0.07606699630681203, -0.388, -1.95, -0.041, 0.691, -0.2] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + - timestamp: 3.9 + positions: [0.07606699630681203, -0.388, -1.95, -0.041, 0.691, -0.2] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + - timestamp: 4.0 + positions: [0.1214794775059318, -0.388, -1.95, -0.041, 0.691, -0.2] + - timestamp: 4.4 + positions: [0.311261469950257, -0.388, -1.95, -0.041, 0.691, -0.2] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + - timestamp: 5.0 + positions: [0.3654601977690858, -0.07357360839843752, -2.331978759765625, -0.1216660156250005, 0.653, -0.2] + small_joint1: 0.24 + - timestamp: 5.5 + positions: [0.3640007556342396, 0.871, -2.407, -0.064, 0.653, 0.645] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + small_joint2: -0.176 \ No newline at end of file diff --git a/engineer_trajectory_teaching/trajectory/small_island2.yaml b/engineer_trajectory_teaching/trajectory/small_island2.yaml new file mode 100644 index 0000000..32c6849 --- /dev/null +++ b/engineer_trajectory_teaching/trajectory/small_island2.yaml @@ -0,0 +1,6 @@ +joint_names: [joint1, joint2, joint3, joint4, joint5, joint6] +points: + - timestamp: 0.000050034 + positions: [0.244, 0.871, -2.407, -0.064, 0.653, 0.645] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + small_joint1: 0.1805 \ No newline at end of file diff --git a/engineer_trajectory_teaching/trajectory/test.yaml b/engineer_trajectory_teaching/trajectory/test.yaml new file mode 100644 index 0000000..1db7d9f --- /dev/null +++ b/engineer_trajectory_teaching/trajectory/test.yaml @@ -0,0 +1,10 @@ +joint_names: [joint1, joint2, joint3, joint4, joint5, joint6] +points: + - timestamp: 4.245500000000001e-05 + positions: [0.091, 1.102, -2.499, -0.14, 0.003, 0.003] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] +# accelerations: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + - timestamp: 0.500042926 + positions: [0.091, -0.741, -2.499, -0.14, 0.003, 0.003] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] +# accelerations: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] \ No newline at end of file diff --git a/engineer_trajectory_teaching/trajectory/transfer.yaml b/engineer_trajectory_teaching/trajectory/transfer.yaml new file mode 100644 index 0000000..461892f --- /dev/null +++ b/engineer_trajectory_teaching/trajectory/transfer.yaml @@ -0,0 +1,49 @@ +joint_names: [joint1, joint2, joint3, joint4, joint5, joint6] +points: + - timestamp: 5.4941e-05 + positions: [0.2300330971860355, 0.871326904296875, -2.407509765625, -0.09061865234374911, 0.6533129882812492, 0.6448383789062495] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + small_joint1: 0.1805 + small_joint2: -0.176 + - timestamp: 0.3 + positions: [0.37, 0.2277874755859375, -2.301079711914062, -0.034, 1.575, 0.381] + middle_pitch: 0.071 + - timestamp: 0.6 + positions: [0.53, -0.64, -2.189, -0.034, 1.575, 0.16] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + - timestamp: 0.9 + positions: [0.53, -1.281, -2.189, -0.034, 1.575, 0.16] + - timestamp: 1.2 + positions: [0.53, -2.142, -2.189, -0.034, 1.66, 0.16] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + - timestamp: 1.5 + positions: [0.53, -2.142, -2.189, -0.034, 1.66, 0.16] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + - timestamp: 1.8 + positions: [0.42, -2.142, -2.189, -0.034, 1.66, 0.16] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + small_gripper: true + transfer_gripper: true + - timestamp: 2.1 + positions: [0.42, -2.142, -2.189, -0.034, 1.66, 0.16] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + main_gripper: false + - timestamp: 2.2 + positions: [0.53, -2.142, -2.189, -0.034, 1.66, 0.16] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + - timestamp: 2.7 + positions: [0.466, -1.242, -2.16, -0.538, 1.247, -0.4] + - timestamp: 3.1 + positions: [0.323, -1.242, -2.13, -1.406, 1.103, -1.447] + main_gripper: true + - timestamp: 3.4 + positions: [0.323, -1.779, -2.13, -1.406, 0.785, -1.447] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + - timestamp: 3.8 + positions: [0.323, -1.779, -2.13, -1.406, 0.785, -1.447] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + - timestamp: 4.3 + positions: [0.4, -1.779, -2.13, -1.406, 0.785, -1.447] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + small_gripper: true + transfer_gripper: false \ No newline at end of file diff --git a/engineer_trajectory_teaching/trajectory/transfer_store_stone.yaml b/engineer_trajectory_teaching/trajectory/transfer_store_stone.yaml new file mode 100644 index 0000000..720ddeb --- /dev/null +++ b/engineer_trajectory_teaching/trajectory/transfer_store_stone.yaml @@ -0,0 +1,41 @@ +joint_names: [joint1, joint2, joint3, joint4, joint5, joint6] +points: + - timestamp: 5.4941e-05 + positions: [0.2300330971860355, 0.871326904296875, -2.407509765625, -0.09061865234374911, 0.6533129882812492, 0.6448383789062495] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + middle_pitch: 0.071 + - timestamp: 0.3 + positions: [0.41, 0.2277874755859375, -2.301079711914062, -0.034, 1.575, 0.381] + small_joint3: -0.01 + small_joint1: 0.238 + small_joint2: -0.176 + - timestamp: 0.6 + positions: [0.55, -0.64, -2.189, -0.034, 1.575, 0.16] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + - timestamp: 0.9 + positions: [0.55, -1.281, -2.189, -0.034, 1.575, -0.307] + - timestamp: 1.2 + positions: [0.55, -2.142, -2.189, -0.034, 1.66, -0.307] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + - timestamp: 1.6 + positions: [0.55, -2.142, -2.189, -0.034, 1.66, -0.307] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + - timestamp: 2.1 + positions: [0.47, -2.142, -2.189, -0.034, 1.66, -0.307] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + small_gripper: true + transfer_gripper: true + - timestamp: 2.4 + positions: [0.47, -2.142, -2.189, -0.034, 1.66, -0.307] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + main_gripper: false + - timestamp: 2.6 + positions: [0.55, -2.142, -2.189, -0.034, 1.66, -0.307] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + - timestamp: 2.9 + positions: [0.55, -1.281, -2.189, -0.034, 1.575, -0.307] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + - timestamp: 4.4 + positions: [0.09671880342092676, 1.096012573242187, -2.492577514648437, -0.0004916992187505853, -0.0009105078125003496, -0.001002441406249166] + velocities: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + small_joint2: -0.01 \ No newline at end of file