├── .gitignore ├── README.md └── system ├── control_manip ├── CMakeLists.txt ├── include │ └── control_manip │ │ ├── gripper.h │ │ ├── manager.h │ │ ├── manipulation.h │ │ ├── object.h │ │ └── utils.h ├── launch │ └── start_manip.launch ├── msg │ ├── Command.msg │ ├── Goal.msg │ ├── GoalArray.msg │ ├── JointsCmd.msg │ ├── Objects.msg │ └── Status.msg ├── package.xml ├── src │ ├── gripper.cpp │ ├── joints_node.cpp │ ├── manager.cpp │ ├── manipulation.cpp │ ├── node_manager.cpp │ ├── object.cpp │ └── utils.cpp └── srv │ ├── InitObj.srv │ ├── JointsMove.srv │ └── Move.srv ├── grasp ├── CMakeLists.txt ├── package.xml ├── setup.py ├── src │ └── grasp │ │ ├── Grasp.py │ │ ├── GraspNode.py │ │ └── __init__.py └── srv │ └── GraspRank.srv ├── predictor ├── predictor_assistance │ ├── CMakeLists.txt │ ├── package.xml │ ├── setup.py │ ├── src │ │ └── predictor_assistance │ │ │ ├── AssistancePolicy.py │ │ │ ├── AssistancePolicyOneGoal.py │ │ │ ├── AssistancePolicyOneTarget.py │ │ │ ├── GoalAssistance.py │ │ │ ├── GoalPredictorAssistance.py │ │ │ ├── HuberAssistancePolicy.py │ │ │ ├── PredictorAssistance.py │ │ │ ├── PredictorAssistanceNode.py │ │ │ ├── PrintOnFile.py │ │ │ ├── RobotAssistancePolicy.py │ │ │ ├── RobotState.py │ │ │ ├── Utils.py │ │ │ └── __init__.py │ └── srv │ │ └── AssistancePredictor.srv └── predictor_distance │ ├── CMakeLists.txt │ ├── package.xml │ ├── setup.py │ ├── src │ └── predictor_distance │ │ ├── GoalPredictor.py │ │ ├── PredictorDistance.py │ │ ├── PredictorDistanceNode.py │ │ └── __init__.py │ └── srv │ └── DistancePredictor.srv ├── shared_control ├── CMakeLists.txt ├── config │ ├── config_params.yaml │ └── potential_field.yaml ├── launch │ └── start.launch ├── msg │ └── InitPredictor.msg ├── package.xml ├── setup.py ├── src │ └── shared_control │ │ ├── Clik.py │ │ ├── Csp.py │ │ ├── Goal.py │ │ ├── PotentialFunction.py │ │ ├── PrintFile.py │ │ ├── RobotKinematics.py │ │ ├── SharedControl.py │ │ ├── SharedControlNode.py │ │ ├── UserInput.py │ │ ├── Utils.py │ │ └── __init__.py └── srv │ └── InitPred.srv └── user_input ├── joystick ├── CMakeLists.txt ├── launch │ └── joystick_control.launch ├── msg │ └── JoyCommand.msg ├── package.xml ├── setup.py └── src │ └── joystick │ ├── Joystick.py │ ├── __init__.py │ └── joystick_node.py ├── keyboard ├── CMakeLists.txt ├── launch │ └── keyboard_control.launch ├── msg │ └── KeyCommand.msg ├── package.xml ├── setup.py └── src │ └── keyboard │ ├── Keyboard.py │ ├── __init__.py │ └── keyboard_node.py └── myo ├── CMakeLists.txt ├── include └── myo │ └── myo_controller.h ├── launch └── myo_control.launch ├── msg └── MyoCommand.msg ├── package.xml ├── src ├── myo_controller.cpp └── myo_node.cpp └── srv └── ResetMyo.srv /.gitignore: -------------------------------------------------------------------------------- 1 | *.vscode 2 | *.pyc 3 | *__pycache__ 4 | stat/ 5 | stat_assist/ 6 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Improved-apf 2 | 3 | Shared Control in Robot Teleoperation With Improved Potential Fields 4 | 5 | 6 | ### Introduction 7 | Shared-control teleoperation framework based on Artificial Potential Fields improved by the generation of escape points around the obstacles. 8 | 9 | Framework is implemented in ROS and works both with Kinetic and Melodic versions. For the hardware we suggest a configuration with at least processor i7, 8GB memory and graphics cards. 10 | 11 | ### Required Python module 12 | - Numpy 13 | - Sympy 14 | - IPython 15 | - PyUserInput 16 | 17 | ### Required ROS pakages 18 | - Simulation Environment setup: https://github.com/Robin4-0/robin_arena 19 | - ps3joy: http://wiki.ros.org/ps3joy 20 | - Rosmyo: https://github.com/ste92uo/ROS_multipleMyo 21 | 22 | ### Commands 23 | 1. Start Gazebo simulation and kinect 24 | ```sh 25 | $ roslaunch robin_arena robin_bringup.launch simulation:=BOOL_SIM gripper_enable:=BOOL_GRIPPER 26 | ``` 27 | where *BOOL_SIM == true* if the simulated environment is launched; otherwise, *BOOL_SIM == false*. 28 | If *BOOL_GRIPPER == true*, the command launches UR5 with a Robotiq 3-finger gripper attached on its end-effector; otherwise, *BOOL_GRIPPER == false* launches UR5 with a magnet. 29 | 30 | 2. Start Apriltag detection 31 | ```sh 32 | $ roslaunch robin_arena apriltag.launch simulation:= BOOL 33 | ``` 34 | where *BOOL* is defined as before. 35 | 36 | 3. Start type of user control 37 | * Keyboard 38 | ```sh 39 | $ roslaunch keyboard keyboard_control.launch 40 | ``` 41 | * Joystick 42 | ```sh 43 | $ roslaunch joystick joystick_control.launch 44 | ``` 45 | * Myo 46 | ```sh 47 | $ roslaunch myo myo_control.launch 48 | ``` 49 | 4. Start system 50 | ```sh 51 | $ roslaunch shared_control start.launch sim:=BOOL_SIM gripper:=BOOL_GRIPPER grasp:=BOOL_GRASP teleop:=BOOL_TELEOP escape:=BOOL_ESCAPE dynamic:=BOOL_DYNAMIC user_type:="USER_TYPE" 52 | ``` 53 | * where *BOOL_SIM == true* if the simulated environment is launched; otherwise *BOOL_SIM == false*. 54 | * where *BOOL_GRIPPER == true* if gripper is attached on the end-effector; otherwise *BOOL_GRIPPER == false*. 55 | * where *BOOL_GRASP == true* to activate grasp routine; otherwise *BOOL_GRASP == false*. 56 | * where *BOOL_TELEOP == true* to activate teleoperation system, otherwise *BOOL_TELEOP == false* to activate shared-control system. 57 | * where *BOOL_ESCAPE == true* to activate our framework with improved APF with escape points; otherwise *BOOL_ESCAPE == false* to activate only APF shared-control system. 58 | * where *BOOL_DYNAMIC == true* to activate dynamic version of our framework; otherwise *BOOL_DYNAMIC == false* to activate static version. 59 | * where *USER_TYPE == "keyboard"* if the keyboard control is launched; *USER_TYPE == "joystick"* if the joystick control is launched; *USER_TYPE == "myo"* if myo control is launched 60 | 61 | 62 | ### Citation 63 | 64 | If you use this code in your project, please cite the associated paper: 65 | 66 | Gottardi, A., Tortora, S., Tosello, E., & Menegatti, E. (2022). Shared Control in Robot Teleoperation With Improved Potential Fields. IEEE Transactions on Human-Machine Systems. 67 | [[DOI]](https://doi.org/10.1109/THMS.2022.3155716). 68 | 69 | ```bibtex 70 | @article{gottardi2022shared, 71 | title={Shared Control in Robot Teleoperation With Improved Potential Fields}, 72 | author={Gottardi, Alberto and Tortora, Stefano and Tosello, Elisa and Menegatti, Emanuele}, 73 | journal={IEEE Transactions on Human-Machine Systems}, 74 | year={2022}, 75 | publisher={IEEE} 76 | } 77 | ``` -------------------------------------------------------------------------------- /system/control_manip/include/control_manip/gripper.h: -------------------------------------------------------------------------------- 1 | #ifndef CONTROL_MANIP_GRIPPER_H 2 | #define CONTROL_MANIP_GRIPPER_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | 10 | namespace control_manip{ 11 | 12 | class Gripper{ 13 | public: 14 | enum Status{ 15 | OPEN, 16 | CLOSE, 17 | ACTIVE, 18 | DEACTIVE, 19 | STOP 20 | }; 21 | 22 | Gripper(std::shared_ptr t_nh_ptr); 23 | ~Gripper() {} 24 | 25 | Status open(); 26 | Status close(); 27 | Status activation(); 28 | Status getStatus(); 29 | 30 | private: 31 | std::shared_ptr m_nh_ptr; 32 | ros::Publisher m_command_pub; 33 | ros::Subscriber m_status_sub; 34 | ros::Rate m_rate; 35 | 36 | robotiq_3f_gripper_articulated_msgs::Robotiq3FGripperRobotInputConstPtr m_status_ptr; 37 | robotiq_3f_gripper_articulated_msgs::Robotiq3FGripperRobotOutput m_command_msg; 38 | 39 | void configure(); 40 | 41 | }; 42 | } 43 | #endif -------------------------------------------------------------------------------- /system/control_manip/include/control_manip/manager.h: -------------------------------------------------------------------------------- 1 | #ifndef CONTROL_MANIP_MANAGER_H 2 | #define CONTROL_MANIP_MANAGER_H 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | namespace control_manip{ 18 | 19 | class Manager{ 20 | public: 21 | Manager(InputParameters t_params, 22 | std::shared_ptr t_nh_ptr, 23 | control_manip::Manipulation t_manipulation); 24 | 25 | ~Manager() {} 26 | void start(); 27 | 28 | private: 29 | bool objService(control_manip::InitObj::Request &req, control_manip::InitObj::Response &res); 30 | 31 | std::shared_ptr m_nh_ptr; 32 | ros::NodeHandle m_nh; 33 | InputParameters m_params; 34 | 35 | //Apriltag message pointer 36 | apriltag_ros::AprilTagDetectionArrayConstPtr m_msg_apriltag_ptr; 37 | 38 | ros::ServiceServer m_objects_server; 39 | Objects m_objects_msg; 40 | Manipulation m_manipulation; 41 | 42 | void callbackApriltag(const apriltag_ros::AprilTagDetectionArrayConstPtr& msg_april); 43 | ros::Publisher m_pub_objs; 44 | ros::Subscriber m_sub_april; 45 | 46 | void setObjectsParameters(); 47 | 48 | moveit_msgs::CollisionObject createWall(); 49 | 50 | void createCollisionForRviz(); 51 | 52 | moveit_msgs::CollisionObject createCollisionObject(Object object); 53 | moveit_msgs::CollisionObject createSupportCollision(Object object); 54 | 55 | 56 | }; 57 | } 58 | 59 | #endif -------------------------------------------------------------------------------- /system/control_manip/include/control_manip/manipulation.h: -------------------------------------------------------------------------------- 1 | #ifndef CONTROL_MANIP_MANIPULATION_H 2 | #define CONTROL_MANIP_MANIPULATION_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include 21 | #include 22 | 23 | namespace control_manip{ 24 | 25 | struct InputParameters{ 26 | bool simulation; 27 | std::string robot_type; 28 | bool gripper_active; 29 | bool dynamic; 30 | }; 31 | 32 | class Manipulation{ 33 | public: 34 | Manipulation(InputParameters t_params, 35 | std::shared_ptr t_nh_ptr, 36 | std::shared_ptr t_mg_ptr, 37 | std::shared_ptr t_pli_ptr, 38 | Gripper t_gripper); 39 | 40 | ~Manipulation() {} 41 | 42 | bool goHome(); 43 | bool cartesianPath(geometry_msgs::Pose next_pose); 44 | bool pick(geometry_msgs::Pose pose); 45 | bool place(geometry_msgs::Pose pose); 46 | void addCollisionObjectToScene(moveit_msgs::CollisionObject collision_object); 47 | void updateCollisionScene(std::vector collision_object_vector); 48 | void updateCollisionScene(std::vector collision_object_vector, std::vector color_object_vector); 49 | std::vector getCurrentJoint(); 50 | 51 | void visualizeMarkerEscape(std::vector escape_points); 52 | void visualizeMarkerGrasping(std::vector grasping_points); 53 | 54 | private: 55 | void configure(); 56 | bool goJoint(std::vector joints); 57 | bool goPose(geometry_msgs::Pose pose); 58 | bool moveService(control_manip::Move::Request &req, control_manip::Move::Response &res); 59 | 60 | InputParameters m_params; 61 | ros::NodeHandle m_nh; 62 | std::shared_ptr m_nh_ptr; 63 | std::shared_ptr m_move_group; 64 | std::shared_ptr m_planning_scene; 65 | moveit::planning_interface::MoveGroupInterface::Plan m_plan; 66 | std::vector m_collision_objects; 67 | ros::ServiceServer m_move_server; 68 | ros::Publisher m_pub_marker; 69 | 70 | const std::string PLANNING_GROUP = "manipulator"; 71 | const std::string UR5 = "/ur5"; 72 | const std::string UR10 = "/ur10"; 73 | const int m_max_attempt = 100; 74 | const double m_jump_threshold = 0.0; 75 | const double m_eef_step = 0.001; 76 | const bool m_avoid_collision = false; 77 | const double m_fraction_threshold = 0.8; 78 | const std::vector HOME_JOINTS_NO_GRIPPER = {-1.8419152908361127, -1.5043811821531188, 2.7216235258970656, -2.7514960178574217, -1.5684341877921888, -0.22205800161406497}; 79 | 80 | Gripper m_gripper; 81 | Gripper::Status m_gripper_status = Gripper::Status::DEACTIVE; 82 | 83 | bool serviceJointsMove(control_manip::JointsMove::Request& req, control_manip::JointsMove::Response& res); 84 | ros::Publisher pub_joints_cmd_; 85 | ros::ServiceServer joints_move_srv_; 86 | 87 | }; 88 | } 89 | 90 | #endif -------------------------------------------------------------------------------- /system/control_manip/include/control_manip/object.h: -------------------------------------------------------------------------------- 1 | #ifndef CONTROL_MANIP_OBJECT__H 2 | #define CONTROL_MANIP_OBJECT__H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | namespace control_manip{ 12 | 13 | class Object{ 14 | public: 15 | Object(int obj_id, geometry_msgs::PoseStamped apriltagDetectedPose); 16 | ~Object() {} 17 | 18 | int getId(); 19 | int getType(); 20 | std::string getName(); 21 | geometry_msgs::Pose getCenter(); 22 | std::vector getDimension(); 23 | 24 | moveit_msgs::CollisionObject getCollisionObject(); 25 | geometry_msgs::PoseArray getObstacle(); 26 | geometry_msgs::PoseArray getEscapePoints(); 27 | 28 | void createGraspingPoints(std::string frame_id); 29 | std::vector getGraspingPoints(); 30 | visualization_msgs::Marker visualizeGraspingPoints(); 31 | 32 | geometry_msgs::PoseArray getSupport(); 33 | 34 | private: 35 | int id; //tag's ID 36 | int type; //type: cube = 0, cyl_prism = 1, triangle = 3 37 | std::string name; //object name 38 | geometry_msgs::Pose center; //object's center 39 | double width; 40 | double length; 41 | double height; 42 | double radius; //only for cylinder 43 | double my_yaw; 44 | const std::string WORLD_FRAME = "world"; 45 | 46 | moveit_msgs::CollisionObject collision; 47 | geometry_msgs::PoseArray obstacle; 48 | geometry_msgs::PoseArray escape_points; 49 | 50 | std::vector grasping_points_list; 51 | visualization_msgs::Marker marker_point; 52 | 53 | void configure(geometry_msgs::PoseStamped apriltagDetectedPose); 54 | geometry_msgs::PoseStamped transform(geometry_msgs::PoseStamped input_pose, std::string out_frame); 55 | void setCenter(geometry_msgs::PoseStamped pose); 56 | geometry_msgs::Quaternion setQuaternion(int index); 57 | void createTargetCollisionObject(std::string frame_id); 58 | void createObstacle(); 59 | void createObstacleCollisionObject(std::string frame_id); 60 | void createEscapePoints(); 61 | 62 | void createSupport(); 63 | geometry_msgs::PoseArray support_points; 64 | 65 | }; 66 | } 67 | 68 | #endif 69 | 70 | -------------------------------------------------------------------------------- /system/control_manip/include/control_manip/utils.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | 21 | 22 | namespace control_manip{ 23 | namespace Utils{ 24 | double computeDistance(geometry_msgs::Pose from, geometry_msgs::Pose to); 25 | geometry_msgs::Pose createPose(double x, double y, double z, double ox, double oy, double oz, double ow); 26 | bool compare(double a, double b, double difference); 27 | Goal createGoalMsg(int id, geometry_msgs::Pose center, std::vector grasping_points); 28 | GoalArray createGoalArrayMsg(std::vector goals); 29 | Objects createObjectsMsg(std::vector goals, std::vector obstacles, std::vector joints, std::vector escape_points); 30 | } 31 | } -------------------------------------------------------------------------------- /system/control_manip/launch/start_manip.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /system/control_manip/msg/Command.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | uint8 HOME = 0 4 | uint8 MOVE = 1 5 | uint8 PICK = 2 6 | uint8 PLACE = 3 7 | uint8 FINISH = 4 8 | 9 | uint8 command 10 | geometry_msgs/Pose to_pose 11 | -------------------------------------------------------------------------------- /system/control_manip/msg/Goal.msg: -------------------------------------------------------------------------------- 1 | int32 id 2 | geometry_msgs/Pose center 3 | geometry_msgs/PoseStamped[] grasping_points 4 | -------------------------------------------------------------------------------- /system/control_manip/msg/GoalArray.msg: -------------------------------------------------------------------------------- 1 | Goal[] goal 2 | -------------------------------------------------------------------------------- /system/control_manip/msg/JointsCmd.msg: -------------------------------------------------------------------------------- 1 | trajectory_msgs/JointTrajectory trajectory 2 | bool publish -------------------------------------------------------------------------------- /system/control_manip/msg/Objects.msg: -------------------------------------------------------------------------------- 1 | GoalArray goals 2 | geometry_msgs/PoseArray obstacles 3 | trajectory_msgs/JointTrajectoryPoint joints 4 | geometry_msgs/PoseArray escape_points 5 | -------------------------------------------------------------------------------- /system/control_manip/msg/Status.msg: -------------------------------------------------------------------------------- 1 | bool ready 2 | -------------------------------------------------------------------------------- /system/control_manip/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | control_manip 4 | 0.0.2 5 | The control_manip package 6 | 7 | 8 | 9 | 10 | Alberto Gottardi 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | Alberto Gottardi 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | geometry_msgs 53 | roscpp 54 | std_msgs 55 | geometry_msgs 56 | roscpp 57 | std_msgs 58 | geometry_msgs 59 | roscpp 60 | std_msgs 61 | 62 | robotiq_3f_gripper_articulated_msgs 63 | robotiq_3f_gripper_articulated_msgs 64 | robotiq_3f_gripper_articulated_msgs 65 | 66 | apriltag_ros 67 | message_generation 68 | message_runtime 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | -------------------------------------------------------------------------------- /system/control_manip/src/gripper.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | /** 4 | * Costructor of Gripper Class 5 | * @param t_nh_ptr NodeHandle Ptr 6 | */ 7 | control_manip::Gripper::Gripper(std::shared_ptr t_nh_ptr) 8 | : m_nh_ptr(t_nh_ptr) 9 | , m_rate(1) 10 | { 11 | configure(); 12 | } 13 | 14 | 15 | /** 16 | * Configure publisher to Activate, Open and Close gripper 17 | */ 18 | void control_manip::Gripper::configure(){ 19 | m_command_pub = m_nh_ptr->advertise("/robotiq_hands/l_hand/Robotiq3FGripperRobotOutput", 1); 20 | } 21 | 22 | 23 | /** 24 | * Activate gripper 25 | * @return Status::ACTIVE if it is active, Status::STOP if activation fails. 26 | */ 27 | control_manip::Gripper::Status control_manip::Gripper::activation(){ 28 | m_command_msg.rACT = 1; 29 | m_command_pub.publish(m_command_msg); 30 | 31 | m_status_ptr = ros::topic::waitForMessage("/robotiq_hands/l_hand/Robotiq3FGripperRobotInput"); 32 | while(m_status_ptr->gIMC != 3){ 33 | if((m_status_ptr->gSTA == 1) || (m_status_ptr->gSTA == 2)){ 34 | ROS_ERROR_STREAM("ACTIVE GRIPPER ERROR: FINGERS STOPPED BEFORE REQUESTED POSITION"); 35 | return control_manip::Gripper::Status::STOP; 36 | } 37 | m_status_ptr = ros::topic::waitForMessage("/robotiq_hands/l_hand/Robotiq3FGripperRobotInput"); 38 | m_rate.sleep(); 39 | } 40 | 41 | return control_manip::Gripper::Status::ACTIVE; 42 | } 43 | 44 | 45 | /** 46 | * Open gripper 47 | * @return Status::OPEN if it is open, Status::STOP if opening fails. 48 | */ 49 | control_manip::Gripper::Status control_manip::Gripper::open(){ 50 | m_command_msg.rACT = 1; 51 | m_command_msg.rGTO = 1; 52 | m_command_msg.rPRA = 0; 53 | m_command_msg.rFRA = 0; 54 | m_command_msg.rSPA = 200; 55 | m_command_pub.publish(m_command_msg); 56 | 57 | m_status_ptr = ros::topic::waitForMessage("/robotiq_hands/l_hand/Robotiq3FGripperRobotInput"); 58 | while((m_status_ptr->gPRA != 0) && (m_status_ptr->gSTA != 3)){ 59 | if((m_status_ptr->gSTA == 1) || (m_status_ptr->gSTA == 2)){ 60 | ROS_ERROR_STREAM("OPEN GRIPPER ERROR: FINGERS STOPPED BEFORE REQUESTED POSITION"); 61 | return control_manip::Gripper::Status::STOP; 62 | } 63 | 64 | m_status_ptr = ros::topic::waitForMessage("/robotiq_hands/l_hand/Robotiq3FGripperRobotInput"); 65 | m_rate.sleep(); 66 | } 67 | 68 | ros::Duration(2.0).sleep(); 69 | 70 | return control_manip::Gripper::Status::OPEN; 71 | } 72 | 73 | 74 | /** 75 | * Close gripper 76 | * @return Status::CLOSE if it is closed, Status::STOP if closure fails. 77 | */ 78 | control_manip::Gripper::Status control_manip::Gripper::close(){ 79 | m_command_msg.rACT = 1; 80 | m_command_msg.rGTO = 1; 81 | m_command_msg.rPRA = 250; 82 | m_command_msg.rSPA = 200; 83 | m_command_msg.rFRA = 200; 84 | m_command_pub.publish(m_command_msg); 85 | 86 | m_status_ptr = ros::topic::waitForMessage("/robotiq_hands/l_hand/Robotiq3FGripperRobotInput"); 87 | while((m_status_ptr->gPRA != 250) && (m_status_ptr->gSTA != 3)){ 88 | if((m_status_ptr->gSTA == 1) || (m_status_ptr->gSTA == 2)){ 89 | ROS_ERROR_STREAM("CLOSE GRIPPER ERROR: FINGERS STOPPED BEFORE REQUESTED POSITION"); 90 | return control_manip::Gripper::Status::STOP; 91 | } 92 | 93 | m_status_ptr = ros::topic::waitForMessage("/robotiq_hands/l_hand/Robotiq3FGripperRobotInput"); 94 | m_rate.sleep(); 95 | } 96 | 97 | ros::Duration(2.0).sleep(); 98 | 99 | return control_manip::Gripper::Status::CLOSE; 100 | } 101 | 102 | 103 | /** 104 | * Get status of the gripper 105 | * @return actual Status: DEACTIVE, OPEN or CLOSE 106 | */ 107 | control_manip::Gripper::Status control_manip::Gripper::getStatus(){ 108 | m_status_ptr = ros::topic::waitForMessage("/robotiq_hands/l_hand/Robotiq3FGripperRobotInput"); 109 | 110 | if(m_status_ptr->gACT == 0){ 111 | return control_manip::Gripper::Status::DEACTIVE; 112 | } 113 | 114 | //gPRA = 0 gripper is open, gPRA = 250 gripper is close 115 | if(m_status_ptr->gPRA == 250){ 116 | return control_manip::Gripper::Status::CLOSE; 117 | } 118 | 119 | return control_manip::Gripper::Status::OPEN; 120 | } 121 | 122 | -------------------------------------------------------------------------------- /system/control_manip/src/joints_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | trajectory_msgs::JointTrajectory joints_msgs_; 7 | bool publish_ = false; 8 | 9 | void callbackJoints(const control_manip::JointsCmdConstPtr& msg){ 10 | joints_msgs_ = msg->trajectory; 11 | publish_ = msg->publish; 12 | } 13 | 14 | int main(int argc, char **argv){ 15 | ros::init(argc, argv, "joints_node"); 16 | auto nh = std::make_shared("~"); 17 | 18 | ros::Subscriber sub_joints_cmd = nh->subscribe("/cmd_joints", 1, callbackJoints); 19 | ros::Publisher pub_joints_cmd = nh->advertise("/scaled_pos_joint_traj_controller/command", 10); 20 | 21 | ros::Rate rate(20); 22 | while(ros::ok()){ 23 | if(publish_){ 24 | pub_joints_cmd.publish(joints_msgs_); 25 | } 26 | ros::spinOnce(); 27 | rate.sleep(); 28 | } 29 | 30 | return EXIT_SUCCESS; 31 | } -------------------------------------------------------------------------------- /system/control_manip/src/node_manager.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | int main(int argc, char **argv){ 8 | 9 | if (ros::console::set_logger_level(ROSCONSOLE_ROOT_LOGGER_NAME, ros::console::levels::Error)){ 10 | ros::console::notifyLoggerLevelsChanged(); 11 | } 12 | 13 | ros::init(argc, argv, "control_manipulator_node"); 14 | auto nh = std::make_shared("~"); 15 | 16 | ros::AsyncSpinner spinner(3); 17 | spinner.start(); 18 | 19 | control_manip::InputParameters params; 20 | nh->getParam("sim", params.simulation); 21 | nh->getParam("gripper", params.gripper_active); 22 | nh->getParam("robot_type", params.robot_type); 23 | nh->getParam("dynamic", params.dynamic); 24 | 25 | auto move_group_manager = std::make_shared("manipulator"); 26 | auto planning_scene_manager = std::make_shared(); 27 | 28 | control_manip::Gripper gripper(nh); 29 | 30 | control_manip::Manipulation manipulation(params, nh, 31 | move_group_manager, 32 | planning_scene_manager, 33 | gripper); 34 | 35 | control_manip::Manager manager(params, nh, manipulation); 36 | manager.start(); 37 | 38 | ros::waitForShutdown(); 39 | return 0; 40 | } -------------------------------------------------------------------------------- /system/control_manip/src/utils.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | /** 4 | * Compute distance between two points 5 | * @param a point 6 | * @param b point 7 | * @return distance from a to b 8 | */ 9 | double control_manip::Utils::computeDistance(geometry_msgs::Pose a, geometry_msgs::Pose b){ 10 | double x_a = a.position.x; 11 | double y_a = a.position.y; 12 | double z_a = a.position.z; 13 | double x_b = b.position.x; 14 | double y_b = b.position.y; 15 | double z_b = b.position.z; 16 | double distance = sqrt(pow(x_b - x_a, 2) + pow(y_b - y_a, 2) + pow(z_b - z_a, 2)); 17 | return distance; 18 | } 19 | 20 | /** 21 | * Create geometry_msgs Pose 22 | * @param (x,y,z) position 23 | * @param (ox, oy, oz, ow) orientation 24 | * @return Pose msg 25 | */ 26 | geometry_msgs::Pose control_manip::Utils::createPose(double x, double y, double z, double ox, double oy, double oz, double ow){ 27 | geometry_msgs::Pose pose_msg; 28 | pose_msg.position.x = x; 29 | pose_msg.position.y = y; 30 | pose_msg.position.z = z; 31 | pose_msg.orientation.x = ox; 32 | pose_msg.orientation.y = oy; 33 | pose_msg.orientation.z = oz; 34 | pose_msg.orientation.w = ow; 35 | return pose_msg; 36 | } 37 | 38 | /** 39 | * Compare two number 40 | * @param a 41 | * @param b 42 | * @param difference threshold difference 43 | * @return true if abs(a-b) < difference, false otherwise 44 | */ 45 | bool control_manip::Utils::compare(double a, double b, double difference){ 46 | if(std::fabs(a - b) > difference){ 47 | return false; 48 | } 49 | return true; 50 | } 51 | 52 | 53 | /** 54 | * Create Goal message 55 | * @param id id of goal 56 | * @param center center of the goal 57 | * @return Goal msg 58 | */ 59 | control_manip::Goal control_manip::Utils::createGoalMsg(int id, geometry_msgs::Pose center, std::vector grasping_points){ 60 | Goal goal_msg; 61 | goal_msg.id = id; 62 | goal_msg.center = center; 63 | goal_msg.grasping_points = grasping_points; 64 | return goal_msg; 65 | } 66 | 67 | /** 68 | * Create GoalArray message 69 | * @param goals array of goal 70 | * @return GoalArray msg 71 | */ 72 | control_manip::GoalArray control_manip::Utils::createGoalArrayMsg(std::vector goals){ 73 | GoalArray goal_array_msg; 74 | goal_array_msg.goal = goals; 75 | return goal_array_msg; 76 | } 77 | 78 | /** 79 | * Create Objects message 80 | * @param goals array of goals 81 | * @param obstacles array of obstacles 82 | * @param joints current joints value 83 | * @param escape_points array of escape points 84 | * @return Objects msg 85 | */ 86 | control_manip::Objects control_manip::Utils::createObjectsMsg(std::vector goals, std::vector obstacles, std::vector joints, std::vector escape_points){ 87 | Objects objects_msg; 88 | objects_msg.goals = createGoalArrayMsg(goals); 89 | //Add obstacles points 90 | for(int i = 0; i < obstacles.size(); i++){ 91 | for(int j = 0; j < obstacles.at(i).poses.size(); j++){ 92 | objects_msg.obstacles.poses.push_back(obstacles.at(i).poses.at(j)); 93 | } 94 | } 95 | //Add joints value 96 | for(int i = 0; i < joints.size(); i++){ 97 | objects_msg.joints.positions.push_back(joints.at(i)); 98 | } 99 | //Add escape points 100 | for(int i = 0; i < escape_points.size(); i++){ 101 | for(int j = 0; j < escape_points.at(i).poses.size(); j++){ 102 | objects_msg.escape_points.poses.push_back(escape_points.at(i).poses.at(j)); 103 | } 104 | } 105 | 106 | return objects_msg; 107 | } 108 | 109 | 110 | -------------------------------------------------------------------------------- /system/control_manip/srv/InitObj.srv: -------------------------------------------------------------------------------- 1 | control_manip/Status status 2 | --- 3 | control_manip/Objects objects -------------------------------------------------------------------------------- /system/control_manip/srv/JointsMove.srv: -------------------------------------------------------------------------------- 1 | trajectory_msgs/JointTrajectory trajectory 2 | --- 3 | bool done -------------------------------------------------------------------------------- /system/control_manip/srv/Move.srv: -------------------------------------------------------------------------------- 1 | control_manip/Command command 2 | --- 3 | bool done -------------------------------------------------------------------------------- /system/grasp/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(grasp) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | geometry_msgs 12 | rospy 13 | std_msgs 14 | message_generation 15 | control_manip 16 | trajectory_msgs 17 | ) 18 | 19 | ## System dependencies are found with CMake's conventions 20 | # find_package(Boost REQUIRED COMPONENTS system) 21 | 22 | 23 | ## Uncomment this if the package has a setup.py. This macro ensures 24 | ## modules and global scripts declared therein get installed 25 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 26 | catkin_python_setup() 27 | 28 | ################################################ 29 | ## Declare ROS messages, services and actions ## 30 | ################################################ 31 | 32 | ## To declare and build messages, services or actions from within this 33 | ## package, follow these steps: 34 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 35 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 36 | ## * In the file package.xml: 37 | ## * add a build_depend tag for "message_generation" 38 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 39 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 40 | ## but can be declared for certainty nonetheless: 41 | ## * add a exec_depend tag for "message_runtime" 42 | ## * In this file (CMakeLists.txt): 43 | ## * add "message_generation" and every package in MSG_DEP_SET to 44 | ## find_package(catkin REQUIRED COMPONENTS ...) 45 | ## * add "message_runtime" and every package in MSG_DEP_SET to 46 | ## catkin_package(CATKIN_DEPENDS ...) 47 | ## * uncomment the add_*_files sections below as needed 48 | ## and list every .msg/.srv/.action file to be processed 49 | ## * uncomment the generate_messages entry below 50 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 51 | 52 | ## Generate messages in the 'msg' folder 53 | # add_message_files( 54 | # FILES 55 | # Message1.msg 56 | # Message2.msg 57 | # ) 58 | 59 | ## Generate services in the 'srv' folder 60 | add_service_files( 61 | FILES 62 | GraspRank.srv 63 | ) 64 | 65 | ## Generate actions in the 'action' folder 66 | # add_action_files( 67 | # FILES 68 | # Action1.action 69 | # Action2.action 70 | # ) 71 | 72 | ## Generate added messages and services with any dependencies listed here 73 | generate_messages( 74 | DEPENDENCIES 75 | geometry_msgs 76 | std_msgs 77 | control_manip 78 | trajectory_msgs 79 | ) 80 | 81 | ################################################ 82 | ## Declare ROS dynamic reconfigure parameters ## 83 | ################################################ 84 | 85 | ## To declare and build dynamic reconfigure parameters within this 86 | ## package, follow these steps: 87 | ## * In the file package.xml: 88 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 89 | ## * In this file (CMakeLists.txt): 90 | ## * add "dynamic_reconfigure" to 91 | ## find_package(catkin REQUIRED COMPONENTS ...) 92 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 93 | ## and list every .cfg file to be processed 94 | 95 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 96 | # generate_dynamic_reconfigure_options( 97 | # cfg/DynReconf1.cfg 98 | # cfg/DynReconf2.cfg 99 | # ) 100 | 101 | ################################### 102 | ## catkin specific configuration ## 103 | ################################### 104 | ## The catkin_package macro generates cmake config files for your package 105 | ## Declare things to be passed to dependent projects 106 | ## INCLUDE_DIRS: uncomment this if your package contains header files 107 | ## LIBRARIES: libraries you create in this project that dependent projects also need 108 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 109 | ## DEPENDS: system dependencies of this project that dependent projects also need 110 | catkin_package( 111 | # INCLUDE_DIRS include 112 | # LIBRARIES grasp 113 | CATKIN_DEPENDS geometry_msgs rospy std_msgs trajectory_msgs message_runtime 114 | # DEPENDS system_lib 115 | ) 116 | 117 | ########### 118 | ## Build ## 119 | ########### 120 | 121 | ## Specify additional locations of header files 122 | ## Your package locations should be listed before other locations 123 | include_directories( 124 | # include 125 | ${catkin_INCLUDE_DIRS} 126 | ) 127 | 128 | ## Declare a C++ library 129 | # add_library(${PROJECT_NAME} 130 | # src/${PROJECT_NAME}/grasp.cpp 131 | # ) 132 | 133 | ## Add cmake target dependencies of the library 134 | ## as an example, code may need to be generated before libraries 135 | ## either from message generation or dynamic reconfigure 136 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 137 | 138 | ## Declare a C++ executable 139 | ## With catkin_make all packages are built within a single CMake context 140 | ## The recommended prefix ensures that target names across packages don't collide 141 | # add_executable(${PROJECT_NAME}_node src/grasp_node.cpp) 142 | 143 | ## Rename C++ executable without prefix 144 | ## The above recommended prefix causes long target names, the following renames the 145 | ## target back to the shorter version for ease of user use 146 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 147 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 148 | 149 | ## Add cmake target dependencies of the executable 150 | ## same as for the library above 151 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 152 | 153 | ## Specify libraries to link a library or executable target against 154 | # target_link_libraries(${PROJECT_NAME}_node 155 | # ${catkin_LIBRARIES} 156 | # ) 157 | 158 | ############# 159 | ## Install ## 160 | ############# 161 | 162 | # all install targets should use catkin DESTINATION variables 163 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 164 | 165 | ## Mark executable scripts (Python etc.) for installation 166 | ## in contrast to setup.py, you can choose the destination 167 | # catkin_install_python(PROGRAMS 168 | # scripts/my_python_script 169 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 170 | # ) 171 | 172 | ## Mark executables for installation 173 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 174 | # install(TARGETS ${PROJECT_NAME}_node 175 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 176 | # ) 177 | 178 | ## Mark libraries for installation 179 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 180 | # install(TARGETS ${PROJECT_NAME} 181 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 182 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 183 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 184 | # ) 185 | 186 | ## Mark cpp header files for installation 187 | # install(DIRECTORY include/${PROJECT_NAME}/ 188 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 189 | # FILES_MATCHING PATTERN "*.h" 190 | # PATTERN ".svn" EXCLUDE 191 | # ) 192 | 193 | ## Mark other files for installation (e.g. launch and bag files, etc.) 194 | # install(FILES 195 | # # myfile1 196 | # # myfile2 197 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 198 | # ) 199 | 200 | ############# 201 | ## Testing ## 202 | ############# 203 | 204 | ## Add gtest based cpp test target and link libraries 205 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_grasp.cpp) 206 | # if(TARGET ${PROJECT_NAME}-test) 207 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 208 | # endif() 209 | 210 | ## Add folders to be run by python nosetests 211 | # catkin_add_nosetests(test) 212 | -------------------------------------------------------------------------------- /system/grasp/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | grasp 4 | 0.0.2 5 | The grasp package 6 | 7 | 8 | 9 | 10 | Alberto Gottardi 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | geometry_msgs 53 | rospy 54 | std_msgs 55 | trajectory_msgs 56 | geometry_msgs 57 | rospy 58 | std_msgs 59 | trajectory_msgs 60 | geometry_msgs 61 | rospy 62 | std_msgs 63 | trajectory_msgs 64 | 65 | message_generation 66 | message_runtime 67 | 68 | control_manip 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | -------------------------------------------------------------------------------- /system/grasp/setup.py: -------------------------------------------------------------------------------- 1 | from distutils.core import setup 2 | from catkin_pkg.python_setup import generate_distutils_setup 3 | 4 | d = generate_distutils_setup( 5 | packages = ['grasp'], 6 | package_dir = {'': 'src'} 7 | ) 8 | 9 | setup(**d) 10 | -------------------------------------------------------------------------------- /system/grasp/src/grasp/Grasp.py: -------------------------------------------------------------------------------- 1 | #coding=utf-8 2 | 3 | import rospy 4 | import numpy as np 5 | import math 6 | import sys 7 | from geometry_msgs.msg import PoseStamped, Pose 8 | from grasp.srv import GraspRank, GraspRankResponse 9 | 10 | class Grasp: 11 | """ 12 | Grasp class \n 13 | """ 14 | def __init__(self): 15 | self._goal_center = None 16 | self._pose_ee = None 17 | self._rank = None 18 | self._service_grasp = rospy.Service("grasp_srv", GraspRank, self.handleGrasp) 19 | 20 | 21 | def handleGrasp(self, request): 22 | """ 23 | Service that compute grasping points rank \n 24 | Args: 25 | request: request of service 26 | Return: response 27 | """ 28 | self._goal_center = request.goal.center 29 | self._pose_ee = request.ee_pose 30 | self.computeRank() 31 | grasping_points = list() 32 | for grasp_point in self._rank[1]: 33 | grasping_points.append(grasp_point) 34 | return GraspRankResponse(grasping_points) 35 | 36 | 37 | def createGraspingPoint(self): 38 | """ 39 | Create Grasping Points for cube \n 40 | Return: grasping points 41 | position: list of numpy array of position 42 | orientation: list of numpy array of orientation 43 | """ 44 | #Cube dimension 45 | height = width = length = 0.1 46 | 47 | #List of position and orientation 48 | position = list() 49 | orientation = list() 50 | 51 | #polar coordinate 52 | rho = 0.17 #0.061 53 | theta = 0 54 | #num points for each cube face 55 | num_point = 3 56 | 57 | #Center coordinate 58 | x_c = self._goal_center.position.x 59 | y_c = self._goal_center.position.y 60 | z_c = self._goal_center.position.z 61 | 62 | #Compute Grasping Points 63 | #Top obj 64 | point_p = np.array([x_c, y_c, z_c + rho])#rho + 0.009]) #+ 0.035 in x,y 65 | position.append(point_p) 66 | point_o = self.setOrientation(0) 67 | orientation.append(point_o) 68 | 69 | ''' 70 | #Other points 71 | for h in range(num_point): 72 | index_p = 1 73 | while(theta < 2*math.pi): 74 | x = self._goal_center.position.x + rho * math.cos(theta) 75 | y = self._goal_center.position.y + rho * math.sin(theta) 76 | z = self._goal_center.position.z + (height/2)*(float(h)/float(num_point)) 77 | point_p = np.array([x, y, z]) 78 | point_o = self.setOrientation(index_p) 79 | 80 | position.append(point_p) 81 | orientation.append(point_o) 82 | 83 | #Updates 84 | theta += math.pi/2 85 | index_p += 1 86 | 87 | theta = 0 88 | ''' 89 | 90 | return position, orientation 91 | 92 | 93 | def setOrientation(self, value): 94 | """ 95 | Set orientation from default value \n 96 | Args: 97 | value: number of cube face 98 | Return: quaternion orientation of point 99 | """ 100 | #Set orientation 101 | if(value == 0): #Top 102 | 103 | 104 | #orientation = np.array([0.7177, 0.0104, -0.6957, 0.0258]) 105 | 106 | orientation = np.array([0.515299, 0.478671, -0.50345, 0.501875]) 107 | 108 | #orientation = np.array([0.702307, -0.110643, -0.69849, -0.0814571]) 109 | #orientation = np.array([-0.719112, 0.0213112, 0.694439, 0.0133605]) 110 | 111 | elif(value == 1): #Front 112 | orientation = np.array([0.00928103, -0.999851, -0.0130359, 0.00649344]) 113 | 114 | elif(value == 2): #Right side 115 | orientation = np.array([0.697931, -0.716051, -0.00507108, 0.0117258]) 116 | 117 | elif(value == 3): #Behind 118 | orientation = np.array([-0.00282146, 0.0156438, 0.0843281, 0.996311]) 119 | 120 | elif(value == 4): #Left side 121 | orientation = np.array([0.00628831, 0.0144797, 0.718782, 0.695056]) 122 | 123 | else: 124 | rospy.logerr("Index not valid!") 125 | rospy.on_shutdown(self.reason) 126 | 127 | return orientation 128 | 129 | 130 | def computeDistance(self, from_ee, to_point): 131 | """ 132 | Compute distance from actual ee to grasp point \n 133 | Args: 134 | from_ee: ee position/orientation 135 | to_point: grasp point position/orientation 136 | Return: distance from ee to grasp point 137 | """ 138 | from_ee = np.array(from_ee) 139 | to_point = np.array(to_point) 140 | distance = np.linalg.norm(from_ee - to_point) 141 | return distance 142 | 143 | 144 | def computeScore(self, point_p, point_o, ee_p, ee_o): 145 | """ 146 | Compute score of a grasping point p \n 147 | Args: all args are numpy vector 148 | point_p: position of point 149 | point_o: orientation of point 150 | ee_p: ee position 151 | ee_o: ee orientation 152 | Return: score of point p 153 | """ 154 | dist_p = self.computeDistance(ee_p, point_p) 155 | dist_o = self.computeDistance(ee_o, point_o) 156 | 157 | score = (1./dist_p) + (1./dist_o) 158 | 159 | return score 160 | 161 | 162 | def computeRank(self): 163 | """ 164 | Use score function to compute rank of the grasping points \n 165 | """ 166 | #Create grasping points 167 | position, orientation = self.createGraspingPoint() 168 | num_points = len(position) 169 | 170 | ee_position = np.array([self._pose_ee.position.x, self._pose_ee.position.y, self._pose_ee.position.z]) 171 | ee_orientation = np.array([self._pose_ee.orientation.x, self._pose_ee.orientation.y, self._pose_ee.orientation.z, self._pose_ee.orientation.w]) 172 | 173 | #Compute rank 174 | disorder_rank = list() 175 | grasping_list = list() 176 | for i in range(num_points): 177 | score = self.computeScore(position[i], orientation[i], ee_position, ee_orientation) 178 | disorder_rank.append(score) 179 | pose_msg = self.createPoseStampedMsg(i, position[i], orientation[i]) 180 | grasping_list.append(pose_msg) 181 | 182 | #Create list(score, pose) 183 | list_score = zip(disorder_rank, grasping_list) 184 | list_score.sort(reverse=True) 185 | #Ordered list 186 | self._rank = zip(*list_score) 187 | 188 | 189 | def createPoseStampedMsg(self, id_p, position, orientation): 190 | """ 191 | Create PoseStamped message \n 192 | Args: 193 | id_p: point ID 194 | position: position numpy array 195 | orientation: orientation numpy array 196 | Return: PoseStamped message 197 | """ 198 | pose_msg = PoseStamped() 199 | pose_msg.header.seq = id_p 200 | pose_msg.header.frame_id = "world" 201 | pose_msg.pose.position.x = position[0] 202 | pose_msg.pose.position.y = position[1] 203 | pose_msg.pose.position.z = position[2] 204 | pose_msg.pose.orientation.x = orientation[0] 205 | pose_msg.pose.orientation.y = orientation[1] 206 | pose_msg.pose.orientation.z = orientation[2] 207 | pose_msg.pose.orientation.w = orientation[3] 208 | return pose_msg 209 | 210 | 211 | def getRank(self): 212 | """ 213 | Get rank \n 214 | Return rank 215 | """ 216 | return self._rank 217 | 218 | 219 | def getGoalCenter(self): 220 | """ 221 | Get center of the goal \n 222 | Return center 223 | """ 224 | return self._goal_center 225 | 226 | 227 | def getEEPose(self): 228 | """ 229 | Get EE pose \n 230 | Return: EE pose 231 | """ 232 | return self._pose_ee 233 | 234 | 235 | def reason(self): 236 | rospy.logerr("Error: Index not valid! Wait to shutdown!") 237 | 238 | -------------------------------------------------------------------------------- /system/grasp/src/grasp/GraspNode.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | #coding=utf-8 3 | 4 | import rospy 5 | from geometry_msgs.msg import PoseStamped, Pose 6 | from control_manip.msg import Goal as goalMsg 7 | import Grasp 8 | import sys 9 | 10 | if __name__ == "__main__": 11 | rospy.init_node("grasping_points_node", anonymous=True) 12 | grasp = Grasp.Grasp() 13 | 14 | rospy.spin() 15 | 16 | -------------------------------------------------------------------------------- /system/grasp/src/grasp/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Shared-control/improved-apf/85ab7ef5f043752794c52e80f4d1d19e10ef87ff/system/grasp/src/grasp/__init__.py -------------------------------------------------------------------------------- /system/grasp/srv/GraspRank.srv: -------------------------------------------------------------------------------- 1 | control_manip/Goal goal 2 | geometry_msgs/Pose ee_pose 3 | --- 4 | geometry_msgs/PoseStamped[] rank 5 | -------------------------------------------------------------------------------- /system/predictor/predictor_assistance/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(predictor_assistance) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | geometry_msgs 12 | rospy 13 | std_msgs 14 | message_generation 15 | shared_control 16 | ) 17 | 18 | ## System dependencies are found with CMake's conventions 19 | # find_package(Boost REQUIRED COMPONENTS system) 20 | 21 | 22 | ## Uncomment this if the package has a setup.py. This macro ensures 23 | ## modules and global scripts declared therein get installed 24 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 25 | catkin_python_setup() 26 | 27 | ################################################ 28 | ## Declare ROS messages, services and actions ## 29 | ################################################ 30 | 31 | ## To declare and build messages, services or actions from within this 32 | ## package, follow these steps: 33 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 34 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 35 | ## * In the file package.xml: 36 | ## * add a build_depend tag for "message_generation" 37 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 38 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 39 | ## but can be declared for certainty nonetheless: 40 | ## * add a exec_depend tag for "message_runtime" 41 | ## * In this file (CMakeLists.txt): 42 | ## * add "message_generation" and every package in MSG_DEP_SET to 43 | ## find_package(catkin REQUIRED COMPONENTS ...) 44 | ## * add "message_runtime" and every package in MSG_DEP_SET to 45 | ## catkin_package(CATKIN_DEPENDS ...) 46 | ## * uncomment the add_*_files sections below as needed 47 | ## and list every .msg/.srv/.action file to be processed 48 | ## * uncomment the generate_messages entry below 49 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 50 | 51 | ## Generate messages in the 'msg' folder 52 | # add_message_files( 53 | # FILES 54 | # Message1.msg 55 | # Message2.msg 56 | # ) 57 | 58 | ## Generate services in the 'srv' folder 59 | add_service_files( 60 | FILES 61 | AssistancePredictor.srv 62 | ) 63 | 64 | ## Generate actions in the 'action' folder 65 | # add_action_files( 66 | # FILES 67 | # Action1.action 68 | # Action2.action 69 | # ) 70 | 71 | ## Generate added messages and services with any dependencies listed here 72 | generate_messages( 73 | DEPENDENCIES 74 | geometry_msgs 75 | std_msgs 76 | ) 77 | 78 | ################################################ 79 | ## Declare ROS dynamic reconfigure parameters ## 80 | ################################################ 81 | 82 | ## To declare and build dynamic reconfigure parameters within this 83 | ## package, follow these steps: 84 | ## * In the file package.xml: 85 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 86 | ## * In this file (CMakeLists.txt): 87 | ## * add "dynamic_reconfigure" to 88 | ## find_package(catkin REQUIRED COMPONENTS ...) 89 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 90 | ## and list every .cfg file to be processed 91 | 92 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 93 | # generate_dynamic_reconfigure_options( 94 | # cfg/DynReconf1.cfg 95 | # cfg/DynReconf2.cfg 96 | # ) 97 | 98 | ################################### 99 | ## catkin specific configuration ## 100 | ################################### 101 | ## The catkin_package macro generates cmake config files for your package 102 | ## Declare things to be passed to dependent projects 103 | ## INCLUDE_DIRS: uncomment this if your package contains header files 104 | ## LIBRARIES: libraries you create in this project that dependent projects also need 105 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 106 | ## DEPENDS: system dependencies of this project that dependent projects also need 107 | catkin_package( 108 | # INCLUDE_DIRS include 109 | # LIBRARIES predictor_assistance 110 | CATKIN_DEPENDS geometry_msgs rospy std_msgs message_runtime 111 | # DEPENDS system_lib 112 | ) 113 | 114 | ########### 115 | ## Build ## 116 | ########### 117 | 118 | ## Specify additional locations of header files 119 | ## Your package locations should be listed before other locations 120 | include_directories( 121 | # include 122 | ${catkin_INCLUDE_DIRS} 123 | ) 124 | 125 | ## Declare a C++ library 126 | # add_library(${PROJECT_NAME} 127 | # src/${PROJECT_NAME}/predictor_assistance.cpp 128 | # ) 129 | 130 | ## Add cmake target dependencies of the library 131 | ## as an example, code may need to be generated before libraries 132 | ## either from message generation or dynamic reconfigure 133 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 134 | 135 | ## Declare a C++ executable 136 | ## With catkin_make all packages are built within a single CMake context 137 | ## The recommended prefix ensures that target names across packages don't collide 138 | # add_executable(${PROJECT_NAME}_node src/predictor_assistance_node.cpp) 139 | 140 | ## Rename C++ executable without prefix 141 | ## The above recommended prefix causes long target names, the following renames the 142 | ## target back to the shorter version for ease of user use 143 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 144 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 145 | 146 | ## Add cmake target dependencies of the executable 147 | ## same as for the library above 148 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 149 | 150 | ## Specify libraries to link a library or executable target against 151 | # target_link_libraries(${PROJECT_NAME}_node 152 | # ${catkin_LIBRARIES} 153 | # ) 154 | 155 | ############# 156 | ## Install ## 157 | ############# 158 | 159 | # all install targets should use catkin DESTINATION variables 160 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 161 | 162 | ## Mark executable scripts (Python etc.) for installation 163 | ## in contrast to setup.py, you can choose the destination 164 | # catkin_install_python(PROGRAMS 165 | # scripts/my_python_script 166 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 167 | # ) 168 | 169 | ## Mark executables for installation 170 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 171 | # install(TARGETS ${PROJECT_NAME}_node 172 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 173 | # ) 174 | 175 | ## Mark libraries for installation 176 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 177 | # install(TARGETS ${PROJECT_NAME} 178 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 179 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 180 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 181 | # ) 182 | 183 | ## Mark cpp header files for installation 184 | # install(DIRECTORY include/${PROJECT_NAME}/ 185 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 186 | # FILES_MATCHING PATTERN "*.h" 187 | # PATTERN ".svn" EXCLUDE 188 | # ) 189 | 190 | ## Mark other files for installation (e.g. launch and bag files, etc.) 191 | # install(FILES 192 | # # myfile1 193 | # # myfile2 194 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 195 | # ) 196 | 197 | ############# 198 | ## Testing ## 199 | ############# 200 | 201 | ## Add gtest based cpp test target and link libraries 202 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_predictor_assistance.cpp) 203 | # if(TARGET ${PROJECT_NAME}-test) 204 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 205 | # endif() 206 | 207 | ## Add folders to be run by python nosetests 208 | # catkin_add_nosetests(test) 209 | -------------------------------------------------------------------------------- /system/predictor/predictor_assistance/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | predictor_assistance 4 | 0.0.2 5 | The predictor_assistance package 6 | 7 | 8 | 9 | 10 | Alberto Gottardi 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | geometry_msgs 53 | rospy 54 | std_msgs 55 | geometry_msgs 56 | rospy 57 | std_msgs 58 | geometry_msgs 59 | rospy 60 | std_msgs 61 | 62 | shared_control 63 | 64 | message_generation 65 | message_runtime 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | -------------------------------------------------------------------------------- /system/predictor/predictor_assistance/setup.py: -------------------------------------------------------------------------------- 1 | from distutils.core import setup 2 | from catkin_pkg.python_setup import generate_distutils_setup 3 | 4 | d = generate_distutils_setup( 5 | packages = ['predictor_assistance'], 6 | package_dir = {'': 'src'} 7 | ) 8 | 9 | setup(**d) 10 | -------------------------------------------------------------------------------- /system/predictor/predictor_assistance/src/predictor_assistance/AssistancePolicy.py: -------------------------------------------------------------------------------- 1 | #Generic assistance policy for multiple goals 2 | import numpy as np 3 | import IPython 4 | import AssistancePolicyOneGoal as GoalPolicy 5 | from RobotState import Action 6 | 7 | 8 | class AssistancePolicy: 9 | """ 10 | Assistance Policy Class \n 11 | Args: 12 | robot_state: RobotState 13 | goals: list of GoalAssistance 14 | """ 15 | ACTION_APPLY_TIME = 0.1 # used when calculating future state 16 | def __init__(self, robot_state, goals): 17 | self.goals = goals 18 | self.robot_state = robot_state 19 | self.goal_assist_policies = [] 20 | for goal in goals: 21 | self.goal_assist_policies.append(GoalPolicy.AssistancePolicyOneGoal(goal)) 22 | 23 | 24 | def update(self, user_action): 25 | """ 26 | Update assistance policy \n 27 | Args: 28 | user_action: Action 29 | """ 30 | robot_state_after_action = self.robot_state.state_after_action(user_action, self.ACTION_APPLY_TIME) 31 | #user action corresponds to the effect of direct teleoperation on the robot 32 | self.user_action = user_action 33 | 34 | for goal_policy in self.goal_assist_policies: 35 | goal_policy.update(self.robot_state, self.user_action, robot_state_after_action) 36 | 37 | def get_values(self): 38 | """ 39 | Get v_values and q_values \n 40 | Return: 41 | v_values 42 | q_values 43 | """ 44 | values = np.ndarray(len(self.goal_assist_policies)) 45 | qvalues = np.ndarray(len(self.goal_assist_policies)) 46 | for ind,goal_policy in enumerate(self.goal_assist_policies): 47 | values[ind] = goal_policy.get_value() 48 | qvalues[ind] = goal_policy.get_qvalue() 49 | 50 | return values,qvalues 51 | 52 | 53 | def get_probs_last_user_action(self): 54 | values,qvalues = self.get_values() 55 | return np.exp(-(qvalues-values)) 56 | 57 | def get_assisted_action(self, goal_distribution, fix_magnitude_user_command=True): 58 | """ 59 | Get assisted action \n 60 | Args: 61 | goal_distribution 62 | Return: assisted action 63 | """ 64 | assert goal_distribution.size == len(self.goal_assist_policies) 65 | 66 | action_dimension = GoalPolicy.TargetPolicy.ACTION_DIMENSION 67 | #TODO how do we handle mode switch vs. not? 68 | total_action_twist = np.zeros(action_dimension) 69 | for goal_policy,goal_prob in zip(self.goal_assist_policies, goal_distribution): 70 | total_action_twist += goal_prob * goal_policy.get_action() 71 | 72 | total_action_twist /= np.sum(goal_distribution) 73 | to_ret_twist = total_action_twist + self.user_action.twist 74 | #print "before magnitude adjustment: " + str(to_ret_twist) 75 | if fix_magnitude_user_command and not np.linalg.norm(self.user_action.twist)==0: 76 | to_ret_twist *= np.linalg.norm(self.user_action.twist)/np.linalg.norm(to_ret_twist) 77 | #print "after magnitude adjustment: " + str(to_ret_twist) 78 | 79 | return to_ret_twist 80 | 81 | 82 | 83 | 84 | -------------------------------------------------------------------------------- /system/predictor/predictor_assistance/src/predictor_assistance/AssistancePolicyOneGoal.py: -------------------------------------------------------------------------------- 1 | #WE DON'T EXPECT TO USE THIS CLASS, BUT RATHER ONE THAT INHERITS FROM IT 2 | #Generic assistance policy for one goal 3 | import numpy as np 4 | import IPython 5 | #import AssistancePolicyOneTarget as TargetPolicy 6 | import HuberAssistancePolicy as TargetPolicy 7 | 8 | TargetPolicyClass = TargetPolicy.HuberAssistancePolicy 9 | 10 | class AssistancePolicyOneGoal: 11 | """ 12 | Assistance Policy One Goal Class \n 13 | Args: 14 | goal: GoalAssistance 15 | """ 16 | def __init__(self, goal): 17 | self.goal = goal 18 | self.target_assist_policies = [] 19 | self.pose = self.goal.getCenterMatrix() 20 | self.target_assist_policies.append(TargetPolicyClass(self.pose)) 21 | self.min_val_ind = 0 22 | 23 | def update(self, robot_state, user_action, robot_state_after_action): 24 | self.last_robot_state = robot_state 25 | self.last_user_action = user_action 26 | 27 | for target_policy in self.target_assist_policies: 28 | target_policy.update(robot_state, user_action, robot_state_after_action) 29 | 30 | values = [targ_policy.get_value() for targ_policy in self.target_assist_policies] 31 | self.min_val_ind = np.argmin(values) 32 | 33 | def get_value(self): 34 | return self.target_assist_policies[self.min_val_ind].get_value() 35 | 36 | def get_qvalue(self): 37 | return self.target_assist_policies[self.min_val_ind].get_qvalue() 38 | 39 | def get_action(self): 40 | values = [targ_policy.get_value() for targ_policy in self.target_assist_policies] 41 | min_val_ind = np.argmin(values) 42 | return self.target_assist_policies[min_val_ind].get_action() 43 | 44 | 45 | def get_min_value_pose(self): 46 | return self.goal.pose_center[self.min_val_ind] 47 | -------------------------------------------------------------------------------- /system/predictor/predictor_assistance/src/predictor_assistance/AssistancePolicyOneTarget.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import tf.transformations as transmethods 3 | import copy 4 | 5 | class AssistancePolicyOneTarget(object): 6 | """ 7 | Aiistance Policy One Target Class \n 8 | Args: 9 | pose: matrix of goal 10 | """ 11 | ACTION_APPLY_TIME = 0.01 12 | 13 | def __init__(self, pose): 14 | self.goal_pose = pose 15 | self.goal_quat = transmethods.quaternion_from_matrix(pose) 16 | self.goal_pos = pose[0:3,3] 17 | 18 | def update(self, robot_state, user_action, robot_state_after_action): 19 | self.robot_state = robot_state 20 | self.user_action = user_action 21 | self.robot_state_after_action = robot_state_after_action 22 | 23 | def get_action(self): 24 | ee_trans = self.last_ee_trans 25 | pos_diff = 5.*(self.goal_pose[0:3,3] - ee_trans[0:3,3]) 26 | 27 | pos_diff_norm = np.linalg.norm(pos_diff) 28 | 29 | clip_norm_val = 0.02 30 | if (pos_diff_norm > clip_norm_val): 31 | pos_diff /= pos_diff_norm/clip_norm_val 32 | 33 | return pos_diff 34 | 35 | 36 | #def pose_after_user_action(self, ee_trans, user_action): 37 | def state_after_user_action(self, robot_state, user_action): 38 | return robot_state.state_after_action(user_action, self.ACTION_APPLY_TIME) 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /system/predictor/predictor_assistance/src/predictor_assistance/GoalAssistance.py: -------------------------------------------------------------------------------- 1 | #coding=utf-8 2 | 3 | import numpy as np 4 | 5 | class GoalAssistance: 6 | """ 7 | Goal Assistance Class \n 8 | Args: 9 | id: ID object 10 | pose: center pose 11 | grasp_points: list of grasping points 12 | """ 13 | def __init__(self, id, pose, grasp_points): 14 | self._id = id 15 | self._pose_center = pose 16 | self._grasp_points = grasp_points 17 | self._probability = 0 18 | self._distance = float('inf') 19 | 20 | def getID(self): 21 | """ 22 | Get ID object associated with the goal \n 23 | Return: ID object associated with the goal 24 | """ 25 | return self._id 26 | 27 | def getCenterPosition(self): 28 | """ 29 | Get center position of object associated with the goal \n 30 | Return: center of object 31 | """ 32 | return self._pose_center[0:3,3] 33 | 34 | def getCenterMatrix(self): 35 | """ 36 | Get center matrix pose of object associated with the goal \n 37 | Return: center matrix pose of object 38 | """ 39 | return self._pose_center 40 | 41 | def getGraspingPoints(self): 42 | """ 43 | Get grasping points associated with the goal \n 44 | Return: grasping points 45 | """ 46 | return self._grasp_points 47 | 48 | def getDistance(self): 49 | """ 50 | Get distance from goal to ee_position \n 51 | Return: distance from goal to ee 52 | """ 53 | return self._distance 54 | 55 | def getProbability(self): 56 | """ 57 | Get probability associated with the goal \n 58 | Return: probability of goal 59 | """ 60 | return self._probability 61 | 62 | def updateDistance(self, new_distance): 63 | """ 64 | Update distance from goal to ee position \n 65 | Args: 66 | new_distance: new distance from goal to ee 67 | """ 68 | self._distance = new_distance 69 | 70 | def printGoal(self): 71 | print("ID: " +str(self._id) ) 72 | print("center: " +str(self._pose_center)) 73 | print("Distance: " +str(self._distance)) 74 | print("Probability: " +str(self._probability)) 75 | 76 | def updateProbability(self, new_prob): 77 | """ 78 | Update probability \n 79 | Args: 80 | new_prob: new probability 81 | """ 82 | self._probability = new_prob 83 | 84 | 85 | 86 | -------------------------------------------------------------------------------- /system/predictor/predictor_assistance/src/predictor_assistance/GoalPredictorAssistance.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import scipy.misc 3 | 4 | logsumexp = scipy.misc.logsumexp 5 | 6 | 7 | class GoalPredictorAssistance(object): 8 | """ 9 | Goal Predictor Assistance Class \n 10 | Args: 11 | goals: list of GoalAssistance 12 | """ 13 | max_prob_any_goal = 0.99 14 | log_max_prob_any_goal = np.log(max_prob_any_goal) 15 | 16 | def __init__(self, goals): 17 | self._goals = goals 18 | self._log_goal_distribution = np.log((1./len(self._goals))*np.ones(len(self._goals))) # log( p(g|xi) ) 19 | 20 | def update_distribution(self, v_values, q_values, w_sc): 21 | """ 22 | Update distribution \n 23 | Args: 24 | v_values 25 | q_values 26 | weight 27 | """ 28 | # log( p(xi|g) ) = log( p(xi|g) )@t-1 + log( exp(V - Q) )@t 29 | self._log_goal_distribution += w_sc * (v_values - q_values) 30 | self.normalize_log_distribution() 31 | 32 | 33 | def normalize_log_distribution(self): 34 | # log( sum( p(xi|g) ) ) 35 | log_normalization_val = logsumexp(self._log_goal_distribution) 36 | # log( p(g|xi) )_i = log( p(xi|g) )_i - log( sum( p(xi|g) ) ) 37 | self._log_goal_distribution = self._log_goal_distribution - log_normalization_val 38 | self.clip_prob() 39 | 40 | 41 | def clip_prob(self): 42 | if len(self._log_goal_distribution) <= 1: 43 | return 44 | #check if any too high 45 | max_prob_ind = np.argmax(self._log_goal_distribution) 46 | if self._log_goal_distribution[max_prob_ind] > self.log_max_prob_any_goal: 47 | #see how much we will remove from probability 48 | diff = np.exp(self._log_goal_distribution[max_prob_ind]) - self.max_prob_any_goal 49 | #want to distribute this evenly among other goals 50 | diff_per = diff/(len(self._log_goal_distribution)-1.) 51 | #distribute this evenly in the probability space...this corresponds to doing so in log space 52 | # e^x_new = e^x_old + diff_per, and this is formulate for log addition 53 | self._log_goal_distribution += np.log( 1. + diff_per/np.exp(self._log_goal_distribution)) 54 | #set old one 55 | self._log_goal_distribution[max_prob_ind] = self.log_max_prob_any_goal 56 | 57 | 58 | def get_distribution(self): 59 | """ 60 | Get distribution \n 61 | Return: distribution 62 | """ 63 | return np.exp(self._log_goal_distribution) 64 | 65 | 66 | def get_prob(self, index): 67 | """ 68 | Get probability of index goal \n 69 | Args: 70 | index: index of the goal 71 | Return: probability of index goal 72 | """ 73 | return np.exp(self._log_goal_distribution[index]) 74 | 75 | 76 | def get_ind_maxes(self): 77 | """ 78 | Get the indices of the two major probabilities \n 79 | Return: 80 | first max index 81 | second max index 82 | """ 83 | amax = np.argmax(self._log_goal_distribution) # This is the maximum value 84 | mask = np.zeros_like(self._log_goal_distribution) 85 | mask[amax] = 1 86 | a = np.ma.masked_array(self._log_goal_distribution, mask=mask) 87 | second_max = np.argmax(a) # Second maximum value 88 | return amax, second_max -------------------------------------------------------------------------------- /system/predictor/predictor_assistance/src/predictor_assistance/PredictorAssistance.py: -------------------------------------------------------------------------------- 1 | #coding=utf-8 2 | 3 | import rospy 4 | from predictor_assistance.srv import AssistancePredictor, AssistancePredictorResponse 5 | from control_manip.msg import GoalArray as goalArrayMsg 6 | from shared_control.srv import InitPred, InitPredResponse 7 | 8 | import Utils 9 | import RobotState as rs 10 | from RobotState import Action 11 | import RobotAssistancePolicy 12 | import PrintOnFile as pof 13 | 14 | import copy 15 | import numpy as np 16 | 17 | class PredictorAssistance: 18 | """ 19 | PredictorAssistance Class \n 20 | Args: 21 | name_srv: name of initPred service 22 | """ 23 | def __init__(self, name_srv): 24 | self._goals = None 25 | self._user_twist = np.zeros(6) 26 | self._ee_pose = np.zeros((4, 4)) #zero matrix 27 | self._ca_twist = np.zeros(6) 28 | self._file = pof.PrintOnFile() 29 | self._print_on_file = False 30 | self._robot_state = None 31 | self._policy = None 32 | self._service_init = rospy.Service(name_srv, InitPred, self.handleInitPred) 33 | self._service = rospy.Service("predictor_assistance_srv", AssistancePredictor, self.handleAssistance) 34 | 35 | 36 | def handleInitPred(self, request): 37 | """ 38 | Service to initialize predictor node \n 39 | Args: request \n 40 | Return: response 41 | """ 42 | self._goals = Utils.getGoal(request.goals) 43 | self._ee_pose = Utils.pose_to_mat(request.ee_pose) 44 | self._robot_state = rs.RobotState(self._ee_pose, 0,0,0) 45 | self._policy = RobotAssistancePolicy.RobotAssistancePolicy(self._goals, self._robot_state, self._print_on_file, self._file) 46 | 47 | print("Predictor node received request!") 48 | return InitPredResponse(True) 49 | 50 | 51 | 52 | 53 | def handleAssistance(self, request): 54 | """ 55 | Service predictor assistance. It receivers twist user and actual EE pose and computes distribution probability and assisted twist. \n 56 | Args: request \n 57 | Return: response 58 | """ 59 | #Set user twist, CA twist and actual EE pose 60 | self.setUserTwist(request.user_input) 61 | self.setEEPose(request.ee_pose) 62 | 63 | #Update Robot State pose 64 | #self._robot_state.updateState(self._ee_pose) 65 | 66 | #Upadate policy 67 | action_u = Action(self._user_twist) 68 | self._policy.update(action_u) 69 | result_action = self._policy.get_action() 70 | 71 | #Get assistance twist, probability distribution and index of goal with max prob 72 | assistance_twist = result_action.getTwist() 73 | assistance_twist_msg = Utils.arrayToTwistMsg(assistance_twist) 74 | prob_distribution = self._policy.getDistribution() 75 | self._policy.visualize_prob() 76 | index_max = self._policy.getIndexMax() 77 | 78 | return AssistancePredictorResponse(assistance_twist_msg, prob_distribution, index_max) 79 | 80 | 81 | def setUserTwist(self, user_twist): 82 | """ 83 | Set user twist \n 84 | Args: 85 | user_twist: Twist message 86 | """ 87 | self._user_twist[0:3] = np.array([user_twist.linear.x, user_twist.linear.y, user_twist.linear.z]) 88 | self._user_twist[3:6] = np.array([user_twist.angular.x, user_twist.angular.y, user_twist.angular.z]) 89 | 90 | 91 | def setCATwist(self, ca_twist): 92 | """ 93 | Set CA twist \n 94 | Args: 95 | ca_twist: Twist message 96 | """ 97 | self._ca_twist[0:3] = np.array([ca_twist.linear.x, ca_twist.linear.y, ca_twist.linear.z]) 98 | self._ca_twist[3:6] = np.array([ca_twist.angular.x, ca_twist.angular.y, ca_twist.angular.z]) 99 | 100 | 101 | def setEEPose(self, pose): 102 | """ 103 | Set pose matrix of EE \n 104 | Args: 105 | pose: pose message of EE 106 | """ 107 | self._ee_pose = Utils.pose_to_mat(pose) -------------------------------------------------------------------------------- /system/predictor/predictor_assistance/src/predictor_assistance/PredictorAssistanceNode.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | #coding=utf-8 3 | 4 | import rospy 5 | import PredictorAssistance as pa 6 | import Utils 7 | import numpy as np 8 | 9 | 10 | 11 | if __name__ == "__main__": 12 | rospy.init_node("prediction_assistance_node", anonymous=True) 13 | robot_type = rospy.get_param("~robot_type", "/ur5") 14 | name_service_init = "init_prediction_srv" 15 | 16 | prediction_assistance = pa.PredictorAssistance(name_service_init) 17 | rospy.spin() 18 | 19 | 20 | -------------------------------------------------------------------------------- /system/predictor/predictor_assistance/src/predictor_assistance/PrintOnFile.py: -------------------------------------------------------------------------------- 1 | import os.path 2 | 3 | class PrintOnFile: 4 | def __init__(self): 5 | self.my_path = os.path.abspath(os.path.dirname(__file__)) 6 | self.path = os.path.split(self.my_path) 7 | self.new_path = os.path.split(self.path[0]) 8 | self.final_path = os.path.join(self.new_path[0], "stat_assist") 9 | if(not os.path.exists(self.final_path)): 10 | os.mkdir(self.final_path) 11 | self.file_name = self.final_path + "/output.txt" 12 | self.file = open(self.file_name,'w+') 13 | 14 | def write(self, info): 15 | string = str(info) + "\n" 16 | self.file.write(string) 17 | 18 | def write_with_title(self, info, title): 19 | string = title + ": " + str(info) + "\n" 20 | self.file.write(string) 21 | 22 | def end_block(self): 23 | string = '===\n' 24 | self.file.write(string) 25 | 26 | def close(self): 27 | self.file.flush() 28 | self.file.close() 29 | -------------------------------------------------------------------------------- /system/predictor/predictor_assistance/src/predictor_assistance/RobotAssistancePolicy.py: -------------------------------------------------------------------------------- 1 | #coding=utf-8 2 | 3 | import GoalAssistance as Goal 4 | import numpy as np 5 | import GoalPredictorAssistance as GoalPredictorAssistance 6 | from RobotState import Action 7 | import AssistancePolicy 8 | 9 | 10 | class RobotAssistancePolicy: 11 | """ 12 | Robot Assistance Policy Class \n 13 | Args: 14 | goals: list of GoalAssistance 15 | robot_state: RobotState 16 | print_on_file: True if print on file info, False otherwise 17 | file: PrintOnFile 18 | """ 19 | def __init__(self, goals, robot_state, print_on_file, file): 20 | self._assist_policy = AssistancePolicy.AssistancePolicy(robot_state, goals) 21 | self._goal_predictor = GoalPredictorAssistance.GoalPredictorAssistance(goals) 22 | self._robot_state = robot_state 23 | self._goals = goals 24 | 25 | self._ind_max = 0 26 | self._weight_sc = 1.0 27 | 28 | # if need to print data to file 29 | self._print_on_file = print_on_file 30 | self._file = file 31 | 32 | 33 | def update(self, user_action=Action()): 34 | """ 35 | Update robot assistance policy \n 36 | Args: 37 | user_action: Action 38 | """ 39 | self._assist_policy.update(user_action) 40 | v_values,q_values = self._assist_policy.get_values() 41 | if self._print_on_file: 42 | self._file.write_with_title(v_values, "v_values") 43 | self._file.write_with_title(q_values, "q_values") 44 | 45 | self._goal_predictor.update_distribution(v_values, q_values, self._weight_sc) 46 | first_max, second_max = self._goal_predictor.get_ind_maxes() 47 | self._ind_max = first_max 48 | 49 | 50 | def get_action(self, goal_distribution=np.array([]), **kwargs): 51 | """ 52 | Get Action \n 53 | Args: 54 | goal_distribution 55 | Return: assisted action Action 56 | """ 57 | if goal_distribution.size == 0: 58 | goal_distribution = self._goal_predictor.get_distribution() 59 | 60 | self._policy_twist = self._assist_policy.get_assisted_action(goal_distribution, **kwargs) 61 | 62 | assisted_action = Action(twist=self._policy_twist, 63 | finger_vel=self._assist_policy.user_action.finger_vel, 64 | switch_mode_to=self._assist_policy.user_action.switch_mode_to) 65 | 66 | return assisted_action 67 | 68 | 69 | def visualize_prob(self): 70 | """ 71 | Visualize goal distribution probability 72 | """ 73 | goal_distribution = self._goal_predictor.get_distribution() 74 | print("=====-- Goals --=====") 75 | for idx, g_prob in enumerate(goal_distribution): 76 | print("= Goal: " + str(self._goals[idx].getID()) + " with prob = " + str(g_prob)) 77 | 78 | 79 | def getDistribution(self): 80 | """ 81 | Get goal distribution \n 82 | Return: goal distribution 83 | """ 84 | goal_distribution = self._goal_predictor.get_distribution() 85 | return goal_distribution 86 | 87 | 88 | def getIndexMax(self): 89 | """ 90 | Get index of max probability \n 91 | Return: index of max probability 92 | """ 93 | return self._ind_max 94 | 95 | 96 | def get_selected_goal(self): 97 | """ 98 | Get goal with max probability \n 99 | Return: goal 100 | """ 101 | return self._goals[self._ind_max] 102 | 103 | -------------------------------------------------------------------------------- /system/predictor/predictor_assistance/src/predictor_assistance/RobotState.py: -------------------------------------------------------------------------------- 1 | #RobotState.py 2 | 3 | # keeps track of the state of the robot 4 | 5 | import copy 6 | import numpy as np 7 | import rospy 8 | 9 | import Utils 10 | 11 | #TODO make this dynamic 12 | #NUM_FINGER_DOFS = rospy.get_param('/ada/num_finger_dofs', 2) 13 | 14 | class RobotState(object): 15 | def __init__(self, ee_trans, finger_dofs, mode=0, num_modes=2): 16 | self.ee_trans = copy.copy(ee_trans) 17 | self.finger_dofs = copy.copy(finger_dofs) 18 | self.mode = mode 19 | self.num_modes = num_modes 20 | self.myee = copy.copy(ee_trans) 21 | 22 | def get_pos(self): 23 | return self.ee_trans[0:3,3] 24 | 25 | def get_finger_dofs(self): 26 | return self.finger_dofs 27 | 28 | def switch_mode(self): 29 | self.mode = self.next_mode() 30 | 31 | def next_mode(self): 32 | return (self.mode+1)%self.num_modes 33 | 34 | def set_mode(self, mode): 35 | assert mode >= 0 and mode <= self.num_modes 36 | self.mode = mode 37 | 38 | def mode_after_action(self, action): 39 | if action.is_no_mode_switch(): 40 | return self.mode 41 | else: 42 | return action.switch_mode_to 43 | 44 | ### 45 | def updateState(self, matrix_pose): 46 | self.ee_trans = matrix_pose 47 | ### 48 | def state_after_action(self, action, time): 49 | state_copy = copy.deepcopy(self) 50 | if not action.is_no_move(): 51 | state_copy.ee_trans = Utils.ApplyTwistToTransform(action.twist, state_copy.ee_trans, time) 52 | 53 | if not action.is_no_mode_switch(): 54 | state_copy.mode = action.switch_mode_to 55 | 56 | self.myee = state_copy 57 | return state_copy 58 | 59 | def num_finger_dofs(self): 60 | return len(self.finger_dofs) 61 | 62 | def printPose(self): 63 | print("STATE ACTION: " +str(self.myee)) 64 | 65 | def __str__(self): 66 | return "STATE ACTION: %s" %(self.myee) 67 | 68 | #actions we can enact on the state 69 | #corresponds to a mode switch and a twist 70 | 71 | 72 | #TODO handling an unspecified number of fingers is messy here. Handle this better 73 | class Action(object): 74 | no_mode_switch=-1 75 | no_move = np.zeros(6) 76 | no_finger_vel = np.zeros(2) 77 | def __init__(self, twist=copy.copy(no_move), finger_vel=None, switch_mode_to=no_mode_switch): 78 | self.twist = twist 79 | if finger_vel is None: 80 | self.finger_vel = Action.no_finger_vel 81 | else: 82 | self.finger_vel = finger_vel 83 | 84 | self.switch_mode_to = switch_mode_to 85 | 86 | def as_tuple(self): 87 | return (self.twist, self.switch_mode_to) 88 | 89 | def is_no_action(self): 90 | return self.twist == self.no_move and self.switch_mode_to == self.no_mode_switch and self.finger_vel == Action.no_finger_vel 91 | 92 | def __eq__(self, other): 93 | return self.twist == other.twist and self.switch_mode_to == other.switch_mode_to and self.finger_vel == other.finger_vel 94 | 95 | def is_no_mode_switch(self): 96 | return self.switch_mode_to == self.no_mode_switch 97 | 98 | def is_no_move(self): 99 | return np.linalg.norm(self.twist) < 1e-10 100 | 101 | def is_no_finger_move(self): 102 | return np.linalg.norm(self.finger_vel) < 1e-10 103 | 104 | def getTwist(self): 105 | return self.twist 106 | 107 | # return the parts of this actions move that can be 108 | # altered by the user given the current robot 109 | # def move_in_mode(self, mode): 110 | # if mode == 0: 111 | # return self.move[:3] 112 | # elif mode == 1: 113 | # return self.move[3:] 114 | # else: 115 | # return self.move 116 | 117 | def __str__(self): 118 | return 'twist:' + str(self.twist) + ' finger:' + str(self.finger_vel) + ' mode to:' + str(self.switch_mode_to) 119 | 120 | @staticmethod 121 | def set_no_finger_vel(num_finger_dofs): 122 | Action.no_finger_vel = np.zeros(num_finger_dofs) 123 | -------------------------------------------------------------------------------- /system/predictor/predictor_assistance/src/predictor_assistance/Utils.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import tf.transformations as transmethods 3 | from geometry_msgs.msg import Pose, Twist 4 | import GoalAssistance 5 | 6 | def RotationMatrixDistance(pose1, pose2): 7 | quat1 = transmethods.quaternion_from_matrix(pose1) 8 | quat2 = transmethods.quaternion_from_matrix(pose2) 9 | return QuaternionDistance(quat1, quat2) 10 | 11 | def QuaternionDistance(quat1, quat2): 12 | quat_between = transmethods.quaternion_multiply( quat2, transmethods.quaternion_inverse(quat1) ) 13 | return AngleFromQuaternionW(quat_between[-1]) 14 | 15 | def AngleFromQuaternionW(w): 16 | w = min(0.9999999, max(-0.999999, w)) 17 | phi = 2.*np.arccos(w) 18 | return min(phi, 2.* np.pi - phi) 19 | 20 | def ApplyTwistToTransform(twist, transform, time=1.): 21 | transform[0:3,3] += time * twist[0:3] 22 | 23 | angular_velocity = twist[3:] 24 | angular_velocity_norm = np.linalg.norm(angular_velocity) 25 | if angular_velocity_norm > 1e-3: 26 | angle = time*angular_velocity_norm 27 | axis = angular_velocity/angular_velocity_norm 28 | transform[0:3,0:3] = np.dot(transmethods.rotation_matrix(angle, axis), transform)[0:3,0:3] 29 | 30 | return transform 31 | 32 | def ApplyAngularVelocityToQuaternion(angular_velocity, quat, time=1.): 33 | angular_velocity_norm = np.linalg.norm(angular_velocity) 34 | angle = time*angular_velocity_norm 35 | axis = angular_velocity/angular_velocity_norm 36 | 37 | #angle axis to quaternion formula 38 | quat_from_velocity = np.append(np.sin(angle/2.)*axis, np.cos(angle/2.)) 39 | 40 | return transmethods.quaternion_multiply(quat_from_velocity, quat) 41 | 42 | 43 | def pose_to_mat(pose): 44 | """ 45 | Convert a Pose msg to a 4x4 numpy matrix \n 46 | Args: 47 | pose: Pose msg 48 | Return: numpy matrix 49 | """ 50 | mat = np.zeros((4, 4)) 51 | mat[3, 3] = 1 52 | mat[0:3, 3] = np.array([pose.position.x, pose.position.y, pose.position.z]) 53 | q = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w] 54 | mat[0:3, 0:3] = np.array(transmethods.quaternion_matrix(q))[0:3, 0:3] 55 | return mat 56 | 57 | 58 | def mat_to_pose(mat): 59 | """ 60 | Convert 4x4 numpy matrix to a Pose msg \n 61 | Args: 62 | mat: numpy matrix 63 | Return: Pose msg 64 | """ 65 | pose = Pose() 66 | pose.position.x = mat[0,3] 67 | pose.position.y = mat[1,3] 68 | pose.position.z = mat[2,3] 69 | quat = transmethods.quaternion_from_matrix(mat) 70 | pose.orientation.x = quat[0] 71 | pose.orientation.y = quat[1] 72 | pose.orientation.z = quat[2] 73 | pose.orientation.w = quat[3] 74 | return pose 75 | 76 | 77 | def arrayToTwistMsg(array): 78 | """ 79 | Convert numpy array into twist message \n 80 | Args: 81 | array: numpy array 82 | Return: twist message 83 | """ 84 | twist = Twist() 85 | twist.linear.x = array[0] 86 | twist.linear.y = array[1] 87 | twist.linear.z = array[2] 88 | twist.angular.x = array[3] 89 | twist.angular.y = array[4] 90 | twist.angular.z = array[5] 91 | return twist 92 | 93 | 94 | def getGoal(goal_msg): 95 | """ 96 | Create and get list of GoalAssistance obj \n 97 | Args: 98 | goal_msg: msg of goal 99 | Return: list of GoalAssistance 100 | """ 101 | goal_list = [] 102 | target_pos = [] 103 | for g in goal_msg.goal: 104 | grasp_points = [] 105 | id = g.id 106 | center = pose_to_mat(g.center) 107 | tmp_c = np.array([g.center.position.x, g.center.position.y, g.center.position.z]) 108 | target_pos.append(tmp_c) 109 | for grasp in g.grasping_points: 110 | grasp_points.append(grasp) 111 | goal = GoalAssistance.GoalAssistance(id, center, grasp_points) 112 | goal_list.append(goal) 113 | 114 | return goal_list 115 | -------------------------------------------------------------------------------- /system/predictor/predictor_assistance/src/predictor_assistance/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Shared-control/improved-apf/85ab7ef5f043752794c52e80f4d1d19e10ef87ff/system/predictor/predictor_assistance/src/predictor_assistance/__init__.py -------------------------------------------------------------------------------- /system/predictor/predictor_assistance/srv/AssistancePredictor.srv: -------------------------------------------------------------------------------- 1 | geometry_msgs/Twist user_input 2 | geometry_msgs/Twist ca_twist 3 | geometry_msgs/Pose ee_pose 4 | --- 5 | geometry_msgs/Twist assisted_twist 6 | float32[] distr_prob 7 | int8 index_max 8 | -------------------------------------------------------------------------------- /system/predictor/predictor_distance/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(predictor_distance) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | geometry_msgs 12 | rospy 13 | std_msgs 14 | message_generation 15 | ) 16 | 17 | ## System dependencies are found with CMake's conventions 18 | # find_package(Boost REQUIRED COMPONENTS system) 19 | 20 | 21 | ## Uncomment this if the package has a setup.py. This macro ensures 22 | ## modules and global scripts declared therein get installed 23 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 24 | catkin_python_setup() 25 | 26 | ################################################ 27 | ## Declare ROS messages, services and actions ## 28 | ################################################ 29 | 30 | ## To declare and build messages, services or actions from within this 31 | ## package, follow these steps: 32 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 33 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 34 | ## * In the file package.xml: 35 | ## * add a build_depend tag for "message_generation" 36 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 37 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 38 | ## but can be declared for certainty nonetheless: 39 | ## * add a exec_depend tag for "message_runtime" 40 | ## * In this file (CMakeLists.txt): 41 | ## * add "message_generation" and every package in MSG_DEP_SET to 42 | ## find_package(catkin REQUIRED COMPONENTS ...) 43 | ## * add "message_runtime" and every package in MSG_DEP_SET to 44 | ## catkin_package(CATKIN_DEPENDS ...) 45 | ## * uncomment the add_*_files sections below as needed 46 | ## and list every .msg/.srv/.action file to be processed 47 | ## * uncomment the generate_messages entry below 48 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 49 | 50 | ## Generate messages in the 'msg' folder 51 | # add_message_files( 52 | # FILES 53 | # Message1.msg 54 | # Message2.msg 55 | # ) 56 | 57 | ## Generate services in the 'srv' folder 58 | add_service_files( 59 | FILES 60 | DistancePredictor.srv 61 | ) 62 | 63 | ## Generate actions in the 'action' folder 64 | # add_action_files( 65 | # FILES 66 | # Action1.action 67 | # Action2.action 68 | # ) 69 | 70 | ## Generate added messages and services with any dependencies listed here 71 | generate_messages( 72 | DEPENDENCIES 73 | geometry_msgs 74 | std_msgs 75 | ) 76 | 77 | ################################################ 78 | ## Declare ROS dynamic reconfigure parameters ## 79 | ################################################ 80 | 81 | ## To declare and build dynamic reconfigure parameters within this 82 | ## package, follow these steps: 83 | ## * In the file package.xml: 84 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 85 | ## * In this file (CMakeLists.txt): 86 | ## * add "dynamic_reconfigure" to 87 | ## find_package(catkin REQUIRED COMPONENTS ...) 88 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 89 | ## and list every .cfg file to be processed 90 | 91 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 92 | # generate_dynamic_reconfigure_options( 93 | # cfg/DynReconf1.cfg 94 | # cfg/DynReconf2.cfg 95 | # ) 96 | 97 | ################################### 98 | ## catkin specific configuration ## 99 | ################################### 100 | ## The catkin_package macro generates cmake config files for your package 101 | ## Declare things to be passed to dependent projects 102 | ## INCLUDE_DIRS: uncomment this if your package contains header files 103 | ## LIBRARIES: libraries you create in this project that dependent projects also need 104 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 105 | ## DEPENDS: system dependencies of this project that dependent projects also need 106 | catkin_package( 107 | # INCLUDE_DIRS include 108 | # LIBRARIES predictor_distance 109 | CATKIN_DEPENDS geometry_msgs rospy std_msgs message_runtime 110 | # DEPENDS system_lib 111 | ) 112 | 113 | ########### 114 | ## Build ## 115 | ########### 116 | 117 | ## Specify additional locations of header files 118 | ## Your package locations should be listed before other locations 119 | include_directories( 120 | # include 121 | ${catkin_INCLUDE_DIRS} 122 | ) 123 | 124 | ## Declare a C++ library 125 | # add_library(${PROJECT_NAME} 126 | # src/${PROJECT_NAME}/predictor_distance.cpp 127 | # ) 128 | 129 | ## Add cmake target dependencies of the library 130 | ## as an example, code may need to be generated before libraries 131 | ## either from message generation or dynamic reconfigure 132 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 133 | 134 | ## Declare a C++ executable 135 | ## With catkin_make all packages are built within a single CMake context 136 | ## The recommended prefix ensures that target names across packages don't collide 137 | # add_executable(${PROJECT_NAME}_node src/predictor_distance_node.cpp) 138 | 139 | ## Rename C++ executable without prefix 140 | ## The above recommended prefix causes long target names, the following renames the 141 | ## target back to the shorter version for ease of user use 142 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 143 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 144 | 145 | ## Add cmake target dependencies of the executable 146 | ## same as for the library above 147 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 148 | 149 | ## Specify libraries to link a library or executable target against 150 | # target_link_libraries(${PROJECT_NAME}_node 151 | # ${catkin_LIBRARIES} 152 | # ) 153 | 154 | ############# 155 | ## Install ## 156 | ############# 157 | 158 | # all install targets should use catkin DESTINATION variables 159 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 160 | 161 | ## Mark executable scripts (Python etc.) for installation 162 | ## in contrast to setup.py, you can choose the destination 163 | # catkin_install_python(PROGRAMS 164 | # scripts/my_python_script 165 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 166 | # ) 167 | 168 | ## Mark executables for installation 169 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 170 | # install(TARGETS ${PROJECT_NAME}_node 171 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 172 | # ) 173 | 174 | ## Mark libraries for installation 175 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 176 | # install(TARGETS ${PROJECT_NAME} 177 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 178 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 179 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 180 | # ) 181 | 182 | ## Mark cpp header files for installation 183 | # install(DIRECTORY include/${PROJECT_NAME}/ 184 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 185 | # FILES_MATCHING PATTERN "*.h" 186 | # PATTERN ".svn" EXCLUDE 187 | # ) 188 | 189 | ## Mark other files for installation (e.g. launch and bag files, etc.) 190 | # install(FILES 191 | # # myfile1 192 | # # myfile2 193 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 194 | # ) 195 | 196 | ############# 197 | ## Testing ## 198 | ############# 199 | 200 | ## Add gtest based cpp test target and link libraries 201 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_predictor_distance.cpp) 202 | # if(TARGET ${PROJECT_NAME}-test) 203 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 204 | # endif() 205 | 206 | ## Add folders to be run by python nosetests 207 | # catkin_add_nosetests(test) 208 | -------------------------------------------------------------------------------- /system/predictor/predictor_distance/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | predictor_distance 4 | 0.0.2 5 | The predictor_distance package 6 | 7 | 8 | 9 | 10 | Alberto Gottardi 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | geometry_msgs 53 | rospy 54 | std_msgs 55 | geometry_msgs 56 | rospy 57 | std_msgs 58 | geometry_msgs 59 | rospy 60 | std_msgs 61 | 62 | shared_control_autonomy 63 | 64 | message_generation 65 | message_runtime 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | -------------------------------------------------------------------------------- /system/predictor/predictor_distance/setup.py: -------------------------------------------------------------------------------- 1 | from distutils.core import setup 2 | from catkin_pkg.python_setup import generate_distutils_setup 3 | 4 | d = generate_distutils_setup( 5 | packages = ['predictor_distance'], 6 | package_dir = {'': 'src'} 7 | ) 8 | 9 | setup(**d) 10 | -------------------------------------------------------------------------------- /system/predictor/predictor_distance/src/predictor_distance/GoalPredictor.py: -------------------------------------------------------------------------------- 1 | #coding=utf-8 2 | 3 | import numpy as np 4 | from shared_control_autonomy.Goal import Goal 5 | 6 | class GoalPredictor(object): 7 | """ 8 | Class goal predictor to compute probability distribution \n 9 | Args: 10 | goals: Goal list 11 | """ 12 | def __init__(self, goals): 13 | self._goals = goals 14 | self._goal_distribution = (1./len(self._goals)) * np.ones(len(self._goals)) 15 | 16 | def updateDistribution(self): 17 | """ 18 | Update distribution probability after new action 19 | """ 20 | if(len(self._goal_distribution) == 0): 21 | return 22 | 23 | elif(len(self._goal_distribution) == 1): 24 | self._goals[0].updateProbability(1) 25 | return 26 | 27 | #Compute sum of all reciprocal distances 28 | sum_distance_recip = 0 29 | #reciprocal distance array 30 | reciprocal = np.zeros(len(self._goals)) 31 | for i in range(len(self._goals)): 32 | reciprocal[i] += 1./self._goals[i].getDistance() 33 | #print("ID: " + str(self.goals[i].getID()) + " distance: " +str(self.goals[i].getDistance())) 34 | sum_distance_recip += reciprocal[i] 35 | 36 | self._goal_distribution = reciprocal/sum_distance_recip 37 | for i in range(len(self._goal_distribution)): 38 | self._goals[i].updateProbability(self._goal_distribution[i]) 39 | 40 | def getDistribution(self): 41 | """ 42 | Get distribution \n 43 | Return: goal distribution 44 | """ 45 | return self._goal_distribution 46 | 47 | def getMaxProbability(self): 48 | """ 49 | Get max probability \n 50 | Return: max probability 51 | """ 52 | idx_max = np.argmax(self._goal_distribution) 53 | max_prob = self._goal_distribution[idx_max] 54 | 55 | return max_prob 56 | 57 | def getIndexMaxProb(self): 58 | """ 59 | Get index of goal with max probability \n 60 | Return: index of goal 61 | """ 62 | idx_max = np.argmax(self._goal_distribution) 63 | return idx_max 64 | 65 | def printDistribution(self): 66 | """ 67 | Print distribution probability 68 | """ 69 | for idx, prob in enumerate(self._goal_distribution): 70 | print("Goal: " + str(self._goals[idx].getID()) + " with prob: " + str(prob)) 71 | 72 | 73 | 74 | -------------------------------------------------------------------------------- /system/predictor/predictor_distance/src/predictor_distance/PredictorDistance.py: -------------------------------------------------------------------------------- 1 | #coding=utf-8 2 | 3 | import rospy 4 | from shared_control_autonomy.Goal import Goal 5 | from shared_control_autonomy import Utils 6 | from control_manip.msg import GoalArray as goalArrayMsg 7 | from predictor_distance.srv import DistancePredictor, DistancePredictorResponse 8 | 9 | from shared_control_autonomy.srv import InitPred, InitPredResponse 10 | 11 | import GoalPredictor as gp 12 | import numpy as np 13 | import copy 14 | 15 | class PredictorDistance: 16 | """ 17 | PredictorDistance Class \n 18 | Args: 19 | name_srv: name of initPred service 20 | """ 21 | def __init__(self, name_srv): 22 | self._goals = None 23 | self._targets_position = None 24 | self._goal_pred = None 25 | self._service_init = rospy.Service(name_srv, InitPred, self.handleInitPred) 26 | self._service = rospy.Service("predictor_distance_srv", DistancePredictor, self.handlePredition) 27 | 28 | 29 | def handleInitPred(self, request): 30 | """ 31 | Service to initialize predictor node \n 32 | Args: 33 | request: request of service 34 | Return: response 35 | """ 36 | self._goals, targets_position = Utils.getGoal(request.goals) 37 | self._goal_pred = gp.GoalPredictor(self._goals) 38 | self._targets_position = np.zeros(len(self._goals)) 39 | 40 | print("Predictor node received request!") 41 | return InitPredResponse(True) 42 | 43 | 44 | def handlePredition(self,request): 45 | """ 46 | Service prediction, receives distances, compute the distribution probability and return distribution and index of goal with max probability \n 47 | Args: 48 | request: request of service 49 | Return: response composed by distribution and index 50 | """ 51 | distances = list(request.distance) 52 | for i in range(len(distances)): 53 | if(distances[i] == 0): 54 | distribution = self._goal_pred.getDistribution() 55 | return DistancePredictorResponse(distribution, i) 56 | self._goals[i].updateDistance(distances[i]) 57 | self._goal_pred.updateDistribution() 58 | self._goal_pred.printDistribution() 59 | distribution = self._goal_pred.getDistribution() 60 | index = self._goal_pred.getIndexMaxProb() 61 | return DistancePredictorResponse(distribution, index) 62 | 63 | 64 | -------------------------------------------------------------------------------- /system/predictor/predictor_distance/src/predictor_distance/PredictorDistanceNode.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | #coding=utf-8 3 | 4 | import rospy 5 | from shared_control_autonomy import Utils 6 | from control_manip.msg import GoalArray as goalArrayMsg 7 | 8 | import GoalPredictor as gp 9 | import PredictorDistance as pd 10 | import numpy as np 11 | 12 | 13 | if __name__ == "__main__": 14 | rospy.init_node("prediction_node", anonymous=True) 15 | robot_type = rospy.get_param("~robot_type", "/ur5") 16 | name_service_init = "init_prediction_srv" 17 | 18 | prediction = pd.PredictorDistance(name_service_init) 19 | 20 | rospy.spin() 21 | 22 | 23 | -------------------------------------------------------------------------------- /system/predictor/predictor_distance/src/predictor_distance/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Shared-control/improved-apf/85ab7ef5f043752794c52e80f4d1d19e10ef87ff/system/predictor/predictor_distance/src/predictor_distance/__init__.py -------------------------------------------------------------------------------- /system/predictor/predictor_distance/srv/DistancePredictor.srv: -------------------------------------------------------------------------------- 1 | float32[] distance 2 | --- 3 | float32[] distr_prob 4 | uint8 index_max 5 | -------------------------------------------------------------------------------- /system/shared_control/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(shared_control) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | geometry_msgs 12 | rospy 13 | std_msgs 14 | trajectory_msgs 15 | control_manip 16 | message_generation 17 | ) 18 | 19 | ## System dependencies are found with CMake's conventions 20 | # find_package(Boost REQUIRED COMPONENTS system) 21 | 22 | 23 | ## Uncomment this if the package has a setup.py. This macro ensures 24 | ## modules and global scripts declared therein get installed 25 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 26 | catkin_python_setup() 27 | 28 | ################################################ 29 | ## Declare ROS messages, services and actions ## 30 | ################################################ 31 | 32 | ## To declare and build messages, services or actions from within this 33 | ## package, follow these steps: 34 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 35 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 36 | ## * In the file package.xml: 37 | ## * add a build_depend tag for "message_generation" 38 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 39 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 40 | ## but can be declared for certainty nonetheless: 41 | ## * add a exec_depend tag for "message_runtime" 42 | ## * In this file (CMakeLists.txt): 43 | ## * add "message_generation" and every package in MSG_DEP_SET to 44 | ## find_package(catkin REQUIRED COMPONENTS ...) 45 | ## * add "message_runtime" and every package in MSG_DEP_SET to 46 | ## catkin_package(CATKIN_DEPENDS ...) 47 | ## * uncomment the add_*_files sections below as needed 48 | ## and list every .msg/.srv/.action file to be processed 49 | ## * uncomment the generate_messages entry below 50 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 51 | 52 | ## Generate messages in the 'msg' folder 53 | add_message_files( 54 | FILES 55 | InitPredictor.msg 56 | ) 57 | 58 | ## Generate services in the 'srv' folder 59 | add_service_files( 60 | FILES 61 | InitPred.srv 62 | ) 63 | 64 | ## Generate actions in the 'action' folder 65 | # add_action_files( 66 | # FILES 67 | # Action1.action 68 | # Action2.action 69 | # ) 70 | 71 | ## Generate added messages and services with any dependencies listed here 72 | generate_messages( 73 | DEPENDENCIES 74 | geometry_msgs 75 | std_msgs 76 | trajectory_msgs 77 | control_manip 78 | ) 79 | 80 | ################################################ 81 | ## Declare ROS dynamic reconfigure parameters ## 82 | ################################################ 83 | 84 | ## To declare and build dynamic reconfigure parameters within this 85 | ## package, follow these steps: 86 | ## * In the file package.xml: 87 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 88 | ## * In this file (CMakeLists.txt): 89 | ## * add "dynamic_reconfigure" to 90 | ## find_package(catkin REQUIRED COMPONENTS ...) 91 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 92 | ## and list every .cfg file to be processed 93 | 94 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 95 | # generate_dynamic_reconfigure_options( 96 | # cfg/DynReconf1.cfg 97 | # cfg/DynReconf2.cfg 98 | # ) 99 | 100 | ################################### 101 | ## catkin specific configuration ## 102 | ################################### 103 | ## The catkin_package macro generates cmake config files for your package 104 | ## Declare things to be passed to dependent projects 105 | ## INCLUDE_DIRS: uncomment this if your package contains header files 106 | ## LIBRARIES: libraries you create in this project that dependent projects also need 107 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 108 | ## DEPENDS: system dependencies of this project that dependent projects also need 109 | catkin_package( 110 | # INCLUDE_DIRS include 111 | # LIBRARIES shared_control 112 | CATKIN_DEPENDS geometry_msgs rospy std_msgs trajectory_msgs message_runtime 113 | # DEPENDS system_lib 114 | ) 115 | 116 | ########### 117 | ## Build ## 118 | ########### 119 | 120 | ## Specify additional locations of header files 121 | ## Your package locations should be listed before other locations 122 | include_directories( 123 | # include 124 | ${catkin_INCLUDE_DIRS} 125 | ) 126 | 127 | ## Declare a C++ library 128 | # add_library(${PROJECT_NAME} 129 | # src/${PROJECT_NAME}/shared_control.cpp 130 | # ) 131 | 132 | ## Add cmake target dependencies of the library 133 | ## as an example, code may need to be generated before libraries 134 | ## either from message generation or dynamic reconfigure 135 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 136 | 137 | ## Declare a C++ executable 138 | ## With catkin_make all packages are built within a single CMake context 139 | ## The recommended prefix ensures that target names across packages don't collide 140 | # add_executable(${PROJECT_NAME}_node src/shared_control_node.cpp) 141 | 142 | ## Rename C++ executable without prefix 143 | ## The above recommended prefix causes long target names, the following renames the 144 | ## target back to the shorter version for ease of user use 145 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 146 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 147 | 148 | ## Add cmake target dependencies of the executable 149 | ## same as for the library above 150 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 151 | 152 | ## Specify libraries to link a library or executable target against 153 | # target_link_libraries(${PROJECT_NAME}_node 154 | # ${catkin_LIBRARIES} 155 | # ) 156 | 157 | ############# 158 | ## Install ## 159 | ############# 160 | 161 | # all install targets should use catkin DESTINATION variables 162 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 163 | 164 | ## Mark executable scripts (Python etc.) for installation 165 | ## in contrast to setup.py, you can choose the destination 166 | # catkin_install_python(PROGRAMS 167 | # scripts/my_python_script 168 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 169 | # ) 170 | 171 | ## Mark executables for installation 172 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 173 | # install(TARGETS ${PROJECT_NAME}_node 174 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 175 | # ) 176 | 177 | ## Mark libraries for installation 178 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 179 | # install(TARGETS ${PROJECT_NAME} 180 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 181 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 182 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 183 | # ) 184 | 185 | ## Mark cpp header files for installation 186 | # install(DIRECTORY include/${PROJECT_NAME}/ 187 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 188 | # FILES_MATCHING PATTERN "*.h" 189 | # PATTERN ".svn" EXCLUDE 190 | # ) 191 | 192 | ## Mark other files for installation (e.g. launch and bag files, etc.) 193 | # install(FILES 194 | # # myfile1 195 | # # myfile2 196 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 197 | # ) 198 | 199 | ############# 200 | ## Testing ## 201 | ############# 202 | 203 | ## Add gtest based cpp test target and link libraries 204 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_shared_control.cpp) 205 | # if(TARGET ${PROJECT_NAME}-test) 206 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 207 | # endif() 208 | 209 | ## Add folders to be run by python nosetests 210 | # catkin_add_nosetests(test) 211 | -------------------------------------------------------------------------------- /system/shared_control/config/config_params.yaml: -------------------------------------------------------------------------------- 1 | delta_time: 0.02 #seconds 2 | max_abs_velocity: 1 #max value is 5 3 | diagonal: 50 4 | goal_radius: 0.08 #0.05 #0.20 #0.15 #m min value = 0.05 -------------------------------------------------------------------------------- /system/shared_control/config/potential_field.yaml: -------------------------------------------------------------------------------- 1 | attractive_gain: 5 2 | repulsive_gain: 1 3 | threshold_distance: 0.14 #0.08 # m 4 | escape_gain: 1000 5 | 6 | -------------------------------------------------------------------------------- /system/shared_control/launch/start.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /system/shared_control/msg/InitPredictor.msg: -------------------------------------------------------------------------------- 1 | control_manip/GoalArray goals 2 | geometry_msgs/Pose ee_pose 3 | -------------------------------------------------------------------------------- /system/shared_control/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | shared_control 4 | 0.0.2 5 | The shared_control package 6 | 7 | 8 | 9 | 10 | Alberto Gottardi 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | geometry_msgs 53 | rospy 54 | std_msgs 55 | trajectory_msgs 56 | geometry_msgs 57 | rospy 58 | std_msgs 59 | trajectory_msgs 60 | geometry_msgs 61 | rospy 62 | std_msgs 63 | trajectory_msgs 64 | 65 | message_generation 66 | message_runtime 67 | 68 | control_manip 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | -------------------------------------------------------------------------------- /system/shared_control/setup.py: -------------------------------------------------------------------------------- 1 | from distutils.core import setup 2 | from catkin_pkg.python_setup import generate_distutils_setup 3 | 4 | d = generate_distutils_setup( 5 | packages = ['shared_control'], 6 | package_dir = {'': 'src'} 7 | ) 8 | 9 | setup(**d) 10 | -------------------------------------------------------------------------------- /system/shared_control/src/shared_control/Clik.py: -------------------------------------------------------------------------------- 1 | #coding=utf-8 2 | 3 | import numpy as np 4 | import Utils 5 | import copy 6 | 7 | class Clik: 8 | """ 9 | Closed Loop Inverse Kinematics algorithm \n 10 | Args: 11 | K: definite positive matrix 12 | delta_t: interval time 13 | rob_kin: RobotKinematics object 14 | init_q: initial joints vector 15 | """ 16 | def __init__(self, K, delta_t, rob_kin, init_q): 17 | self._K = K 18 | self._delta_t = delta_t 19 | self._rob_kin = rob_kin 20 | self._init_q = init_q 21 | self._q_clik = np.zeros(6) 22 | self._ee_pose_clik = np.zeros((4,4)) 23 | self._indexEE = len(init_q) 24 | 25 | self._limit_z = 0.90 26 | 27 | def computeCLIK(self, actual_position, twist_ee, jacobian, new_q): 28 | """ 29 | Use CLIK to compute new joints vector q and ee pose \n 30 | Args: 31 | actual_position: actual EE position (x,y,z) 32 | twist_ee: twist for EE 33 | jacobian: jacobian matrix 34 | Return: 35 | new joints vector q 36 | new pose of EE 37 | final twist used to compute new joints vector and ee pose 38 | """ 39 | self._init_q = new_q 40 | twist_f = np.zeros(6) 41 | done = self.doCLIK(actual_position, twist_ee, jacobian) 42 | if(not done): 43 | tw_ee = copy.copy(twist_ee) 44 | tw_ee[2] = 0 45 | print("New FINAL TWIST: " +str(tw_ee)) 46 | done = self.doCLIK(actual_position, tw_ee, jacobian) 47 | twist_f = tw_ee 48 | 49 | return self._q_clik, self._ee_pose_clik, twist_f 50 | 51 | def doCLIK(self, actual_position, twist_ee, jacobian): 52 | """ 53 | CLIK computation \n 54 | Args: 55 | actual_position: actual EE position (x,y,z) 56 | twist_ee: twist for EE 57 | jacobian: jacobian matrix 58 | Return: True if computation is done, False otherwise 59 | """ 60 | #compute inverse jacobian 61 | jacob_inv = self._rob_kin.computeInverseJacobian(jacobian) 62 | #compute vel joints 63 | vel_joints = np.dot(jacob_inv, twist_ee) 64 | #compute new joints angles q 65 | new_q = self._init_q + vel_joints * self._delta_t 66 | #compute error e = x_d - x_e 67 | x_e = self._rob_kin.getPosition(new_q, self._indexEE) 68 | x_d = self.nextPoint(twist_ee, actual_position) 69 | err = Utils.computeError(x_e, x_d) 70 | #compute new joints velocities and angles 71 | st = twist_ee + np.dot(self._K, err) 72 | vel_joints_clik = np.dot(jacob_inv, st) 73 | #Euler Integration 74 | self._q_clik = self._init_q + vel_joints_clik * self._delta_t 75 | #Matrix EE pose 76 | self._ee_pose_clik = self._rob_kin.get_pose(self._q_clik, self._indexEE) 77 | 78 | #Check the limit in Z 79 | if(self._ee_pose_clik[2,3] <= self._limit_z): 80 | print("LIMITE IN Z") 81 | return False 82 | 83 | #Update 84 | self._init_q = self._q_clik 85 | return True 86 | 87 | def getQClik(self): 88 | """ 89 | Get joints vector q from CLIK \n 90 | Return: joints vector q 91 | """ 92 | return copy.copy(self._q_clik) 93 | 94 | def getEEPoseClik(self): 95 | """ 96 | Get EE pose from CLIK \n 97 | Return: end effector pose 98 | """ 99 | return copy.copy(self._ee_pose_clik) 100 | 101 | def nextPoint(self, twist, actual_position): 102 | """ 103 | Compute next position in ideal trajectory \n 104 | Args: 105 | twist_user: End_Effector input velocity 106 | Return: next position 107 | """ 108 | d = twist[0:3] * self._delta_t 109 | #compute next position 110 | new_position = actual_position + d 111 | return new_position 112 | 113 | 114 | -------------------------------------------------------------------------------- /system/shared_control/src/shared_control/Csp.py: -------------------------------------------------------------------------------- 1 | #coding=utf-8 2 | 3 | import numpy as np 4 | import sys 5 | 6 | 7 | class CSP: 8 | """ 9 | CSP class 10 | """ 11 | def __init__(self): 12 | self._goals = list() 13 | self._escape_points = list() 14 | self._constraints_arc = list() 15 | 16 | 17 | def setGoals(self, goals): 18 | """ 19 | id_goal -> probability \n 20 | self._goals is a list of tuple(id_goal, probability) \n 21 | Args: 22 | goals: list of tuple(id_goal, probability) 23 | """ 24 | self._goals = goals 25 | 26 | 27 | def setEscapePoints(self, escape_p): 28 | """ 29 | id_escape_point -> value = number in [0,1] of the reciprocal of the distance from EE to escape point \n 30 | self._escape_points is a list of tuple(id_escape_p, value) \n 31 | Args: 32 | escape_p: list of tuple(id_escape_p, value) 33 | """ 34 | self._escape_points = escape_p 35 | 36 | 37 | def setConstraintsArc(self, distances): 38 | """ 39 | (id_goal, id_escape_point) -> value = number in [0,1] of the reciprocal of the distance from goal to escape point \n 40 | self._constraints_arc is a list of tuple(id_goal, id_escape_p, value) \n 41 | Args: 42 | distances: list of tuple(id_goal, id_escape_p, value) 43 | """ 44 | self._constraints_arc = distances 45 | 46 | 47 | def fuzzySCSP(self, goals, escape_p, distances): 48 | """ 49 | Start fuzzy soft CSP \n 50 | Args: 51 | goals: list of tuple(id_goal, probability) 52 | escape_p: list of tuple(id_escape_p, value) 53 | distances: list of tuple(id_goal, id_escape_p, value) 54 | Return: tuple (id_goal, id_escape_point, value) result of projection 55 | """ 56 | #Set constraints 57 | self.setGoals(goals) 58 | self.setEscapePoints(escape_p) 59 | self.setConstraintsArc(distances) 60 | 61 | #Combining 62 | result_comb = self.fuzzyCombining() 63 | #Projection 64 | result_proj = self.fuzzyProjection(result_comb) 65 | 66 | return result_proj 67 | 68 | 69 | def fuzzyCombining(self): 70 | """ 71 | Combining = take min value \n 72 | Return: list of results of combining 73 | """ 74 | #result_combining is a list of tuple(id_goal, id_escape_p, min) 75 | results_combining = list() 76 | for c1 in self._goals: 77 | id_goal = c1[0] 78 | prob_goal = c1[1] 79 | for c2 in self._escape_points: 80 | id_escape_p = c2[0] 81 | value_escape_p = c2[1] 82 | #Compute c3 value 83 | value_constraint = self.findC3Value(id_goal, id_escape_p) 84 | if(value_constraint is None): 85 | print("Error: c3 value is NONE") 86 | sys.exit() 87 | #Compute min 88 | min_value = min(prob_goal, value_escape_p, value_constraint) 89 | #create tuple (id_goal, id_escape_p, min) and insert it into results_combining list 90 | tmp_tuple = (id_goal, id_escape_p, min_value) 91 | results_combining.append(tmp_tuple) 92 | 93 | return results_combining 94 | 95 | 96 | def fuzzyProjection(self, combining_result): 97 | """ 98 | Projection = take max value \n 99 | Return: tuple (id_goal, id_escape_point, value) result of projection 100 | """ 101 | tmp_max = -1 102 | final_tuple = None 103 | for tpe in combining_result: 104 | if(tpe[2] > tmp_max): 105 | tmp_max = tpe[2] 106 | final_tuple = tpe 107 | 108 | return final_tuple 109 | 110 | 111 | def findC3Value(self, id_g, id_ep): 112 | """ 113 | C3 is the tuple in self._constraints_arc in the form (id_goal, id_escape_p, value) \n 114 | Args: 115 | id_g: id of the goal 116 | id_ep: id of the escape point 117 | Return: value of c3 with (id_g, id_ep) 118 | """ 119 | for c3 in self._constraints_arc: 120 | if((c3[0] == id_g) and (c3[1] == id_ep)): 121 | c3_value = c3[2] 122 | return c3_value 123 | 124 | return None 125 | 126 | -------------------------------------------------------------------------------- /system/shared_control/src/shared_control/Goal.py: -------------------------------------------------------------------------------- 1 | #coding=utf-8 2 | 3 | import numpy as np 4 | 5 | class Goal: 6 | """ 7 | Goal Class \n 8 | Args: 9 | id: ID object 10 | pose: center pose 11 | grasp_points: list of grasping points 12 | """ 13 | def __init__(self, id, pose, grasp_points): 14 | self._id = id 15 | self._pose_center = pose 16 | self._grasp_points = grasp_points 17 | self._probability = 0 18 | self._distance = float('inf') 19 | 20 | def getID(self): 21 | """ 22 | Get ID object associated with the goal \n 23 | Return: ID object associated with the goal 24 | """ 25 | return self._id 26 | 27 | def getCenterPosition(self): 28 | """ 29 | Get center position of object associated with the goal \n 30 | Return: center of object 31 | """ 32 | return self._pose_center[0:3,3] 33 | 34 | def getCenterMatrix(self): 35 | """ 36 | Get center matrix pose of object associated with the goal \n 37 | Return: center matrix pose of object 38 | """ 39 | return self._pose_center 40 | 41 | def getGraspingPoints(self): 42 | """ 43 | Get grasping points associated with the goal \n 44 | Return: grasping points 45 | """ 46 | return self._grasp_points 47 | 48 | def getDistance(self): 49 | """ 50 | Get distance from goal to ee_position \n 51 | Return: distance from goal to ee 52 | """ 53 | return self._distance 54 | 55 | def getProbability(self): 56 | """ 57 | Get probability associated with the goal \n 58 | Return: probability of goal 59 | """ 60 | return self._probability 61 | 62 | def updateDistance(self, new_distance): 63 | """ 64 | Update distance from goal to ee position \n 65 | Args: 66 | new_distance: new distance from goal to ee 67 | """ 68 | self._distance = new_distance 69 | 70 | def printGoal(self): 71 | print("ID: " +str(self._id) ) 72 | print("center: " +str(self._pose_center)) 73 | print("Grasp: " +str(self._grasp_points)) 74 | 75 | def updateProbability(self, new_prob): 76 | """ 77 | Update probability \n 78 | Args: 79 | new_prob: new probability 80 | """ 81 | self._probability = new_prob 82 | 83 | 84 | 85 | -------------------------------------------------------------------------------- /system/shared_control/src/shared_control/PrintFile.py: -------------------------------------------------------------------------------- 1 | import os.path 2 | import numpy as np 3 | 4 | class PrintFile: 5 | """ 6 | Class PrintFile \n 7 | Args: 8 | predictor_type: type of predictor 9 | index_test: index of the test 10 | name_user_test: name of the user 11 | input_type: name of user input control type 12 | """ 13 | def __init__(self, predictor_type, index_test, name_user_test, input_type): 14 | self._my_path = os.path.abspath(os.path.dirname(__file__)) 15 | self._path = os.path.split(self._my_path) 16 | self._new_path = os.path.split(self._path[0]) 17 | self._final_path = None 18 | self._prefix_name = None 19 | self._file = None 20 | 21 | #Create directory 22 | self.createDir(predictor_type, name_user_test, input_type) 23 | 24 | #Create and open file 25 | self.createFile(predictor_type, index_test, name_user_test, input_type) 26 | 27 | 28 | def createFile(self, predictor_type, index_test, name_user_test, input_type): 29 | """ 30 | Create and open file file \n 31 | Args: 32 | predictor_type: type of predictor 33 | index_test: index of the test 34 | name_user_test: name of the user 35 | input_type: name of user input control type 36 | """ 37 | len_predictor = len(predictor_type) 38 | prefix_predictor = None 39 | if(len_predictor < 3): 40 | prefix_predictor = predictor_type[0:len_predictor] 41 | else: 42 | prefix_predictor = predictor_type[0:3] 43 | 44 | prefix_user = None 45 | len_user = len(name_user_test) 46 | if(len_user < 5): 47 | prefix_user = name_user_test[0:len_user] 48 | else: 49 | prefix_user = name_user_test[0:5] 50 | 51 | prefix_input = None 52 | len_input = len(input_type) 53 | if(len_input < 3): 54 | prefix_input = input_type[0:len_input] 55 | else: 56 | prefix_input = input_type[0:3] 57 | 58 | self._prefix_name = prefix_predictor + "_" + prefix_user + "_" + prefix_input 59 | 60 | suffix_name = "_test_" + str(index_test) + ".txt" 61 | 62 | #Create name of the file as suffix_name + prefix_name 63 | final_doc_name = self._prefix_name + suffix_name 64 | 65 | #String with path of the file 66 | final_path_doc_name = self._final_path + "/" + final_doc_name 67 | 68 | #Open 69 | self._file = open(final_path_doc_name, 'w+') 70 | 71 | 72 | def createDir(self, predictor_type, name_user_test, input_type): 73 | """ 74 | Create directory \n 75 | Args: 76 | predictor_type: type of predictor 77 | index_test: index of the test 78 | input_type: name of user input control type 79 | """ 80 | final_path = os.path.join(self._new_path[0], "stat") 81 | if(not os.path.exists(final_path)): 82 | os.mkdir(final_path) 83 | 84 | final_path = os.path.join(final_path, name_user_test) 85 | if(not os.path.exists(final_path)): 86 | os.mkdir(final_path) 87 | 88 | final_path = os.path.join(final_path, predictor_type) 89 | if(not os.path.exists(final_path)): 90 | os.mkdir(final_path) 91 | 92 | 93 | self._final_path = os.path.join(final_path, input_type) 94 | if(not os.path.exists(self._final_path)): 95 | os.mkdir(self._final_path) 96 | 97 | 98 | def write(self, info): 99 | """ 100 | Write info \n 101 | Args: 102 | info: info to write in the file 103 | """ 104 | string = str(info) + "\n" 105 | self._file.write(string) 106 | 107 | 108 | def write_with_title(self, info, title): 109 | """ 110 | Write info with title \n 111 | Args: 112 | info: info to write in the file 113 | title: title of the info 114 | """ 115 | arr = None 116 | if(isinstance(info, np.ndarray)): 117 | arr = str(info[0]) + " " + str(info[1]) + " " + str(info[2]) 118 | elif(isinstance(info, list) or isinstance(info, tuple)): 119 | arr = ' '.join(map(str, info)) 120 | else: 121 | arr = info 122 | string = str(title) + ": " + str(arr) + "\n" 123 | self._file.write(string) 124 | 125 | 126 | def end_block(self): 127 | """ 128 | Write === to divide two blocks 129 | """ 130 | string = '===\n' 131 | self._file.write(string) 132 | 133 | 134 | def close(self): 135 | self._file.flush() 136 | self._file.close() 137 | 138 | 139 | def newFile(self, index_test): 140 | """ 141 | Create and open a new file \n 142 | Args: 143 | index_test: index of the test to save 144 | """ 145 | suffix_name = "_test_" + str(index_test) + ".txt" 146 | 147 | #Create name of the file as suffix_name + prefix_name 148 | final_doc_name = self._prefix_name + suffix_name 149 | 150 | #String with path of the file 151 | final_path_doc_name = self._final_path + "/" + final_doc_name 152 | 153 | #Open 154 | self._file = open(final_path_doc_name, 'w+') 155 | 156 | -------------------------------------------------------------------------------- /system/shared_control/src/shared_control/SharedControlNode.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | #coding=utf-8 3 | 4 | import rospy 5 | import SharedControl 6 | import numpy as np 7 | import sympy as smp 8 | import sys 9 | 10 | 11 | 12 | def getDHParams(robot_type): 13 | """ 14 | Get Denavit-Hartenberg params of UR 5/10 \n 15 | Args: 16 | robot_type: robot type 17 | Return: DH params 18 | qsym: theta 19 | a: a 20 | d: d 21 | alpha: alpha 22 | """ 23 | theta1, theta2, theta3, theta4, theta5, theta6 = smp.symbols('theta1, theta2, theta3, theta4, theta5, theta6') 24 | #list of angles (symbolic thetas) 25 | qsym = [theta1, theta2, theta3, theta4, theta5, theta6] 26 | if((robot_type == "ur10") or (robot_type == "/ur10")): 27 | d = [0.1273, 0, 0, 0.163941, 0.1157, 0.0922] 28 | a = [0, -0.612, -0.5723, 0, 0, 0] 29 | alpha = [smp.pi/2, 0, 0, smp.pi/2, -smp.pi/2, 0] 30 | 31 | elif((robot_type == "ur5") or (robot_type == "/ur5")): 32 | d = [0.089159, 0, 0, 0.10915, 0.09465, 0.0823] 33 | a = [0, -0.425, -0.39225, 0, 0, 0] 34 | alpha = [smp.pi/2, 0, 0, smp.pi/2, -smp.pi/2, 0] 35 | 36 | else: 37 | rospy.logerr("Error: Invalid robot name!") 38 | rospy.on_shutdown("Invalid robot name!") 39 | sys.exit() 40 | 41 | dhparams = [qsym, a, d, alpha] 42 | 43 | return dhparams 44 | 45 | if __name__ == "__main__": 46 | rospy.init_node("shared_control_node", anonymous=True) 47 | 48 | #Get all configuration parameters 49 | #Potential field params 50 | threshold_distance = rospy.get_param("threshold_distance", 0.1) 51 | attractive_gain = rospy.get_param("attractive_gain", 5) 52 | repulsive_gain = rospy.get_param("repulsive_gain", 1) 53 | escape_gain = rospy.get_param("escape_gain", 1000) 54 | potential_params = [threshold_distance, repulsive_gain, attractive_gain, escape_gain] 55 | print("Threshold distance: " +str(potential_params[0])) 56 | 57 | #CLIK params 58 | delta_time = rospy.get_param("delta_time", 0.02) 59 | vmax = rospy.get_param("max_abs_velocity", 1) 60 | diag = rospy.get_param("diagonal", 50) 61 | goal_radius = rospy.get_param("goal_radius", 0.20) 62 | config_params = [delta_time, vmax, diag, goal_radius] 63 | print("Goal radius: " +str(goal_radius)) 64 | 65 | #Only teleoperation 66 | teleop = rospy.get_param("~teleop", False) 67 | print("Only teleoperation control: " +str(teleop)) 68 | 69 | #Type of predictor 70 | predictor_type = "distance" 71 | distance_pred = rospy.get_param("~distance_predictor", False) 72 | if(not distance_pred): 73 | predictor_type = "max_ent_ioc" 74 | print("Type of predictor: " +str(predictor_type)) 75 | 76 | #Robot type 77 | robot_type = rospy.get_param("~robot_type", "/ur5") 78 | print("Robot Type: " + str(robot_type)) 79 | 80 | #Grasping 81 | grasp = rospy.get_param("~grasp", False) 82 | print("Grasp node active: " +str(grasp)) 83 | 84 | #User control 85 | user_input_type = rospy.get_param("~user_type", "keyboard") 86 | print("User control type: " +str(user_input_type)) 87 | 88 | #Only for save test 89 | index_test = rospy.get_param("~index_test", 0) 90 | print("Index test: " + str(index_test)) 91 | 92 | #Denavit-Hartenberg params 93 | DH_params = getDHParams(robot_type) 94 | 95 | #Name of the user that esecute test 96 | name_user_test = rospy.get_param("~name_user_test", "user") 97 | print("Name user: " +str(name_user_test)) 98 | 99 | dynamic_system = rospy.get_param("~dynamic", False) 100 | print("Dynamic system: " +str(dynamic_system)) 101 | 102 | gripper_active = rospy.get_param("~gripper", False) 103 | print("Gripper enable: " +str(gripper_active)) 104 | 105 | escape_active = rospy.get_param("~escape", True) 106 | print("Escape active: " +str(escape_active)) 107 | 108 | #Start 109 | shared_control = SharedControl.SharedControl(potential_params, config_params, teleop, predictor_type, robot_type, grasp, user_input_type, index_test, 110 | DH_params, name_user_test, dynamic_system, gripper_active, escape_active) 111 | 112 | 113 | shared_control.start() 114 | 115 | 116 | print("Finish: turn off system") 117 | 118 | -------------------------------------------------------------------------------- /system/shared_control/src/shared_control/UserInput.py: -------------------------------------------------------------------------------- 1 | #coding=utf-8 2 | 3 | import rospy 4 | from std_msgs.msg import String 5 | from geometry_msgs.msg import TwistStamped 6 | 7 | from keyboard.msg import KeyCommand 8 | from joystick.msg import JoyCommand 9 | from myo.msg import MyoCommand 10 | 11 | import sys 12 | import time 13 | import numpy as np 14 | import copy 15 | 16 | class UserInput: 17 | """ 18 | Class to manage User Input Command \n 19 | Args: 20 | robot type: robot type 21 | user_input_type: type of user input 22 | """ 23 | def __init__(self, robot_type, user_input_type): 24 | self._robot_type = robot_type 25 | self._user_input_type = user_input_type 26 | self._name_topic_user_command = robot_type + "/user_command" 27 | 28 | #Subscriber user command 29 | if(self._user_input_type == "myo"): 30 | rospy.Subscriber(self._name_topic_user_command, MyoCommand, self.callbackUserMyoCommand) 31 | elif(self._user_input_type == "joystick"): 32 | rospy.Subscriber(self._name_topic_user_command, JoyCommand, self.callbackUserJoyCommand) 33 | else: 34 | rospy.Subscriber(self._name_topic_user_command, KeyCommand, self.callbackUserKeyCommand) 35 | 36 | self._twist_user = np.zeros(6) 37 | self._twist_user_time = rospy.Time.now() 38 | self._command = None 39 | 40 | 41 | def callbackUserMyoCommand(self, msg): 42 | if(msg.command == MyoCommand.TWIST): 43 | self.setTwist(msg.twist) 44 | self._command = msg.command 45 | 46 | 47 | def callbackUserJoyCommand(self, msg): 48 | if(msg.command == JoyCommand.TWIST): 49 | self.setTwist(msg.twist) 50 | self._command = msg.command 51 | 52 | 53 | def callbackUserKeyCommand(self, msg): 54 | if(msg.command == KeyCommand.TWIST): 55 | self.setTwist(msg.twist) 56 | self._command = msg.command 57 | 58 | 59 | def setTwist(self, twist): 60 | #twist with linear and angular components 61 | self._twist_user[0:3] = 10 * np.array([twist.linear.x, twist.linear.y, twist.linear.z]) 62 | self._twist_user[3:6] = 10 * np.array([twist.angular.x, twist.angular.y, twist.angular.z]) 63 | 64 | 65 | def getTwist(self): 66 | return copy.copy(self._twist_user) 67 | 68 | 69 | def getCommand(self): 70 | return copy.copy(self._command) 71 | 72 | -------------------------------------------------------------------------------- /system/shared_control/src/shared_control/Utils.py: -------------------------------------------------------------------------------- 1 | #coding=utf-8 2 | 3 | import numpy as np 4 | import sympy as smp 5 | import copy 6 | import math 7 | import sys 8 | import tf.transformations as transmethods 9 | from geometry_msgs.msg import Pose, Twist, PoseArray 10 | import Goal 11 | 12 | 13 | def computeDistanceXY(from_position, to_position): 14 | """ 15 | Compute distance in (x,y) \n 16 | Args: 17 | from_position: actual position 18 | to_position: target position 19 | Return: distance 20 | """ 21 | position_from = copy.copy(from_position[0:2]) 22 | position_to = copy.copy(to_position[0:2]) 23 | 24 | act_pos = np.array(position_from) 25 | tar_pos = np.array(position_to) 26 | distance = np.linalg.norm(tar_pos - act_pos) 27 | return distance 28 | 29 | def computeDistance(from_position, to_position): 30 | """ 31 | Compute distance in (x,y,z) \n 32 | Args: 33 | from_position: actual position 34 | to_position: target position 35 | Return: distance 36 | """ 37 | act_pos = np.array(from_position) 38 | tar_pos = np.array(to_position) 39 | distance = np.linalg.norm(tar_pos - act_pos) 40 | return distance 41 | 42 | def computeError(x_e, x_d): 43 | """ 44 | Compute error from desired and actual position \n 45 | Args: 46 | x_e: actual position 47 | x_d: desired position 48 | Return: error e = x_d - x_e 49 | """ 50 | err = np.zeros(6) 51 | err[0:3] = x_d[0:3] - x_e[0:3] 52 | return err 53 | 54 | def twistMsgToArray(msg): 55 | """ 56 | Convert twist msg into numpy array \n 57 | Args: 58 | msg: twist msg 59 | Return: twist numpy array 60 | """ 61 | twist = np.zeros(6) 62 | twist[0:3] = np.array([msg.linear.x, msg.linear.y, msg.linear.z]) 63 | twist[3:6] = np.array([msg.angular.x, msg.angular.y, msg.angular.z]) 64 | return twist 65 | 66 | def arrayToTwistMsg(array): 67 | """ 68 | Convert numpy array into twist message \n 69 | Args: 70 | array: numpy array 71 | Return: twist message 72 | """ 73 | twist = Twist() 74 | twist.linear.x = array[0] 75 | twist.linear.y = array[1] 76 | twist.linear.z = array[2] 77 | twist.angular.x = array[3] 78 | twist.angular.y = array[4] 79 | twist.angular.z = array[5] 80 | return twist 81 | 82 | def matrixToPoseMsg(matrix): 83 | """ 84 | Convert a 4x4 numpy matrix to a Pose message \n 85 | Args: 86 | matrix: 4x4 numpy matrix 87 | Return: Pose msg 88 | """ 89 | pose = Pose() 90 | pose.position.x = matrix[0,3] 91 | pose.position.y = matrix[1,3] 92 | pose.position.z = matrix[2,3] 93 | quat = transmethods.quaternion_from_matrix(matrix) 94 | pose.orientation.x = quat[0] 95 | pose.orientation.y = quat[1] 96 | pose.orientation.z = quat[2] 97 | pose.orientation.w = quat[3] 98 | return pose 99 | 100 | def PoseMsgToMatrix(pose_msg): 101 | """ 102 | Convert Pose message to a 4x4 numpy matrix \n 103 | Args: 104 | pose_msg: Pose message 105 | Return: 4x4 numpy matrix 106 | """ 107 | matrix = np.zeros((4,4)) 108 | matrix[3,3] = 1 109 | matrix[0:3, 3] = np.array([pose_msg.position.x, pose_msg.position.y, pose_msg.position.z]) 110 | q = [pose_msg.orientation.x, pose_msg.orientation.y, pose_msg.orientation.z, pose_msg.orientation.w] 111 | matrix[0:3, 0:3] = np.array(transmethods.quaternion_matrix(q))[0:3, 0:3] 112 | 113 | return matrix 114 | 115 | def setTwist(twist, vmax): 116 | """ 117 | Set limits for ee twist \n 118 | Args: 119 | twist: twist ee 120 | vmax: absolute value of velocity 121 | Return: twist 122 | """ 123 | x = twist[0] 124 | y = twist[1] 125 | z = twist[2] 126 | 127 | module_v = math.sqrt(x**2 + y**2 + z**2) 128 | x = vmax * x/module_v 129 | y = vmax * y/module_v 130 | z = vmax * z/module_v 131 | 132 | #only linear components 133 | newTwist = np.zeros(6) 134 | newTwist[0] = x 135 | newTwist[1] = y 136 | newTwist[2] = z 137 | #newTwist[3:6] = twist[3:6] 138 | 139 | return newTwist 140 | 141 | def getGoal(goal_msg): 142 | """ 143 | Create and get list of Goal obj \n 144 | Args: 145 | goal_msg: msg of goal 146 | Return: list of goal and positions 147 | goal_list: list of Goal obj 148 | target_pos: positions of goals 149 | """ 150 | goal_list = [] 151 | target_pos = [] 152 | for g in goal_msg.goal: 153 | grasp_points = [] 154 | id = g.id 155 | center = PoseMsgToMatrix(g.center) 156 | #tmp_c = np.array([g.center.position.x, g.center.position.y, g.center.position.z]) 157 | tmp_c = np.array([g.grasping_points[0].pose.position.x, g.grasping_points[0].pose.position.y, g.grasping_points[0].pose.position.z]) 158 | target_pos.append(tmp_c) 159 | for grasp in g.grasping_points: 160 | grasp_points.append(grasp) 161 | goal = Goal.Goal(id, center, grasp_points) 162 | goal_list.append(goal) 163 | 164 | return goal_list, target_pos 165 | 166 | def getListPoints(msg): 167 | """ 168 | Get list of points(numpy vector) from msg \n 169 | Args: 170 | msg: geometry_msgs PoseArray 171 | Return: list of points 172 | """ 173 | point_list = [] 174 | for point in msg.poses: 175 | tmp = np.array([point.position.x, point.position.y, point.position.z]) 176 | point_list.append(tmp) 177 | return point_list 178 | 179 | def getInitJoints(joints_msg): 180 | """ 181 | Return numpy array of initial value of joints (vector q) \n 182 | Args: 183 | joints_msg: joints msg 184 | Return: initial value of joints 185 | """ 186 | joint = np.array([joints_msg.positions[0], joints_msg.positions[1], joints_msg.positions[2], 187 | joints_msg.positions[3], joints_msg.positions[4], joints_msg.positions[5]]) 188 | return joint 189 | 190 | def getMinDistance(points_set, point_x): 191 | """ 192 | Get min distance from point x to set of points (x not in the set) \n 193 | Args: 194 | points_set: list of points 195 | point_x: point x 196 | Return: minimum distance 197 | """ 198 | distance = float('inf') 199 | for p in points_set: 200 | dist = computeDistance(point_x, p) 201 | if(dist < distance): 202 | distance = dist 203 | 204 | return distance 205 | 206 | def checkDimension(point1, point2): 207 | """ 208 | Check if the two points are same shape and dimension (size) \n 209 | Args: 210 | point1: point 1 211 | point2: point 2 212 | Return: True if two points are same dimsnione and shape, False otherwise 213 | """ 214 | if((not isinstance(point1, np.ndarray)) or (not isinstance(point2, np.ndarray))): 215 | print("Type of point1: " +str(type(point1))) 216 | print("Type of point2: " +str(type(point2))) 217 | print("One of the points aren't a numpy ndarray!") 218 | return False 219 | if(np.shape(point1) != np.shape(point2)): 220 | print("Shape of the two points is different!") 221 | return False 222 | if(np.size(point1) != np.size(point2)): 223 | print("Two points are different dimensions") 224 | return False 225 | return True 226 | 227 | def compareVector(a, b): 228 | """ 229 | Compare two numpy array (a, b) \n 230 | Args: 231 | a: numpy array 232 | b: numpy array 233 | Return: True if a = b, False otherwise 234 | """ 235 | if((not isinstance(a, np.ndarray)) or (not isinstance(b, np.ndarray))): 236 | print("Type of a: " +str(type(a))) 237 | print("Type of b: " +str(type(b))) 238 | print("One of the points aren't a numpy ndarray!") 239 | return False 240 | 241 | comparison = a == b 242 | if(not comparison.all()): 243 | return False 244 | return True 245 | 246 | def truncate(number, decimals=5): 247 | """ 248 | Truncate number \n 249 | Args: 250 | number: number 251 | decimals: number of decimals (default = 5) 252 | Return: truncate number 253 | """ 254 | stepper = 10.0 ** decimals 255 | return math.trunc(stepper * number) / stepper 256 | 257 | 258 | -------------------------------------------------------------------------------- /system/shared_control/src/shared_control/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Shared-control/improved-apf/85ab7ef5f043752794c52e80f4d1d19e10ef87ff/system/shared_control/src/shared_control/__init__.py -------------------------------------------------------------------------------- /system/shared_control/srv/InitPred.srv: -------------------------------------------------------------------------------- 1 | control_manip/GoalArray goals 2 | geometry_msgs/Pose ee_pose 3 | --- 4 | bool done -------------------------------------------------------------------------------- /system/user_input/joystick/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(joystick) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | geometry_msgs 12 | rospy 13 | sensor_msgs 14 | std_msgs 15 | message_generation 16 | ) 17 | 18 | ## System dependencies are found with CMake's conventions 19 | # find_package(Boost REQUIRED COMPONENTS system) 20 | 21 | 22 | ## Uncomment this if the package has a setup.py. This macro ensures 23 | ## modules and global scripts declared therein get installed 24 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 25 | catkin_python_setup() 26 | 27 | ################################################ 28 | ## Declare ROS messages, services and actions ## 29 | ################################################ 30 | 31 | ## To declare and build messages, services or actions from within this 32 | ## package, follow these steps: 33 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 34 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 35 | ## * In the file package.xml: 36 | ## * add a build_depend tag for "message_generation" 37 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 38 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 39 | ## but can be declared for certainty nonetheless: 40 | ## * add a exec_depend tag for "message_runtime" 41 | ## * In this file (CMakeLists.txt): 42 | ## * add "message_generation" and every package in MSG_DEP_SET to 43 | ## find_package(catkin REQUIRED COMPONENTS ...) 44 | ## * add "message_runtime" and every package in MSG_DEP_SET to 45 | ## catkin_package(CATKIN_DEPENDS ...) 46 | ## * uncomment the add_*_files sections below as needed 47 | ## and list every .msg/.srv/.action file to be processed 48 | ## * uncomment the generate_messages entry below 49 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 50 | 51 | ## Generate messages in the 'msg' folder 52 | add_message_files( 53 | FILES 54 | JoyCommand.msg 55 | ) 56 | 57 | ## Generate services in the 'srv' folder 58 | # add_service_files( 59 | # FILES 60 | # Service1.srv 61 | # Service2.srv 62 | # ) 63 | 64 | ## Generate actions in the 'action' folder 65 | # add_action_files( 66 | # FILES 67 | # Action1.action 68 | # Action2.action 69 | # ) 70 | 71 | ## Generate added messages and services with any dependencies listed here 72 | generate_messages( 73 | DEPENDENCIES 74 | geometry_msgs 75 | std_msgs 76 | ) 77 | 78 | ################################################ 79 | ## Declare ROS dynamic reconfigure parameters ## 80 | ################################################ 81 | 82 | ## To declare and build dynamic reconfigure parameters within this 83 | ## package, follow these steps: 84 | ## * In the file package.xml: 85 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 86 | ## * In this file (CMakeLists.txt): 87 | ## * add "dynamic_reconfigure" to 88 | ## find_package(catkin REQUIRED COMPONENTS ...) 89 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 90 | ## and list every .cfg file to be processed 91 | 92 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 93 | # generate_dynamic_reconfigure_options( 94 | # cfg/DynReconf1.cfg 95 | # cfg/DynReconf2.cfg 96 | # ) 97 | 98 | ################################### 99 | ## catkin specific configuration ## 100 | ################################### 101 | ## The catkin_package macro generates cmake config files for your package 102 | ## Declare things to be passed to dependent projects 103 | ## INCLUDE_DIRS: uncomment this if your package contains header files 104 | ## LIBRARIES: libraries you create in this project that dependent projects also need 105 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 106 | ## DEPENDS: system dependencies of this project that dependent projects also need 107 | catkin_package( 108 | # INCLUDE_DIRS include 109 | # LIBRARIES joystick 110 | CATKIN_DEPENDS geometry_msgs rospy sensor_msgs std_msgs message_runtime 111 | # DEPENDS system_lib 112 | ) 113 | 114 | ########### 115 | ## Build ## 116 | ########### 117 | 118 | ## Specify additional locations of header files 119 | ## Your package locations should be listed before other locations 120 | include_directories( 121 | # include 122 | ${catkin_INCLUDE_DIRS} 123 | ) 124 | 125 | ## Declare a C++ library 126 | # add_library(${PROJECT_NAME} 127 | # src/${PROJECT_NAME}/joystick.cpp 128 | # ) 129 | 130 | ## Add cmake target dependencies of the library 131 | ## as an example, code may need to be generated before libraries 132 | ## either from message generation or dynamic reconfigure 133 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 134 | 135 | ## Declare a C++ executable 136 | ## With catkin_make all packages are built within a single CMake context 137 | ## The recommended prefix ensures that target names across packages don't collide 138 | # add_executable(${PROJECT_NAME}_node src/joystick_node.cpp) 139 | 140 | ## Rename C++ executable without prefix 141 | ## The above recommended prefix causes long target names, the following renames the 142 | ## target back to the shorter version for ease of user use 143 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 144 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 145 | 146 | ## Add cmake target dependencies of the executable 147 | ## same as for the library above 148 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 149 | 150 | ## Specify libraries to link a library or executable target against 151 | # target_link_libraries(${PROJECT_NAME}_node 152 | # ${catkin_LIBRARIES} 153 | # ) 154 | 155 | ############# 156 | ## Install ## 157 | ############# 158 | 159 | # all install targets should use catkin DESTINATION variables 160 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 161 | 162 | ## Mark executable scripts (Python etc.) for installation 163 | ## in contrast to setup.py, you can choose the destination 164 | # catkin_install_python(PROGRAMS 165 | # scripts/my_python_script 166 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 167 | # ) 168 | 169 | ## Mark executables for installation 170 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 171 | # install(TARGETS ${PROJECT_NAME}_node 172 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 173 | # ) 174 | 175 | ## Mark libraries for installation 176 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 177 | # install(TARGETS ${PROJECT_NAME} 178 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 179 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 180 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 181 | # ) 182 | 183 | ## Mark cpp header files for installation 184 | # install(DIRECTORY include/${PROJECT_NAME}/ 185 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 186 | # FILES_MATCHING PATTERN "*.h" 187 | # PATTERN ".svn" EXCLUDE 188 | # ) 189 | 190 | ## Mark other files for installation (e.g. launch and bag files, etc.) 191 | # install(FILES 192 | # # myfile1 193 | # # myfile2 194 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 195 | # ) 196 | 197 | ############# 198 | ## Testing ## 199 | ############# 200 | 201 | ## Add gtest based cpp test target and link libraries 202 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_joystick.cpp) 203 | # if(TARGET ${PROJECT_NAME}-test) 204 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 205 | # endif() 206 | 207 | ## Add folders to be run by python nosetests 208 | # catkin_add_nosetests(test) 209 | -------------------------------------------------------------------------------- /system/user_input/joystick/launch/joystick_control.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /system/user_input/joystick/msg/JoyCommand.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | uint8 HOME = 0 4 | uint8 TWIST = 1 5 | uint8 PICK = 2 6 | uint8 PLACE = 3 7 | uint8 FINISH = 4 8 | 9 | uint8 command 10 | geometry_msgs/Twist twist 11 | -------------------------------------------------------------------------------- /system/user_input/joystick/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | joystick 4 | 0.0.2 5 | The joystick package 6 | 7 | 8 | 9 | 10 | Alberto Gottardi 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | geometry_msgs 53 | rospy 54 | sensor_msgs 55 | std_msgs 56 | geometry_msgs 57 | rospy 58 | sensor_msgs 59 | std_msgs 60 | geometry_msgs 61 | rospy 62 | sensor_msgs 63 | std_msgs 64 | 65 | message_generation 66 | message_runtime 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | -------------------------------------------------------------------------------- /system/user_input/joystick/setup.py: -------------------------------------------------------------------------------- 1 | from distutils.core import setup 2 | from catkin_pkg.python_setup import generate_distutils_setup 3 | 4 | d = generate_distutils_setup( 5 | packages = ['joystick'], 6 | package_dir = {'': 'src'} 7 | ) 8 | 9 | setup(**d) 10 | -------------------------------------------------------------------------------- /system/user_input/joystick/src/joystick/Joystick.py: -------------------------------------------------------------------------------- 1 | #coding=utf-8 2 | 3 | import rospy 4 | from geometry_msgs.msg import Twist 5 | from sensor_msgs.msg import Joy 6 | from joystick.msg import JoyCommand 7 | 8 | class Joystick: 9 | def __init__(self): 10 | self.sub_joy = rospy.Subscriber("joy", Joy, self.callback) 11 | self.pub_command = rospy.Publisher("joy_command", JoyCommand, queue_size=1) 12 | 13 | self.twist_msg = Twist() 14 | self.command_msg = JoyCommand() 15 | 16 | def callback(self, joy_msg): 17 | self.command_msg.header.stamp = rospy.Time.now() 18 | 19 | #Create zero twist message 20 | self.twist_msg.linear.x = 0 21 | self.twist_msg.linear.y = 0 22 | self.twist_msg.linear.z = 0 23 | self.twist_msg.angular.x = 0 24 | self.twist_msg.angular.y = 0 25 | self.twist_msg.angular.z = 0 26 | 27 | #Manage command message 28 | X_button = joy_msg.buttons[0] 29 | O_button = joy_msg.buttons[1] 30 | T_button = joy_msg.buttons[2] 31 | S_button = joy_msg.buttons[3] 32 | 33 | #COMMAND 34 | # X = PICK 35 | # O = PLACE 36 | # Triangle = HOME 37 | # Square = FINISH 38 | # Left and right Sticks, L1, L2, R1, R2 = TWIST 39 | 40 | if(X_button != 0): 41 | self.command_msg.command = self.command_msg.PICK 42 | 43 | elif(O_button != 0): 44 | self.command_msg.command = self.command_msg.PLACE 45 | 46 | elif(T_button != 0): 47 | self.command_msg.command = self.command_msg.HOME 48 | 49 | elif(S_button != 0): 50 | self.command_msg.command = self.command_msg.FINISH 51 | 52 | else: 53 | self.command_msg.command = self.command_msg.TWIST 54 | 55 | #For twist linear (x, y) used left stick 56 | self.twist_msg.linear.x = joy_msg.axes[0] 57 | self.twist_msg.linear.y = - joy_msg.axes[1] 58 | #For twist linear z used L1 and R1 buttons: L1 = -1.0, R1 = 1.0 59 | self.twist_msg.linear.z = - joy_msg.buttons[4] + joy_msg.buttons[5] 60 | 61 | #For twist angular (x, y) used right stick 62 | self.twist_msg.angular.x = joy_msg.axes[3] 63 | self.twist_msg.angular.y = joy_msg.axes[4] 64 | #For twist angular z used L2 and R2 buttons: L2 = -1.0, R2 = 1.0 65 | self.twist_msg.angular.z = - joy_msg.buttons[6] + joy_msg.buttons[7] 66 | 67 | self.command_msg.twist = self.twist_msg 68 | 69 | #Publish command message 70 | self.pub_command.publish(self.command_msg) 71 | 72 | -------------------------------------------------------------------------------- /system/user_input/joystick/src/joystick/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Shared-control/improved-apf/85ab7ef5f043752794c52e80f4d1d19e10ef87ff/system/user_input/joystick/src/joystick/__init__.py -------------------------------------------------------------------------------- /system/user_input/joystick/src/joystick/joystick_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | #coding=utf-8 3 | 4 | import rospy 5 | import Joystick 6 | 7 | if __name__ == "__main__": 8 | rospy.init_node("joystick_control_node", anonymous=True) 9 | 10 | rate = rospy.Rate(100) 11 | 12 | #Start joystick publisher 13 | joystick_publisher = Joystick.Joystick() 14 | 15 | rospy.loginfo("Joystick_publisher_node started") 16 | while(not rospy.is_shutdown()): 17 | rate.sleep() 18 | 19 | -------------------------------------------------------------------------------- /system/user_input/keyboard/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(keyboard) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | geometry_msgs 12 | rospy 13 | std_msgs 14 | message_generation 15 | ) 16 | 17 | ## System dependencies are found with CMake's conventions 18 | # find_package(Boost REQUIRED COMPONENTS system) 19 | 20 | 21 | ## Uncomment this if the package has a setup.py. This macro ensures 22 | ## modules and global scripts declared therein get installed 23 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 24 | catkin_python_setup() 25 | 26 | ################################################ 27 | ## Declare ROS messages, services and actions ## 28 | ################################################ 29 | 30 | ## To declare and build messages, services or actions from within this 31 | ## package, follow these steps: 32 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 33 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 34 | ## * In the file package.xml: 35 | ## * add a build_depend tag for "message_generation" 36 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 37 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 38 | ## but can be declared for certainty nonetheless: 39 | ## * add a exec_depend tag for "message_runtime" 40 | ## * In this file (CMakeLists.txt): 41 | ## * add "message_generation" and every package in MSG_DEP_SET to 42 | ## find_package(catkin REQUIRED COMPONENTS ...) 43 | ## * add "message_runtime" and every package in MSG_DEP_SET to 44 | ## catkin_package(CATKIN_DEPENDS ...) 45 | ## * uncomment the add_*_files sections below as needed 46 | ## and list every .msg/.srv/.action file to be processed 47 | ## * uncomment the generate_messages entry below 48 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 49 | 50 | ## Generate messages in the 'msg' folder 51 | add_message_files( 52 | FILES 53 | KeyCommand.msg 54 | ) 55 | 56 | ## Generate services in the 'srv' folder 57 | # add_service_files( 58 | # FILES 59 | # Service1.srv 60 | # Service2.srv 61 | # ) 62 | 63 | ## Generate actions in the 'action' folder 64 | # add_action_files( 65 | # FILES 66 | # Action1.action 67 | # Action2.action 68 | # ) 69 | 70 | ## Generate added messages and services with any dependencies listed here 71 | generate_messages( 72 | DEPENDENCIES 73 | geometry_msgs 74 | std_msgs 75 | ) 76 | 77 | ################################################ 78 | ## Declare ROS dynamic reconfigure parameters ## 79 | ################################################ 80 | 81 | ## To declare and build dynamic reconfigure parameters within this 82 | ## package, follow these steps: 83 | ## * In the file package.xml: 84 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 85 | ## * In this file (CMakeLists.txt): 86 | ## * add "dynamic_reconfigure" to 87 | ## find_package(catkin REQUIRED COMPONENTS ...) 88 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 89 | ## and list every .cfg file to be processed 90 | 91 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 92 | # generate_dynamic_reconfigure_options( 93 | # cfg/DynReconf1.cfg 94 | # cfg/DynReconf2.cfg 95 | # ) 96 | 97 | ################################### 98 | ## catkin specific configuration ## 99 | ################################### 100 | ## The catkin_package macro generates cmake config files for your package 101 | ## Declare things to be passed to dependent projects 102 | ## INCLUDE_DIRS: uncomment this if your package contains header files 103 | ## LIBRARIES: libraries you create in this project that dependent projects also need 104 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 105 | ## DEPENDS: system dependencies of this project that dependent projects also need 106 | catkin_package( 107 | # INCLUDE_DIRS include 108 | # LIBRARIES keyboard 109 | CATKIN_DEPENDS geometry_msgs rospy std_msgs message_runtime 110 | # DEPENDS system_lib 111 | ) 112 | 113 | ########### 114 | ## Build ## 115 | ########### 116 | 117 | ## Specify additional locations of header files 118 | ## Your package locations should be listed before other locations 119 | include_directories( 120 | # include 121 | ${catkin_INCLUDE_DIRS} 122 | ) 123 | 124 | ## Declare a C++ library 125 | # add_library(${PROJECT_NAME} 126 | # src/${PROJECT_NAME}/keyboard.cpp 127 | # ) 128 | 129 | ## Add cmake target dependencies of the library 130 | ## as an example, code may need to be generated before libraries 131 | ## either from message generation or dynamic reconfigure 132 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 133 | 134 | ## Declare a C++ executable 135 | ## With catkin_make all packages are built within a single CMake context 136 | ## The recommended prefix ensures that target names across packages don't collide 137 | # add_executable(${PROJECT_NAME}_node src/keyboard_node.cpp) 138 | 139 | ## Rename C++ executable without prefix 140 | ## The above recommended prefix causes long target names, the following renames the 141 | ## target back to the shorter version for ease of user use 142 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 143 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 144 | 145 | ## Add cmake target dependencies of the executable 146 | ## same as for the library above 147 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 148 | 149 | ## Specify libraries to link a library or executable target against 150 | # target_link_libraries(${PROJECT_NAME}_node 151 | # ${catkin_LIBRARIES} 152 | # ) 153 | 154 | ############# 155 | ## Install ## 156 | ############# 157 | 158 | # all install targets should use catkin DESTINATION variables 159 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 160 | 161 | ## Mark executable scripts (Python etc.) for installation 162 | ## in contrast to setup.py, you can choose the destination 163 | # catkin_install_python(PROGRAMS 164 | # scripts/my_python_script 165 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 166 | # ) 167 | 168 | ## Mark executables for installation 169 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 170 | # install(TARGETS ${PROJECT_NAME}_node 171 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 172 | # ) 173 | 174 | ## Mark libraries for installation 175 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 176 | # install(TARGETS ${PROJECT_NAME} 177 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 178 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 179 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 180 | # ) 181 | 182 | ## Mark cpp header files for installation 183 | # install(DIRECTORY include/${PROJECT_NAME}/ 184 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 185 | # FILES_MATCHING PATTERN "*.h" 186 | # PATTERN ".svn" EXCLUDE 187 | # ) 188 | 189 | ## Mark other files for installation (e.g. launch and bag files, etc.) 190 | # install(FILES 191 | # # myfile1 192 | # # myfile2 193 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 194 | # ) 195 | 196 | ############# 197 | ## Testing ## 198 | ############# 199 | 200 | ## Add gtest based cpp test target and link libraries 201 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_keyboard.cpp) 202 | # if(TARGET ${PROJECT_NAME}-test) 203 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 204 | # endif() 205 | 206 | ## Add folders to be run by python nosetests 207 | # catkin_add_nosetests(test) 208 | -------------------------------------------------------------------------------- /system/user_input/keyboard/launch/keyboard_control.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /system/user_input/keyboard/msg/KeyCommand.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | uint8 HOME = 0 4 | uint8 TWIST = 1 5 | uint8 PICK = 2 6 | uint8 PLACE = 3 7 | uint8 FINISH = 4 8 | 9 | uint8 command 10 | geometry_msgs/Twist twist 11 | 12 | -------------------------------------------------------------------------------- /system/user_input/keyboard/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | keyboard 4 | 0.0.2 5 | The keyboard package 6 | 7 | 8 | 9 | 10 | Alberto Gottardi 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | geometry_msgs 53 | rospy 54 | std_msgs 55 | geometry_msgs 56 | rospy 57 | std_msgs 58 | geometry_msgs 59 | rospy 60 | std_msgs 61 | 62 | message_generation 63 | message_runtime 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | -------------------------------------------------------------------------------- /system/user_input/keyboard/setup.py: -------------------------------------------------------------------------------- 1 | from distutils.core import setup 2 | from catkin_pkg.python_setup import generate_distutils_setup 3 | 4 | d = generate_distutils_setup( 5 | packages = ['keyboard'], 6 | package_dir = {'': 'src'} 7 | ) 8 | 9 | setup(**d) 10 | -------------------------------------------------------------------------------- /system/user_input/keyboard/src/keyboard/Keyboard.py: -------------------------------------------------------------------------------- 1 | #coding=utf-8 2 | 3 | from pykeyboard import PyKeyboardEvent 4 | from threading import Thread 5 | 6 | import rospy 7 | from keyboard.msg import KeyCommand 8 | from geometry_msgs.msg import Twist 9 | 10 | class Keyboard(PyKeyboardEvent): 11 | def __init__(self): 12 | PyKeyboardEvent.__init__(self) 13 | 14 | self.pub_command = rospy.Publisher("key_command", KeyCommand, queue_size=1) 15 | self.twist_msg = Twist() 16 | self.command_msg = KeyCommand() 17 | 18 | self.thread = Thread(target=self.run) 19 | self.thread.start() 20 | 21 | def stop(self): 22 | super(Keyboard, self).stop() 23 | self.thread.join() 24 | 25 | def tap(self, keycode, character, press): 26 | self.command_msg.header.stamp = rospy.Time.now() 27 | 28 | #Create zero twist message 29 | self.twist_msg.linear.x = 0 30 | self.twist_msg.linear.y = 0 31 | self.twist_msg.linear.z = 0 32 | self.twist_msg.angular.x = 0 33 | self.twist_msg.angular.y = 0 34 | self.twist_msg.angular.z = 0 35 | 36 | key_char = str(character) 37 | key_pressed = press 38 | 39 | #COMMAND 40 | # "Up" = PLACE 41 | # "Down" = PICK 42 | # "BackSpace" = HOME 43 | # " " (Space) = FINISH 44 | # "Alphabet" = TWIST 45 | 46 | #Manage command message 47 | if(key_char == "Up"): 48 | self.command_msg.command = self.command_msg.PLACE 49 | 50 | elif(key_char == "Down"): 51 | self.command_msg.command = self.command_msg.PICK 52 | 53 | elif(key_char == "BackSpace"): 54 | self.command_msg.command = self.command_msg.HOME 55 | 56 | elif(key_char == " "): 57 | self.command_msg.command = self.command_msg.FINISH 58 | 59 | else: 60 | self.command_msg.command = self.command_msg.TWIST 61 | 62 | if((key_char == "q") and (key_pressed)): 63 | self.twist_msg.linear.z = 0.2 64 | 65 | elif((key_char == "w") and (key_pressed)): 66 | self.twist_msg.linear.z = -0.2 67 | 68 | elif((key_char == "s") and (key_pressed)): 69 | self.twist_msg.linear.y = 0.2 70 | 71 | elif((key_char == "a") and (key_pressed)): 72 | self.twist_msg.linear.y = -0.2 73 | 74 | elif((key_char == "z") and (key_pressed)): 75 | self.twist_msg.linear.x = 0.2 76 | 77 | elif((key_char == "x") and (key_pressed)): 78 | self.twist_msg.linear.x = -0.2 79 | 80 | elif((key_char == "e") and (key_pressed)): 81 | self.twist_msg.linear.y = -0.2 82 | self.twist_msg.linear.z = 0.2 83 | 84 | elif((key_char == "c") and (key_pressed)): 85 | self.twist_msg.linear.y = -0.2 86 | self.twist_msg.linear.x = 0.2 87 | 88 | elif((key_char == "v") and (key_pressed)): 89 | self.twist_msg.linear.y = -0.2 90 | self.twist_msg.linear.x = -0.2 91 | 92 | elif((key_char == "r") and (key_pressed)): 93 | self.twist_msg.linear.y = -0.2 94 | self.twist_msg.linear.z = -0.2 95 | 96 | elif((key_char == "b") and (key_pressed)): 97 | self.twist_msg.linear.y = 0.2 98 | self.twist_msg.linear.x = 0.2 99 | 100 | elif((key_char == "t") and (key_pressed)): 101 | self.twist_msg.linear.y = 0.2 102 | self.twist_msg.linear.z = 0.2 103 | 104 | elif((key_char == "y") and (key_pressed)): 105 | self.twist_msg.linear.y = 0.2 106 | self.twist_msg.linear.z = -0.2 107 | 108 | elif((key_char == "n") and (key_pressed)): 109 | self.twist_msg.linear.y = 0.2 110 | self.twist_msg.linear.x = -0.2 111 | 112 | elif((key_char == "d") and (key_pressed)): 113 | self.twist_msg.linear.x = 0.2 114 | self.twist_msg.linear.z = 0.2 115 | 116 | elif((key_char == "f") and (key_pressed)): 117 | self.twist_msg.linear.x = 0.2 118 | self.twist_msg.linear.z = -0.2 119 | 120 | elif((key_char == "g") and (key_pressed)): 121 | self.twist_msg.linear.x = -0.2 122 | self.twist_msg.linear.z = 0.2 123 | 124 | elif((key_char == "h") and (key_pressed)): 125 | self.twist_msg.linear.x = -0.2 126 | self.twist_msg.linear.z = -0.2 127 | 128 | elif((key_char == "u") and (key_pressed)): 129 | self.twist_msg.linear.x = 0.2 130 | self.twist_msg.linear.y = 0.2 131 | self.twist_msg.linear.z = 0.2 132 | 133 | elif((key_char == "i") and (key_pressed)): 134 | self.twist_msg.linear.x = 0.2 135 | self.twist_msg.linear.y = 0.2 136 | self.twist_msg.linear.z = -0.2 137 | 138 | elif((key_char == "o") and (key_pressed)): 139 | self.twist_msg.linear.x = 0.2 140 | self.twist_msg.linear.y = -0.2 141 | self.twist_msg.linear.z = 0.2 142 | 143 | elif((key_char == "p") and (key_pressed)): 144 | self.twist_msg.linear.x = 0.2 145 | self.twist_msg.linear.y = -0.2 146 | self.twist_msg.linear.z = -0.2 147 | 148 | elif((key_char == "j") and (key_pressed)): 149 | self.twist_msg.linear.x = -0.2 150 | self.twist_msg.linear.y = 0.2 151 | self.twist_msg.linear.z = 0.2 152 | 153 | elif((key_char == "k") and (key_pressed)): 154 | self.twist_msg.linear.x = -0.2 155 | self.twist_msg.linear.y = 0.2 156 | self.twist_msg.linear.z = -0.2 157 | 158 | elif((key_char == "l") and (key_pressed)): 159 | self.twist_msg.linear.x = -0.2 160 | self.twist_msg.linear.y = -0.2 161 | self.twist_msg.linear.z = 0.2 162 | 163 | elif((key_char == "m") and (key_pressed)): 164 | self.twist_msg.linear.x = -0.2 165 | self.twist_msg.linear.y = -0.2 166 | self.twist_msg.linear.z = -0.2 167 | 168 | #Set twist command message 169 | self.command_msg.twist = self.twist_msg 170 | #END else 171 | 172 | #Publish command message 173 | self.pub_command.publish(self.command_msg) 174 | 175 | -------------------------------------------------------------------------------- /system/user_input/keyboard/src/keyboard/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Shared-control/improved-apf/85ab7ef5f043752794c52e80f4d1d19e10ef87ff/system/user_input/keyboard/src/keyboard/__init__.py -------------------------------------------------------------------------------- /system/user_input/keyboard/src/keyboard/keyboard_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | #coding=utf-8 3 | 4 | import rospy 5 | import Keyboard 6 | 7 | if __name__ == "__main__": 8 | rospy.init_node("keyboard_node", anonymous=True) 9 | 10 | rate = rospy.Rate(100) 11 | 12 | #Start keyboard publisher 13 | keyboard_publisher = Keyboard.Keyboard() 14 | 15 | rospy.loginfo("Keyboard_publisher_node started") 16 | while(not rospy.is_shutdown()): 17 | rate.sleep() 18 | 19 | keyboard_publisher.stop() 20 | rospy.loginfo("Keyboard_publisher_node stopped") 21 | 22 | 23 | -------------------------------------------------------------------------------- /system/user_input/myo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(myo) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | geometry_msgs 12 | roscpp 13 | std_msgs 14 | rosmyo 15 | message_generation 16 | ) 17 | 18 | ## System dependencies are found with CMake's conventions 19 | # find_package(Boost REQUIRED COMPONENTS system) 20 | 21 | 22 | ## Uncomment this if the package has a setup.py. This macro ensures 23 | ## modules and global scripts declared therein get installed 24 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 25 | # catkin_python_setup() 26 | 27 | ################################################ 28 | ## Declare ROS messages, services and actions ## 29 | ################################################ 30 | 31 | ## To declare and build messages, services or actions from within this 32 | ## package, follow these steps: 33 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 34 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 35 | ## * In the file package.xml: 36 | ## * add a build_depend tag for "message_generation" 37 | ## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET 38 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 39 | ## but can be declared for certainty nonetheless: 40 | ## * add a exec_depend tag for "message_runtime" 41 | ## * In this file (CMakeLists.txt): 42 | ## * add "message_generation" and every package in MSG_DEP_SET to 43 | ## find_package(catkin REQUIRED COMPONENTS ...) 44 | ## * add "message_runtime" and every package in MSG_DEP_SET to 45 | ## catkin_package(CATKIN_DEPENDS ...) 46 | ## * uncomment the add_*_files sections below as needed 47 | ## and list every .msg/.srv/.action file to be processed 48 | ## * uncomment the generate_messages entry below 49 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 50 | 51 | ## Generate messages in the 'msg' folder 52 | add_message_files( 53 | FILES 54 | MyoCommand.msg 55 | ) 56 | 57 | ## Generate services in the 'srv' folder 58 | add_service_files( 59 | FILES 60 | ResetMyo.srv 61 | ) 62 | 63 | ## Generate actions in the 'action' folder 64 | # add_action_files( 65 | # FILES 66 | # Action1.action 67 | # Action2.action 68 | # ) 69 | 70 | ## Generate added messages and services with any dependencies listed here 71 | generate_messages( 72 | DEPENDENCIES 73 | geometry_msgs 74 | std_msgs 75 | ) 76 | 77 | ################################################ 78 | ## Declare ROS dynamic reconfigure parameters ## 79 | ################################################ 80 | 81 | ## To declare and build dynamic reconfigure parameters within this 82 | ## package, follow these steps: 83 | ## * In the file package.xml: 84 | ## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" 85 | ## * In this file (CMakeLists.txt): 86 | ## * add "dynamic_reconfigure" to 87 | ## find_package(catkin REQUIRED COMPONENTS ...) 88 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 89 | ## and list every .cfg file to be processed 90 | 91 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 92 | # generate_dynamic_reconfigure_options( 93 | # cfg/DynReconf1.cfg 94 | # cfg/DynReconf2.cfg 95 | # ) 96 | 97 | ################################### 98 | ## catkin specific configuration ## 99 | ################################### 100 | ## The catkin_package macro generates cmake config files for your package 101 | ## Declare things to be passed to dependent projects 102 | ## INCLUDE_DIRS: uncomment this if your package contains header files 103 | ## LIBRARIES: libraries you create in this project that dependent projects also need 104 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 105 | ## DEPENDS: system dependencies of this project that dependent projects also need 106 | catkin_package( 107 | INCLUDE_DIRS include 108 | # LIBRARIES myo 109 | CATKIN_DEPENDS geometry_msgs roscpp std_msgs message_runtime 110 | # DEPENDS system_lib 111 | ) 112 | 113 | ########### 114 | ## Build ## 115 | ########### 116 | 117 | ## Specify additional locations of header files 118 | ## Your package locations should be listed before other locations 119 | include_directories( 120 | include 121 | ${catkin_INCLUDE_DIRS} 122 | ) 123 | 124 | ## Declare a C++ library 125 | add_library(${PROJECT_NAME}_myo_controller 126 | src/myo_controller.cpp 127 | ) 128 | target_link_libraries( 129 | ${PROJECT_NAME}_myo_controller 130 | ${catkin_LIBRARIES} 131 | ) 132 | 133 | ## Add cmake target dependencies of the library 134 | ## as an example, code may need to be generated before libraries 135 | ## either from message generation or dynamic reconfigure 136 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 137 | 138 | ## Declare a C++ executable 139 | ## With catkin_make all packages are built within a single CMake context 140 | ## The recommended prefix ensures that target names across packages don't collide 141 | add_executable(${PROJECT_NAME}_node src/myo_node.cpp) 142 | 143 | ## Rename C++ executable without prefix 144 | ## The above recommended prefix causes long target names, the following renames the 145 | ## target back to the shorter version for ease of user use 146 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 147 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 148 | 149 | ## Add cmake target dependencies of the executable 150 | ## same as for the library above 151 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 152 | 153 | ## Specify libraries to link a library or executable target against 154 | target_link_libraries( 155 | ${PROJECT_NAME}_node 156 | ${catkin_LIBRARIES} 157 | ${PROJECT_NAME}_myo_controller 158 | ) 159 | 160 | ############# 161 | ## Install ## 162 | ############# 163 | 164 | # all install targets should use catkin DESTINATION variables 165 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 166 | 167 | ## Mark executable scripts (Python etc.) for installation 168 | ## in contrast to setup.py, you can choose the destination 169 | # catkin_install_python(PROGRAMS 170 | # scripts/my_python_script 171 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 172 | # ) 173 | 174 | ## Mark executables for installation 175 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html 176 | # install(TARGETS ${PROJECT_NAME}_node 177 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 178 | # ) 179 | 180 | ## Mark libraries for installation 181 | ## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html 182 | # install(TARGETS ${PROJECT_NAME} 183 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 184 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 185 | # RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 186 | # ) 187 | 188 | ## Mark cpp header files for installation 189 | # install(DIRECTORY include/${PROJECT_NAME}/ 190 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 191 | # FILES_MATCHING PATTERN "*.h" 192 | # PATTERN ".svn" EXCLUDE 193 | # ) 194 | 195 | ## Mark other files for installation (e.g. launch and bag files, etc.) 196 | # install(FILES 197 | # # myfile1 198 | # # myfile2 199 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 200 | # ) 201 | 202 | ############# 203 | ## Testing ## 204 | ############# 205 | 206 | ## Add gtest based cpp test target and link libraries 207 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_myo.cpp) 208 | # if(TARGET ${PROJECT_NAME}-test) 209 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 210 | # endif() 211 | 212 | ## Add folders to be run by python nosetests 213 | # catkin_add_nosetests(test) 214 | -------------------------------------------------------------------------------- /system/user_input/myo/include/myo/myo_controller.h: -------------------------------------------------------------------------------- 1 | #ifndef MYO_MYO_CONTROLLER_H 2 | #define MYO_MYO_CONTROLLER_H 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | #include 16 | #include 17 | 18 | namespace myo{ 19 | 20 | class MyoController{ 21 | public: 22 | MyoController(ros::NodeHandle t_nh, std::string t_axis [], std::vector t_threshold); 23 | ~MyoController() {} 24 | 25 | private: 26 | ros::NodeHandle m_nh; 27 | void configure(std::string t_axis []); 28 | 29 | tf2::Quaternion q_start_offset; 30 | tf2::Quaternion q_now; 31 | int follow; 32 | int m_axis [3]; //maps roll pitch yaw to axis x y z; roll=0, pitch=1, yaw =2, not used=-1 33 | int sign [3]; //inverts direction over related axis if setted to -1 34 | std::vector m_angle_th; //threshold over axis in degree, axis0 = x 35 | double b_roll, b_pitch, b_yaw; 36 | 37 | MyoCommand command_msg; 38 | geometry_msgs::Twist twist_msg; 39 | 40 | ros::Publisher pub_command; 41 | ros::Subscriber sub_imu; 42 | ros::Subscriber sub_gesture; 43 | ros::ServiceServer m_reset_server; 44 | 45 | //subscriber 46 | void callbackImu(const sensor_msgs::Imu::ConstPtr& msg); 47 | void callbackGesture(const rosmyo::ClassifierPose::ConstPtr& msg); 48 | 49 | //publisher 50 | void publishCommand(); 51 | 52 | double deltaDeg(double current, double centre); 53 | void setTwist(); 54 | 55 | void reset_msg(); 56 | bool resetMyoService(myo::ResetMyo::Request &req, myo::ResetMyo::Response &res); 57 | }; 58 | 59 | } 60 | 61 | 62 | 63 | #endif -------------------------------------------------------------------------------- /system/user_input/myo/launch/myo_control.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /system/user_input/myo/msg/MyoCommand.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | uint8 HOME = 0 4 | uint8 TWIST = 1 5 | uint8 PICK = 2 6 | uint8 PLACE = 3 7 | uint8 FINISH = 4 8 | 9 | uint8 command 10 | geometry_msgs/Twist twist 11 | 12 | -------------------------------------------------------------------------------- /system/user_input/myo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | myo 4 | 0.0.2 5 | The myo package 6 | 7 | 8 | 9 | 10 | Alberto Gottardi 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | geometry_msgs 53 | roscpp 54 | std_msgs 55 | geometry_msgs 56 | roscpp 57 | std_msgs 58 | geometry_msgs 59 | roscpp 60 | std_msgs 61 | 62 | rosmyo 63 | 64 | message_generation 65 | message_runtime 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | -------------------------------------------------------------------------------- /system/user_input/myo/src/myo_controller.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | /** 4 | * Costructor of MyoController class 5 | * @param t_nh NodeHandle 6 | * @param t_axis axis 7 | * @param t_threshold threshold 8 | */ 9 | myo::MyoController::MyoController(ros::NodeHandle t_nh, std::string t_axis [], std::vector t_threshold) 10 | : m_nh(t_nh) 11 | , m_angle_th(t_threshold) 12 | { 13 | configure(t_axis); 14 | } 15 | 16 | /** 17 | * Set axis, Publisher and Subscribers 18 | * @param t_axis axis 19 | */ 20 | void myo::MyoController::configure(std::string t_axis []){ 21 | follow = 0; 22 | 23 | for(int i = 0; i < 3; i++){ 24 | //map axis i to correct angle 25 | char ax_angle = t_axis[i][0]; 26 | char ax_direction = t_axis[i][1]; 27 | 28 | if(ax_angle == 'r'){ 29 | m_axis[i] = 0; 30 | } 31 | else if(ax_angle == 'p'){ 32 | m_axis[i] = 1; 33 | } 34 | else if(ax_angle == 'y'){ 35 | m_axis[i] = 2; 36 | } 37 | else{ 38 | m_axis[i] = -1; //not used 39 | } 40 | 41 | sign[i] = 1; 42 | if(ax_direction == '-'){ 43 | sign[i] = -1; 44 | } 45 | } 46 | 47 | //Set Publisher and Subscribers 48 | pub_command = m_nh.advertise("myo_command", 1); 49 | sub_imu = m_nh.subscribe("imu", 20, &myo::MyoController::callbackImu, this); 50 | //sub_gesture = m_nh.subscribe("gesture", 20, &myo::MyoController::callbackGesture, this); 51 | 52 | m_reset_server = m_nh.advertiseService("reset_myo_srv", &myo::MyoController::resetMyoService, this); 53 | 54 | ROS_INFO_STREAM("Myo Controller started!"); 55 | } 56 | 57 | /** 58 | * Callback Imu 59 | * @param msg sensor_msgs::Imu::ConstPtr& 60 | */ 61 | void myo::MyoController::callbackImu(const sensor_msgs::Imu::ConstPtr& msg){ 62 | if(follow == 0){ 63 | reset_msg(); 64 | return; 65 | } 66 | 67 | //If no starting offset, save this as offset 68 | if(follow == 1){ 69 | q_start_offset = tf2::Quaternion(msg->orientation.x, 70 | msg->orientation.y, 71 | msg->orientation.z, 72 | msg->orientation.w); 73 | q_start_offset.normalize(); 74 | follow = 2; 75 | ROS_INFO_STREAM("Publishing started"); 76 | } 77 | tf2::Quaternion now(msg->orientation.x, 78 | msg->orientation.y, 79 | msg->orientation.z, 80 | msg->orientation.w); 81 | now.normalize(); 82 | q_now = now; 83 | 84 | command_msg.command = command_msg.TWIST; 85 | publishCommand(); 86 | } 87 | 88 | /** 89 | * Callback gesture 90 | * @param msg rosmyo::ClassifierPose::ConstPtr& 91 | */ 92 | void myo::MyoController::callbackGesture(const rosmyo::ClassifierPose::ConstPtr& msg){ 93 | //If gesture is DOUBLE_TAP then modified offset 94 | if(msg->pose == msg->DOUBLE_TAP){ 95 | if(follow == 2){ 96 | //Stop publishing 97 | follow = 0; 98 | ROS_INFO_STREAM("Publishing stopped"); 99 | } 100 | else{ 101 | //Restart publishing 102 | follow = 1; 103 | ROS_INFO_STREAM("Publishing waiting first message"); 104 | } 105 | return; 106 | } 107 | 108 | if(msg->pose == msg->FIST){ 109 | command_msg.command = command_msg.PICK; 110 | } 111 | else if(msg->pose == msg->FINGERS_SPREAD){ 112 | command_msg.command = command_msg.PLACE; 113 | } 114 | else if(msg->pose == msg->WAVE_IN){ 115 | command_msg.command = command_msg.HOME; 116 | } 117 | else if(msg->pose == msg->WAVE_OUT){ 118 | command_msg.command = command_msg.FINISH; 119 | } 120 | publishCommand(); 121 | } 122 | 123 | /** 124 | * Set delta deg 125 | * @param current 126 | * @param centre 127 | * @return delta 128 | */ 129 | double myo::MyoController::deltaDeg(double current, double centre){ 130 | double delta = current - centre; 131 | if(delta > 180){ 132 | delta = delta - 360; 133 | } 134 | else if(delta < -180){ 135 | delta = delta + 360; 136 | } 137 | return delta; 138 | } 139 | 140 | /** 141 | * Publish MyoCommand message 142 | */ 143 | void myo::MyoController::publishCommand(){ 144 | //Manage command message 145 | command_msg.header.stamp = ros::Time::now(); 146 | 147 | twist_msg.linear.x = 0; 148 | twist_msg.linear.y = 0; 149 | twist_msg.linear.z = 0; 150 | twist_msg.angular.x = 0; 151 | twist_msg.angular.y = 0; 152 | twist_msg.angular.z = 0; 153 | 154 | //Set twist 155 | if(command_msg.command == command_msg.TWIST){ 156 | setTwist(); 157 | command_msg.twist = twist_msg; 158 | } 159 | 160 | //Publish command message 161 | pub_command.publish(command_msg); 162 | } 163 | 164 | /** 165 | * Set twist for myo command message 166 | */ 167 | void myo::MyoController::setTwist(){ 168 | //Use the last q_now to publish twist-command message 169 | double roll, pitch, yaw; 170 | tf2::Matrix3x3(q_now).getRPY(roll, pitch, yaw); 171 | 172 | double ro, po, yo; 173 | tf2::Matrix3x3(q_start_offset).getRPY(ro, po, yo); 174 | 175 | double dRoll = deltaDeg(roll * 180/M_PI, ro * 180/M_PI); 176 | double dPitch = deltaDeg(pitch * 180/M_PI, po * 180/M_PI); 177 | double dYaw = deltaDeg(yaw * 180/M_PI, yo * 180/M_PI); 178 | 179 | double angles [] {dRoll, dPitch, dYaw}; 180 | 181 | geometry_msgs::Twist tmp_twist; 182 | 183 | //For now use these settings 184 | //If m_axis[i] < 0 axis i is not used 185 | //Z axis 186 | if(m_axis[0] >= 0){ 187 | //takes the correct angle from r,p,y indicated previously in m_axis vector 188 | double angle_z = angles[m_axis[0]]; 189 | if(angle_z > m_angle_th[0]){ 190 | tmp_twist.linear.z = -sign[0] * 0.15; 191 | } 192 | else if(angle_z < -m_angle_th[0]){ 193 | tmp_twist.linear.z = sign[0] * 0.15; 194 | } 195 | } 196 | 197 | //Y axis 198 | if(m_axis[1] >= 0){ 199 | //takes the correct angle from r,p,y indicated previously in m_axis vector 200 | double angle_x = angles[m_axis[1]]; 201 | if(angle_x > m_angle_th[1]){ 202 | tmp_twist.linear.x = sign[1] * 0.15; 203 | } 204 | else if(angle_x < -m_angle_th[1]){ 205 | tmp_twist.linear.x = -sign[1] * 0.15; 206 | } 207 | 208 | } 209 | 210 | //X axis 211 | if(m_axis[2] >= 0){ 212 | //takes the correct angle from r,p,y indicated previously in m_axis vector 213 | double angle_y = angles[m_axis[2]]; 214 | if(angle_y > m_angle_th[2]){ 215 | tmp_twist.linear.y = sign[2] * 0.15; 216 | } 217 | else if(angle_y < -m_angle_th[2]){ 218 | tmp_twist.linear.y = -sign[2] * 0.15; 219 | } 220 | } 221 | 222 | twist_msg = tmp_twist; 223 | } 224 | 225 | /** 226 | * Reset TWIST message 227 | */ 228 | void myo::MyoController::reset_msg(){ 229 | //Manage command message 230 | command_msg.header.stamp = ros::Time::now(); 231 | 232 | twist_msg.linear.x = 0; 233 | twist_msg.linear.y = 0; 234 | twist_msg.linear.z = 0; 235 | twist_msg.angular.x = 0; 236 | twist_msg.angular.y = 0; 237 | twist_msg.angular.z = 0; 238 | 239 | command_msg.command = command_msg.TWIST; 240 | command_msg.twist = twist_msg; 241 | 242 | //Publish command message 243 | pub_command.publish(command_msg); 244 | } 245 | 246 | 247 | /** 248 | * Service to reset Myo 249 | * @param req request 250 | * @param res response 251 | * @return True if Myo has reset its initial pose, False otherwise 252 | */ 253 | bool myo::MyoController::resetMyoService(myo::ResetMyo::Request &req, myo::ResetMyo::Response &res){ 254 | if(!req.reset){ 255 | ROS_ERROR_STREAM("ERROR: REQUEST_RESET IS FALSE!"); 256 | res.ready = false; 257 | return res.ready; 258 | } 259 | std::cout << "Press ENTER to stop and reset Myo" << std::endl; 260 | std::cin.ignore(); 261 | follow = 0; 262 | ROS_INFO_STREAM("Publishing stopped"); 263 | for(int t = 0; t < 100; t++){ 264 | reset_msg(); 265 | ros::Duration(0.01).sleep(); 266 | } 267 | std::cout << "Press ENTER to restart Myo" << std::endl; 268 | std::cin.ignore(); 269 | //Restart publishing 270 | follow = 1; 271 | ROS_INFO_STREAM("Publishing waiting first message"); 272 | 273 | res.ready = true; 274 | return res.ready; 275 | } -------------------------------------------------------------------------------- /system/user_input/myo/src/myo_node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | int main(int argc, char **argv){ 5 | ros::init(argc, argv, "myo_control_node"); 6 | ros::NodeHandle nh; 7 | 8 | std::string axis[3]; 9 | //For now 10 | axis[0] = "p-"; 11 | axis[1] = "y+"; 12 | axis[2] = "r+"; 13 | 14 | /* 15 | axis[0] = "r+"; //roll 16 | axis[1] = "p-"; //pitch 17 | axis[2] = "y+"; //yaw 18 | */ 19 | std::vector threshold_angle = {15, 15, 15}; 20 | 21 | myo::MyoController myo_control(nh, axis, threshold_angle); 22 | 23 | ros::spin(); 24 | return 0; 25 | } -------------------------------------------------------------------------------- /system/user_input/myo/srv/ResetMyo.srv: -------------------------------------------------------------------------------- 1 | bool reset 2 | --- 3 | bool ready --------------------------------------------------------------------------------