├── LICENSE ├── Note for German decimal point ├── README.md ├── dual_arm_robot_applications ├── CMakeLists.txt ├── include │ ├── dual_arm_demonstrator_iml │ │ ├── DualArmRobot.h │ │ └── SceneManager.h │ ├── dual_arm_toolbox │ │ ├── TrajectoryProcessor.h │ │ └── Transform.h │ └── ur_logging │ │ ├── Stopwatch.h │ │ ├── UrLogger.h │ │ └── UrMessageListener.h ├── launch │ ├── dual_arm_robot_offline_rviz.launch │ ├── dual_arm_robot_ros_control_rviz.launch │ ├── dual_arm_robot_rviz.launch │ ├── dual_arm_robot_tcp_wrench_publisher.launch │ ├── ur10_ros_control_rviz.launch │ ├── ur5_olddriver_rviz.launch │ ├── ur5_ros_control_hi.launch │ ├── ur5_ros_control_rviz.launch │ └── ur5_rviz.launch ├── meshes │ └── shelf │ │ └── shelf.stl ├── package.xml └── src │ ├── dual_arm_demonstrator_iml │ ├── DualArmRobot.cpp │ └── SceneManager.cpp │ ├── dual_arm_robot_vel_force_experiment.cpp │ ├── dual_arm_toolbox │ ├── TrajectoryProcessor.cpp │ └── Transform.cpp │ ├── executables │ ├── demonstration_setup_helper.cpp │ ├── dual_arm_robot_demonstration.cpp │ ├── dual_arm_robot_lift_experiment_force.cpp │ ├── dual_arm_robot_lift_experiment_force_1.cpp │ ├── dual_arm_robot_lift_experiment_force_2.cpp │ ├── dual_arm_robot_lift_experiment_planning.cpp │ └── dual_arm_robot_logger.cpp │ ├── ur10_force_hold_exp.cpp │ └── ur_logging │ ├── Stopwatch.cpp │ ├── UrLogger.cpp │ └── UrMessageListener.cpp ├── dual_arm_robot_description ├── CMakeLists.txt ├── launch │ ├── display_urdf.launch │ └── dual_arm_robot_upload.launch ├── meshes │ ├── endeffector │ │ ├── ee_frame.stl │ │ └── ee_spring.stl │ └── table │ │ ├── table.stl │ │ ├── table_back.stl │ │ ├── table_camerahanger.stl │ │ ├── table_front.stl │ │ ├── table_ground.stl │ │ ├── table_platform_ur10.stl │ │ ├── table_platform_ur5.stl │ │ └── table_side.stl ├── package.xml ├── src │ └── parser.cpp └── urdf │ ├── dual_arm_robot.urdf.xacro │ ├── endeffector.urdf.xacro │ ├── ikfast_commands.txt │ ├── table.urdf.xacro │ ├── ur10.urdf.xacro │ └── ur5.urdf.xacro ├── dual_arm_robot_driver ├── CMakeLists.txt ├── config │ ├── ur10_controllers.yaml │ └── ur5_controllers.yaml ├── launch │ ├── dual_arm_robot_bringup.launch │ └── dual_arm_robot_bringup_ros_control.launch └── package.xml ├── dual_arm_robot_moveit_config ├── CMakeLists.txt ├── README ├── config │ ├── controllers.yaml │ ├── dual_arm_robot.srdf │ ├── fake_controllers.yaml │ ├── joint_limits.yaml │ ├── kinematics.yaml │ └── ompl_planning.yaml ├── launch │ ├── default_warehouse_db.launch │ ├── demo.launch │ ├── dual_arm_robot_moveit_controller_manager.launch.xml │ ├── dual_arm_robot_moveit_planning_execution.launch │ ├── dual_arm_robot_moveit_sensor_manager.launch.xml │ ├── fake_moveit_controller_manager.launch.xml │ ├── joystick_control.launch │ ├── move_group.launch │ ├── moveit.rviz │ ├── moveit_rviz.launch │ ├── ompl_planning_pipeline.launch.xml │ ├── planning_context.launch │ ├── planning_pipeline.launch.xml │ ├── run_benchmark_ompl.launch │ ├── sensor_manager.launch.xml │ ├── setup_assistant.launch │ ├── trajectory_execution.launch.xml │ ├── warehouse.launch │ └── warehouse_settings.launch.xml └── package.xml ├── eeff_foam.stl ├── eeff_frame.stl ├── joint_trajectory_admittance_controller ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── controller_plugins.xml ├── images │ ├── new_trajectory.png │ ├── new_trajectory.svg │ ├── trajectory_replacement_future.png │ ├── trajectory_replacement_future.svg │ ├── trajectory_replacement_now.png │ ├── trajectory_replacement_now.svg │ ├── trajectory_replacement_past.png │ └── trajectory_replacement_past.svg ├── include │ ├── admittance_control │ │ └── admittance_control.h │ ├── joint_trajectory_admittance_controller │ │ ├── hardware_interface_adapter.h │ │ ├── init_joint_trajectory.h │ │ ├── joint_trajectory_admittance_controller.h │ │ ├── joint_trajectory_admittance_controller_impl.h │ │ ├── joint_trajectory_msg_utils.h │ │ ├── joint_trajectory_segment.h │ │ └── tolerances.h │ └── trajectory_interface_admittance │ │ ├── pos_vel_acc_state.h │ │ ├── quintic_spline_segment.h │ │ └── trajectory_interface.h ├── mainpage.dox ├── package.xml ├── ros_control_plugins.xml~ ├── rosdoc.yaml ├── src │ └── joint_trajectory_admittance_controller.cpp └── test │ ├── init_joint_trajectory_test.cpp │ ├── joint_trajectory_controller.test │ ├── joint_trajectory_controller_test.cpp │ ├── joint_trajectory_msg_utils_test.cpp │ ├── joint_trajectory_segment_test.cpp │ ├── quintic_spline_segment_test.cpp │ ├── rrbot.cpp │ ├── rrbot.xacro │ ├── rrbot_controllers.yaml │ ├── tolerances.test │ ├── tolerances.yaml │ ├── tolerances_test.cpp │ └── trajectory_interface_test.cpp ├── my_cart_controller_pkg ├── CMakeLists.txt ├── config │ └── ur10_controller_setup.yaml ├── controller_plugins.xml ├── include │ └── my_controller_pkg │ │ └── my_controller_file.h ├── package.xml └── src │ └── my_controller_file.cpp └── my_controller_pkg ├── CMakeLists.txt ├── config └── ur10_controller_setup.yaml ├── controller_plugins.xml ├── include └── my_controller_pkg │ └── my_controller_file.h ├── package.xml └── src └── my_controller_file.cpp /Note for German decimal point: -------------------------------------------------------------------------------- 1 | Um bei dem Moveit! Setup Assistenten das korrekte Dezimaltrennzeichen zu verwenden muss dieser Befehl eingegeben werden. Andernfalls werden Dezimalzahlen falsch gespeichert. 2 | export LC_NUMERIC=C 3 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Dual-Arm-Robot-Force-Controlled-Object-Manipulation 2 | This Repository is the complete workspace of a project that realizes force-controlled object manipulation with a dual-arm-robot (UR5 and UR10). You can watch a demonstration video of the system by following this link: https://www.youtube.com/watch?v=O0-cC2ST5L8 3 | 4 | 5 | # Package overview: 6 | dual_arm_robot_applications: This package contains all important Files to execute using rosrun and roslaunch and start the demonstration. 7 | 8 | dual_arm_robot_description: This package contains the description files of the robot and the environment including the table and the side walls. 9 | 10 | dual_arm_robot_driver: This package contains configuration files for the controller. 11 | 12 | dual_arm_robot_moveit_config: This is the package generated by the MoveIt! Setup Assistant. It’s configured to function with ROS Control. 13 | 14 | joint_trajectory_admittance_controller: This package contains the source code of the controller that is used for the force-controlled grasp. It's based on the 15 | 16 | 17 | # Run demonstration: 18 | 1. roslaunch dual_arm_robot_ros_control_rviz.launch/dual_arm_robot_offline_rviz.launch 19 | 2. rosrun dual_arm_robot_demonstration 20 | 21 | 22 | # Documentation: 23 | For German speakers my master's thesis provides detailed information. For everyone else I hope some of the diagrams are useful! 24 | https://drive.google.com/file/d/0B-7XIb6B00m6WUFaQndJS2NzZUE/view?usp=sharing&resourcekey=0-DCqlb-sG6d3JmOpbJA15Mw 25 | -------------------------------------------------------------------------------- /dual_arm_robot_applications/include/dual_arm_demonstrator_iml/DualArmRobot.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by daniel on 27.12.16. 3 | // 4 | 5 | #ifndef PROJECT_DUALARMROBOT_H 6 | #define PROJECT_DUALARMROBOT_H 7 | 8 | // Robot status 9 | //#define OFFLINE 10 | 11 | // Dual Arm Toolbox 12 | #include "dual_arm_toolbox/TrajectoryProcessor.h" 13 | #include "dual_arm_toolbox/Transform.h" 14 | 15 | // Controller Manager 16 | #include 17 | #include 18 | #include 19 | 20 | // MoveIt! 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | // Geometry 37 | #include 38 | #include 39 | #include 40 | 41 | // tf 42 | #include 43 | 44 | // ROS 45 | #include 46 | #include 47 | 48 | // KDL 49 | #include 50 | #include 51 | 52 | // Controller Interface 53 | #include 54 | 55 | 56 | namespace dual_arm_demonstrator_iml { 57 | class DualArmRobot { 58 | protected: 59 | ros::NodeHandle nh_; 60 | std::string ur5_controller_; 61 | std::string ur10_controller_; 62 | moveit_msgs::RobotState ur5_last_goal_state_; 63 | double ee_dist_; // distance to object because of endeffector size 64 | KDL::Frame arms_offset_; // offset between arms 65 | bool try_again_question(); 66 | planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; 67 | 68 | public: 69 | DualArmRobot(ros::NodeHandle &nh); 70 | 71 | geometry_msgs::PoseStamped ur5_last_goal_pose_; 72 | geometry_msgs::PoseStamped ur5_last_goal_pose_temp_; 73 | 74 | moveit::planning_interface::MoveGroup ur5_; 75 | moveit::planning_interface::MoveGroup ur10_; 76 | moveit::planning_interface::MoveGroup arms_; 77 | 78 | // workaround for moveGroup method does not return attached objects correctly (issue) 79 | robot_state::RobotState getCurrentRobotState(); 80 | moveit_msgs::RobotState getCurrentRobotStateMsg(); 81 | 82 | // calculates a trajectory for both arms based on the trajectory of one arm 83 | bool adaptTrajectory(moveit_msgs::RobotTrajectory ur5_trajectory, KDL::Frame offset, 84 | moveit_msgs::RobotTrajectory &both_arms_trajectory, double jump_threshold = 0.4); 85 | 86 | // returns the Offset-Vector between both end effectors 87 | KDL::Frame getCurrentOffset(); 88 | 89 | // executes a pick: Moves both arms to the object, grasps it, moves up 90 | bool pickBox(std::string object_id, geometry_msgs::Vector3Stamped lift_direction); 91 | 92 | // pace Methods 93 | bool placeBox(std::string object_id, geometry_msgs::PoseStamped ur5_place_pose, 94 | geometry_msgs::Vector3 close_direction); 95 | 96 | bool pushPlaceBox(std::string object_id, geometry_msgs::PoseStamped box_pose, geometry_msgs::Vector3 direction); 97 | 98 | bool moveObject(std::string object_id, geometry_msgs::PoseStamped ur5_pose, double scale=0.2); 99 | bool planMoveObject(std::string object_id, geometry_msgs::PoseStamped ur5_pose, double scale=0.2); // Only plan an visualize without executing. For use in validation. 100 | 101 | // enable/disable collision check between robot arms 102 | void allowedArmCollision(bool enable, std::string ur5_attachedObject); 103 | 104 | bool place(geometry_msgs::Pose pose_object); 105 | 106 | bool switch_controller(std::string stop_name, std::string start_name, std::string ur_namespace); 107 | 108 | bool graspMove(double distance, bool avoid_collisions = true, bool use_ur5=true, bool use_ur10=true); //both arms will be moved closer together 109 | 110 | bool linearMove(geometry_msgs::Vector3Stamped direction, bool avoid_collisions = true, bool use_ur5=true, bool use_ur10=true); 111 | 112 | bool linearMoveParallel(geometry_msgs::Vector3Stamped direction, std::string object_id, double traj_scale=1, bool avoid_collisions = true); 113 | 114 | bool execute(moveit::planning_interface::MoveGroup::Plan plan); 115 | 116 | bool moveHome(); 117 | }; 118 | }//ns 119 | #endif //PROJECT_DUALARMROBOT_H 120 | -------------------------------------------------------------------------------- /dual_arm_robot_applications/include/dual_arm_demonstrator_iml/SceneManager.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by daniel on 27.12.16. 3 | // 4 | 5 | #ifndef PROJECT_SCENEMANAGER_H 6 | #define PROJECT_SCENEMANAGER_H 7 | 8 | // Dual Arm Toolbox 9 | #include "dual_arm_toolbox/TrajectoryProcessor.h" 10 | #include "dual_arm_toolbox/Transform.h" 11 | 12 | // Controller Manager 13 | #include 14 | #include 15 | #include 16 | 17 | // MoveIt! 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | // Geometry 35 | #include 36 | #include 37 | #include 38 | 39 | // tf 40 | #include 41 | 42 | // ROS 43 | #include 44 | #include 45 | 46 | // KDL 47 | #include 48 | #include 49 | 50 | // Controller Interface 51 | #include 52 | 53 | namespace dual_arm_demonstrator_iml { 54 | class SceneManager { 55 | protected: 56 | ros::NodeHandle nh_; 57 | 58 | // Publisher 59 | ros::Publisher planning_scene_diff_publisher_; 60 | 61 | public: 62 | SceneManager(ros::NodeHandle& nh); 63 | 64 | // constants 65 | shape_msgs::SolidPrimitive box_; 66 | 67 | void addBox(std::string id, geometry_msgs::Pose pose, std::string link_name="world", std::string frame_id="world"); 68 | void addShelf(std::string id, geometry_msgs::Pose pose, std::string link_name="world", std::string frame_id="world"); 69 | void setupScene(); 70 | void setupSceneLift(); 71 | void setupSceneLiftCO(); 72 | }; 73 | }//namespace 74 | 75 | #endif //PROJECT_SCENEMANAGER_H 76 | -------------------------------------------------------------------------------- /dual_arm_robot_applications/include/dual_arm_toolbox/TrajectoryProcessor.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by daniel on 27.12.16. 3 | // 4 | 5 | #ifndef PROJECT_TRAJECTORYPROCESSOR_H 6 | #define PROJECT_TRAJECTORYPROCESSOR_H 7 | 8 | // ROS 9 | #include 10 | #include 11 | 12 | // Controller Manager 13 | #include 14 | #include 15 | #include 16 | 17 | // MoveIt! 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | //#include 25 | //#include 26 | 27 | // Rviz 28 | #include 29 | 30 | // Trajectory tools 31 | #include 32 | #include 33 | 34 | // KDL 35 | #include 36 | 37 | 38 | namespace dual_arm_toolbox { 39 | class TrajectoryProcessor { 40 | protected: 41 | public: 42 | // fuses two trajectories into one; both should be of same length 43 | static bool fuse(moveit_msgs::RobotTrajectory &arms_trajectory, 44 | moveit_msgs::RobotTrajectory arm1_trajectory, 45 | moveit_msgs::RobotTrajectory arm2_trajectory); 46 | 47 | // splits one trajectory for both arms into two trajectory for each arm 48 | static bool split(moveit_msgs::RobotTrajectory arms_trajectory, 49 | moveit_msgs::RobotTrajectory &arm1_trajectory, 50 | moveit_msgs::RobotTrajectory &arm2_trajectory, 51 | std::string arm1_prefix, 52 | std::string arm2_prefix); 53 | 54 | // moveit sometimes generates trajectories with two time the same time stamp. This method eliminates one entry. 55 | static void clean(moveit_msgs::RobotTrajectory &trajectory); 56 | 57 | static void computeTimeFromStart(moveit_msgs::RobotTrajectory &trajectory, double step_t); 58 | 59 | static void scaleTrajectorySpeed(moveit_msgs::RobotTrajectory &trajectory, double scale); 60 | 61 | static bool 62 | computeVelocities(moveit_msgs::RobotTrajectory &trajectory, moveit::planning_interface::MoveGroup &moveGroup); 63 | 64 | static void visualizePlan(moveit::planning_interface::MoveGroup::Plan &plan, unsigned int sec); 65 | }; 66 | }//namespace 67 | #endif //PROJECT_TRAJECTORYPROCESSOR_H 68 | -------------------------------------------------------------------------------- /dual_arm_robot_applications/include/dual_arm_toolbox/Transform.h: -------------------------------------------------------------------------------- 1 | // 2 | // Created by daniel on 27.12.16. 3 | // 4 | 5 | #ifndef PROJECT_TRANSFORM_H 6 | #define PROJECT_TRANSFORM_H 7 | 8 | // ROS 9 | #include 10 | #include 11 | 12 | // Controller Manager 13 | #include 14 | #include 15 | #include 16 | 17 | // MoveIt! 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | //#include 25 | //#include 26 | 27 | // Rviz 28 | #include 29 | 30 | // Trajectory tools 31 | #include 32 | #include 33 | 34 | // KDL 35 | #include 36 | 37 | 38 | namespace dual_arm_toolbox{ 39 | class Transform { 40 | protected: 41 | public: 42 | static void transformPoseToKDL(geometry_msgs::Pose pose, KDL::Frame& kdl_frame); 43 | static void transformKDLtoPose(KDL::Frame kdl_frame, geometry_msgs::Pose& pose); 44 | }; 45 | } 46 | 47 | #endif //PROJECT_TRANSFORM_H 48 | -------------------------------------------------------------------------------- /dual_arm_robot_applications/include/ur_logging/Stopwatch.h: -------------------------------------------------------------------------------- 1 | #include 2 | //#include 3 | 4 | class Stopwatch{ 5 | protected: 6 | public: 7 | Stopwatch(); 8 | void restart(); 9 | ros::Duration elapsed(); 10 | private: 11 | ros::Time start_; 12 | }; 13 | -------------------------------------------------------------------------------- /dual_arm_robot_applications/include/ur_logging/UrLogger.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | 14 | #include "ur_logging/Stopwatch.h" 15 | #include "ur_logging/UrMessageListener.h" 16 | 17 | class UR_Logger{ //Logs all important messages from ur. It can handle several robots at a time 18 | protected: 19 | ros::NodeHandle nh_; 20 | std::vector ur_listeners_; 21 | 22 | public: 23 | UR_Logger(ros::NodeHandle& nh, std::vector ur_namespaces); 24 | ~UR_Logger(); 25 | 26 | Stopwatch stopwatch_; 27 | std::string logfile_name_; 28 | char delimiter_; 29 | 30 | void generate_logfile_name(); //automatically generate a name 31 | void start(int log_rate); //log_rate=[Hz] 32 | void stop(); 33 | std::string headline(UR_Message_Listener ur_listener); //return a headline 34 | std::string data_line(UR_Message_Listener ur_listener); //return formattet data 35 | private: 36 | ros::Timer timer_; 37 | std::ofstream logfile_; 38 | ros::Time log_start_time_; 39 | 40 | void logCallback(const ros::TimerEvent&); 41 | }; 42 | -------------------------------------------------------------------------------- /dual_arm_robot_applications/include/ur_logging/UrMessageListener.h: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | class UR_Message_Listener{ //handles callbacks and saves last received messages 8 | protected: 9 | ros::NodeHandle nh_; 10 | ros::Subscriber wrench_sub_; 11 | ros::Subscriber speed_traj_sub_; 12 | ros::Subscriber state_sub_; 13 | public: 14 | UR_Message_Listener(ros::NodeHandle& nh, std::string ur_namespace); 15 | 16 | std::string ur_namespace_; 17 | trajectory_msgs::JointTrajectory last_speed_traj_msg_; 18 | geometry_msgs::WrenchStamped last_wrench_msg_; 19 | sensor_msgs::JointState last_state_msg_; 20 | std::string ur_prefix_; 21 | private: 22 | void wrenchCallback(const geometry_msgs::WrenchStamped::Ptr& msg); 23 | void speed_trajCallback(const trajectory_msgs::JointTrajectory::Ptr& msg); 24 | void stateCallback(const sensor_msgs::JointState::Ptr& msg); 25 | }; 26 | -------------------------------------------------------------------------------- /dual_arm_robot_applications/launch/dual_arm_robot_offline_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | [/move_group/fake_controller_joint_states] 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | -------------------------------------------------------------------------------- /dual_arm_robot_applications/launch/dual_arm_robot_ros_control_rviz.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 | -------------------------------------------------------------------------------- /dual_arm_robot_applications/launch/dual_arm_robot_rviz.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 | -------------------------------------------------------------------------------- /dual_arm_robot_applications/launch/dual_arm_robot_tcp_wrench_publisher.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /dual_arm_robot_applications/launch/ur10_ros_control_rviz.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 | 37 | 38 | 39 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /dual_arm_robot_applications/launch/ur5_olddriver_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | -------------------------------------------------------------------------------- /dual_arm_robot_applications/launch/ur5_ros_control_hi.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 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /dual_arm_robot_applications/launch/ur5_ros_control_rviz.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 | 37 | 38 | 39 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /dual_arm_robot_applications/launch/ur5_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 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 | -------------------------------------------------------------------------------- /dual_arm_robot_applications/meshes/shelf/shelf.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/danielhoeltgen/Dual-Arm-Robot-Force-Controlled-Object-Manipulation/53761ef144e67e65b8580179ec0a451ba537313f/dual_arm_robot_applications/meshes/shelf/shelf.stl -------------------------------------------------------------------------------- /dual_arm_robot_applications/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | dual_arm_robot_applications 4 | 1.0.0 5 | 6 | Applications for the dual arm robot 7 | 8 | 9 | Daniel Hoeltgen 10 | Daniel Hoeltgen 11 | 12 | BSD 13 | 14 | catkin 15 | roscpp 16 | tf 17 | roscpp 18 | tf 19 | rospy 20 | actionlib 21 | control_msgs 22 | sensor_msgs 23 | trajectory_msgs 24 | ur_msgs 25 | python-lxml 26 | 27 | -------------------------------------------------------------------------------- /dual_arm_robot_applications/src/dual_arm_robot_vel_force_experiment.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Daniel Höltgen. 3 | // 4 | 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | #include "ur_logging/UrLogger.h" 13 | 14 | /* TODO 15 | * vel ansteigen, auf null fallen, rückwärts bewegen 16 | */ 17 | 18 | // m/s ; m 19 | void run_experiment(ros::NodeHandle &nh, ros::Publisher &ur10_speed_pub, moveit::planning_interface::MoveGroup &ur10, double velocity, double moving_distance){ 20 | // create ur_logger. Use this namespace 21 | std::vector ur_namespaces; 22 | ur_namespaces.push_back(""); 23 | UR_Logger ur_logger(nh, ur_namespaces); 24 | 25 | // stop controller 26 | ros::ServiceClient ur10_srv_switch_controller = nh.serviceClient("controller_manager/switch_controller"); 27 | controller_manager_msgs::SwitchController srv_req; 28 | srv_req.request.strictness = controller_manager_msgs::SwitchController::Request::BEST_EFFORT; 29 | srv_req.request.stop_controllers.push_back("vel_based_pos_traj_controller"); 30 | bool success = ur10_srv_switch_controller.call(srv_req); 31 | ROS_INFO("Stopping controller %s",success?"SUCCEDED":"FAILED"); 32 | srv_req.request.stop_controllers.clear(); 33 | 34 | geometry_msgs::PoseStamped current_pose = ur10.getCurrentPose(); 35 | std::cout << "x: " << current_pose.pose.position.x << "\ty: " << current_pose.pose.position.y << "\tz: " << current_pose.pose.position.z << std::endl; 36 | double radius = sqrt(current_pose.pose.position.x*current_pose.pose.position.x + current_pose.pose.position.y*current_pose.pose.position.y); 37 | std::cout << "RADIUS: " << radius << std::endl; 38 | double omega = velocity/radius; 39 | double moving_time = moving_distance/velocity; //move 3cm, obstacle is around 1-2 cm away. 40 | 41 | // Output Message Speed = const 42 | trajectory_msgs::JointTrajectory joint_traj; //containing speed command 43 | trajectory_msgs::JointTrajectoryPoint traj_point; 44 | traj_point.velocities.assign(6,0); 45 | traj_point.velocities[0] = -omega; 46 | joint_traj.points.push_back(traj_point); 47 | 48 | // start logging 49 | ur_logger.start(50); 50 | 51 | ROS_INFO("Publishing velocity commands to ur10 at 125Hz"); 52 | 53 | // publish messages 54 | ros::Rate loop_rate(125); // velocity-message publish rate 55 | 56 | Stopwatch stopwatch; 57 | 58 | while (ros::ok() && (stopwatch.elapsed().toSec()("ur_driver/joint_speed", 1); 119 | ros::AsyncSpinner asyncSpinner(2); 120 | asyncSpinner.start(); 121 | 122 | // MoveGroup 123 | moveit::planning_interface::MoveGroup ur10("manipulator"); 124 | moveit::planning_interface::MoveGroup::Plan plan; 125 | ur10.setPlanningTime(30); 126 | 127 | ROS_WARN("robot is moving without collision checking. BE CAREFUL!"); 128 | ROS_INFO("waiting 10 Seconds. Press Ctrl-C if Robot is in the wrong start position"); 129 | ros::Duration(10).sleep(); 130 | 131 | //long distance position 132 | /* 133 | ur10.setJointValueTarget("elbow_joint", -1.366541959239651); 134 | ur10.setJointValueTarget("shoulder_lift_joint", -2.573810648739345); 135 | ur10.setJointValueTarget("shoulder_pan_joint", 0.5943102022167164); 136 | ur10.setJointValueTarget("wrist_1_joint", -0.7533539281232803); 137 | ur10.setJointValueTarget("wrist_2_joint", -0.0); 138 | ur10.setJointValueTarget("wrist_3_joint", 0.00015758105264953662);*/ 139 | 140 | // short distance postion 141 | ur10.setJointValueTarget("elbow_joint", -1.9646600642315206); 142 | ur10.setJointValueTarget("shoulder_lift_joint", -2.2494529549924893); 143 | ur10.setJointValueTarget("shoulder_pan_joint", -1.1800545151007107); 144 | ur10.setJointValueTarget("wrist_1_joint", -0.41466027950402257); 145 | ur10.setJointValueTarget("wrist_2_joint", -0.0); 146 | ur10.setJointValueTarget("wrist_3_joint", 0.00300112795031922); 147 | 148 | //ur10.setJointValueTarget(ur10JointTarget); 149 | ur10.plan(plan); 150 | ROS_WARN("visualizing plan. STRG+C to interrupt."); 151 | sleep(4); 152 | ur10.execute(plan); 153 | sleep(3); 154 | 155 | // ::::::: Run Experiments ::::::: 156 | /* 157 | ROS_INFO("::: Experiment 1 ::: vel = 0.02m/s; s = 0.03m"); 158 | run_experiment(nh, ur10_speed_pub, ur10, 0.02, 0.03); 159 | 160 | ROS_INFO("moveing to start"); 161 | ur10.plan(plan); 162 | ur10.execute(plan); 163 | 164 | ROS_INFO("::: Experiment 2 ::: vel = 0.01m/s; s = 0.03m"); 165 | run_experiment(nh, ur10_speed_pub, ur10, 0.01, 0.03); 166 | 167 | ROS_INFO("moveing to start"); 168 | ur10.plan(plan); 169 | ur10.execute(plan); 170 | 171 | ROS_INFO("::: Experiment 3 ::: vel = 0.005m/s; s = 0.03m"); 172 | run_experiment(nh, ur10_speed_pub, ur10, 0.005, 0.03); 173 | 174 | ROS_INFO("moveing to start"); 175 | ur10.plan(plan); 176 | ur10.execute(plan); 177 | */ 178 | ROS_INFO("::: Experiment 4 ::: vel = 0.001m/s; s = 0.03m"); 179 | run_experiment(nh, ur10_speed_pub, ur10, 0.001, 0.03); 180 | 181 | ROS_INFO("moveing to start"); 182 | ur10.plan(plan); 183 | ur10.execute(plan); 184 | 185 | 186 | ROS_INFO("finished. shutting down."); 187 | 188 | ros::shutdown(); 189 | return 0; 190 | } 191 | -------------------------------------------------------------------------------- /dual_arm_robot_applications/src/dual_arm_toolbox/Transform.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Daniel Höltgen on 17.10.16. 3 | // 4 | 5 | #include "dual_arm_toolbox/Transform.h" 6 | 7 | using namespace dual_arm_toolbox; 8 | 9 | void Transform::transformPoseToKDL(geometry_msgs::Pose pose, KDL::Frame& kdl_frame){ 10 | kdl_frame.M = kdl_frame.M.Quaternion( 11 | pose.orientation.x, 12 | pose.orientation.y, 13 | pose.orientation.z, 14 | pose.orientation.w 15 | ); 16 | kdl_frame.p.x(pose.position.x); 17 | kdl_frame.p.y(pose.position.y); 18 | kdl_frame.p.z(pose.position.z); 19 | } 20 | 21 | void Transform::transformKDLtoPose(KDL::Frame kdl_frame, geometry_msgs::Pose& pose){ 22 | kdl_frame.M.GetQuaternion( 23 | pose.orientation.x, 24 | pose.orientation.y, 25 | pose.orientation.z, 26 | pose.orientation.w 27 | ); 28 | pose.position.x = kdl_frame.p.x(); 29 | pose.position.y = kdl_frame.p.y(); 30 | pose.position.z = kdl_frame.p.z(); 31 | } -------------------------------------------------------------------------------- /dual_arm_robot_applications/src/executables/demonstration_setup_helper.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Daniel Höltgen on 23.12.16. 3 | // 4 | 5 | // ROS 6 | #include 7 | #include 8 | 9 | // MoveIt! 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | // Rviz 16 | #include 17 | 18 | // Dual Arm Tools 19 | #include "dual_arm_toolbox/TrajectoryProcessor.h" 20 | #include "dual_arm_toolbox/Transform.h" 21 | 22 | // Dual Arm Demonstrator 23 | #include "dual_arm_demonstrator_iml/DualArmRobot.h" 24 | #include "dual_arm_demonstrator_iml/SceneManager.h" 25 | 26 | 27 | int main(int argc, char **argv) { 28 | ros::init(argc, argv, "shelf_setup_helper"); 29 | ros::AsyncSpinner spinner(2); 30 | spinner.start(); 31 | ros::NodeHandle nh; 32 | 33 | // Dual Arm Robot Setup 34 | dual_arm_demonstrator_iml::DualArmRobot dualArmRobot(nh); 35 | 36 | // Scene Setup 37 | dual_arm_demonstrator_iml::SceneManager sceneManager(nh); 38 | sceneManager.setupScene(); 39 | 40 | // variables 41 | moveit::planning_interface::MoveGroup::Plan ur5_plan; 42 | moveit::planning_interface::MoveGroup::Plan ur10_plan; 43 | moveit::planning_interface::MoveItErrorCode error; 44 | error.val = -1; 45 | 46 | // move ur5 to shelf 47 | while (error.val != 1){ 48 | geometry_msgs::PoseStamped ur5_pose; 49 | ur5_pose.header.frame_id = "shelf"; 50 | ur5_pose.pose.position.x = -0.07; 51 | ur5_pose.pose.position.y = 0.125; 52 | ur5_pose.pose.position.z = 0.53; 53 | KDL::Rotation ur5_rot; // generated to easily assign quaternion of pose 54 | ur5_rot.DoRotY(3.14 / 2); 55 | ur5_rot.GetQuaternion(ur5_pose.pose.orientation.x, ur5_pose.pose.orientation.y, ur5_pose.pose.orientation.z, 56 | ur5_pose.pose.orientation.w); 57 | dualArmRobot.ur5_.setPoseTarget(ur5_pose, dualArmRobot.ur5_.getEndEffectorLink()); 58 | 59 | error = dualArmRobot.ur5_.plan(ur5_plan); 60 | 61 | } 62 | dualArmRobot.execute(ur5_plan); 63 | 64 | error.val = -1; 65 | while (error.val != 1){ 66 | // move ur10 to shelf 67 | geometry_msgs::PoseStamped ur10_pose; 68 | ur10_pose.header.frame_id = "shelf"; 69 | ur10_pose.pose.position.x = 0.125; 70 | ur10_pose.pose.position.y = 0.25 + 0.07; 71 | ur10_pose.pose.position.z = 0.53; 72 | KDL::Rotation ur10_rot; // generated to easily assign quaternion of pose 73 | ur10_rot.DoRotX(3.14 / 2); 74 | ur10_rot.GetQuaternion(ur10_pose.pose.orientation.x, ur10_pose.pose.orientation.y, ur10_pose.pose.orientation.z, 75 | ur10_pose.pose.orientation.w); 76 | dualArmRobot.ur10_.setPoseTarget(ur10_pose, dualArmRobot.ur10_.getEndEffectorLink()); 77 | 78 | error = dualArmRobot.ur10_.plan(ur10_plan); 79 | 80 | } 81 | dualArmRobot.execute(ur10_plan); 82 | 83 | // move closer 84 | dualArmRobot.graspMove(0.07); 85 | 86 | // END 87 | ROS_INFO("Place the shelf between the robot arms as displayed in rviz!"); 88 | ros::shutdown(); 89 | return 0; 90 | } -------------------------------------------------------------------------------- /dual_arm_robot_applications/src/executables/dual_arm_robot_lift_experiment_force.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Daniel Höltgen on 08.04.17. 3 | // 4 | 5 | // ROS 6 | #include 7 | #include 8 | 9 | // MoveIt! 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | // Rviz 16 | #include 17 | 18 | // Dual Arm Tools 19 | #include "dual_arm_toolbox/TrajectoryProcessor.h" 20 | #include "dual_arm_toolbox/Transform.h" 21 | 22 | // Dual Arm Demonstrator 23 | #include "dual_arm_demonstrator_iml/DualArmRobot.h" 24 | #include "dual_arm_demonstrator_iml/SceneManager.h" 25 | 26 | // UR Logger 27 | #include "ur_logging/UrLogger.h" 28 | 29 | 30 | int main(int argc, char **argv) { 31 | ros::init(argc, argv, "dual_arm_robot_demonstration"); 32 | ros::AsyncSpinner spinner(2); 33 | spinner.start(); 34 | ros::NodeHandle nh; 35 | 36 | #ifdef OFFLINE 37 | ROS_WARN("Robot offline"); 38 | #endif 39 | 40 | // Dual Arm Robot Setup 41 | dual_arm_demonstrator_iml::DualArmRobot dualArmRobot(nh); 42 | 43 | // Scene Setup 44 | dual_arm_demonstrator_iml::SceneManager sceneManager(nh); 45 | sceneManager.setupSceneLift(); 46 | 47 | // move home 48 | // dualArmRobot.moveHome(); 49 | 50 | // setup constraints 51 | moveit_msgs::JointConstraint jcm; 52 | moveit_msgs::Constraints ur5_constraints; 53 | moveit_msgs::Constraints ur10_constraints; 54 | 55 | jcm.joint_name="ur10_shoulder_pan_joint"; 56 | jcm.position = 2.4; 57 | jcm.tolerance_above = 0.7; 58 | jcm.tolerance_below = 2.5; 59 | jcm.weight = 1.0; 60 | ur10_constraints.joint_constraints.push_back(jcm); 61 | dualArmRobot.ur10_.setPathConstraints(ur10_constraints); 62 | 63 | jcm.joint_name="ur5_shoulder_pan_joint"; 64 | jcm.position = 0.0; 65 | jcm.tolerance_above = 1.0; 66 | jcm.tolerance_below = 1.0; 67 | jcm.weight = 1.0; 68 | ur5_constraints.joint_constraints.push_back(jcm); 69 | dualArmRobot.ur5_.setPathConstraints(ur5_constraints); 70 | 71 | // Pick box1 72 | geometry_msgs::Vector3Stamped direction; 73 | direction.header.frame_id = "/table_ground"; 74 | direction.vector.x = 0; 75 | direction.vector.y = 0; 76 | direction.vector.z = 0;//0.01; 77 | if (!dualArmRobot.pickBox("box1", direction)) { 78 | ROS_WARN("Pick failed"); 79 | ROS_ERROR("Can't execute demonstration without successful pick. Demonstration aborted."); 80 | return 0; 81 | } 82 | /* 83 | geometry_msgs::PoseStamped place_pose_ur5 = dualArmRobot.ur5_.getCurrentPose(dualArmRobot.ur5_.getEndEffectorLink()); 84 | place_pose_ur5.pose.position.z = place_pose_ur5.pose.position.z - direction.vector.z;*/ 85 | 86 | // clear path constraints 87 | dualArmRobot.ur5_.clearPathConstraints(); 88 | dualArmRobot.ur10_.clearPathConstraints(); 89 | 90 | // plan move and execute 91 | //geometry_msgs::PoseStamped start_pose = dualArmRobot.ur5_.getCurrentPose(dualArmRobot.ur5_.getEndEffectorLink()); 92 | //geometry_msgs::PoseStamped lift_pose = start_pose; 93 | //lift_pose.pose.position.z = lift_pose.pose.position.z +1; 94 | geometry_msgs::Vector3Stamped linear_move_direction; 95 | linear_move_direction.vector.z = 0.02; 96 | linear_move_direction.header.frame_id = "table_ground"; 97 | 98 | #ifndef OFFLINE 99 | // experiment just hold 100 | ROS_INFO("Starting Log in 3"); 101 | sleep(1); 102 | ROS_INFO("Starting Log in 2"); 103 | sleep(1); 104 | ROS_INFO("Starting Log in 1"); 105 | sleep(1); 106 | ROS_INFO("Starting Log now"); 107 | // start log 108 | std::vector ur_namespaces; 109 | ur_namespaces.push_back("ur5"); 110 | UR_Logger ur_logger(nh, ur_namespaces); 111 | ur_logger.start(20); 112 | 113 | ROS_INFO("Waiting 10 seconds"); 114 | ros::Duration(7).sleep(); 115 | ROS_INFO("Stop Log in 3"); 116 | sleep(1); 117 | ROS_INFO("Stop Log in 2"); 118 | sleep(1); 119 | ROS_INFO("Stop Log in 1"); 120 | sleep(1); 121 | 122 | ur_logger.stop(); 123 | 124 | // experiment hold and linear move 125 | ROS_INFO("Starting next experiment in 10 Seconds"); 126 | sleep(7); 127 | ROS_INFO("Starting Log in 3"); 128 | sleep(1); 129 | ROS_INFO("Starting Log in 2"); 130 | sleep(1); 131 | ROS_INFO("Starting Log in 1"); 132 | sleep(1); 133 | ROS_INFO("Starting Log now"); 134 | 135 | ROS_INFO("Waiting 5 seconds"); 136 | sleep(5); 137 | #endif 138 | ROS_INFO("Linear Move up"); 139 | dualArmRobot.linearMoveParallel(linear_move_direction, "box1", 0.5); 140 | 141 | #ifndef OFFLINE 142 | ROS_INFO("Waiting 5 seconds"); 143 | sleep(2); 144 | ROS_INFO("Stop Log in 3"); 145 | sleep(1); 146 | ROS_INFO("Stop Log in 2"); 147 | sleep(1); 148 | ROS_INFO("Stop Log in 1"); 149 | sleep(1); 150 | ur_logger.stop(); 151 | #endif 152 | 153 | // place box 154 | /* 155 | geometry_msgs::Vector3Stamped close_direction; 156 | close_direction.vector.z = -direction.vector.z; 157 | dualArmRobot.placeBox("box1", place_pose_ur5, close_direction.vector);*/ 158 | 159 | // END 160 | ROS_INFO("Finished demonstration"); 161 | ros::shutdown(); 162 | return 0; 163 | } -------------------------------------------------------------------------------- /dual_arm_robot_applications/src/executables/dual_arm_robot_lift_experiment_force_1.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Daniel Höltgen on 08.04.17. 3 | // 4 | 5 | // ROS 6 | #include 7 | #include 8 | 9 | // MoveIt! 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | // Rviz 16 | #include 17 | 18 | // Dual Arm Tools 19 | #include "dual_arm_toolbox/TrajectoryProcessor.h" 20 | #include "dual_arm_toolbox/Transform.h" 21 | 22 | // Dual Arm Demonstrator 23 | #include "dual_arm_demonstrator_iml/DualArmRobot.h" 24 | #include "dual_arm_demonstrator_iml/SceneManager.h" 25 | 26 | // UR Logger 27 | #include "ur_logging/UrLogger.h" 28 | 29 | 30 | int main(int argc, char **argv) { 31 | ros::init(argc, argv, "dual_arm_robot_demonstration"); 32 | ros::AsyncSpinner spinner(2); 33 | spinner.start(); 34 | ros::NodeHandle nh; 35 | 36 | #ifdef OFFLINE 37 | ROS_WARN("Robot offline"); 38 | #endif 39 | 40 | // Dual Arm Robot Setup 41 | dual_arm_demonstrator_iml::DualArmRobot dualArmRobot(nh); 42 | 43 | // Scene Setup 44 | dual_arm_demonstrator_iml::SceneManager sceneManager(nh); 45 | sceneManager.setupSceneLift(); 46 | 47 | // move home 48 | // dualArmRobot.moveHome(); 49 | 50 | // setup constraints 51 | moveit_msgs::JointConstraint jcm; 52 | moveit_msgs::Constraints ur5_constraints; 53 | moveit_msgs::Constraints ur10_constraints; 54 | 55 | jcm.joint_name="ur10_shoulder_pan_joint"; 56 | jcm.position = 2.4; 57 | jcm.tolerance_above = 0.7; 58 | jcm.tolerance_below = 2.5; 59 | jcm.weight = 1.0; 60 | ur10_constraints.joint_constraints.push_back(jcm); 61 | dualArmRobot.ur10_.setPathConstraints(ur10_constraints); 62 | 63 | jcm.joint_name="ur5_shoulder_pan_joint"; 64 | jcm.position = 0.0; 65 | jcm.tolerance_above = 1.0; 66 | jcm.tolerance_below = 1.0; 67 | jcm.weight = 1.0; 68 | ur5_constraints.joint_constraints.push_back(jcm); 69 | dualArmRobot.ur5_.setPathConstraints(ur5_constraints); 70 | 71 | 72 | /* 73 | geometry_msgs::PoseStamped place_pose_ur5 = dualArmRobot.ur5_.getCurrentPose(dualArmRobot.ur5_.getEndEffectorLink()); 74 | place_pose_ur5.pose.position.z = place_pose_ur5.pose.position.z - direction.vector.z;*/ 75 | 76 | // clear path constraints 77 | // dualArmRobot.ur5_.clearPathConstraints(); 78 | // dualArmRobot.ur10_.clearPathConstraints(); 79 | 80 | // plan move and execute 81 | //geometry_msgs::PoseStamped start_pose = dualArmRobot.ur5_.getCurrentPose(dualArmRobot.ur5_.getEndEffectorLink()); 82 | //geometry_msgs::PoseStamped lift_pose = start_pose; 83 | //lift_pose.pose.position.z = lift_pose.pose.position.z +1; 84 | geometry_msgs::Vector3Stamped linear_move_direction; 85 | linear_move_direction.vector.z = 0.02; 86 | linear_move_direction.header.frame_id = "table_ground"; 87 | 88 | //#ifndef OFFLINE 89 | // experiment just hold 90 | ROS_INFO("Starting Log in 3"); 91 | sleep(1); 92 | ROS_INFO("Starting Log in 2"); 93 | sleep(1); 94 | ROS_INFO("Starting Log in 1"); 95 | sleep(1); 96 | ROS_INFO("Starting Log now"); 97 | // start log 98 | std::vector ur_namespaces; 99 | ur_namespaces.push_back("ur5"); 100 | UR_Logger ur_logger(nh, ur_namespaces); 101 | ur_logger.start(100); 102 | 103 | // Pick box1 104 | geometry_msgs::Vector3Stamped direction; 105 | direction.header.frame_id = "/table_ground"; 106 | direction.vector.x = 0; 107 | direction.vector.y = 0; 108 | direction.vector.z = 0.6;//0.01; 109 | if (!dualArmRobot.pickBox("box1", direction)) { 110 | ROS_WARN("Pick failed"); 111 | ROS_ERROR("Can't execute demonstration without successful pick. Demonstration aborted."); 112 | return 0; 113 | } 114 | 115 | ROS_INFO("Waiting 5 seconds"); 116 | ros::Duration(5).sleep(); 117 | ROS_INFO("Stop Log in 3"); 118 | sleep(1); 119 | ROS_INFO("Stop Log in 2"); 120 | sleep(1); 121 | ROS_INFO("Stop Log in 1"); 122 | sleep(1); 123 | 124 | ur_logger.stop(); 125 | /* 126 | // experiment hold and linear move 127 | ROS_INFO("Starting next experiment in 10 Seconds"); 128 | sleep(7); 129 | ROS_INFO("Starting Log in 3"); 130 | sleep(1); 131 | ROS_INFO("Starting Log in 2"); 132 | sleep(1); 133 | ROS_INFO("Starting Log in 1"); 134 | sleep(1); 135 | ROS_INFO("Starting Log now"); 136 | 137 | ROS_INFO("Waiting 5 seconds"); 138 | sleep(5); 139 | #endif 140 | ROS_INFO("Linear Move up"); 141 | dualArmRobot.linearMoveParallel(linear_move_direction, "box1", 0.5); 142 | 143 | #ifndef OFFLINE 144 | ROS_INFO("Waiting 5 seconds"); 145 | sleep(2); 146 | ROS_INFO("Stop Log in 3"); 147 | sleep(1); 148 | ROS_INFO("Stop Log in 2"); 149 | sleep(1); 150 | ROS_INFO("Stop Log in 1"); 151 | sleep(1); 152 | ur_logger.stop(); 153 | #endif 154 | */ 155 | // place box 156 | /* 157 | geometry_msgs::Vector3Stamped close_direction; 158 | close_direction.vector.z = -direction.vector.z; 159 | dualArmRobot.placeBox("box1", place_pose_ur5, close_direction.vector);*/ 160 | 161 | // END 162 | ROS_INFO("Finished demonstration"); 163 | ros::shutdown(); 164 | return 0; 165 | } -------------------------------------------------------------------------------- /dual_arm_robot_applications/src/executables/dual_arm_robot_lift_experiment_force_2.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Daniel Höltgen on 08.04.17. 3 | // 4 | 5 | // ROS 6 | #include 7 | #include 8 | 9 | // MoveIt! 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | // Rviz 16 | #include 17 | 18 | // Dual Arm Tools 19 | #include "dual_arm_toolbox/TrajectoryProcessor.h" 20 | #include "dual_arm_toolbox/Transform.h" 21 | 22 | // Dual Arm Demonstrator 23 | #include "dual_arm_demonstrator_iml/DualArmRobot.h" 24 | #include "dual_arm_demonstrator_iml/SceneManager.h" 25 | 26 | // UR Logger 27 | #include "ur_logging/UrLogger.h" 28 | 29 | 30 | int main(int argc, char **argv) { 31 | ros::init(argc, argv, "dual_arm_robot_demonstration"); 32 | ros::AsyncSpinner spinner(2); 33 | spinner.start(); 34 | ros::NodeHandle nh; 35 | 36 | #ifdef OFFLINE 37 | ROS_WARN("Robot offline"); 38 | #endif 39 | 40 | // Dual Arm Robot Setup 41 | dual_arm_demonstrator_iml::DualArmRobot dualArmRobot(nh); 42 | 43 | // Scene Setup 44 | dual_arm_demonstrator_iml::SceneManager sceneManager(nh); 45 | sceneManager.setupSceneLift(); 46 | 47 | // move home 48 | // dualArmRobot.moveHome(); 49 | 50 | // setup constraints 51 | moveit_msgs::JointConstraint jcm; 52 | moveit_msgs::Constraints ur5_constraints; 53 | moveit_msgs::Constraints ur10_constraints; 54 | 55 | jcm.joint_name="ur10_shoulder_pan_joint"; 56 | jcm.position = 2.4; 57 | jcm.tolerance_above = 0.7; 58 | jcm.tolerance_below = 2.5; 59 | jcm.weight = 1.0; 60 | ur10_constraints.joint_constraints.push_back(jcm); 61 | dualArmRobot.ur10_.setPathConstraints(ur10_constraints); 62 | 63 | jcm.joint_name="ur5_shoulder_pan_joint"; 64 | jcm.position = 0.0; 65 | jcm.tolerance_above = 1.0; 66 | jcm.tolerance_below = 1.0; 67 | jcm.weight = 1.0; 68 | ur5_constraints.joint_constraints.push_back(jcm); 69 | dualArmRobot.ur5_.setPathConstraints(ur5_constraints); 70 | 71 | // Pick box1 72 | geometry_msgs::Vector3Stamped direction; 73 | direction.header.frame_id = "/table_ground"; 74 | direction.vector.x = 0; 75 | direction.vector.y = 0; 76 | direction.vector.z = 0;//0.01; 77 | if (!dualArmRobot.pickBox("box1", direction)) { 78 | ROS_WARN("Pick failed"); 79 | ROS_ERROR("Can't execute demonstration without successful pick. Demonstration aborted."); 80 | return 0; 81 | } 82 | /* 83 | geometry_msgs::PoseStamped place_pose_ur5 = dualArmRobot.ur5_.getCurrentPose(dualArmRobot.ur5_.getEndEffectorLink()); 84 | place_pose_ur5.pose.position.z = place_pose_ur5.pose.position.z - direction.vector.z;*/ 85 | 86 | // clear path constraints 87 | dualArmRobot.ur5_.clearPathConstraints(); 88 | dualArmRobot.ur10_.clearPathConstraints(); 89 | 90 | // plan move and execute 91 | //geometry_msgs::PoseStamped start_pose = dualArmRobot.ur5_.getCurrentPose(dualArmRobot.ur5_.getEndEffectorLink()); 92 | //geometry_msgs::PoseStamped lift_pose = start_pose; 93 | //lift_pose.pose.position.z = lift_pose.pose.position.z +1; 94 | geometry_msgs::Vector3Stamped linear_move_direction; 95 | linear_move_direction.vector.z = 0.00; 96 | linear_move_direction.header.frame_id = "table_ground"; 97 | 98 | //#ifndef OFFLINE 99 | // experiment just hold 100 | ROS_INFO("Starting Log in 10"); 101 | sleep(7); 102 | ROS_INFO("Starting Log in 3"); 103 | sleep(1); 104 | ROS_INFO("Starting Log in 2"); 105 | sleep(1); 106 | ROS_INFO("Starting Log in 1"); 107 | sleep(1); 108 | ROS_INFO("Starting Log now"); 109 | // start log 110 | std::vector ur_namespaces; 111 | ur_namespaces.push_back("ur5"); 112 | UR_Logger ur_logger(nh, ur_namespaces); 113 | ur_logger.start(100); 114 | 115 | ROS_INFO("Waiting 10 seconds"); 116 | ros::Duration(7).sleep(); 117 | ROS_INFO("Stop Log in 3"); 118 | sleep(1); 119 | ROS_INFO("Stop Log in 2"); 120 | sleep(1); 121 | ROS_INFO("Stop Log in 1"); 122 | sleep(1); 123 | 124 | ur_logger.stop(); 125 | /* 126 | // experiment hold and linear move 127 | ROS_INFO("Starting next experiment in 10 Seconds"); 128 | sleep(7); 129 | ROS_INFO("Starting Log in 3"); 130 | sleep(1); 131 | ROS_INFO("Starting Log in 2"); 132 | sleep(1); 133 | ROS_INFO("Starting Log in 1"); 134 | sleep(1); 135 | ROS_INFO("Starting Log now"); 136 | 137 | ROS_INFO("Waiting 5 seconds"); 138 | sleep(5); 139 | #endif 140 | ROS_INFO("Linear Move up"); 141 | dualArmRobot.linearMoveParallel(linear_move_direction, "box1", 0.5); 142 | 143 | #ifndef OFFLINE 144 | ROS_INFO("Waiting 5 seconds"); 145 | sleep(2); 146 | ROS_INFO("Stop Log in 3"); 147 | sleep(1); 148 | ROS_INFO("Stop Log in 2"); 149 | sleep(1); 150 | ROS_INFO("Stop Log in 1"); 151 | sleep(1); 152 | ur_logger.stop(); 153 | #endif 154 | */ 155 | // place box 156 | /* 157 | geometry_msgs::Vector3Stamped close_direction; 158 | close_direction.vector.z = -direction.vector.z; 159 | dualArmRobot.placeBox("box1", place_pose_ur5, close_direction.vector);*/ 160 | 161 | // END 162 | ROS_INFO("Finished demonstration"); 163 | ros::shutdown(); 164 | return 0; 165 | } -------------------------------------------------------------------------------- /dual_arm_robot_applications/src/executables/dual_arm_robot_lift_experiment_planning.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by Daniel Höltgen on 08.04.17. 3 | // 4 | 5 | // ROS 6 | #include 7 | #include 8 | 9 | // MoveIt! 10 | #include 11 | #include 12 | #include 13 | #include 14 | 15 | // Rviz 16 | #include 17 | 18 | // Dual Arm Tools 19 | #include "dual_arm_toolbox/TrajectoryProcessor.h" 20 | #include "dual_arm_toolbox/Transform.h" 21 | 22 | // Dual Arm Demonstrator 23 | #include "dual_arm_demonstrator_iml/DualArmRobot.h" 24 | #include "dual_arm_demonstrator_iml/SceneManager.h" 25 | 26 | 27 | int main(int argc, char **argv) { 28 | ros::init(argc, argv, "dual_arm_robot_demonstration"); 29 | ros::AsyncSpinner spinner(2); 30 | spinner.start(); 31 | ros::NodeHandle nh; 32 | 33 | #ifdef OFFLINE 34 | ROS_WARN("Robot offline"); 35 | #endif 36 | 37 | // Dual Arm Robot Setup 38 | dual_arm_demonstrator_iml::DualArmRobot dualArmRobot(nh); 39 | 40 | // Scene Setup 41 | dual_arm_demonstrator_iml::SceneManager sceneManager(nh); 42 | // with or without collision objects? 43 | sceneManager.setupSceneLiftCO(); 44 | // sceneManager.setupSceneLift(); 45 | 46 | // move home 47 | dualArmRobot.moveHome(); 48 | 49 | // setup constraints 50 | moveit_msgs::JointConstraint jcm; 51 | moveit_msgs::Constraints ur5_constraints; 52 | moveit_msgs::Constraints ur10_constraints; 53 | jcm.joint_name="ur10_shoulder_pan_joint"; 54 | jcm.position = 2.4; 55 | jcm.tolerance_above = 0.7; 56 | jcm.tolerance_below = 2.5; 57 | jcm.weight = 1.0; 58 | ur10_constraints.joint_constraints.push_back(jcm); 59 | dualArmRobot.ur10_.setPathConstraints(ur10_constraints); 60 | 61 | /* 62 | moveit_msgs::JointConstraint jcm; 63 | moveit_msgs::Constraints ur5_constraints; 64 | moveit_msgs::Constraints ur10_constraints; 65 | jcm.joint_name="ur10_shoulder_pan_joint"; 66 | jcm.position = -2.4; 67 | jcm.tolerance_above = 2.5; 68 | jcm.tolerance_below = 0.7; 69 | jcm.weight = 1.0; 70 | ur10_constraints.joint_constraints.push_back(jcm); 71 | dualArmRobot.ur10_.setPathConstraints(ur10_constraints);*/ 72 | /* 73 | jcm.joint_name="ur10_wrist_2_joint"; 74 | jcm.position = 3.0; 75 | jcm.tolerance_above = 1.0; 76 | jcm.tolerance_below = 4.0; 77 | jcm.weight = 1.0; 78 | ur10_constraints.joint_constraints.push_back(jcm); 79 | dualArmRobot.ur10_.setPathConstraints(ur10_constraints);*/ 80 | /* 81 | jcm.joint_name="ur5_shoulder_pan_joint"; 82 | jcm.position = 0.0; 83 | jcm.tolerance_above = 1.0; 84 | jcm.tolerance_below = 1.0; 85 | jcm.weight = 1.0; 86 | ur5_constraints.joint_constraints.push_back(jcm); 87 | dualArmRobot.ur5_.setPathConstraints(ur5_constraints);*/ 88 | 89 | jcm.joint_name="ur5_shoulder_pan_joint"; 90 | jcm.position = 0.0; 91 | jcm.tolerance_above = 0.4; 92 | jcm.tolerance_below = 0.4; 93 | jcm.weight = 1.0; 94 | ur5_constraints.joint_constraints.push_back(jcm); 95 | dualArmRobot.ur5_.setPathConstraints(ur5_constraints); 96 | 97 | // Pick box1 98 | geometry_msgs::Vector3Stamped direction; 99 | direction.header.frame_id = "/table_ground"; 100 | direction.vector.x = 0; 101 | direction.vector.y = 0; 102 | direction.vector.z = 0.1; 103 | if (!dualArmRobot.pickBox("box1", direction)) { 104 | ROS_WARN("Pick failed"); 105 | ROS_ERROR("Can't execute demonstration without successful pick. Demonstration aborted."); 106 | return 0; 107 | } 108 | 109 | // clear path constraints 110 | dualArmRobot.ur5_.clearPathConstraints(); 111 | dualArmRobot.ur10_.clearPathConstraints(); 112 | 113 | // plan move up 114 | geometry_msgs::PoseStamped start_pose = dualArmRobot.ur5_.getCurrentPose(dualArmRobot.ur5_.getEndEffectorLink()); 115 | geometry_msgs::PoseStamped lift_pose = start_pose; 116 | lift_pose.pose.position.z = lift_pose.pose.position.z +0.6; 117 | 118 | for (unsigned int i =0; i < 15 ; i++){ 119 | ROS_INFO(":::::: START EVALUATION %i::::::", i); 120 | dualArmRobot.planMoveObject("box1", lift_pose, 0.2); 121 | ROS_INFO(":::::: END EVALUATION %i::::::", i); 122 | } 123 | 124 | 125 | // END 126 | ROS_INFO("Finished demonstration"); 127 | ros::shutdown(); 128 | return 0; 129 | } -------------------------------------------------------------------------------- /dual_arm_robot_applications/src/executables/dual_arm_robot_logger.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Created by daniel on 24.03.17. 3 | // 4 | 5 | // ROS 6 | #include 7 | #include 8 | 9 | //#include "../../include/ur_logging/UrLogger.h" 10 | #include "ur_logging/UrLogger.h" 11 | 12 | 13 | int main(int argc, char **argv){ 14 | ros::init(argc, argv, "dual_arm_robot_logger"); 15 | ros::NodeHandle nh; 16 | 17 | ros::AsyncSpinner asyncSpinner(1); 18 | asyncSpinner.start(); 19 | 20 | std::vector ur_namespaces; 21 | ur_namespaces.push_back("ur5"); 22 | ur_namespaces.push_back("ur10"); 23 | UR_Logger ur_logger(nh, ur_namespaces); 24 | 25 | ur_logger.start(20); 26 | ros::Duration(6).sleep(); 27 | ur_logger.stop(); 28 | 29 | ros::shutdown(); 30 | return 0; 31 | } -------------------------------------------------------------------------------- /dual_arm_robot_applications/src/ur_logging/Stopwatch.cpp: -------------------------------------------------------------------------------- 1 | #include "ur_logging/Stopwatch.h" 2 | 3 | Stopwatch::Stopwatch(){ 4 | start_ = ros::Time::now(); 5 | } 6 | 7 | void Stopwatch::restart(){ 8 | start_ = ros::Time::now(); 9 | } 10 | 11 | ros::Duration Stopwatch::elapsed(){ 12 | return (ros::Time::now() - start_); 13 | } 14 | -------------------------------------------------------------------------------- /dual_arm_robot_applications/src/ur_logging/UrMessageListener.cpp: -------------------------------------------------------------------------------- 1 | # include "ur_logging/UrMessageListener.h" 2 | 3 | 4 | //UR_Message_Listener::UR_Message_Listener(ros::NodeHandle& nh, std::string ur_namespace) : nh_(nh), ur_namespace_(ur_namespace){ 5 | UR_Message_Listener::UR_Message_Listener(ros::NodeHandle& nh, std::string ur_namespace) : nh_(nh), ur_namespace_(ur_namespace){ 6 | wrench_sub_ = nh_.subscribe(ur_namespace+"/tcp_wrench", 1, &UR_Message_Listener::wrenchCallback, this); 7 | speed_traj_sub_ = nh_.subscribe(ur_namespace+"/ur_driver/joint_speed", 1, &UR_Message_Listener::speed_trajCallback, this); 8 | state_sub_ = nh_.subscribe("/joint_states", 1, &UR_Message_Listener::stateCallback, this); 9 | 10 | if (ur_namespace_.size() > 0){ 11 | ur_prefix_ = ur_namespace_+"_"; 12 | } 13 | } 14 | 15 | void UR_Message_Listener::wrenchCallback(const geometry_msgs::WrenchStamped::Ptr& msg){ 16 | last_wrench_msg_ = *msg; 17 | } 18 | 19 | 20 | void UR_Message_Listener::speed_trajCallback(const trajectory_msgs::JointTrajectory::Ptr& msg){ 21 | last_speed_traj_msg_ = *msg; 22 | } 23 | 24 | 25 | void UR_Message_Listener::stateCallback(const sensor_msgs::JointState::Ptr& msg){ 26 | std::string name = (*msg).name[0]; 27 | if (name.compare(0,ur_prefix_.size(), ur_prefix_) == 0){ // "joint_states" receives messages of all robots. This is seperating the searched one from the others. 28 | last_state_msg_ = *msg; 29 | } 30 | } 31 | -------------------------------------------------------------------------------- /dual_arm_robot_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(dual_arm_robot_description) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS 8 | roscpp 9 | std_msgs 10 | geometry_msgs 11 | urdf 12 | ) 13 | 14 | ## System dependencies are found with CMake's conventions 15 | # find_package(Boost REQUIRED COMPONENTS system) 16 | 17 | 18 | ## Uncomment this if the package has a setup.py. This macro ensures 19 | ## modules and global scripts declared therein get installed 20 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 21 | # catkin_python_setup() 22 | 23 | ################################################ 24 | ## Declare ROS messages, services and actions ## 25 | ################################################ 26 | 27 | ## To declare and build messages, services or actions from within this 28 | ## package, follow these steps: 29 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 30 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 31 | ## * In the file package.xml: 32 | ## * add a build_depend tag for "message_generation" 33 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 34 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 35 | ## but can be declared for certainty nonetheless: 36 | ## * add a run_depend tag for "message_runtime" 37 | ## * In this file (CMakeLists.txt): 38 | ## * add "message_generation" and every package in MSG_DEP_SET to 39 | ## find_package(catkin REQUIRED COMPONENTS ...) 40 | ## * add "message_runtime" and every package in MSG_DEP_SET to 41 | ## catkin_package(CATKIN_DEPENDS ...) 42 | ## * uncomment the add_*_files sections below as needed 43 | ## and list every .msg/.srv/.action file to be processed 44 | ## * uncomment the generate_messages entry below 45 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 46 | 47 | ## Generate messages in the 'msg' folder 48 | # add_message_files( 49 | # FILES 50 | # Message1.msg 51 | # Message2.msg 52 | # ) 53 | 54 | ## Generate services in the 'srv' folder 55 | # add_service_files( 56 | # FILES 57 | # Service1.srv 58 | # Service2.srv 59 | # ) 60 | 61 | ## Generate actions in the 'action' folder 62 | # add_action_files( 63 | # FILES 64 | # Action1.action 65 | # Action2.action 66 | # ) 67 | 68 | ## Generate added messages and services with any dependencies listed here 69 | # generate_messages( 70 | # DEPENDENCIES 71 | # std_msgs # Or other packages containing msgs 72 | # ) 73 | 74 | ################################################ 75 | ## Declare ROS dynamic reconfigure parameters ## 76 | ################################################ 77 | 78 | ## To declare and build dynamic reconfigure parameters within this 79 | ## package, follow these steps: 80 | ## * In the file package.xml: 81 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 82 | ## * In this file (CMakeLists.txt): 83 | ## * add "dynamic_reconfigure" to 84 | ## find_package(catkin REQUIRED COMPONENTS ...) 85 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 86 | ## and list every .cfg file to be processed 87 | 88 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 89 | # generate_dynamic_reconfigure_options( 90 | # cfg/DynReconf1.cfg 91 | # cfg/DynReconf2.cfg 92 | # ) 93 | 94 | ################################### 95 | ## catkin specific configuration ## 96 | ################################### 97 | ## The catkin_package macro generates cmake config files for your package 98 | ## Declare things to be passed to dependent projects 99 | ## INCLUDE_DIRS: uncomment this if you package contains header files 100 | ## LIBRARIES: libraries you create in this project that dependent projects also need 101 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 102 | ## DEPENDS: system dependencies of this project that dependent projects also need 103 | catkin_package( 104 | # INCLUDE_DIRS include 105 | # LIBRARIES dual_arm_robot_description 106 | # CATKIN_DEPENDS other_catkin_pkg 107 | # DEPENDS system_lib 108 | ) 109 | 110 | ########### 111 | ## Build ## 112 | ########### 113 | 114 | ## Specify additional locations of header files 115 | ## Your package locations should be listed before other locations 116 | # include_directories(include) 117 | include_directories( 118 | ${catkin_INCLUDE_DIRS} 119 | ) 120 | 121 | 122 | ## Declare a C++ library 123 | # add_library(dual_arm_robot_description 124 | # src/${PROJECT_NAME}/dual_arm_robot_description.cpp 125 | # ) 126 | 127 | ## Add cmake target dependencies of the library 128 | ## as an example, code may need to be generated before libraries 129 | ## either from message generation or dynamic reconfigure 130 | # add_dependencies(dual_arm_robot_description ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 131 | 132 | ## Declare a C++ executable 133 | # add_executable(dual_arm_robot_description_node src/dual_arm_robot_description_node.cpp) 134 | 135 | ## Add cmake target dependencies of the executable 136 | ## same as for the library above 137 | # add_dependencies(dual_arm_robot_description_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 138 | 139 | ## Specify libraries to link a library or executable target against 140 | # target_link_libraries(dual_arm_robot_description_node 141 | # ${catkin_LIBRARIES} 142 | # ) 143 | 144 | ############# 145 | ## Install ## 146 | ############# 147 | 148 | # all install targets should use catkin DESTINATION variables 149 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 150 | 151 | ## Mark executable scripts (Python etc.) for installation 152 | ## in contrast to setup.py, you can choose the destination 153 | # install(PROGRAMS 154 | # scripts/my_python_script 155 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 156 | # ) 157 | 158 | ## Mark executables and/or libraries for installation 159 | # install(TARGETS dual_arm_robot_description dual_arm_robot_description_node 160 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 161 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 162 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 163 | # ) 164 | 165 | ## Mark cpp header files for installation 166 | # install(DIRECTORY include/${PROJECT_NAME}/ 167 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 168 | # FILES_MATCHING PATTERN "*.h" 169 | # PATTERN ".svn" EXCLUDE 170 | # ) 171 | 172 | ## Mark other files for installation (e.g. launch and bag files, etc.) 173 | # install(FILES 174 | # # myfile1 175 | # # myfile2 176 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 177 | # ) 178 | 179 | ############# 180 | ## Testing ## 181 | ############# 182 | 183 | ## Add gtest based cpp test target and link libraries 184 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_dual_arm_robot_description.cpp) 185 | # if(TARGET ${PROJECT_NAME}-test) 186 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 187 | # endif() 188 | 189 | ## Add folders to be run by python nosetests 190 | # catkin_add_nosetests(test) 191 | 192 | add_executable(parser src/parser.cpp) 193 | target_link_libraries(parser ${catkin_LIBRARIES}) 194 | -------------------------------------------------------------------------------- /dual_arm_robot_description/launch/display_urdf.launch: -------------------------------------------------------------------------------- 1 | 2 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /dual_arm_robot_description/launch/dual_arm_robot_upload.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /dual_arm_robot_description/meshes/endeffector/ee_frame.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/danielhoeltgen/Dual-Arm-Robot-Force-Controlled-Object-Manipulation/53761ef144e67e65b8580179ec0a451ba537313f/dual_arm_robot_description/meshes/endeffector/ee_frame.stl -------------------------------------------------------------------------------- /dual_arm_robot_description/meshes/endeffector/ee_spring.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/danielhoeltgen/Dual-Arm-Robot-Force-Controlled-Object-Manipulation/53761ef144e67e65b8580179ec0a451ba537313f/dual_arm_robot_description/meshes/endeffector/ee_spring.stl -------------------------------------------------------------------------------- /dual_arm_robot_description/meshes/table/table.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/danielhoeltgen/Dual-Arm-Robot-Force-Controlled-Object-Manipulation/53761ef144e67e65b8580179ec0a451ba537313f/dual_arm_robot_description/meshes/table/table.stl -------------------------------------------------------------------------------- /dual_arm_robot_description/meshes/table/table_back.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/danielhoeltgen/Dual-Arm-Robot-Force-Controlled-Object-Manipulation/53761ef144e67e65b8580179ec0a451ba537313f/dual_arm_robot_description/meshes/table/table_back.stl -------------------------------------------------------------------------------- /dual_arm_robot_description/meshes/table/table_camerahanger.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/danielhoeltgen/Dual-Arm-Robot-Force-Controlled-Object-Manipulation/53761ef144e67e65b8580179ec0a451ba537313f/dual_arm_robot_description/meshes/table/table_camerahanger.stl -------------------------------------------------------------------------------- /dual_arm_robot_description/meshes/table/table_front.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/danielhoeltgen/Dual-Arm-Robot-Force-Controlled-Object-Manipulation/53761ef144e67e65b8580179ec0a451ba537313f/dual_arm_robot_description/meshes/table/table_front.stl -------------------------------------------------------------------------------- /dual_arm_robot_description/meshes/table/table_ground.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/danielhoeltgen/Dual-Arm-Robot-Force-Controlled-Object-Manipulation/53761ef144e67e65b8580179ec0a451ba537313f/dual_arm_robot_description/meshes/table/table_ground.stl -------------------------------------------------------------------------------- /dual_arm_robot_description/meshes/table/table_platform_ur10.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/danielhoeltgen/Dual-Arm-Robot-Force-Controlled-Object-Manipulation/53761ef144e67e65b8580179ec0a451ba537313f/dual_arm_robot_description/meshes/table/table_platform_ur10.stl -------------------------------------------------------------------------------- /dual_arm_robot_description/meshes/table/table_platform_ur5.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/danielhoeltgen/Dual-Arm-Robot-Force-Controlled-Object-Manipulation/53761ef144e67e65b8580179ec0a451ba537313f/dual_arm_robot_description/meshes/table/table_platform_ur5.stl -------------------------------------------------------------------------------- /dual_arm_robot_description/meshes/table/table_side.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/danielhoeltgen/Dual-Arm-Robot-Force-Controlled-Object-Manipulation/53761ef144e67e65b8580179ec0a451ba537313f/dual_arm_robot_description/meshes/table/table_side.stl -------------------------------------------------------------------------------- /dual_arm_robot_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | dual_arm_robot_description 4 | 0.0.0 5 | The dual_arm_robot_description package 6 | 7 | 8 | 9 | 10 | daniel 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 | catkin 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | -------------------------------------------------------------------------------- /dual_arm_robot_description/src/parser.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "ros/ros.h" 3 | 4 | int main(int argc, char** argv){ 5 | ros::init(argc, argv, "my_parser"); 6 | if (argc != 2){ 7 | ROS_ERROR("Need a urdf file as argument"); 8 | return -1; 9 | } 10 | std::string urdf_file = argv[1]; 11 | 12 | urdf::Model model; 13 | if (!model.initFile(urdf_file)){ 14 | ROS_ERROR("Failed to parse urdf file"); 15 | return -1; 16 | } 17 | ROS_INFO("Successfully parsed urdf file"); 18 | return 0; 19 | } 20 | 21 | -------------------------------------------------------------------------------- /dual_arm_robot_description/urdf/dual_arm_robot.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 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 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 82 | 83 | -------------------------------------------------------------------------------- /dual_arm_robot_description/urdf/endeffector.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 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 | -------------------------------------------------------------------------------- /dual_arm_robot_description/urdf/ikfast_commands.txt: -------------------------------------------------------------------------------- 1 | EXPORT PYTHON PATH 2 | export PYTHONPATH=$PYTHONPATH:`openrave-config --python-dir` 3 | 4 | ROUND COLLADA VALUES 5 | rosrun moveit_ikfast round_collada_numbers.py dual_arm_robot.dae dual_arm_robot.rounded.dae 5 6 | 7 | CREATE IK FAST SOLUTION 8 | python `openrave-config --python-dir`/openravepy/_openravepy_/ikfast.py --robot=dual_arm_robot.rounded.dae --iktype=transform6d --baselink=8 --eelink=16 --savefile=ikfast61_ur10_manipulator.cpp 9 | 10 | CREATE PLUGIN 11 | catkin_create_pkg dual_arm_robot_ik_fast_ur10_manipulator_plugin 12 | 13 | GENERATE CODE FOR PLUGIN (move ikfast61_ur10.cpp first) 14 | rosrun moveit_ikfast create_ikfast_moveit_plugin.py dual_arm_robot ur10_manipulator dual_arm_robot_ik_fast_ur10_manipulator_plugin src/ikfast61_ur10.cpp 15 | 16 | 17 | 18 | WEBSITE: 19 | http://docs.ros.org/indigo/api/moveit_tutorials/html/doc/ikfast_tutorial.html?highlight=fast 20 | 21 | TO UPDATE CHANGES EASILY: 22 | update_ikfast_plugin.sh 23 | 24 | ??? 25 | export OPENRAVE_PLUGINS="/home/daniel/catkin_ws/devel/share/openrave-0.9/plugins" 26 | 27 | 28 | DID NOT WORK 29 | [plugindatabase.h:577 Create] Failed to create name fcl_, interface collisionchecker 30 | [plugindatabase.h:577 Create] Failed to create name ode, interface collisionchecker 31 | 32 | -------------------------------------------------------------------------------- /dual_arm_robot_description/urdf/table.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 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 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | -------------------------------------------------------------------------------- /dual_arm_robot_driver/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(dual_arm_robot_driver) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS 8 | roscpp 9 | std_msgs 10 | ) 11 | 12 | ## System dependencies are found with CMake's conventions 13 | # find_package(Boost REQUIRED COMPONENTS system) 14 | 15 | 16 | ## Uncomment this if the package has a setup.py. This macro ensures 17 | ## modules and global scripts declared therein get installed 18 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 19 | # catkin_python_setup() 20 | 21 | ################################################ 22 | ## Declare ROS messages, services and actions ## 23 | ################################################ 24 | 25 | ## To declare and build messages, services or actions from within this 26 | ## package, follow these steps: 27 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 28 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 29 | ## * In the file package.xml: 30 | ## * add a build_depend tag for "message_generation" 31 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 32 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 33 | ## but can be declared for certainty nonetheless: 34 | ## * add a run_depend tag for "message_runtime" 35 | ## * In this file (CMakeLists.txt): 36 | ## * add "message_generation" and every package in MSG_DEP_SET to 37 | ## find_package(catkin REQUIRED COMPONENTS ...) 38 | ## * add "message_runtime" and every package in MSG_DEP_SET to 39 | ## catkin_package(CATKIN_DEPENDS ...) 40 | ## * uncomment the add_*_files sections below as needed 41 | ## and list every .msg/.srv/.action file to be processed 42 | ## * uncomment the generate_messages entry below 43 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 44 | 45 | ## Generate messages in the 'msg' folder 46 | # add_message_files( 47 | # FILES 48 | # Message1.msg 49 | # Message2.msg 50 | # ) 51 | 52 | ## Generate services in the 'srv' folder 53 | # add_service_files( 54 | # FILES 55 | # Service1.srv 56 | # Service2.srv 57 | # ) 58 | 59 | ## Generate actions in the 'action' folder 60 | # add_action_files( 61 | # FILES 62 | # Action1.action 63 | # Action2.action 64 | # ) 65 | 66 | ## Generate added messages and services with any dependencies listed here 67 | # generate_messages( 68 | # DEPENDENCIES 69 | # std_msgs 70 | # ) 71 | 72 | ################################################ 73 | ## Declare ROS dynamic reconfigure parameters ## 74 | ################################################ 75 | 76 | ## To declare and build dynamic reconfigure parameters within this 77 | ## package, follow these steps: 78 | ## * In the file package.xml: 79 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 80 | ## * In this file (CMakeLists.txt): 81 | ## * add "dynamic_reconfigure" to 82 | ## find_package(catkin REQUIRED COMPONENTS ...) 83 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 84 | ## and list every .cfg file to be processed 85 | 86 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 87 | # generate_dynamic_reconfigure_options( 88 | # cfg/DynReconf1.cfg 89 | # cfg/DynReconf2.cfg 90 | # ) 91 | 92 | ################################### 93 | ## catkin specific configuration ## 94 | ################################### 95 | ## The catkin_package macro generates cmake config files for your package 96 | ## Declare things to be passed to dependent projects 97 | ## INCLUDE_DIRS: uncomment this if you package contains header files 98 | ## LIBRARIES: libraries you create in this project that dependent projects also need 99 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 100 | ## DEPENDS: system dependencies of this project that dependent projects also need 101 | catkin_package( 102 | # INCLUDE_DIRS include 103 | # LIBRARIES dual_arm_robot_driver 104 | # CATKIN_DEPENDS roscpp std_msgs 105 | # DEPENDS system_lib 106 | ) 107 | 108 | ########### 109 | ## Build ## 110 | ########### 111 | 112 | ## Specify additional locations of header files 113 | ## Your package locations should be listed before other locations 114 | # include_directories(include) 115 | include_directories( 116 | ${catkin_INCLUDE_DIRS} 117 | ) 118 | 119 | ## Declare a C++ library 120 | # add_library(dual_arm_robot_driver 121 | # src/${PROJECT_NAME}/dual_arm_robot_driver.cpp 122 | # ) 123 | 124 | ## Add cmake target dependencies of the library 125 | ## as an example, code may need to be generated before libraries 126 | ## either from message generation or dynamic reconfigure 127 | # add_dependencies(dual_arm_robot_driver ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 128 | 129 | ## Declare a C++ executable 130 | # add_executable(dual_arm_robot_driver_node src/dual_arm_robot_driver_node.cpp) 131 | 132 | ## Add cmake target dependencies of the executable 133 | ## same as for the library above 134 | # add_dependencies(dual_arm_robot_driver_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 135 | 136 | ## Specify libraries to link a library or executable target against 137 | # target_link_libraries(dual_arm_robot_driver_node 138 | # ${catkin_LIBRARIES} 139 | # ) 140 | 141 | ############# 142 | ## Install ## 143 | ############# 144 | 145 | # all install targets should use catkin DESTINATION variables 146 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 147 | 148 | ## Mark executable scripts (Python etc.) for installation 149 | ## in contrast to setup.py, you can choose the destination 150 | # install(PROGRAMS 151 | # scripts/my_python_script 152 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 153 | # ) 154 | 155 | ## Mark executables and/or libraries for installation 156 | # install(TARGETS dual_arm_robot_driver dual_arm_robot_driver_node 157 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 158 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 159 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 160 | # ) 161 | 162 | ## Mark cpp header files for installation 163 | # install(DIRECTORY include/${PROJECT_NAME}/ 164 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 165 | # FILES_MATCHING PATTERN "*.h" 166 | # PATTERN ".svn" EXCLUDE 167 | # ) 168 | 169 | ## Mark other files for installation (e.g. launch and bag files, etc.) 170 | # install(FILES 171 | # # myfile1 172 | # # myfile2 173 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 174 | # ) 175 | 176 | ############# 177 | ## Testing ## 178 | ############# 179 | 180 | ## Add gtest based cpp test target and link libraries 181 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_dual_arm_robot_driver.cpp) 182 | # if(TARGET ${PROJECT_NAME}-test) 183 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 184 | # endif() 185 | 186 | ## Add folders to be run by python nosetests 187 | # catkin_add_nosetests(test) 188 | -------------------------------------------------------------------------------- /dual_arm_robot_driver/config/ur10_controllers.yaml: -------------------------------------------------------------------------------- 1 | # Settings for ros_control control loop 2 | hardware_control_loop: 3 | loop_hz: 125 4 | 5 | # Settings for ros_control hardware interface 6 | hardware_interface: 7 | joints: 8 | - ur10_shoulder_pan_joint 9 | - ur10_shoulder_lift_joint 10 | - ur10_elbow_joint 11 | - ur10_wrist_1_joint 12 | - ur10_wrist_2_joint 13 | - ur10_wrist_3_joint 14 | 15 | # Publish all joint states ---------------------------------- 16 | ur10_joint_state_controller: 17 | type: joint_state_controller/JointStateController 18 | publish_rate: 125 19 | 20 | # Publish wrench ---------------------------------- 21 | ur10_force_torque_sensor_controller: 22 | type: force_torque_sensor_controller/ForceTorqueSensorController 23 | publish_rate: 125 24 | 25 | 26 | # Joint Trajectory Controller ------------------------------- 27 | # For detailed explanations of parameter see http://wiki.ros.org/ 28 | ur10_vel_based_pos_traj_controller: 29 | type: velocity_controllers/JointTrajectoryController 30 | joints: 31 | - ur10_shoulder_pan_joint 32 | - ur10_shoulder_lift_joint 33 | - ur10_elbow_joint 34 | - ur10_wrist_1_joint 35 | - ur10_wrist_2_joint 36 | - ur10_wrist_3_joint 37 | constraints: 38 | goal_time: 0.6 39 | stopped_velocity_tolerance: 0.05 40 | ur10_shoulder_pan_joint: {trajectory: 0.1, goal: 0.1} 41 | ur10_shoulder_lift_joint: {trajectory: 0.1, goal: 0.1} 42 | ur10_elbow_joint: {trajectory: 0.1, goal: 0.1} 43 | ur10_wrist_1_joint: {trajectory: 0.1, goal: 0.1} 44 | ur10_wrist_2_joint: {trajectory: 0.1, goal: 0.1} 45 | ur10_wrist_3_joint: {trajectory: 0.1, goal: 0.1} 46 | stop_trajectory_duration: 0.5 47 | state_publish_rate: 125 48 | action_monitor_rate: 10 49 | gains: 50 | #!!These values are useable, but maybe not optimal 51 | ur10_shoulder_pan_joint: {p: 20.0, i: 0.0, d: 0.1, i_clamp: 1} 52 | ur10_shoulder_lift_joint: {p: 20.0, i: 0.0, d: 0.1, i_clamp: 1} 53 | ur10_elbow_joint: {p: 20.0, i: 0.0, d: 0.1, i_clamp: 1} 54 | ur10_wrist_1_joint: {p: 20.0, i: 0.0, d: 0.1, i_clamp: 1} 55 | ur10_wrist_2_joint: {p: 20.0, i: 0.0, d: 0.1, i_clamp: 1} 56 | ur10_wrist_3_joint: {p: 20.0, i: 0.0, d: 0.1, i_clamp: 1} 57 | 58 | 59 | # state_publish_rate: 50 # Defaults to 50 60 | # action_monitor_rate: 20 # Defaults to 20 61 | #hold_trajectory_duration: 0 # Defaults to 0.5 62 | 63 | # Joint Trajectory Controller ------------------------------- 64 | # For detailed explanations of parameter see http://wiki.ros.org/ 65 | ur10_vel_based_admittance_traj_controller: 66 | type: velocity_controllers/JointTrajectoryAdmittanceController 67 | root_name: ur10_base 68 | tip_name: ur10_tool0 69 | contact_force: 25 #[N] 70 | max_velocity: 0.2 71 | max_shift: 0.05 72 | wrench_topic: /ur10/wrench 73 | wrench_tolerance: 5 74 | admittance_pid: 75 | p: 0.0005 76 | i: 0.0000 77 | d: 0.0000 78 | i_clamp_min: -0.0001 79 | i_clamp_max: 0.0001 80 | joints: 81 | - ur10_shoulder_pan_joint 82 | - ur10_shoulder_lift_joint 83 | - ur10_elbow_joint 84 | - ur10_wrist_1_joint 85 | - ur10_wrist_2_joint 86 | - ur10_wrist_3_joint 87 | constraints: 88 | goal_time: 0.6 89 | stopped_velocity_tolerance: 0.05 90 | ur10_shoulder_pan_joint: {trajectory: 0.1, goal: 0.1} 91 | ur10_shoulder_lift_joint: {trajectory: 0.1, goal: 0.1} 92 | ur10_elbow_joint: {trajectory: 0.1, goal: 0.1} 93 | ur10_wrist_1_joint: {trajectory: 0.1, goal: 0.1} 94 | ur10_wrist_2_joint: {trajectory: 0.1, goal: 0.1} 95 | ur10_wrist_3_joint: {trajectory: 0.1, goal: 0.1} 96 | stop_trajectory_duration: 0.5 97 | state_publish_rate: 125 98 | action_monitor_rate: 10 99 | gains: 100 | #!!These values are useable, but maybe not optimal 101 | ur10_shoulder_pan_joint: {p: 20.0, i: 0.0, d: 0.1, i_clamp: 1} 102 | ur10_shoulder_lift_joint: {p: 20.0, i: 0.0, d: 0.1, i_clamp: 1} 103 | ur10_elbow_joint: {p: 20.0, i: 0.0, d: 0.1, i_clamp: 1} 104 | ur10_wrist_1_joint: {p: 20.0, i: 0.0, d: 0.1, i_clamp: 1} 105 | ur10_wrist_2_joint: {p: 20.0, i: 0.0, d: 0.1, i_clamp: 1} 106 | ur10_wrist_3_joint: {p: 20.0, i: 0.0, d: 0.1, i_clamp: 1} 107 | 108 | 109 | # state_publish_rate: 50 # Defaults to 50 110 | # action_monitor_rate: 20 # Defaults to 20 111 | #hold_trajectory_duration: 0 # Defaults to 0.5 112 | 113 | -------------------------------------------------------------------------------- /dual_arm_robot_driver/config/ur5_controllers.yaml: -------------------------------------------------------------------------------- 1 | # Settings for ros_control control loop 2 | hardware_control_loop: 3 | loop_hz: 125 4 | 5 | # Settings for ros_control hardware interface 6 | hardware_interface: 7 | joints: 8 | - ur5_shoulder_pan_joint 9 | - ur5_shoulder_lift_joint 10 | - ur5_elbow_joint 11 | - ur5_wrist_1_joint 12 | - ur5_wrist_2_joint 13 | - ur5_wrist_3_joint 14 | 15 | # Publish all joint states ---------------------------------- 16 | ur5_joint_state_controller: 17 | type: joint_state_controller/JointStateController 18 | publish_rate: 125 19 | 20 | # Publish wrench ---------------------------------- 21 | ur5_force_torque_sensor_controller: 22 | type: force_torque_sensor_controller/ForceTorqueSensorController 23 | publish_rate: 125 24 | 25 | # Joint Trajectory Controller - velocity based ------------------------------- 26 | # For detailed explanations of parameter see http://wiki.ros.org/ 27 | ur5_vel_based_pos_traj_controller: 28 | type: velocity_controllers/JointTrajectoryController 29 | joints: 30 | - ur5_shoulder_pan_joint 31 | - ur5_shoulder_lift_joint 32 | - ur5_elbow_joint 33 | - ur5_wrist_1_joint 34 | - ur5_wrist_2_joint 35 | - ur5_wrist_3_joint 36 | constraints: 37 | goal_time: 0.6 38 | stopped_velocity_tolerance: 0.05 39 | ur5_shoulder_pan_joint: {trajectory: 0.1, goal: 0.1} 40 | ur5_shoulder_lift_joint: {trajectory: 0.1, goal: 0.1} 41 | ur5_elbow_joint: {trajectory: 0.1, goal: 0.1} 42 | ur5_wrist_1_joint: {trajectory: 0.1, goal: 0.1} 43 | ur5_wrist_2_joint: {trajectory: 0.1, goal: 0.1} 44 | ur5_wrist_3_joint: {trajectory: 0.1, goal: 0.1} 45 | stop_trajectory_duration: 0.5 46 | state_publish_rate: 125 47 | action_monitor_rate: 10 48 | gains: 49 | #!!These values are useable, but maybe not optimal, default: p:1.2 50 | ur5_shoulder_pan_joint: {p: 20, i: 0.0, d: 0.1, i_clamp: 1} 51 | ur5_shoulder_lift_joint: {p: 20, i: 0.0, d: 0.1, i_clamp: 1} 52 | ur5_elbow_joint: {p: 20, i: 0.0, d: 0.1, i_clamp: 1} 53 | ur5_wrist_1_joint: {p: 20, i: 0.0, d: 0.1, i_clamp: 1} 54 | ur5_wrist_2_joint: {p: 20, i: 0.0, d: 0.1, i_clamp: 1} 55 | ur5_wrist_3_joint: {p: 20, i: 0.0, d: 0.1, i_clamp: 1} 56 | 57 | # state_publish_rate: 50 # Defaults to 50 58 | # action_monitor_rate: 20 # Defaults to 20 59 | #hold_trajectory_duration: 0 # Defaults to 0.5 60 | 61 | # Joint Trajectory Controller ------------------------------- 62 | # For detailed explanations of parameter see http://wiki.ros.org/ 63 | ur5_vel_based_admittance_traj_controller: 64 | type: velocity_controllers/JointTrajectoryAdmittanceController 65 | root_name: ur5_base 66 | tip_name: ur5_tool0 67 | contact_force: 30 #[N] 68 | max_velocity: 0.2 69 | max_shift: 0.15 70 | wrench_topic: /ur5/wrench 71 | wrench_tolerance: 0 #5 72 | admittance_pid: 73 | p: 0.0005 74 | i: 0.0000 75 | d: 0.0000 76 | i_clamp_min: -0.0001 77 | i_clamp_max: 0.0001 78 | joints: 79 | - ur5_shoulder_pan_joint 80 | - ur5_shoulder_lift_joint 81 | - ur5_elbow_joint 82 | - ur5_wrist_1_joint 83 | - ur5_wrist_2_joint 84 | - ur5_wrist_3_joint 85 | constraints: 86 | goal_time: 0.6 87 | stopped_velocity_tolerance: 0.05 88 | ur5_shoulder_pan_joint: {trajectory: 0.1, goal: 0.1} 89 | ur5_shoulder_lift_joint: {trajectory: 0.1, goal: 0.1} 90 | ur5_elbow_joint: {trajectory: 0.1, goal: 0.1} 91 | ur5_wrist_1_joint: {trajectory: 0.1, goal: 0.1} 92 | ur5_wrist_2_joint: {trajectory: 0.1, goal: 0.1} 93 | ur5_wrist_3_joint: {trajectory: 0.1, goal: 0.1} 94 | stop_trajectory_duration: 0.5 95 | state_publish_rate: 125 96 | action_monitor_rate: 10 97 | gains: 98 | #!!These values are useable, but maybe not optimal, default: p:1.2 99 | ur5_shoulder_pan_joint: {p: 20, i: 0.0, d: 0.1, i_clamp: 1} 100 | ur5_shoulder_lift_joint: {p: 20, i: 0.0, d: 0.1, i_clamp: 1} 101 | ur5_elbow_joint: {p: 20, i: 0.0, d: 0.1, i_clamp: 1} 102 | ur5_wrist_1_joint: {p: 20, i: 0.0, d: 0.1, i_clamp: 1} 103 | ur5_wrist_2_joint: {p: 20, i: 0.0, d: 0.1, i_clamp: 1} 104 | ur5_wrist_3_joint: {p: 20, i: 0.0, d: 0.1, i_clamp: 1} 105 | 106 | # state_publish_rate: 50 # Defaults to 50 107 | # action_monitor_rate: 20 # Defaults to 20 108 | #hold_trajectory_duration: 0 # Defaults to 0.5 109 | -------------------------------------------------------------------------------- /dual_arm_robot_driver/launch/dual_arm_robot_bringup.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 | -------------------------------------------------------------------------------- /dual_arm_robot_driver/launch/dual_arm_robot_bringup_ros_control.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 | 47 | 48 | 49 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 77 | 78 | 79 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | -------------------------------------------------------------------------------- /dual_arm_robot_driver/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | dual_arm_robot_driver 4 | 0.0.0 5 | The dual_arm_robot_driver package 6 | 7 | 8 | 9 | 10 | daniel 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 | catkin 43 | roscpp 44 | std_msgs 45 | roscpp 46 | std_msgs 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | -------------------------------------------------------------------------------- /dual_arm_robot_moveit_config/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(dual_arm_robot_moveit_config) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | catkin_package() 7 | 8 | install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 9 | PATTERN "setup_assistant.launch" EXCLUDE) 10 | install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 11 | -------------------------------------------------------------------------------- /dual_arm_robot_moveit_config/README: -------------------------------------------------------------------------------- 1 | The controllers.yaml has to be configured depending on the used controller. 2 | Setup is made for ros-control. 3 | -------------------------------------------------------------------------------- /dual_arm_robot_moveit_config/config/controllers.yaml: -------------------------------------------------------------------------------- 1 | ### ros-control settings: 2 | controller_list: 3 | - name: ur5/ur5_vel_based_pos_traj_controller #or pos_based_pos_traj_controller 4 | action_ns: follow_joint_trajectory 5 | type: FollowJointTrajectory 6 | joints: 7 | - ur5_shoulder_pan_joint 8 | - ur5_shoulder_lift_joint 9 | - ur5_elbow_joint 10 | - ur5_wrist_1_joint 11 | - ur5_wrist_2_joint 12 | - ur5_wrist_3_joint 13 | # - name: ur5/ur5_vel_based_admittance_traj_controller #or pos_based_pos_traj_controller 14 | # action_ns: follow_joint_trajectory 15 | # type: FollowJointTrajectory 16 | # joints: 17 | # - ur5_shoulder_pan_joint 18 | # - ur5_shoulder_lift_joint 19 | # - ur5_elbow_joint 20 | # - ur5_wrist_1_joint 21 | # - ur5_wrist_2_joint 22 | # - ur5_wrist_3_joint 23 | - name: ur10/ur10_vel_based_pos_traj_controller #or pos_based_pos_traj_controller 24 | action_ns: follow_joint_trajectory 25 | type: FollowJointTrajectory 26 | joints: 27 | - ur10_shoulder_pan_joint 28 | - ur10_shoulder_lift_joint 29 | - ur10_elbow_joint 30 | - ur10_wrist_1_joint 31 | - ur10_wrist_2_joint 32 | - ur10_wrist_3_joint 33 | # - name: ur10/ur10_vel_based_admittance_traj_controller #or pos_based_pos_traj_controller 34 | # action_ns: follow_joint_trajectory 35 | # type: FollowJointTrajectory 36 | # joints: 37 | # - ur10_shoulder_pan_joint 38 | # - ur10_shoulder_lift_joint 39 | # - ur10_elbow_joint 40 | # - ur10_wrist_1_joint 41 | # - ur10_wrist_2_joint 42 | # - ur10_wrist_3_joint 43 | 44 | ### without ros-control: 45 | #controller_list: 46 | # - name: ur5 47 | # action_ns: follow_joint_trajectory 48 | # type: FollowJointTrajectory 49 | # joints: 50 | # - ur5_shoulder_pan_joint 51 | # - ur5_shoulder_lift_joint 52 | # - ur5_elbow_joint 53 | # - ur5_wrist_1_joint 54 | # - ur5_wrist_2_joint 55 | # - ur5_wrist_3_joint 56 | # - name: ur10 57 | # action_ns: follow_joint_trajectory 58 | # type: FollowJointTrajectory 59 | # joints: 60 | # - ur10_shoulder_pan_joint 61 | # - ur10_shoulder_lift_joint 62 | # - ur10_elbow_joint 63 | # - ur10_wrist_1_joint 64 | # - ur10_wrist_2_joint 65 | # - ur10_wrist_3_joint 66 | -------------------------------------------------------------------------------- /dual_arm_robot_moveit_config/config/fake_controllers.yaml: -------------------------------------------------------------------------------- 1 | fake_interpolating_controller_rate: 10 (Hz) 2 | controller_list: 3 | - name: fake_ur5_manipulator_controller 4 | type: interpolate #interpolate | via points | last point 5 | joints: 6 | - ur5_shoulder_pan_joint 7 | - ur5_shoulder_lift_joint 8 | - ur5_elbow_joint 9 | - ur5_wrist_1_joint 10 | - ur5_wrist_2_joint 11 | - ur5_wrist_3_joint 12 | - name: fake_ur10_manipulator_controller 13 | type: interpolate 14 | joints: 15 | - ur10_shoulder_pan_joint 16 | - ur10_shoulder_lift_joint 17 | - ur10_elbow_joint 18 | - ur10_wrist_1_joint 19 | - ur10_wrist_2_joint 20 | - ur10_wrist_3_joint 21 | - name: fake_ur5_endeffector_controller 22 | joints: 23 | [] 24 | - name: fake_ur10_endeffector_controller 25 | joints: 26 | [] 27 | initial: 28 | - group: arms 29 | pose: arms_up 30 | 31 | 32 | # - name: fake_arms_controller 33 | # joints: 34 | # - ur10_shoulder_pan_joint 35 | # - ur10_shoulder_lift_joint 36 | # - ur10_elbow_joint 37 | # - ur10_wrist_1_joint 38 | # - ur10_wrist_2_joint 39 | # - ur10_wrist_3_joint 40 | # - ur5_shoulder_pan_joint 41 | # - ur5_shoulder_lift_joint 42 | # - ur5_elbow_joint 43 | # - ur5_wrist_1_joint 44 | # - ur5_wrist_2_joint 45 | # - ur5_wrist_3_joint 46 | -------------------------------------------------------------------------------- /dual_arm_robot_moveit_config/config/joint_limits.yaml: -------------------------------------------------------------------------------- 1 | # joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed 2 | # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] 3 | # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] 4 | joint_limits: 5 | ur10_elbow_joint: 6 | has_velocity_limits: true 7 | max_velocity: 3.15 8 | has_acceleration_limits: false 9 | max_acceleration: 0 10 | ur10_shoulder_lift_joint: 11 | has_velocity_limits: true 12 | max_velocity: 2.16 13 | has_acceleration_limits: false 14 | max_acceleration: 0 15 | ur10_shoulder_pan_joint: 16 | has_velocity_limits: true 17 | max_velocity: 2.16 18 | has_acceleration_limits: false 19 | max_acceleration: 0 20 | ur10_wrist_1_joint: 21 | has_velocity_limits: true 22 | max_velocity: 3.2 23 | has_acceleration_limits: false 24 | max_acceleration: 0 25 | ur10_wrist_2_joint: 26 | has_velocity_limits: true 27 | max_velocity: 3.2 28 | has_acceleration_limits: false 29 | max_acceleration: 0 30 | ur10_wrist_3_joint: 31 | has_velocity_limits: true 32 | max_velocity: 3.2 33 | has_acceleration_limits: false 34 | max_acceleration: 0 35 | ur5_elbow_joint: 36 | has_velocity_limits: true 37 | max_velocity: 3.15 38 | has_acceleration_limits: false 39 | max_acceleration: 0 40 | ur5_shoulder_lift_joint: 41 | has_velocity_limits: true 42 | max_velocity: 3.15 43 | has_acceleration_limits: false 44 | max_acceleration: 0 45 | ur5_shoulder_pan_joint: 46 | has_velocity_limits: true 47 | max_velocity: 3.15 48 | has_acceleration_limits: false 49 | max_acceleration: 0 50 | ur5_wrist_1_joint: 51 | has_velocity_limits: true 52 | max_velocity: 3.2 53 | has_acceleration_limits: false 54 | max_acceleration: 0 55 | ur5_wrist_2_joint: 56 | has_velocity_limits: true 57 | max_velocity: 3.2 58 | has_acceleration_limits: false 59 | max_acceleration: 0 60 | ur5_wrist_3_joint: 61 | has_velocity_limits: true 62 | max_velocity: 3.2 63 | has_acceleration_limits: false 64 | max_acceleration: 0 -------------------------------------------------------------------------------- /dual_arm_robot_moveit_config/config/kinematics.yaml: -------------------------------------------------------------------------------- 1 | ur5_manipulator: 2 | kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin 3 | kinematics_solver_search_resolution: 0.001 4 | kinematics_solver_timeout: 0.005 5 | kinematics_solver_attempts: 3 6 | ur10_manipulator: 7 | kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin 8 | #kinematics_solver: dual_arm_robot_ur10_manipulator_kinematics/IKFastKinematicsPlugin 9 | kinematics_solver_search_resolution: 0.001 10 | kinematics_solver_timeout: 0.005 11 | kinematics_solver_attempts: 3 12 | -------------------------------------------------------------------------------- /dual_arm_robot_moveit_config/config/ompl_planning.yaml: -------------------------------------------------------------------------------- 1 | planner_configs: 2 | SBLkConfigDefault: 3 | type: geometric::SBL 4 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 5 | ESTkConfigDefault: 6 | type: geometric::EST 7 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup() 8 | goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 9 | LBKPIECEkConfigDefault: 10 | type: geometric::LBKPIECE 11 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 12 | border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 13 | min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 14 | BKPIECEkConfigDefault: 15 | type: geometric::BKPIECE 16 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 17 | border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 18 | failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 19 | min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 20 | KPIECEkConfigDefault: 21 | type: geometric::KPIECE 22 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 23 | goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 24 | border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.] 25 | failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 26 | min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 27 | RRTkConfigDefault: 28 | type: geometric::RRT 29 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 30 | goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 31 | RRTConnectkConfigDefault: 32 | type: geometric::RRTConnect 33 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 34 | RRTstarkConfigDefault: 35 | type: geometric::RRTstar 36 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 37 | goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 38 | delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1 39 | TRRTkConfigDefault: 40 | type: geometric::TRRT 41 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 42 | goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 43 | max_states_failed: 10 # when to start increasing temp. default: 10 44 | temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0 45 | min_temperature: 10e-10 # lower limit of temp change. default: 10e-10 46 | init_temperature: 10e-6 # initial temperature. default: 10e-6 47 | frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() 48 | frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 49 | k_constant: 0.0 # value used to normalize expresssion. default: 0.0 set in setup() 50 | PRMkConfigDefault: 51 | type: geometric::PRM 52 | max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 53 | PRMstarkConfigDefault: 54 | type: geometric::PRMstar 55 | ur5_manipulator: 56 | planner_configs: 57 | - SBLkConfigDefault 58 | - ESTkConfigDefault 59 | - LBKPIECEkConfigDefault 60 | - BKPIECEkConfigDefault 61 | - KPIECEkConfigDefault 62 | - RRTkConfigDefault 63 | - RRTConnectkConfigDefault 64 | - RRTstarkConfigDefault 65 | - TRRTkConfigDefault 66 | - PRMkConfigDefault 67 | - PRMstarkConfigDefault 68 | projection_evaluator: joints(ur5_shoulder_pan_joint,ur5_shoulder_lift_joint) 69 | longest_valid_segment_fraction: 0.005 # in rad maximaler wert ergibt sich aus summe_der_joint_ranges*dieser_wert 70 | ur10_manipulator: 71 | planner_configs: 72 | - SBLkConfigDefault 73 | - ESTkConfigDefault 74 | - LBKPIECEkConfigDefault 75 | - BKPIECEkConfigDefault 76 | - KPIECEkConfigDefault 77 | - RRTkConfigDefault 78 | - RRTConnectkConfigDefault 79 | - RRTstarkConfigDefault 80 | - TRRTkConfigDefault 81 | - PRMkConfigDefault 82 | - PRMstarkConfigDefault 83 | projection_evaluator: joints(ur10_shoulder_pan_joint,ur10_shoulder_lift_joint) 84 | longest_valid_segment_fraction: 0.005 85 | ur5_endeffector: 86 | planner_configs: 87 | - SBLkConfigDefault 88 | - ESTkConfigDefault 89 | - LBKPIECEkConfigDefault 90 | - BKPIECEkConfigDefault 91 | - KPIECEkConfigDefault 92 | - RRTkConfigDefault 93 | - RRTConnectkConfigDefault 94 | - RRTstarkConfigDefault 95 | - TRRTkConfigDefault 96 | - PRMkConfigDefault 97 | - PRMstarkConfigDefault 98 | ur10_endeffector: 99 | planner_configs: 100 | - SBLkConfigDefault 101 | - ESTkConfigDefault 102 | - LBKPIECEkConfigDefault 103 | - BKPIECEkConfigDefault 104 | - KPIECEkConfigDefault 105 | - RRTkConfigDefault 106 | - RRTConnectkConfigDefault 107 | - RRTstarkConfigDefault 108 | - TRRTkConfigDefault 109 | - PRMkConfigDefault 110 | - PRMstarkConfigDefault 111 | arms: 112 | planner_configs: 113 | - SBLkConfigDefault 114 | - ESTkConfigDefault 115 | - LBKPIECEkConfigDefault 116 | - BKPIECEkConfigDefault 117 | - KPIECEkConfigDefault 118 | - RRTkConfigDefault 119 | - RRTConnectkConfigDefault 120 | - RRTstarkConfigDefault 121 | - TRRTkConfigDefault 122 | - PRMkConfigDefault 123 | - PRMstarkConfigDefault 124 | projection_evaluator: joints(ur10_shoulder_pan_joint,ur10_shoulder_lift_joint) 125 | longest_valid_segment_fraction: 0.005 126 | -------------------------------------------------------------------------------- /dual_arm_robot_moveit_config/launch/default_warehouse_db.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /dual_arm_robot_moveit_config/launch/demo.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 | [/move_group/fake_controller_joint_states] 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 | -------------------------------------------------------------------------------- /dual_arm_robot_moveit_config/launch/dual_arm_robot_moveit_controller_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /dual_arm_robot_moveit_config/launch/dual_arm_robot_moveit_planning_execution.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /dual_arm_robot_moveit_config/launch/dual_arm_robot_moveit_sensor_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /dual_arm_robot_moveit_config/launch/fake_moveit_controller_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /dual_arm_robot_moveit_config/launch/joystick_control.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /dual_arm_robot_moveit_config/launch/move_group.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 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 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | -------------------------------------------------------------------------------- /dual_arm_robot_moveit_config/launch/moveit_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /dual_arm_robot_moveit_config/launch/ompl_planning_pipeline.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 8 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /dual_arm_robot_moveit_config/launch/planning_context.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 | -------------------------------------------------------------------------------- /dual_arm_robot_moveit_config/launch/planning_pipeline.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /dual_arm_robot_moveit_config/launch/run_benchmark_ompl.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /dual_arm_robot_moveit_config/launch/sensor_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /dual_arm_robot_moveit_config/launch/setup_assistant.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /dual_arm_robot_moveit_config/launch/trajectory_execution.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /dual_arm_robot_moveit_config/launch/warehouse.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /dual_arm_robot_moveit_config/launch/warehouse_settings.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /dual_arm_robot_moveit_config/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | dual_arm_robot_moveit_config 4 | 0.3.0 5 | 6 | An automatically generated package with all the configuration and launch files for using the dual_arm_robot with the MoveIt! Motion Planning Framework 7 | 8 | Daniel Hoeltgen 9 | Daniel Hoeltgen 10 | 11 | BSD 12 | 13 | http://moveit.ros.org/ 14 | https://github.com/ros-planning/moveit/issues 15 | https://github.com/ros-planning/moveit 16 | 17 | catkin 18 | 19 | moveit_ros_move_group 20 | moveit_kinematics 21 | moveit_planners_ompl 22 | moveit_ros_visualization 23 | joint_state_publisher 24 | robot_state_publisher 25 | xacro 26 | dual_arm_robot_description 27 | dual_arm_robot_description 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /eeff_foam.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/danielhoeltgen/Dual-Arm-Robot-Force-Controlled-Object-Manipulation/53761ef144e67e65b8580179ec0a451ba537313f/eeff_foam.stl -------------------------------------------------------------------------------- /eeff_frame.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/danielhoeltgen/Dual-Arm-Robot-Force-Controlled-Object-Manipulation/53761ef144e67e65b8580179ec0a451ba537313f/eeff_frame.stl -------------------------------------------------------------------------------- /joint_trajectory_admittance_controller/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package joint_trajectory_admittance_controller 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.9.3 (2016-02-12) 6 | ------------------ 7 | * Add joint limits spec to rrbot test robot 8 | * Address -Wunused-parameter warnings 9 | * reset to semantic zero in HardwareInterfaceAdapter for PositionJointInterface 10 | * Contributors: Adolfo Rodriguez Tsouroukdissian, ipa-fxm 11 | 12 | 0.9.2 (2015-05-04) 13 | ------------------ 14 | 15 | 0.9.1 (2014-11-03) 16 | ------------------ 17 | 18 | 0.9.0 (2014-10-31) 19 | ------------------ 20 | * Check that waypoint times are strictly increasing before accepting a command 21 | * velocity_controllers::JointTrajectoryController: New plugin variant for 22 | velocity-controlled joints 23 | * Buildsystem fixes 24 | * Contributors: Adolfo Rodriguez Tsouroukdissian, Lukas Bulwahn, ipa-fxm, Dave Coleman 25 | 26 | 0.8.1 (2014-07-11) 27 | ------------------ 28 | * joint_trajectory_admittance_controller: Critical targets declared before calling catkin_package 29 | * check for CATKIN_ENABLE_TESTING 30 | * Contributors: Jonathan Bohren, Lukas Bulwahn 31 | 32 | 0.8.0 (2014-05-12) 33 | ------------------ 34 | * Remove rosbuild artifacts. Fix `#90 `_. 35 | * Contributors: Adolfo Rodriguez Tsouroukdissian 36 | 37 | 0.7.2 (2014-04-01) 38 | ------------------ 39 | 40 | 0.7.1 (2014-03-31) 41 | ------------------ 42 | 43 | 0.7.0 (2014-03-28) 44 | ------------------ 45 | * Add support for an joint interfaces are not inherited from JointHandle. 46 | * Contributors: Igorec 47 | 48 | 0.6.0 (2014-02-05) 49 | ------------------ 50 | * Merge pull request `#72 `_ from pal-robotics/minor-maintenance 51 | Minor maintenance 52 | * Default stop_trajectory_duration to zero. Refs `#73 `_ 53 | * Better logs when dropping traj points. Refs `#68 `_. 54 | * Fix class member reorder warning in constructor. 55 | * Add missing headers to target files. 56 | * Action interface rejects empty goals. Fixes `#70 `_. 57 | * Reorder how time and traj data are updated. 58 | In the update method, fetching the currently executed trajectory should be done 59 | before updating the time data to prevent a potential scenario in which there 60 | is no trajectory defined for the current control cycle. 61 | * Work tolerance checking methods. 62 | Until now we used the currently active goal handle for performing tolerance 63 | checks. Using the goal handle stored in segments is more robust to unexpected 64 | goal updates by the non-rt thread. 65 | * Refactor how the currrent trajectory is stored. 66 | - Handle concurrency in the current trajectory between rt and non-rt threads 67 | using the simpler RealtimeBox instead of the RealtimeBuffer, because our 68 | usecase does not fit well the non-rt->writes / rt->reads semantics. 69 | - As a consequence we no longer need to store the msg_trajectory member, but 70 | only the hold_trajectory, which must still be preallocated. 71 | * Honor unspecified vel/acc in ROS message. Fix `#65 `_. 72 | * Fixes per Adolfo 73 | * Added verbose flag 74 | * Fixing realtime issues 75 | * Merge branch 'hydro-devel' into joint_trajectory_tweaks 76 | * Tweaked error messages 77 | * Added more debug info 78 | * Fix for microsecond delay that caused header time=0 (now) to start too late 79 | * Reworded debug message 80 | * Image update. 81 | * Update README.md 82 | Factor out user documentation to the ROS wiki. 83 | * Merge branch 'hydro-devel' of https://github.com/willowgarage/ros_controllers into hydro-devel 84 | * Rename hold_trajectory_duration 85 | - hold_trajectory_duration -> stop_trajectory_duration for more clarity. 86 | - During Hydro, hold_trajectory_duration will still work, giving a deprecation 87 | warning. 88 | * Add basic description in package.xml. 89 | * Add images used in the ROS wiki doc. 90 | * Added better debug info 91 | * Throttled debug output 92 | * Added more debug and error information 93 | * Contributors: Adolfo Rodriguez Tsouroukdissian, Dave Coleman 94 | 95 | 0.5.4 (2013-09-30) 96 | ------------------ 97 | * Added install rules for plugin.xml 98 | * Remove PID sign flip. 99 | This is now done in the state error computation. 100 | * Merge pull request `#45 `_ from ros-controls/effort_fixes 101 | Added check for ~/robot_description and fixed hardware interface abstraction bug 102 | * Flip state error sign. 103 | * PID sign was wrong 104 | * Added check for ~/robot_description and fixed hardware interface abstraction bug 105 | * Update README.md 106 | * Create README.md 107 | * Fix license header string for some files. 108 | * Less verbose init logging. 109 | Statement detailing controller joint count, as well as segment and hardware 110 | interface types moved from INFO to DEBUG severity. 111 | 112 | 0.5.3 (2013-09-04) 113 | ------------------ 114 | * joint_trajectory_admittance_controller: New package implementing a controller for executing joint-space trajectories on a 115 | set of joints. 116 | 117 | * ROS API 118 | 119 | * Commands: FollowJointTrajectory action and trajectory_msgs::JointTrajectory topic. 120 | * Current controller state is available in a control_msgs::JointTrajectoryControllerState topic. 121 | * Controller state at any future time can be queried through a control_msgs::JointTrajectoryControllerState 122 | service. 123 | 124 | * Trajectory segment type 125 | 126 | * Controller is templated on the segment type. 127 | * Multi-dof quintic spline segment implementation provided by default. 128 | 129 | * Hardware interface type 130 | 131 | * Controller is templated on the hardware interface type. 132 | * Position and effort control joint interfaces provided by default. 133 | 134 | * Other 135 | 136 | * Realtime-safe. 137 | * Proper handling of wrapping (continuous) joints. 138 | * Discontinuous system clock changes do not cause discontinuities in the execution of already queued 139 | trajectory segments. -------------------------------------------------------------------------------- /joint_trajectory_admittance_controller/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(joint_trajectory_admittance_controller) 3 | 4 | # Load catkin and all dependencies required for this package 5 | find_package(catkin 6 | REQUIRED COMPONENTS 7 | actionlib 8 | angles 9 | cmake_modules 10 | roscpp 11 | urdf 12 | control_toolbox 13 | controller_interface 14 | hardware_interface 15 | realtime_tools 16 | control_msgs 17 | trajectory_msgs 18 | controller_manager 19 | xacro 20 | pluginlib 21 | ) 22 | 23 | find_package(orocos_kdl) 24 | find_package(kdl_parser) 25 | 26 | # Declare catkin package 27 | catkin_package( 28 | CATKIN_DEPENDS 29 | pluginlib 30 | actionlib 31 | angles 32 | roscpp 33 | urdf 34 | control_toolbox 35 | controller_interface 36 | hardware_interface 37 | realtime_tools 38 | control_msgs 39 | trajectory_msgs 40 | controller_manager 41 | xacro 42 | INCLUDE_DIRS include 43 | LIBRARIES ${PROJECT_NAME} 44 | ) 45 | 46 | SET( EIGEN3_INCLUDE_DIR "$ENV{EIGEN3_INCLUDE_DIR}" ) 47 | IF( NOT EIGEN3_INCLUDE_DIR ) 48 | MESSAGE( FATAL_ERROR "Please point the environment variable EIGEN3_INCLUDE_DIR to the include directory of your Eigen3 installation.") 49 | ENDIF() 50 | INCLUDE_DIRECTORIES ( "${EIGEN3_INCLUDE_DIR}" ) 51 | 52 | include_directories(${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS} "${EIGEN3_INCLUDE_DIR}" include) 53 | 54 | add_library(${PROJECT_NAME} src/joint_trajectory_admittance_controller.cpp 55 | include/joint_trajectory_admittance_controller/hardware_interface_adapter.h 56 | include/joint_trajectory_admittance_controller/init_joint_trajectory.h 57 | include/joint_trajectory_admittance_controller/joint_trajectory_admittance_controller.h 58 | include/joint_trajectory_admittance_controller/joint_trajectory_admittance_controller_impl.h 59 | include/joint_trajectory_admittance_controller/joint_trajectory_msg_utils.h 60 | include/joint_trajectory_admittance_controller/joint_trajectory_segment.h 61 | include/joint_trajectory_admittance_controller/tolerances.h 62 | include/trajectory_interface_admittance/trajectory_interface.h 63 | include/trajectory_interface_admittance/quintic_spline_segment.h 64 | include/trajectory_interface_admittance/pos_vel_acc_state.h 65 | include/admittance_control/admittance_control.h 66 | ) 67 | 68 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${orocos_kdl_LIBRARIES} ${kdl_parser_LIBRARIES}) 69 | 70 | 71 | if(CATKIN_ENABLE_TESTING) 72 | find_package(rostest REQUIRED) 73 | 74 | catkin_add_gtest(quintic_spline_segment_test test/quintic_spline_segment_test.cpp) 75 | target_link_libraries(quintic_spline_segment_test ${catkin_LIBRARIES}) 76 | 77 | catkin_add_gtest(trajectory_interface_test test/trajectory_interface_test.cpp) 78 | target_link_libraries(trajectory_interface_test ${catkin_LIBRARIES}) 79 | 80 | catkin_add_gtest(joint_trajectory_segment_test test/joint_trajectory_segment_test.cpp) 81 | target_link_libraries(joint_trajectory_segment_test ${catkin_LIBRARIES}) 82 | 83 | catkin_add_gtest(joint_trajectory_msg_utils_test test/joint_trajectory_msg_utils_test.cpp) 84 | target_link_libraries(joint_trajectory_msg_utils_test ${catkin_LIBRARIES}) 85 | 86 | catkin_add_gtest(init_joint_trajectory_test test/init_joint_trajectory_test.cpp) 87 | target_link_libraries(init_joint_trajectory_test ${catkin_LIBRARIES}) 88 | 89 | add_rostest_gtest(tolerances_test 90 | test/tolerances.test 91 | test/tolerances_test.cpp) 92 | target_link_libraries(tolerances_test ${catkin_LIBRARIES}) 93 | 94 | add_executable(rrbot test/rrbot.cpp) 95 | target_link_libraries(rrbot ${catkin_LIBRARIES}) 96 | 97 | add_dependencies(tests rrbot) 98 | add_dependencies(tests ${PROJECT_NAME}) 99 | 100 | add_rostest_gtest(joint_trajectory_controller_test 101 | test/joint_trajectory_controller.test 102 | test/joint_trajectory_controller_test.cpp) 103 | target_link_libraries(joint_trajectory_controller_test ${catkin_LIBRARIES}) 104 | endif() 105 | 106 | # Install 107 | install(DIRECTORY include/${PROJECT_NAME}/ 108 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) 109 | 110 | install(DIRECTORY include/trajectory_interface_admittance/ 111 | DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}/trajectory_interface/) 112 | 113 | install(TARGETS ${PROJECT_NAME} 114 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 115 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 116 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 117 | ) 118 | 119 | install(FILES controller_plugins.xml 120 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 121 | 122 | # TODO: Install test resource files as well? 123 | -------------------------------------------------------------------------------- /joint_trajectory_admittance_controller/README.md: -------------------------------------------------------------------------------- 1 | ## Joint Trajectory Controller ## 2 | 3 | Controller for executing joint-space trajectories on a group of joints. 4 | 5 | Detailed user documentation can be found in the controller's [ROS wiki page](http://wiki.ros.org/joint_trajectory_admittance_controller). 6 | 7 | 8 | -------------------------------------------------------------------------------- /joint_trajectory_admittance_controller/controller_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | 7 | The JointTrajectoryController executes joint-space trajectories on a set of joints. 8 | This variant represents trajectory segments as quintic splines and sends commands to a position interface. 9 | 10 | 11 | 12 | 15 | 16 | The JointTrajectoryController executes joint-space trajectories on a set of joints. 17 | This variant represents trajectory segments as quintic splines and sends commands to a velocity interface. 18 | 19 | 20 | 21 | 24 | 25 | The JointTrajectoryController executes joint-space trajectories on a set of joints. 26 | This variant represents trajectory segments as quintic splines and sends commands to an effort interface. 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /joint_trajectory_admittance_controller/images/new_trajectory.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/danielhoeltgen/Dual-Arm-Robot-Force-Controlled-Object-Manipulation/53761ef144e67e65b8580179ec0a451ba537313f/joint_trajectory_admittance_controller/images/new_trajectory.png -------------------------------------------------------------------------------- /joint_trajectory_admittance_controller/images/trajectory_replacement_future.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/danielhoeltgen/Dual-Arm-Robot-Force-Controlled-Object-Manipulation/53761ef144e67e65b8580179ec0a451ba537313f/joint_trajectory_admittance_controller/images/trajectory_replacement_future.png -------------------------------------------------------------------------------- /joint_trajectory_admittance_controller/images/trajectory_replacement_now.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/danielhoeltgen/Dual-Arm-Robot-Force-Controlled-Object-Manipulation/53761ef144e67e65b8580179ec0a451ba537313f/joint_trajectory_admittance_controller/images/trajectory_replacement_now.png -------------------------------------------------------------------------------- /joint_trajectory_admittance_controller/images/trajectory_replacement_past.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/danielhoeltgen/Dual-Arm-Robot-Force-Controlled-Object-Manipulation/53761ef144e67e65b8580179ec0a451ba537313f/joint_trajectory_admittance_controller/images/trajectory_replacement_past.png -------------------------------------------------------------------------------- /joint_trajectory_admittance_controller/include/trajectory_interface_admittance/pos_vel_acc_state.h: -------------------------------------------------------------------------------- 1 | 2 | /////////////////////////////////////////////////////////////////////////////// 3 | // Copyright (C) 2013, PAL Robotics S.L. 4 | // Copyright (c) 2008, Willow Garage, Inc. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // * Redistributions of source code must retain the above copyright notice, 9 | // this list of conditions and the following disclaimer. 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // * Neither the name of PAL Robotics S.L. nor the names of its 14 | // contributors may be used to endorse or promote products derived from 15 | // this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | // POSSIBILITY OF SUCH DAMAGE. 28 | ////////////////////////////////////////////////////////////////////////////// 29 | 30 | /// \author Adolfo Rodriguez Tsouroukdissian 31 | 32 | #ifndef TRAJECTORY_INTERFACE_POS_VEL_ACC_STATE_H 33 | #define TRAJECTORY_INTERFACE_POS_VEL_ACC_STATE_H 34 | 35 | #include 36 | 37 | namespace trajectory_interface 38 | { 39 | 40 | /** 41 | * \brief Multi-dof trajectory state containing position, velocity and acceleration data. 42 | */ 43 | template 44 | struct PosVelAccState 45 | { 46 | typedef ScalarType Scalar; 47 | 48 | PosVelAccState() {} 49 | 50 | /** 51 | * \brief Resource-preallocating constructor. 52 | * 53 | * Position, velocity and acceleration vectors are resized to \p size, and their values are set to zero. 54 | * Note that these two situations are different: 55 | * \code 56 | * // 2-dimensional state specifying zero position, velocity and acceleration 57 | * State zero_pos_vel_acc(2); 58 | * 59 | * // 2-dimensional state specifying zero position 60 | * State zero_pos; 61 | * zero_pos.position.resize(2); 62 | * \endcode 63 | */ 64 | PosVelAccState(const typename std::vector::size_type size) 65 | : position( std::vector(size, static_cast(0))), 66 | velocity( std::vector(size, static_cast(0))), 67 | acceleration(std::vector(size, static_cast(0))) 68 | {} 69 | 70 | std::vector position; 71 | std::vector velocity; 72 | std::vector acceleration; 73 | }; 74 | 75 | } // namespace 76 | 77 | #endif // header guard 78 | -------------------------------------------------------------------------------- /joint_trajectory_admittance_controller/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b joint_trajectory_admittance_controller 6 | 7 | 10 | 11 | --> 12 | 13 | 14 | */ 15 | -------------------------------------------------------------------------------- /joint_trajectory_admittance_controller/package.xml: -------------------------------------------------------------------------------- 1 | 2 | joint_trajectory_admittance_controller 3 | Controller for executing joint-space trajectories on a group of joints. 4 | 0.9.3 5 | 6 | Daniel Höltgen 7 | 8 | BSD 9 | 10 | https://github.com/ros-controls/ros_controllers/wiki 11 | https://github.com/ros-controls/ros_controllers/issues 12 | https://github.com/ros-controls/ros_controllers 13 | 14 | Daniel Höltgen 15 | 16 | catkin 17 | 18 | cmake_modules 19 | actionlib 20 | angles 21 | roscpp 22 | urdf 23 | 24 | control_toolbox 25 | controller_interface 26 | hardware_interface 27 | realtime_tools 28 | 29 | control_msgs 30 | trajectory_msgs 31 | 32 | rostest 33 | controller_manager 34 | xacro 35 | 36 | pluginlib 37 | 38 | kdl 39 | 40 | 41 | actionlib 42 | angles 43 | roscpp 44 | urdf 45 | 46 | control_toolbox 47 | controller_interface 48 | hardware_interface 49 | realtime_tools 50 | 51 | control_msgs 52 | trajectory_msgs 53 | 54 | rostest 55 | controller_manager 56 | xacro 57 | rqt_plot 58 | 59 | pluginlib 60 | 61 | kdl 62 | 63 | 64 | 65 | 66 | 67 | -------------------------------------------------------------------------------- /joint_trajectory_admittance_controller/ros_control_plugins.xml~: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | 7 | The JointTrajectoryController executes joint-space trajectories on a set of joints. 8 | This variant represents trajectory segments as quintic splines and sends commands to a position interface. 9 | 10 | 11 | 12 | 15 | 16 | The JointTrajectoryController executes joint-space trajectories on a set of joints. 17 | This variant represents trajectory segments as quintic splines and sends commands to a velocity interface. 18 | 19 | 20 | 21 | 24 | 25 | The JointTrajectoryController executes joint-space trajectories on a set of joints. 26 | This variant represents trajectory segments as quintic splines and sends commands to an effort interface. 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /joint_trajectory_admittance_controller/rosdoc.yaml: -------------------------------------------------------------------------------- 1 | - builder: doxygen 2 | name: C++ API 3 | output_dir: c++ 4 | file_patterns: '*.c *.cpp *.h *.cc *.hh *.dox' 5 | exclude_patterns: 'test' -------------------------------------------------------------------------------- /joint_trajectory_admittance_controller/src/joint_trajectory_admittance_controller.cpp: -------------------------------------------------------------------------------- 1 | /////////////////////////////////////////////////////////////////////////////// 2 | // Copyright (C) 2013, PAL Robotics S.L. 3 | // Copyright (c) 2008, Willow Garage, Inc. 4 | // 5 | // Redistribution and use in source and binary forms, with or without 6 | // modification, are permitted provided that the following conditions are met: 7 | // * Redistributions of source code must retain the above copyright notice, 8 | // this list of conditions and the following disclaimer. 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // * Neither the name of PAL Robotics S.L. nor the names of its 13 | // contributors may be used to endorse or promote products derived from 14 | // this software without specific prior written permission. 15 | // 16 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 19 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 20 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 21 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 22 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 23 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 24 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 25 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 26 | // POSSIBILITY OF SUCH DAMAGE. 27 | ////////////////////////////////////////////////////////////////////////////// 28 | 29 | // Pluginlib 30 | #include 31 | 32 | // Project 33 | #include "trajectory_interface_admittance/quintic_spline_segment.h" 34 | #include "joint_trajectory_admittance_controller/joint_trajectory_admittance_controller.h" 35 | 36 | namespace position_controllers 37 | { 38 | /** 39 | * \brief Joint trajectory controller that represents trajectory segments as quintic splines and sends 40 | * commands to a \b position interface. 41 | */ 42 | typedef joint_trajectory_admittance_controller::JointTrajectoryAdmittanceController, 43 | hardware_interface::PositionJointInterface> 44 | JointTrajectoryAdmittanceController; 45 | } 46 | 47 | namespace velocity_controllers 48 | { 49 | /** 50 | * \brief Joint trajectory controller that represents trajectory segments as quintic splines and sends 51 | * commands to a \b velocity interface. 52 | */ 53 | typedef joint_trajectory_admittance_controller::JointTrajectoryAdmittanceController, 54 | hardware_interface::VelocityJointInterface> 55 | JointTrajectoryAdmittanceController; 56 | } 57 | 58 | namespace effort_controllers 59 | { 60 | /** 61 | * \brief Joint trajectory controller that represents trajectory segments as quintic splines and sends 62 | * commands to an \b effort interface. 63 | */ 64 | typedef joint_trajectory_admittance_controller::JointTrajectoryAdmittanceController, 65 | hardware_interface::EffortJointInterface> 66 | JointTrajectoryAdmittanceController; 67 | } 68 | 69 | PLUGINLIB_EXPORT_CLASS(position_controllers::JointTrajectoryAdmittanceController, controller_interface::ControllerBase) 70 | PLUGINLIB_EXPORT_CLASS(velocity_controllers::JointTrajectoryAdmittanceController, controller_interface::ControllerBase) 71 | PLUGINLIB_EXPORT_CLASS(effort_controllers::JointTrajectoryAdmittanceController, controller_interface::ControllerBase) 72 | -------------------------------------------------------------------------------- /joint_trajectory_admittance_controller/test/joint_trajectory_controller.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 10 | 11 | 12 | 13 | 14 | 15 | 18 | 19 | 20 | 24 | 25 | 29 | 30 | 31 | 35 | 36 | -------------------------------------------------------------------------------- /joint_trajectory_admittance_controller/test/rrbot.cpp: -------------------------------------------------------------------------------- 1 | /////////////////////////////////////////////////////////////////////////////// 2 | // Copyright (C) 2013, PAL Robotics S.L. 3 | // 4 | // Redistribution and use in source and binary forms, with or without 5 | // modification, are permitted provided that the following conditions are met: 6 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright 9 | // notice, this list of conditions and the following disclaimer in the 10 | // documentation and/or other materials provided with the distribution. 11 | // * Neither the name of PAL Robotics S.L. nor the names of its 12 | // contributors may be used to endorse or promote products derived from 13 | // this software without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 18 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 19 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 20 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 21 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 22 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 23 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 24 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 25 | // POSSIBILITY OF SUCH DAMAGE. 26 | ////////////////////////////////////////////////////////////////////////////// 27 | 28 | // NOTE: The contents of this file have been taken largely from the ros_control wiki tutorials 29 | 30 | // ROS 31 | #include 32 | #include 33 | 34 | // ros_control 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | 41 | class RRbot : public hardware_interface::RobotHW 42 | { 43 | public: 44 | RRbot() 45 | { 46 | // Intialize raw data 47 | pos_[0] = 0.0; pos_[1] = 0.0; 48 | vel_[0] = 0.0; vel_[1] = 0.0; 49 | eff_[0] = 0.0; eff_[1] = 0.0; 50 | cmd_[0] = 0.0; cmd_[1] = 0.0; 51 | 52 | // Connect and register the joint state interface 53 | hardware_interface::JointStateHandle state_handle_1("joint1", &pos_[0], &vel_[0], &eff_[0]); 54 | jnt_state_interface_.registerHandle(state_handle_1); 55 | 56 | hardware_interface::JointStateHandle state_handle_2("joint2", &pos_[1], &vel_[1], &eff_[1]); 57 | jnt_state_interface_.registerHandle(state_handle_2); 58 | 59 | registerInterface(&jnt_state_interface_); 60 | 61 | // Connect and register the joint position interface 62 | hardware_interface::JointHandle pos_handle_1(jnt_state_interface_.getHandle("joint1"), &cmd_[0]); 63 | jnt_pos_interface_.registerHandle(pos_handle_1); 64 | 65 | hardware_interface::JointHandle pos_handle_2(jnt_state_interface_.getHandle("joint2"), &cmd_[1]); 66 | jnt_pos_interface_.registerHandle(pos_handle_2); 67 | 68 | registerInterface(&jnt_pos_interface_); 69 | 70 | // Smoothing subscriber 71 | smoothing_sub_ = ros::NodeHandle().subscribe("smoothing", 1, &RRbot::smoothingCB, this); 72 | smoothing_.initRT(0.0); 73 | } 74 | 75 | ros::Time getTime() const {return ros::Time::now();} 76 | ros::Duration getPeriod() const {return ros::Duration(0.01);} 77 | 78 | void read() {} 79 | 80 | void write() 81 | { 82 | const double smoothing = *(smoothing_.readFromRT()); 83 | for (unsigned int i = 0; i < 2; ++i) 84 | { 85 | vel_[i] = (cmd_[i] - pos_[i]) / getPeriod().toSec(); 86 | 87 | const double next_pos = smoothing * pos_[i] + (1.0 - smoothing) * cmd_[i]; 88 | pos_[i] = next_pos; 89 | } 90 | } 91 | 92 | private: 93 | hardware_interface::JointStateInterface jnt_state_interface_; 94 | hardware_interface::PositionJointInterface jnt_pos_interface_; 95 | double cmd_[2]; 96 | double pos_[2]; 97 | double vel_[2]; 98 | double eff_[2]; 99 | 100 | realtime_tools::RealtimeBuffer smoothing_; 101 | void smoothingCB(const std_msgs::Float64& smoothing) {smoothing_.writeFromNonRT(smoothing.data);} 102 | 103 | ros::Subscriber smoothing_sub_; 104 | }; 105 | 106 | int main(int argc, char **argv) 107 | { 108 | ros::init(argc, argv, "rrbot"); 109 | ros::NodeHandle nh; 110 | 111 | RRbot robot; 112 | controller_manager::ControllerManager cm(&robot, nh); 113 | 114 | ros::Rate rate(1.0 / robot.getPeriod().toSec()); 115 | ros::AsyncSpinner spinner(1); 116 | spinner.start(); 117 | while (ros::ok()) 118 | { 119 | robot.read(); 120 | cm.update(robot.getTime(), robot.getPeriod()); 121 | robot.write(); 122 | rate.sleep(); 123 | } 124 | spinner.stop(); 125 | 126 | return 0; 127 | } 128 | -------------------------------------------------------------------------------- /joint_trajectory_admittance_controller/test/rrbot.xacro: -------------------------------------------------------------------------------- 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 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 126 | 127 | 128 | 129 | 130 | -------------------------------------------------------------------------------- /joint_trajectory_admittance_controller/test/rrbot_controllers.yaml: -------------------------------------------------------------------------------- 1 | rrbot_controller: 2 | type: "position_controllers/JointTrajectoryController" 3 | joints: 4 | - joint1 5 | - joint2 6 | 7 | constraints: 8 | goal_time: 0.5 9 | joint1: 10 | goal: 0.01 11 | trajectory: 0.05 12 | joint2: 13 | goal: 0.01 14 | trajectory: 0.05 15 | -------------------------------------------------------------------------------- /joint_trajectory_admittance_controller/test/tolerances.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /joint_trajectory_admittance_controller/test/tolerances.yaml: -------------------------------------------------------------------------------- 1 | constraints: 2 | goal_time: 1.0 # Defaults to zero 3 | stopped_velocity_tolerance: 0.02 # Defaults to 0.01 4 | foo_joint: 5 | trajectory: 0.05 # Not enforced if unspecified 6 | goal: 0.03 # Not enforced if unspecified 7 | bar_joint: 8 | goal: 0.01 9 | 10 | baz_joint: # Not part of joints we're looking for 11 | trajectory: 1.0 12 | goal: 2.0 13 | 14 | 15 | -------------------------------------------------------------------------------- /my_cart_controller_pkg/config/ur10_controller_setup.yaml: -------------------------------------------------------------------------------- 1 | my_controller: 2 | type: my_cart_controller_pkg/MyCartControllerPlugin 3 | joints: 4 | - shoulder_pan_joint 5 | - shoulder_lift_joint 6 | - elbow_joint 7 | - wrist_1_joint 8 | - wrist_2_joint 9 | - wrist_3_joint 10 | constraints: 11 | goal_time: 0.6 12 | stopped_velocity_tolerance: 0.05 13 | shoulder_pan_joint: {trajectory: 0.1, goal: 0.1} 14 | shoulder_lift_joint: {trajectory: 0.1, goal: 0.1} 15 | elbow_joint: {trajectory: 0.1, goal: 0.1} 16 | wrist_1_joint: {trajectory: 0.1, goal: 0.1} 17 | wrist_2_joint: {trajectory: 0.1, goal: 0.1} 18 | wrist_3_joint: {trajectory: 0.1, goal: 0.1} 19 | stop_trajectory_duration: 0.5 20 | state_publish_rate: 125 21 | action_monitor_rate: 10 22 | gains: 23 | #!!These values are useable, but maybe not optimal 24 | shoulder_pan_joint: {p: 20.0, i: 0.0, d: 0.1, i_clamp: 1} 25 | shoulder_lift_joint: {p: 20.0, i: 0.0, d: 0.1, i_clamp: 1} 26 | elbow_joint: {p: 20.0, i: 0.0, d: 0.1, i_clamp: 1} 27 | wrist_1_joint: {p: 20.0, i: 0.0, d: 0.1, i_clamp: 1} 28 | wrist_2_joint: {p: 20.0, i: 0.0, d: 0.1, i_clamp: 1} 29 | wrist_3_joint: {p: 20.0, i: 0.0, d: 0.1, i_clamp: 1} 30 | -------------------------------------------------------------------------------- /my_cart_controller_pkg/controller_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | -------------------------------------------------------------------------------- /my_cart_controller_pkg/include/my_controller_pkg/my_controller_file.h: -------------------------------------------------------------------------------- 1 | namespace my_controller_ns{ 2 | 3 | class MyControllerClass 4 | { 5 | private: 6 | pr2_mechanism_model::JointState* joint_state_; 7 | double init_pos_; 8 | 9 | public: 10 | virtual bool init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n); 11 | virtual void starting(); 12 | virtual void update(); 13 | virtual void stopping(); 14 | }; 15 | } 16 | -------------------------------------------------------------------------------- /my_cart_controller_pkg/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | my_cart_controller_pkg 4 | 0.0.0 5 | The my_controller_pkg package 6 | 7 | 8 | 9 | 10 | daniel 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 | catkin 43 | roscpp 44 | roscpp 45 | pluginlib 46 | pluginlib 47 | controller_interface 48 | controller_interface 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | -------------------------------------------------------------------------------- /my_controller_pkg/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(my_controller_pkg) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS 8 | roscpp 9 | pluginlib 10 | ) 11 | 12 | include_directories(SYSTEM ${Boost_INCLUDE_DIR} ${EIGEN_INCLUDE_DIRS}) 13 | 14 | ## System dependencies are found with CMake's conventions 15 | # find_package(Boost REQUIRED COMPONENTS system) 16 | 17 | 18 | ## Uncomment this if the package has a setup.py. This macro ensures 19 | ## modules and global scripts declared therein get installed 20 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 21 | # catkin_python_setup() 22 | 23 | ################################################ 24 | ## Declare ROS messages, services and actions ## 25 | ################################################ 26 | 27 | ## To declare and build messages, services or actions from within this 28 | ## package, follow these steps: 29 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 30 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 31 | ## * In the file package.xml: 32 | ## * add a build_depend tag for "message_generation" 33 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 34 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 35 | ## but can be declared for certainty nonetheless: 36 | ## * add a run_depend tag for "message_runtime" 37 | ## * In this file (CMakeLists.txt): 38 | ## * add "message_generation" and every package in MSG_DEP_SET to 39 | ## find_package(catkin REQUIRED COMPONENTS ...) 40 | ## * add "message_runtime" and every package in MSG_DEP_SET to 41 | ## catkin_package(CATKIN_DEPENDS ...) 42 | ## * uncomment the add_*_files sections below as needed 43 | ## and list every .msg/.srv/.action file to be processed 44 | ## * uncomment the generate_messages entry below 45 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 46 | 47 | ## Generate messages in the 'msg' folder 48 | # add_message_files( 49 | # FILES 50 | # Message1.msg 51 | # Message2.msg 52 | # ) 53 | 54 | ## Generate services in the 'srv' folder 55 | # add_service_files( 56 | # FILES 57 | # Service1.srv 58 | # Service2.srv 59 | # ) 60 | 61 | ## Generate actions in the 'action' folder 62 | # add_action_files( 63 | # FILES 64 | # Action1.action 65 | # Action2.action 66 | # ) 67 | 68 | ## Generate added messages and services with any dependencies listed here 69 | # generate_messages( 70 | # DEPENDENCIES 71 | # std_msgs # Or other packages containing msgs 72 | # ) 73 | 74 | ################################################ 75 | ## Declare ROS dynamic reconfigure parameters ## 76 | ################################################ 77 | 78 | ## To declare and build dynamic reconfigure parameters within this 79 | ## package, follow these steps: 80 | ## * In the file package.xml: 81 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 82 | ## * In this file (CMakeLists.txt): 83 | ## * add "dynamic_reconfigure" to 84 | ## find_package(catkin REQUIRED COMPONENTS ...) 85 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 86 | ## and list every .cfg file to be processed 87 | 88 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 89 | # generate_dynamic_reconfigure_options( 90 | # cfg/DynReconf1.cfg 91 | # cfg/DynReconf2.cfg 92 | # ) 93 | 94 | ################################### 95 | ## catkin specific configuration ## 96 | ################################### 97 | ## The catkin_package macro generates cmake config files for your package 98 | ## Declare things to be passed to dependent projects 99 | ## INCLUDE_DIRS: uncomment this if you package contains header files 100 | ## LIBRARIES: libraries you create in this project that dependent projects also need 101 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 102 | ## DEPENDS: system dependencies of this project that dependent projects also need 103 | catkin_package( 104 | INCLUDE_DIRS include 105 | # LIBRARIES my_controller_pkg 106 | CATKIN_DEPENDS roscpp pluginlib 107 | # DEPENDS system_lib 108 | ) 109 | 110 | ########### 111 | ## Build ## 112 | ########### 113 | 114 | ## Specify additional locations of header files 115 | ## Your package locations should be listed before other locations 116 | # include_directories(include) 117 | include_directories(include 118 | ${catkin_INCLUDE_DIRS} 119 | ) 120 | 121 | ## Declare a C++ library 122 | # add_library(my_controller_pkg 123 | # src/${PROJECT_NAME}/my_controller_pkg.cpp 124 | # ) 125 | 126 | ## Add cmake target dependencies of the library 127 | ## as an example, code may need to be generated before libraries 128 | ## either from message generation or dynamic reconfigure 129 | # add_dependencies(my_controller_pkg ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 130 | 131 | ## Declare a C++ executable 132 | # add_executable(my_controller_pkg_node src/my_controller_pkg_node.cpp) 133 | 134 | ## Add cmake target dependencies of the executable 135 | ## same as for the library above 136 | # add_dependencies(my_controller_pkg_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 137 | 138 | ## Specify libraries to link a library or executable target against 139 | # target_link_libraries(my_controller_pkg_node 140 | # ${catkin_LIBRARIES} 141 | # ) 142 | 143 | ############# 144 | ## Install ## 145 | ############# 146 | 147 | # all install targets should use catkin DESTINATION variables 148 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 149 | 150 | ## Mark executable scripts (Python etc.) for installation 151 | ## in contrast to setup.py, you can choose the destination 152 | # install(PROGRAMS 153 | # scripts/my_python_script 154 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 155 | # ) 156 | 157 | ## Mark executables and/or libraries for installation 158 | # install(TARGETS my_controller_pkg my_controller_pkg_node 159 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 160 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 161 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 162 | # ) 163 | 164 | ## Mark cpp header files for installation 165 | #install(DIRECTORY include/${PROJECT_NAME}/ 166 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 167 | # FILES_MATCHING PATTERN "*.h" 168 | # PATTERN ".svn" EXCLUDE 169 | #) 170 | 171 | install(DIRECTORY include/${PROJECT_NAME}/ 172 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 173 | FILES_MATCHING PATTERN "*.h" 174 | ) 175 | 176 | ## Mark other files for installation (e.g. launch and bag files, etc.) 177 | # install(FILES 178 | # # myfile1 179 | # # myfile2 180 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 181 | # ) 182 | 183 | ############# 184 | ## Testing ## 185 | ############# 186 | 187 | ## Add gtest based cpp test target and link libraries 188 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_my_controller_pkg.cpp) 189 | # if(TARGET ${PROJECT_NAME}-test) 190 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 191 | # endif() 192 | 193 | ## Add folders to be run by python nosetests 194 | # catkin_add_nosetests(test) 195 | 196 | add_library(my_controller_lib src/my_controller_file.cpp) 197 | target_link_libraries(my_controller_lib ${catkin_LIBRARIES}) 198 | -------------------------------------------------------------------------------- /my_controller_pkg/config/ur10_controller_setup.yaml: -------------------------------------------------------------------------------- 1 | my_controller: 2 | type: my_controller_pkg/MyControllerPlugin 3 | joint: shoulder_pan_joint 4 | -------------------------------------------------------------------------------- /my_controller_pkg/controller_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | -------------------------------------------------------------------------------- /my_controller_pkg/include/my_controller_pkg/my_controller_file.h: -------------------------------------------------------------------------------- 1 | namespace my_controller_ns{ 2 | 3 | class MyControllerClass 4 | { 5 | private: 6 | pr2_mechanism_model::JointState* joint_state_; 7 | double init_pos_; 8 | 9 | public: 10 | virtual bool init(pr2_mechanism_model::RobotState *robot, ros::NodeHandle &n); 11 | virtual void starting(); 12 | virtual void update(); 13 | virtual void stopping(); 14 | }; 15 | } 16 | -------------------------------------------------------------------------------- /my_controller_pkg/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | my_controller_pkg 4 | 0.0.0 5 | The my_controller_pkg package 6 | 7 | 8 | 9 | 10 | daniel 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 | catkin 43 | roscpp 44 | roscpp 45 | pluginlib 46 | pluginlib 47 | controller_interface 48 | controller_interface 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | -------------------------------------------------------------------------------- /my_controller_pkg/src/my_controller_file.cpp: -------------------------------------------------------------------------------- 1 | //#include "my_controller_pkg/my_controller_file.h" 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | namespace controller_ns{ 8 | 9 | class PositionController : public controller_interface::Controller 10 | { 11 | public: 12 | bool init(hardware_interface::VelocityJointInterface* hw, ros::NodeHandle &nh) 13 | { 14 | // get joint name from the parameter server 15 | std::string my_joint; 16 | if (!nh.getParam("joint", my_joint)){ 17 | ROS_ERROR("Could not find joint name"); 18 | return false; 19 | } 20 | 21 | // get the joint object to use in the realtime loop 22 | joint_ = hw->getHandle(my_joint); // throws on failure 23 | return true; 24 | } 25 | 26 | void update(const ros::Time& time, const ros::Duration& period) 27 | { 28 | //double error = setpoint_ - joint_.getPosition(); 29 | joint_.setCommand(command_); 30 | } 31 | 32 | void starting(const ros::Time& time) { } 33 | void stopping(const ros::Time& time) { } 34 | 35 | private: 36 | hardware_interface::JointHandle joint_; 37 | static const double gain_ = 1.25; 38 | static const double setpoint_ = 3.00; 39 | static const double command_ = 0.02; 40 | }; 41 | 42 | //PLUGINLIB_DECLARE_CLASS(my_controller_pkg, MyControllerPlugin, controller_ns::PositionController, controller_interface::ControllerBase); 43 | 44 | PLUGINLIB_EXPORT_CLASS(controller_ns::PositionController, controller_interface::ControllerBase) 45 | 46 | }//namespace 47 | --------------------------------------------------------------------------------