├── marm_moveit_config ├── launch │ ├── arm_moveit_sensor_manager.launch.xml │ ├── planning_pipeline.launch.xml │ ├── fake_moveit_controller_manager.launch.xml │ ├── warehouse.launch │ ├── setup_assistant.launch │ ├── arm_moveit_controller_manager.launch.xml │ ├── sensor_manager.launch.xml │ ├── joystick_control.launch │ ├── moveit_rviz.launch │ ├── warehouse_settings.launch.xml │ ├── default_warehouse_db.launch │ ├── moveit_planning_execution.launch │ ├── run_benchmark_ompl.launch │ ├── ompl_planning_pipeline.launch.xml │ ├── planning_context.launch │ ├── trajectory_execution.launch.xml │ ├── demo.launch │ ├── move_group.launch │ └── moveit.rviz ├── config │ ├── kinematics.yaml │ ├── fake_controllers.yaml │ ├── controllers.yaml │ ├── controllers_gazebo.yaml │ ├── joint_limits.yaml │ ├── arm.srdf │ └── ompl_planning.yaml ├── .setup_assistant ├── CMakeLists.txt └── package.xml ├── marm_gazebo ├── config │ ├── arm_gazebo_joint_states.yaml │ ├── trajectory_control.yaml │ └── arm_gazebo_control.yaml ├── launch │ ├── arm_gazebo_control.launch │ ├── arm_trajectory_controller.launch │ ├── arm_bringup_moveit.launch │ ├── arm_gazebo_states.launch │ ├── arm_gazebo_controller.launch │ └── arm_world.launch ├── package.xml └── CMakeLists.txt ├── marm_planning ├── src │ ├── test_random.cpp │ ├── remove_collision_objct.cpp │ ├── test_custom.cpp │ ├── test_cartesian_path.cpp │ ├── add_collision_objct.cpp │ └── check_collision.cpp ├── launch │ ├── arm_planning.launch │ └── arm_planning_with_trail.launch ├── scripts │ ├── moveit_fk_demo.py │ ├── trajectory_demo.py │ ├── moveit_ik_demo.py │ ├── moveit_cartesian_demo.py │ ├── moveit_obstacles_demo.py │ └── moveit_pick_and_place_demo.py ├── package.xml ├── CMakeLists.txt └── config │ ├── arm_paths.rviz │ └── pick_and_place.rviz └── marm_description ├── config └── arm.yaml ├── launch ├── view_arm.launch └── fake_arm.launch ├── package.xml ├── urdf.rviz ├── CMakeLists.txt └── urdf └── arm.xacro /marm_moveit_config/launch/arm_moveit_sensor_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /marm_moveit_config/config/kinematics.yaml: -------------------------------------------------------------------------------- 1 | arm: 2 | kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin 3 | kinematics_solver_search_resolution: 0.005 4 | kinematics_solver_timeout: 0.05 5 | kinematics_solver_attempts: 3 -------------------------------------------------------------------------------- /marm_gazebo/config/arm_gazebo_joint_states.yaml: -------------------------------------------------------------------------------- 1 | arm: 2 | # Publish all joint states ----------------------------------- 3 | joint_state_controller: 4 | type: joint_state_controller/JointStateController 5 | publish_rate: 50 6 | 7 | -------------------------------------------------------------------------------- /marm_gazebo/launch/arm_gazebo_control.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /marm_moveit_config/config/fake_controllers.yaml: -------------------------------------------------------------------------------- 1 | controller_list: 2 | - name: fake_arm_controller 3 | joints: 4 | - joint1 5 | - joint2 6 | - joint3 7 | - joint4 8 | - joint5 9 | - joint6 10 | - name: fake_gripper_controller 11 | joints: 12 | - finger_joint1 -------------------------------------------------------------------------------- /marm_moveit_config/.setup_assistant: -------------------------------------------------------------------------------- 1 | moveit_setup_assistant_config: 2 | URDF: 3 | package: marm_description 4 | relative_path: urdf/arm.xacro 5 | SRDF: 6 | relative_path: config/arm.srdf 7 | CONFIG: 8 | author_name: Hu Chunxu 9 | author_email: huchunxu@hust.edu.cn 10 | generated_timestamp: 1501848702 -------------------------------------------------------------------------------- /marm_gazebo/launch/arm_trajectory_controller.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /marm_moveit_config/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(marm_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 | -------------------------------------------------------------------------------- /marm_moveit_config/launch/planning_pipeline.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /marm_moveit_config/launch/fake_moveit_controller_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /marm_moveit_config/config/controllers.yaml: -------------------------------------------------------------------------------- 1 | controller_list: 2 | - name: arm_controller 3 | action_ns: follow_joint_trajectory 4 | type: FollowJointTrajectory 5 | default: true 6 | joints: 7 | - joint1 8 | - joint2 9 | - joint3 10 | - joint4 11 | - joint5 12 | - joint6 13 | 14 | - name: gripper_controller 15 | action_ns: gripper_action 16 | type: GripperCommand 17 | default: true 18 | joints: 19 | - finger_joint1 20 | 21 | -------------------------------------------------------------------------------- /marm_moveit_config/config/controllers_gazebo.yaml: -------------------------------------------------------------------------------- 1 | controller_manager_ns: controller_manager 2 | controller_list: 3 | - name: arm/arm_joint_controller 4 | action_ns: follow_joint_trajectory 5 | type: FollowJointTrajectory 6 | default: true 7 | joints: 8 | - joint1 9 | - joint2 10 | - joint3 11 | - joint4 12 | - joint5 13 | - joint6 14 | 15 | - name: arm/gripper_controller 16 | action_ns: follow_joint_trajectory 17 | type: FollowJointTrajectory 18 | default: true 19 | joints: 20 | - finger_joint1 21 | - finger_joint2 22 | -------------------------------------------------------------------------------- /marm_gazebo/launch/arm_bringup_moveit.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /marm_planning/src/test_random.cpp: -------------------------------------------------------------------------------- 1 | //首先要包含API的头文件 2 | #include 3 | 4 | int main(int argc, char **argv) 5 | { 6 | ros::init(argc, argv, "moveit_random_demo", ros::init_options::AnonymousName); 7 | // 创建一个异步的自旋线程（spinning thread） 8 | ros::AsyncSpinner spinner(1); 9 | spinner.start(); 10 | // 连接move_group节点中的机械臂实例组，这里的组名arm是我们之前在setup assistant中设置的 11 | moveit::planning_interface::MoveGroupInterface group("arm"); 12 | 13 | // 随机产生一个目标位置 14 | group.setRandomTarget(); 15 | // 开始运动规划，并且让机械臂移动到目标位置 16 | group.move(); 17 | ros::waitForShutdown(); 18 | } 19 | -------------------------------------------------------------------------------- /marm_moveit_config/launch/warehouse.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /marm_moveit_config/launch/setup_assistant.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /marm_moveit_config/launch/arm_moveit_controller_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /marm_gazebo/launch/arm_gazebo_states.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 7 | 8 | 9 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /marm_moveit_config/launch/sensor_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /marm_moveit_config/launch/joystick_control.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /marm_description/config/arm.yaml: -------------------------------------------------------------------------------- 1 | joints: { 2 | joint1: {id: 1, neutral: 205, max_angle: 169.6, min_angle: -169.6, max_speed: 90}, 3 | joint2: {id: 2, max_angle: 134.6, min_angle: -134.6, max_speed: 90}, 4 | joint3: {id: 3, max_angle: 150.1, min_angle: -150.1, max_speed: 90}, 5 | joint4: {id: 4, max_angle: 150.1, min_angle: -150.1, max_speed: 90}, 6 | joint5: {id: 5, max_angle: 150.1, min_angle: -150.1, max_speed: 90}, 7 | joint6: {id: 6, max_angle: 360, min_angle: -360, max_speed: 90}, 8 | finger_joint1: {id: 7, max_speed: 90}, 9 | } 10 | controllers: { 11 | arm_controller: {type: follow_controller, joints: [joint1, joint2, joint3, joint4, joint5, joint6], action_name: arm_controller/follow_joint_trajectory, onboard: False } 12 | } 13 | -------------------------------------------------------------------------------- /marm_moveit_config/launch/moveit_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /marm_description/launch/view_arm.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /marm_moveit_config/launch/warehouse_settings.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /marm_moveit_config/launch/default_warehouse_db.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /marm_planning/src/remove_collision_objct.cpp: -------------------------------------------------------------------------------- 1 | // 包含API的头文件 2 | #include 3 | #include 4 | 5 | int main(int argc, char **argv) 6 | { 7 | // 初始化ros节点 8 | ros::init(argc, argv, "remove_collision_objct"); 9 | ros::NodeHandle nh; 10 | 11 | ros::AsyncSpinner spin(1); 12 | spin.start(); 13 | 14 | // 创建运动规划的情景，等待创建完成 15 | moveit::planning_interface::PlanningSceneInterface current_scene; 16 | sleep(5.0); 17 | 18 | // 添加个需要删除的障碍物名称，然后通过planning scene interface完成删除 19 | std::vector object_ids; 20 | object_ids.push_back("arm_cylinder"); 21 | current_scene.removeCollisionObjects(object_ids); 22 | 23 | ros::shutdown(); 24 | 25 | return 0; 26 | } 27 | -------------------------------------------------------------------------------- /marm_moveit_config/launch/moveit_planning_execution.launch: -------------------------------------------------------------------------------- 1 | 2 | # The planning and execution components of MoveIt! configured to 3 | # publish the current configuration of the robot (simulated or real) 4 | # and the current state of the world as seen by the planner 5 | 6 | 7 | 8 | # The visualization component of MoveIt! 9 | 10 | 11 | 12 | 13 | 14 | [/arm/joint_states] 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /marm_gazebo/config/trajectory_control.yaml: -------------------------------------------------------------------------------- 1 | arm: 2 | arm_joint_controller: 3 | type: "position_controllers/JointTrajectoryController" 4 | joints: 5 | - joint1 6 | - joint2 7 | - joint3 8 | - joint4 9 | - joint5 10 | - joint6 11 | 12 | gains: 13 | joint1: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0} 14 | joint2: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0} 15 | joint3: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0} 16 | joint4: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0} 17 | joint5: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0} 18 | joint6: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0} 19 | 20 | 21 | gripper_controller: 22 | type: "position_controllers/JointTrajectoryController" 23 | joints: 24 | - finger_joint1 25 | gains: 26 | finger_joint1: {p: 50.0, d: 1.0, i: 0.01, i_clamp: 1.0} 27 | 28 | -------------------------------------------------------------------------------- /marm_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 | -------------------------------------------------------------------------------- /marm_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 | -------------------------------------------------------------------------------- /marm_gazebo/launch/arm_gazebo_controller.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 15 | 16 | 17 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /marm_planning/src/test_custom.cpp: -------------------------------------------------------------------------------- 1 | // 包含miveit的API头文件 2 | #include 3 | 4 | int main(int argc, char **argv) 5 | { 6 | ros::init(argc, argv, "moveit_custom_demo"); 7 | ros::NodeHandle node_handle; 8 | ros::AsyncSpinner spinner(1); 9 | spinner.start(); 10 | 11 | moveit::planning_interface::MoveGroupInterface group("arm"); 12 | 13 | // 设置机器人终端的目标位置 14 | geometry_msgs::Pose target_pose1; 15 | target_pose1.orientation.w = 0.726282; 16 | target_pose1.orientation.x= 4.04423e-07; 17 | target_pose1.orientation.y = -0.687396; 18 | target_pose1.orientation.z = 4.81813e-07; 19 | 20 | target_pose1.position.x = 0.03; 21 | target_pose1.position.y = 0.2; 22 | target_pose1.position.z = 0.5; 23 | group.setPoseTarget(target_pose1); 24 | 25 | // 进行运动规划，计算机器人移动到目标的运动轨迹，此时只是计算出轨迹，并不会控制机械臂运动 26 | moveit::planning_interface::MoveGroupInterface::Plan my_plan; 27 | moveit::planning_interface::MoveItErrorCode success = group.plan(my_plan); 28 | 29 | ROS_INFO("Visualizing plan 1 (pose goal) %s",success?"":"FAILED"); 30 | 31 | //让机械臂按照规划的轨迹开始运动。 32 | if(success) 33 | group.execute(my_plan); 34 | 35 | ros::shutdown(); 36 | return 0; 37 | } 38 | -------------------------------------------------------------------------------- /marm_gazebo/launch/arm_world.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 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /marm_gazebo/config/arm_gazebo_control.yaml: -------------------------------------------------------------------------------- 1 | arm: 2 | # Publish all joint states ----------------------------------- 3 | joint_state_controller: 4 | type: joint_state_controller/JointStateController 5 | publish_rate: 50 6 | 7 | # Position Controllers --------------------------------------- 8 | joint1_position_controller: 9 | type: position_controllers/JointPositionController 10 | joint: joint1 11 | pid: {p: 100.0, i: 0.01, d: 10.0} 12 | joint2_position_controller: 13 | type: position_controllers/JointPositionController 14 | joint: joint2 15 | pid: {p: 100.0, i: 0.01, d: 10.0} 16 | joint3_position_controller: 17 | type: position_controllers/JointPositionController 18 | joint: joint3 19 | pid: {p: 100.0, i: 0.01, d: 10.0} 20 | joint4_position_controller: 21 | type: position_controllers/JointPositionController 22 | joint: joint4 23 | pid: {p: 100.0, i: 0.01, d: 10.0} 24 | joint5_position_controller: 25 | type: position_controllers/JointPositionController 26 | joint: joint5 27 | pid: {p: 100.0, i: 0.01, d: 10.0} 28 | joint6_position_controller: 29 | type: position_controllers/JointPositionController 30 | joint: joint6 31 | pid: {p: 100.0, i: 0.01, d: 10.0} 32 | 33 | -------------------------------------------------------------------------------- /marm_description/launch/fake_arm.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | model: singlesided 18 | invert: false 19 | center: 0.0 20 | pad_width: 0.004 21 | finger_length: 0.08 22 | min_opening: 0.0 23 | max_opening: 0.06 24 | joint: finger_joint1 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /marm_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 | -------------------------------------------------------------------------------- /marm_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 | finger_joint1: 6 | has_velocity_limits: true 7 | max_velocity: 1 8 | has_acceleration_limits: false 9 | max_acceleration: 0 10 | joint1: 11 | has_velocity_limits: true 12 | max_velocity: 1 13 | has_acceleration_limits: false 14 | max_acceleration: 0 15 | joint2: 16 | has_velocity_limits: true 17 | max_velocity: 1 18 | has_acceleration_limits: false 19 | max_acceleration: 0 20 | joint3: 21 | has_velocity_limits: true 22 | max_velocity: 1 23 | has_acceleration_limits: false 24 | max_acceleration: 0 25 | joint4: 26 | has_velocity_limits: true 27 | max_velocity: 1 28 | has_acceleration_limits: false 29 | max_acceleration: 0 30 | joint5: 31 | has_velocity_limits: true 32 | max_velocity: 1 33 | has_acceleration_limits: false 34 | max_acceleration: 0 35 | joint6: 36 | has_velocity_limits: true 37 | max_velocity: 1 38 | has_acceleration_limits: false 39 | max_acceleration: 0 -------------------------------------------------------------------------------- /marm_planning/launch/arm_planning.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | model: singlesided 18 | invert: false 19 | center: 0.0 20 | pad_width: 0.004 21 | finger_length: 0.08 22 | min_opening: 0.0 23 | max_opening: 0.06 24 | joint: finger_joint1 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /marm_planning/launch/arm_planning_with_trail.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | model: singlesided 18 | invert: false 19 | center: 0.0 20 | pad_width: 0.004 21 | finger_length: 0.08 22 | min_opening: 0.0 23 | max_opening: 0.06 24 | joint: finger_joint1 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /marm_planning/src/test_cartesian_path.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | int main(int argc, char **argv) 5 | { 6 | ros::init(argc, argv, "cartesian_path_demo", ros::init_options::AnonymousName); 7 | 8 | // 创建一个异步的自旋线程（spinning thread） 9 | ros::AsyncSpinner spinner(1); 10 | spinner.start(); 11 | 12 | moveit::planning_interface::MoveGroupInterface move_group("arm"); 13 | 14 | // 设置一个目标位置（路点） 15 | geometry_msgs::Pose target_pose; 16 | target_pose.orientation.w = 0.726282; 17 | target_pose.orientation.x = 4.04423e-07; 18 | target_pose.orientation.y = -0.687396; 19 | target_pose.orientation.z = 4.81813e-07; 20 | target_pose.position.x = 0.03; 21 | target_pose.position.y = 0.2; 22 | target_pose.position.z = 0.5; 23 | std::vector waypoints; 24 | waypoints.push_back(target_pose); 25 | 26 | // 笛卡尔空间下的路径规划 27 | moveit_msgs::RobotTrajectory trajectory; 28 | const double jump_threshold = 0.0; 29 | const double eef_step = 0.01; 30 | double fraction = move_group.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory); 31 | 32 | // 生成机械臂的运动规划数据 33 | moveit::planning_interface::MoveGroupInterface::Plan plan; 34 | plan.trajectory_ = trajectory; 35 | 36 | // 执行运动 37 | move_group.execute(plan); 38 | 39 | ros::shutdown(); 40 | return 0; 41 | } 42 | -------------------------------------------------------------------------------- /marm_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 | 20 | 21 | -------------------------------------------------------------------------------- /marm_moveit_config/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | marm_moveit_config 4 | 0.3.0 5 | 6 | An automatically generated package with all the configuration and launch files for using the arm with the MoveIt! Motion Planning Framework 7 | 8 | Hu Chunxu 9 | Hu Chunxu 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 | 27 | 28 | marm_description 29 | marm_description 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /marm_planning/scripts/moveit_fk_demo.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | import rospy, sys 5 | import moveit_commander 6 | from control_msgs.msg import GripperCommand 7 | 8 | class MoveItFkDemo: 9 | def __init__(self): 10 | # 初始化move_group的API 11 | moveit_commander.roscpp_initialize(sys.argv) 12 | 13 | # 初始化ROS节点 14 | rospy.init_node('moveit_fk_demo', anonymous=True) 15 | 16 | # 初始化需要使用move group控制的机械臂中的arm group 17 | arm = moveit_commander.MoveGroupCommander('arm') 18 | 19 | # 初始化需要使用move group控制的机械臂中的gripper group 20 | gripper = moveit_commander.MoveGroupCommander('gripper') 21 | 22 | # 设置机械臂和夹爪的允许误差值 23 | arm.set_goal_joint_tolerance(0.001) 24 | gripper.set_goal_joint_tolerance(0.001) 25 | 26 | # 控制机械臂先回到初始化位置 27 | arm.set_named_target('home') 28 | arm.go() 29 | rospy.sleep(2) 30 | 31 | # 设置夹爪的目标位置，并控制夹爪运动 32 | gripper.set_joint_value_target([0.01]) 33 | gripper.go() 34 | rospy.sleep(1) 35 | 36 | # 设置机械臂的目标位置，使用六轴的位置数据进行描述（单位：弧度） 37 | joint_positions = [-0.0867, -1.274, 0.02832, 0.0820, -1.273, -0.003] 38 | arm.set_joint_value_target(joint_positions) 39 | 40 | # 控制机械臂完成运动 41 | arm.go() 42 | rospy.sleep(1) 43 | 44 | # 关闭并退出moveit 45 | moveit_commander.roscpp_shutdown() 46 | moveit_commander.os._exit(0) 47 | 48 | if __name__ == "__main__": 49 | try: 50 | MoveItFkDemo() 51 | except rospy.ROSInterruptException: 52 | pass 53 | -------------------------------------------------------------------------------- /marm_planning/src/add_collision_objct.cpp: -------------------------------------------------------------------------------- 1 | // 包含API的头文件 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | int main(int argc, char **argv) 8 | { 9 | ros::init(argc, argv, "add_collision_objct"); 10 | ros::NodeHandle nh; 11 | ros::AsyncSpinner spin(1); 12 | spin.start(); 13 | 14 | // 创建运动规划的情景，等待创建完成 15 | moveit::planning_interface::PlanningSceneInterface current_scene; 16 | sleep(5.0); 17 | 18 | // 声明一个障碍物的实例，并且为其设置一个id，方便对其进行操作，该实例会发布到当前的情景实例中 19 | moveit_msgs::CollisionObject cylinder; 20 | cylinder.id = "arm_cylinder"; 21 | 22 | // 设置障碍物的外形、尺寸等属性 23 | shape_msgs::SolidPrimitive primitive; 24 | primitive.type = primitive.CYLINDER; 25 | primitive.dimensions.resize(3); 26 | primitive.dimensions[0] = 0.6; 27 | primitive.dimensions[1] = 0.2; 28 | 29 | // 设置障碍物的位置 30 | geometry_msgs::Pose pose; 31 | pose.orientation.w = 1.0; 32 | pose.position.x = 0.0; 33 | pose.position.y = -0.4; 34 | pose.position.z = 0.4; 35 | 36 | // 将障碍物的属性、位置加入到障碍物的实例中 37 | cylinder.primitives.push_back(primitive); 38 | cylinder.primitive_poses.push_back(pose); 39 | cylinder.operation = cylinder.ADD; 40 | 41 | // 创建一个障碍物的列表，把之前创建的障碍物实例加入其中 42 | std::vector collision_objects; 43 | collision_objects.push_back(cylinder); 44 | 45 | // 所有障碍物加入列表后（这里只有一个障碍物），再把障碍物加入到当前的情景中，如果要删除障碍物，使用removeCollisionObjects(collision_objects) 46 | current_scene.addCollisionObjects(collision_objects); 47 | 48 | ros::shutdown(); 49 | 50 | return 0; 51 | } 52 | -------------------------------------------------------------------------------- /marm_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | marm_description 4 | 0.0.0 5 | The marm_description package 6 | 7 | 8 | 9 | 10 | hcx 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 | tf 45 | urdf 46 | xacro 47 | roscpp 48 | tf 49 | urdf 50 | xacro 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | -------------------------------------------------------------------------------- /marm_gazebo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | marm_gazebo 4 | 0.0.0 5 | The marm_gazebo package 6 | 7 | 8 | 9 | 10 | hcx 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 | gazebo_msgs 44 | gazebo_plugins 45 | gazebo_ros 46 | gazebo_ros_control 47 | gazebo_msgs 48 | gazebo_plugins 49 | gazebo_ros 50 | gazebo_ros_control 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | -------------------------------------------------------------------------------- /marm_planning/scripts/trajectory_demo.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | import rospy 5 | import actionlib 6 | 7 | from control_msgs.msg import FollowJointTrajectoryAction, FollowJointTrajectoryGoal 8 | from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint 9 | 10 | class TrajectoryDemo(): 11 | def __init__(self): 12 | rospy.init_node('trajectory_demo') 13 | 14 | # 是否需要回到初始化的位置 15 | reset = rospy.get_param('~reset', False) 16 | 17 | # 机械臂中joint的命名 18 | arm_joints = ['joint1', 19 | 'joint2', 20 | 'joint3', 21 | 'joint4', 22 | 'joint5', 23 | 'joint6'] 24 | 25 | if reset: 26 | # 如果需要回到初始化位置，需要将目标位置设置为初始化位置的六轴角度 27 | arm_goal = [0, 0, 0, 0, 0, 0] 28 | 29 | else: 30 | # 如果不需要回初始化位置，则设置目标位置的六轴角度 31 | arm_goal = [-0.3, -1.0, 0.5, 0.8, 1.0, -0.7] 32 | 33 | # 连接机械臂轨迹规划的trajectory action server 34 | rospy.loginfo('Waiting for arm trajectory controller...') 35 | arm_client = actionlib.SimpleActionClient('arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction) 36 | arm_client.wait_for_server() 37 | rospy.loginfo('...connected.') 38 | 39 | # 使用设置的目标位置创建一条轨迹数据 40 | arm_trajectory = JointTrajectory() 41 | arm_trajectory.joint_names = arm_joints 42 | arm_trajectory.points.append(JointTrajectoryPoint()) 43 | arm_trajectory.points[0].positions = arm_goal 44 | arm_trajectory.points[0].velocities = [0.0 for i in arm_joints] 45 | arm_trajectory.points[0].accelerations = [0.0 for i in arm_joints] 46 | arm_trajectory.points[0].time_from_start = rospy.Duration(3.0) 47 | 48 | rospy.loginfo('Moving the arm to goal position...') 49 | 50 | # 创建一个轨迹目标的空对象 51 | arm_goal = FollowJointTrajectoryGoal() 52 | 53 | # 将之前创建好的轨迹数据加入轨迹目标对象中 54 | arm_goal.trajectory = arm_trajectory 55 | 56 | # 设置执行时间的允许误差值 57 | arm_goal.goal_time_tolerance = rospy.Duration(0.0) 58 | 59 | # 将轨迹目标发送到action server进行处理，实现机械臂的运动控制 60 | arm_client.send_goal(arm_goal) 61 | 62 | # 等待机械臂运动结束 63 | arm_client.wait_for_result(rospy.Duration(5.0)) 64 | 65 | rospy.loginfo('...done') 66 | 67 | if __name__ == '__main__': 68 | try: 69 | TrajectoryDemo() 70 | except rospy.ROSInterruptException: 71 | pass 72 | 73 | -------------------------------------------------------------------------------- /marm_moveit_config/launch/demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | [/move_group/fake_controller_joint_states] 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 | -------------------------------------------------------------------------------- /marm_planning/scripts/moveit_ik_demo.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | import rospy, sys 5 | import moveit_commander 6 | from moveit_msgs.msg import RobotTrajectory 7 | from trajectory_msgs.msg import JointTrajectoryPoint 8 | 9 | from geometry_msgs.msg import PoseStamped, Pose 10 | from tf.transformations import euler_from_quaternion, quaternion_from_euler 11 | 12 | class MoveItIkDemo: 13 | def __init__(self): 14 | # 初始化move_group的API 15 | moveit_commander.roscpp_initialize(sys.argv) 16 | 17 | # 初始化ROS节点 18 | rospy.init_node('moveit_ik_demo') 19 | 20 | # 初始化需要使用move group控制的机械臂中的arm group 21 | arm = moveit_commander.MoveGroupCommander('arm') 22 | 23 | # 获取终端link的名称 24 | end_effector_link = arm.get_end_effector_link() 25 | 26 | # 设置目标位置所使用的参考坐标系 27 | reference_frame = 'base_link' 28 | arm.set_pose_reference_frame(reference_frame) 29 | 30 | # 当运动规划失败后，允许重新规划 31 | arm.allow_replanning(True) 32 | 33 | # 设置位置(单位：米)和姿态（单位：弧度）的允许误差 34 | arm.set_goal_position_tolerance(0.01) 35 | arm.set_goal_orientation_tolerance(0.05) 36 | 37 | # 控制机械臂先回到初始化位置 38 | arm.set_named_target('home') 39 | arm.go() 40 | rospy.sleep(2) 41 | 42 | # 设置机械臂工作空间中的目标位姿，位置使用x、y、z坐标描述， 43 | # 姿态使用四元数描述，基于base_link坐标系 44 | target_pose = PoseStamped() 45 | target_pose.header.frame_id = reference_frame 46 | target_pose.header.stamp = rospy.Time.now() 47 | target_pose.pose.position.x = 0.191995 48 | target_pose.pose.position.y = 0.213868 49 | target_pose.pose.position.z = 0.520436 50 | target_pose.pose.orientation.x = 0.911822 51 | target_pose.pose.orientation.y = -0.0269758 52 | target_pose.pose.orientation.z = 0.285694 53 | target_pose.pose.orientation.w = -0.293653 54 | 55 | # 设置机器臂当前的状态作为运动初始状态 56 | arm.set_start_state_to_current_state() 57 | 58 | # 设置机械臂终端运动的目标位姿 59 | arm.set_pose_target(target_pose, end_effector_link) 60 | 61 | # 规划运动路径 62 | traj = arm.plan() 63 | 64 | # 按照规划的运动路径控制机械臂运动 65 | arm.execute(traj) 66 | rospy.sleep(1) 67 | 68 | # 控制机械臂终端向右移动5cm 69 | arm.shift_pose_target(1, -0.05, end_effector_link) 70 | arm.go() 71 | rospy.sleep(1) 72 | 73 | # 控制机械臂终端反向旋转90度 74 | arm.shift_pose_target(3, -1.57, end_effector_link) 75 | arm.go() 76 | rospy.sleep(1) 77 | 78 | # 控制机械臂回到初始化位置 79 | arm.set_named_target('home') 80 | arm.go() 81 | 82 | # 关闭并退出moveit 83 | moveit_commander.roscpp_shutdown() 84 | moveit_commander.os._exit(0) 85 | 86 | if __name__ == "__main__": 87 | MoveItIkDemo() 88 | 89 | 90 | 91 | -------------------------------------------------------------------------------- /marm_planning/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | marm_planning 4 | 0.0.0 5 | The marm_planning package 6 | 7 | 8 | 9 | 10 | hcx 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 | actionlib 44 | control_msgs 45 | diagnostic_msgs 46 | geometry_msgs 47 | manipulation_msgs 48 | moveit_msgs 49 | roscpp 50 | rospy 51 | sensor_msgs 52 | shape_msgs 53 | std_msgs 54 | std_srvs 55 | tf 56 | trajectory_msgs 57 | actionlib 58 | control_msgs 59 | diagnostic_msgs 60 | geometry_msgs 61 | manipulation_msgs 62 | moveit_msgs 63 | roscpp 64 | rospy 65 | sensor_msgs 66 | shape_msgs 67 | std_msgs 68 | std_srvs 69 | tf 70 | trajectory_msgs 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | -------------------------------------------------------------------------------- /marm_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 | 56 | 57 | 58 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | -------------------------------------------------------------------------------- /marm_moveit_config/config/arm.srdf: -------------------------------------------------------------------------------- 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 | -------------------------------------------------------------------------------- /marm_planning/src/check_collision.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | // 包含moveit的API 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | int main(int argc, char **argv) 9 | { 10 | ros::init (argc, argv, "check_collision"); 11 | ros::AsyncSpinner spinner(1); 12 | spinner.start(); 13 | 14 | // 加载机器人的运动学模型到情景实例中 15 | robot_model_loader::RobotModelLoader robot_model_loader("robot_description"); 16 | robot_model::RobotModelPtr kinematic_model = robot_model_loader.getModel(); 17 | planning_scene::PlanningScene planning_scene(kinematic_model); 18 | 19 | // 自身碰撞检测 20 | // 首先需要创建一个碰撞检测的请求对象和响应对象，然后调用碰撞检测的API checkSelfCollision，检测结果会放到collision_result中 21 | collision_detection::CollisionRequest collision_request; 22 | collision_detection::CollisionResult collision_result; 23 | planning_scene.checkSelfCollision(collision_request, collision_result); 24 | ROS_INFO_STREAM("1. Self collision Test: "<< (collision_result.collision ? "in" : "not in") 25 | << " self collision"); 26 | 27 | // 修改机器人的状态 28 | // 我们可以使用场景实例的getCurrentStateNonConst()获取当前机器人的状态，然后修改机器人的状态到一个随机的位置， 29 | // 清零collision_result的结果后再次检测机器人是否发生滋生碰撞 30 | robot_state::RobotState& current_state = planning_scene.getCurrentStateNonConst(); 31 | current_state.setToRandomPositions(); 32 | collision_result.clear(); 33 | planning_scene.checkSelfCollision(collision_request, collision_result); 34 | ROS_INFO_STREAM("2. Self collision Test(Change the state): "<< (collision_result.collision ? "in" : "not in")); 35 | 36 | // 我们也可以指定查询一个group是否和其他部分发生碰撞，只需要在collision_request中修改group_name属性 37 | collision_request.group_name = "arm"; 38 | current_state.setToRandomPositions(); 39 | collision_result.clear(); 40 | planning_scene.checkSelfCollision(collision_request, collision_result); 41 | ROS_INFO_STREAM("3. Self collision Test(In a group): "<< (collision_result.collision ? "in" : "not in")); 42 | 43 | // 获取碰撞关系 44 | // 首先，我们先让机器人发生自身碰撞 45 | std::vector joint_values; 46 | const robot_model::JointModelGroup* joint_model_group = 47 | current_state.getJointModelGroup("arm"); 48 | current_state.copyJointGroupPositions(joint_model_group, joint_values); 49 | joint_values[2] = 1.57; //原来的代码这里是joint_values[0]，并不会导致碰撞，我改成了joint_values[2]，在该状态下机器人会发生碰撞 50 | current_state.setJointGroupPositions(joint_model_group, joint_values); 51 | ROS_INFO_STREAM("4. Collision points " 52 | << (current_state.satisfiesBounds(joint_model_group) ? "valid" : "not valid")); 53 | 54 | // 然后我们再来检测机器人是否发生了自身碰撞，已经发生碰撞的是哪两个部分 55 | collision_request.contacts = true; 56 | collision_request.max_contacts = 1000; 57 | collision_result.clear(); 58 | planning_scene.checkSelfCollision(collision_request, collision_result); 59 | ROS_INFO_STREAM("5. Self collision Test: "<< (collision_result.collision ? "in" : "not in") 60 | << " self collision"); 61 | 62 | collision_detection::CollisionResult::ContactMap::const_iterator it; 63 | for(it = collision_result.contacts.begin(); 64 | it != collision_result.contacts.end(); 65 | ++it) 66 | { 67 | ROS_INFO("6 . Contact between: %s and %s", 68 | it->first.first.c_str(), 69 | it->first.second.c_str()); 70 | } 71 | 72 | // 修改允许碰撞矩阵（Allowed Collision Matrix，acm) 73 | // 我们可以通过修改acm来指定机器人是否检测自身碰撞和与障碍物的碰撞，在不检测的状态下，即使发生碰撞，检测结果也会显示未发生碰撞 74 | collision_detection::AllowedCollisionMatrix acm = planning_scene.getAllowedCollisionMatrix(); 75 | robot_state::RobotState copied_state = planning_scene.getCurrentState(); 76 | collision_detection::CollisionResult::ContactMap::const_iterator it2; 77 | for(it2 = collision_result.contacts.begin(); 78 | it2 != collision_result.contacts.end(); 79 | ++it2) 80 | { 81 | acm.setEntry(it2->first.first, it2->first.second, true); 82 | } 83 | collision_result.clear(); 84 | planning_scene.checkSelfCollision(collision_request, collision_result, copied_state, acm); 85 | 86 | ROS_INFO_STREAM("6. Self collision Test after modified ACM: "<< (collision_result.collision ? "in" : "not in") 87 | << " self collision"); 88 | 89 | // 完整的碰撞检测 90 | // 同时检测自身碰撞和与障碍物的碰撞 91 | collision_result.clear(); 92 | planning_scene.checkCollision(collision_request, collision_result, copied_state, acm); 93 | 94 | ROS_INFO_STREAM("6. Full collision Test: "<< (collision_result.collision ? "in" : "not in") 95 | << " collision"); 96 | 97 | ros::shutdown(); 98 | return 0; 99 | } 100 | -------------------------------------------------------------------------------- /marm_planning/scripts/moveit_cartesian_demo.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | import rospy, sys 5 | import moveit_commander 6 | from moveit_commander import MoveGroupCommander 7 | from geometry_msgs.msg import Pose 8 | from copy import deepcopy 9 | 10 | class MoveItCartesianDemo: 11 | def __init__(self): 12 | # 初始化move_group的API 13 | moveit_commander.roscpp_initialize(sys.argv) 14 | 15 | # 初始化ROS节点 16 | rospy.init_node('moveit_cartesian_demo', anonymous=True) 17 | 18 | # 是否需要使用笛卡尔空间的运动规划 19 | cartesian = rospy.get_param('~cartesian', True) 20 | 21 | # 初始化需要使用move group控制的机械臂中的arm group 22 | arm = MoveGroupCommander('arm') 23 | 24 | # 当运动规划失败后，允许重新规划 25 | arm.allow_replanning(True) 26 | 27 | # 设置目标位置所使用的参考坐标系 28 | arm.set_pose_reference_frame('base_link') 29 | 30 | # 设置位置(单位：米)和姿态（单位：弧度）的允许误差 31 | arm.set_goal_position_tolerance(0.01) 32 | arm.set_goal_orientation_tolerance(0.1) 33 | 34 | # 获取终端link的名称 35 | end_effector_link = arm.get_end_effector_link() 36 | 37 | # 控制机械臂运动到之前设置的“forward”姿态 38 | arm.set_named_target('forward') 39 | arm.go() 40 | 41 | # 获取当前位姿数据最为机械臂运动的起始位姿 42 | start_pose = arm.get_current_pose(end_effector_link).pose 43 | 44 | # 初始化路点列表 45 | waypoints = [] 46 | 47 | # 将初始位姿加入路点列表 48 | if cartesian: 49 | waypoints.append(start_pose) 50 | 51 | # 设置第二个路点数据，并加入路点列表 52 | # 第二个路点需要向后运动0.2米，向右运动0.2米 53 | wpose = deepcopy(start_pose) 54 | wpose.position.x -= 0.2 55 | wpose.position.y -= 0.2 56 | 57 | if cartesian: 58 | waypoints.append(deepcopy(wpose)) 59 | else: 60 | arm.set_pose_target(wpose) 61 | arm.go() 62 | rospy.sleep(1) 63 | 64 | # 设置第三个路点数据，并加入路点列表 65 | wpose.position.x += 0.05 66 | wpose.position.y += 0.15 67 | wpose.position.z -= 0.15 68 | 69 | if cartesian: 70 | waypoints.append(deepcopy(wpose)) 71 | else: 72 | arm.set_pose_target(wpose) 73 | arm.go() 74 | rospy.sleep(1) 75 | 76 | # 设置第四个路点数据，回到初始位置，并加入路点列表 77 | if cartesian: 78 | waypoints.append(deepcopy(start_pose)) 79 | else: 80 | arm.set_pose_target(start_pose) 81 | arm.go() 82 | rospy.sleep(1) 83 | 84 | if cartesian: 85 | fraction = 0.0 #路径规划覆盖率 86 | maxtries = 100 #最大尝试规划次数 87 | attempts = 0 #已经尝试规划次数 88 | 89 | # 设置机器臂当前的状态作为运动初始状态 90 | arm.set_start_state_to_current_state() 91 | 92 | # 尝试规划一条笛卡尔空间下的路径，依次通过所有路点 93 | while fraction < 1.0 and attempts < maxtries: 94 | (plan, fraction) = arm.compute_cartesian_path ( 95 | waypoints, # waypoint poses，路点列表 96 | 0.01, # eef_step，终端步进值 97 | 0.0, # jump_threshold，跳跃阈值 98 | True) # avoid_collisions，避障规划 99 | 100 | # 尝试次数累加 101 | attempts += 1 102 | 103 | # 打印运动规划进程 104 | if attempts % 10 == 0: 105 | rospy.loginfo("Still trying after " + str(attempts) + " attempts...") 106 | 107 | # 如果路径规划成功（覆盖率100%）,则开始控制机械臂运动 108 | if fraction == 1.0: 109 | rospy.loginfo("Path computed successfully. Moving the arm.") 110 | arm.execute(plan) 111 | rospy.loginfo("Path execution complete.") 112 | # 如果路径规划失败，则打印失败信息 113 | else: 114 | rospy.loginfo("Path planning failed with only " + str(fraction) + " success after " + str(maxtries) + " attempts.") 115 | 116 | # 控制机械臂回到初始化位置 117 | arm.set_named_target('home') 118 | arm.go() 119 | rospy.sleep(1) 120 | 121 | # 关闭并退出moveit 122 | moveit_commander.roscpp_shutdown() 123 | moveit_commander.os._exit(0) 124 | 125 | if __name__ == "__main__": 126 | try: 127 | MoveItCartesianDemo() 128 | except rospy.ROSInterruptException: 129 | pass 130 | -------------------------------------------------------------------------------- /marm_planning/scripts/moveit_obstacles_demo.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | import rospy, sys 5 | import moveit_commander 6 | from moveit_commander import MoveGroupCommander, PlanningSceneInterface 7 | from moveit_msgs.msg import PlanningScene, ObjectColor 8 | from geometry_msgs.msg import PoseStamped, Pose 9 | 10 | class MoveItObstaclesDemo: 11 | def __init__(self): 12 | # 初始化move_group的API 13 | moveit_commander.roscpp_initialize(sys.argv) 14 | 15 | # 初始化ROS节点 16 | rospy.init_node('moveit_obstacles_demo') 17 | 18 | # 初始化场景对象 19 | scene = PlanningSceneInterface() 20 | 21 | # 创建一个发布场景变化信息的发布者 22 | self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5) 23 | 24 | # 创建一个存储物体颜色的字典对象 25 | self.colors = dict() 26 | 27 | # 等待场景准备就绪 28 | rospy.sleep(1) 29 | 30 | # 初始化需要使用move group控制的机械臂中的arm group 31 | arm = MoveGroupCommander('arm') 32 | 33 | # 获取终端link的名称 34 | end_effector_link = arm.get_end_effector_link() 35 | 36 | # 设置位置(单位：米)和姿态（单位：弧度）的允许误差 37 | arm.set_goal_position_tolerance(0.01) 38 | arm.set_goal_orientation_tolerance(0.05) 39 | 40 | # 当运动规划失败后，允许重新规划 41 | arm.allow_replanning(True) 42 | 43 | # 设置目标位置所使用的参考坐标系 44 | reference_frame = 'base_link' 45 | arm.set_pose_reference_frame(reference_frame) 46 | 47 | # 设置每次运动规划的时间限制：5s 48 | arm.set_planning_time(5) 49 | 50 | # 设置场景物体的名称 51 | table_id = 'table' 52 | box1_id = 'box1' 53 | box2_id = 'box2' 54 | 55 | # 移除场景中之前运行残留的物体 56 | scene.remove_world_object(table_id) 57 | scene.remove_world_object(box1_id) 58 | scene.remove_world_object(box2_id) 59 | rospy.sleep(1) 60 | 61 | # 控制机械臂先回到初始化位置 62 | arm.set_named_target('home') 63 | arm.go() 64 | rospy.sleep(2) 65 | 66 | # 设置桌面的高度 67 | table_ground = 0.25 68 | 69 | # 设置table、box1和box2的三维尺寸 70 | table_size = [0.2, 0.7, 0.01] 71 | box1_size = [0.1, 0.05, 0.05] 72 | box2_size = [0.05, 0.05, 0.15] 73 | 74 | # 将三个物体加入场景当中 75 | table_pose = PoseStamped() 76 | table_pose.header.frame_id = reference_frame 77 | table_pose.pose.position.x = 0.26 78 | table_pose.pose.position.y = 0.0 79 | table_pose.pose.position.z = table_ground + table_size[2] / 2.0 80 | table_pose.pose.orientation.w = 1.0 81 | scene.add_box(table_id, table_pose, table_size) 82 | 83 | box1_pose = PoseStamped() 84 | box1_pose.header.frame_id = reference_frame 85 | box1_pose.pose.position.x = 0.21 86 | box1_pose.pose.position.y = -0.1 87 | box1_pose.pose.position.z = table_ground + table_size[2] + box1_size[2] / 2.0 88 | box1_pose.pose.orientation.w = 1.0 89 | scene.add_box(box1_id, box1_pose, box1_size) 90 | 91 | box2_pose = PoseStamped() 92 | box2_pose.header.frame_id = reference_frame 93 | box2_pose.pose.position.x = 0.19 94 | box2_pose.pose.position.y = 0.15 95 | box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0 96 | box2_pose.pose.orientation.w = 1.0 97 | scene.add_box(box2_id, box2_pose, box2_size) 98 | 99 | # 将桌子设置成红色，两个box设置成橙色 100 | self.setColor(table_id, 0.8, 0, 0, 1.0) 101 | self.setColor(box1_id, 0.8, 0.4, 0, 1.0) 102 | self.setColor(box2_id, 0.8, 0.4, 0, 1.0) 103 | 104 | # 将场景中的颜色设置发布 105 | self.sendColors() 106 | 107 | # 设置机械臂的运动目标位置，位于桌面之上两个盒子之间 108 | target_pose = PoseStamped() 109 | target_pose.header.frame_id = reference_frame 110 | target_pose.pose.position.x = 0.2 111 | target_pose.pose.position.y = 0.0 112 | target_pose.pose.position.z = table_pose.pose.position.z + table_size[2] + 0.05 113 | target_pose.pose.orientation.w = 1.0 114 | 115 | # 控制机械臂运动到目标位置 116 | arm.set_pose_target(target_pose, end_effector_link) 117 | arm.go() 118 | rospy.sleep(2) 119 | 120 | # 设置机械臂的运动目标位置，进行避障规划 121 | target_pose2 = PoseStamped() 122 | target_pose2.header.frame_id = reference_frame 123 | target_pose2.pose.position.x = 0.2 124 | target_pose2.pose.position.y = -0.25 125 | target_pose2.pose.position.z = table_pose.pose.position.z + table_size[2] + 0.05 126 | target_pose2.pose.orientation.w = 1.0 127 | 128 | # 控制机械臂运动到目标位置 129 | arm.set_pose_target(target_pose2, end_effector_link) 130 | arm.go() 131 | rospy.sleep(2) 132 | 133 | # 控制机械臂回到初始化位置 134 | arm.set_named_target('home') 135 | arm.go() 136 | 137 | # 关闭并退出moveit 138 | moveit_commander.roscpp_shutdown() 139 | moveit_commander.os._exit(0) 140 | 141 | # 设置场景物体的颜色 142 | def setColor(self, name, r, g, b, a = 0.9): 143 | # 初始化moveit颜色对象 144 | color = ObjectColor() 145 | 146 | # 设置颜色值 147 | color.id = name 148 | color.color.r = r 149 | color.color.g = g 150 | color.color.b = b 151 | color.color.a = a 152 | 153 | # 更新颜色字典 154 | self.colors[name] = color 155 | 156 | # 将颜色设置发送并应用到moveit场景当中 157 | def sendColors(self): 158 | # 初始化规划场景对象 159 | p = PlanningScene() 160 | 161 | # 需要设置规划场景是否有差异 162 | p.is_diff = True 163 | 164 | # 从颜色字典中取出颜色设置 165 | for color in self.colors.values(): 166 | p.object_colors.append(color) 167 | 168 | # 发布场景物体颜色设置 169 | self.scene_pub.publish(p) 170 | 171 | if __name__ == "__main__": 172 | try: 173 | MoveItObstaclesDemo() 174 | except KeyboardInterrupt: 175 | raise 176 | 177 | 178 | 179 | -------------------------------------------------------------------------------- /marm_description/urdf.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | Splitter Ratio: 0.5 10 | Tree Height: 503 11 | - Class: rviz/Selection 12 | Name: Selection 13 | - Class: rviz/Tool Properties 14 | Expanded: 15 | - /2D Pose Estimate1 16 | - /2D Nav Goal1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.588679016 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz/Time 26 | Experimental: false 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: "" 30 | Visualization Manager: 31 | Class: "" 32 | Displays: 33 | - Alpha: 0.5 34 | Cell Size: 1 35 | Class: rviz/Grid 36 | Color: 160; 160; 164 37 | Enabled: true 38 | Line Style: 39 | Line Width: 0.0299999993 40 | Value: Lines 41 | Name: Grid 42 | Normal Cell Count: 0 43 | Offset: 44 | X: 0 45 | Y: 0 46 | Z: 0 47 | Plane: XY 48 | Plane Cell Count: 10 49 | Reference Frame: 50 | Value: true 51 | - Alpha: 1 52 | Class: rviz/RobotModel 53 | Collision Enabled: false 54 | Enabled: true 55 | Links: 56 | All Links Enabled: true 57 | Expand Joint Details: false 58 | Expand Link Details: false 59 | Expand Tree: false 60 | Link Tree Style: Links in Alphabetic Order 61 | base_link: 62 | Alpha: 1 63 | Show Axes: false 64 | Show Trail: false 65 | Value: true 66 | bottom_link: 67 | Alpha: 1 68 | Show Axes: false 69 | Show Trail: false 70 | Value: true 71 | grasping_frame: 72 | Alpha: 1 73 | Show Axes: false 74 | Show Trail: false 75 | gripper_finger_link1: 76 | Alpha: 1 77 | Show Axes: false 78 | Show Trail: false 79 | Value: true 80 | gripper_finger_link2: 81 | Alpha: 1 82 | Show Axes: false 83 | Show Trail: false 84 | Value: true 85 | link1: 86 | Alpha: 1 87 | Show Axes: false 88 | Show Trail: false 89 | Value: true 90 | link2: 91 | Alpha: 1 92 | Show Axes: false 93 | Show Trail: false 94 | Value: true 95 | link3: 96 | Alpha: 1 97 | Show Axes: false 98 | Show Trail: false 99 | Value: true 100 | link4: 101 | Alpha: 1 102 | Show Axes: false 103 | Show Trail: false 104 | Value: true 105 | link5: 106 | Alpha: 1 107 | Show Axes: false 108 | Show Trail: false 109 | Value: true 110 | link6: 111 | Alpha: 1 112 | Show Axes: false 113 | Show Trail: false 114 | Value: true 115 | Name: RobotModel 116 | Robot Description: robot_description 117 | TF Prefix: "" 118 | Update Interval: 0 119 | Value: true 120 | Visual Enabled: true 121 | - Class: rviz/TF 122 | Enabled: false 123 | Frame Timeout: 15 124 | Frames: 125 | All Enabled: true 126 | Marker Scale: 1 127 | Name: TF 128 | Show Arrows: true 129 | Show Axes: true 130 | Show Names: true 131 | Tree: 132 | {} 133 | Update Interval: 0 134 | Value: false 135 | Enabled: true 136 | Global Options: 137 | Background Color: 48; 48; 48 138 | Fixed Frame: base_link 139 | Frame Rate: 30 140 | Name: root 141 | Tools: 142 | - Class: rviz/Interact 143 | Hide Inactive Objects: true 144 | - Class: rviz/MoveCamera 145 | - Class: rviz/Select 146 | - Class: rviz/FocusCamera 147 | - Class: rviz/Measure 148 | - Class: rviz/SetInitialPose 149 | Topic: /initialpose 150 | - Class: rviz/SetGoal 151 | Topic: /move_base_simple/goal 152 | - Class: rviz/PublishPoint 153 | Single click: true 154 | Topic: /clicked_point 155 | Value: true 156 | Views: 157 | Current: 158 | Class: rviz/Orbit 159 | Distance: 1.40504098 160 | Enable Stereo Rendering: 161 | Stereo Eye Separation: 0.0599999987 162 | Stereo Focal Distance: 1 163 | Swap Stereo Eyes: false 164 | Value: false 165 | Focal Point: 166 | X: -0.138732001 167 | Y: -0.0432205014 168 | Z: 0.292795986 169 | Focal Shape Fixed Size: true 170 | Focal Shape Size: 0.0500000007 171 | Name: Current View 172 | Near Clip Distance: 0.00999999978 173 | Pitch: 0.230397999 174 | Target Frame: 175 | Value: Orbit (rviz) 176 | Yaw: 0.400398016 177 | Saved: ~ 178 | Window Geometry: 179 | Displays: 180 | collapsed: false 181 | Height: 784 182 | Hide Left Dock: false 183 | Hide Right Dock: false 184 | QMainWindow State: 000000ff00000000fd00000004000000000000016a00000286fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000286000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000001d9fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000001d9000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004a00000003efc0100000002fb0000000800540069006d00650100000000000004a00000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000003300000028600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 185 | Selection: 186 | collapsed: false 187 | Time: 188 | collapsed: false 189 | Tool Properties: 190 | collapsed: false 191 | Views: 192 | collapsed: false 193 | Width: 1184 194 | X: 65 195 | Y: 24 196 | -------------------------------------------------------------------------------- /marm_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(marm_description) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | roscpp 12 | tf 13 | urdf 14 | xacro 15 | ) 16 | 17 | ## System dependencies are found with CMake's conventions 18 | # find_package(Boost REQUIRED COMPONENTS system) 19 | 20 | 21 | ## Uncomment this if the package has a setup.py. This macro ensures 22 | ## modules and global scripts declared therein get installed 23 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 24 | # catkin_python_setup() 25 | 26 | ################################################ 27 | ## Declare ROS messages, services and actions ## 28 | ################################################ 29 | 30 | ## To declare and build messages, services or actions from within this 31 | ## package, follow these steps: 32 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 33 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 34 | ## * In the file package.xml: 35 | ## * add a build_depend tag for "message_generation" 36 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 37 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 38 | ## but can be declared for certainty nonetheless: 39 | ## * add a run_depend tag for "message_runtime" 40 | ## * In this file (CMakeLists.txt): 41 | ## * add "message_generation" and every package in MSG_DEP_SET to 42 | ## find_package(catkin REQUIRED COMPONENTS ...) 43 | ## * add "message_runtime" and every package in MSG_DEP_SET to 44 | ## catkin_package(CATKIN_DEPENDS ...) 45 | ## * uncomment the add_*_files sections below as needed 46 | ## and list every .msg/.srv/.action file to be processed 47 | ## * uncomment the generate_messages entry below 48 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 49 | 50 | ## Generate messages in the 'msg' folder 51 | # add_message_files( 52 | # FILES 53 | # Message1.msg 54 | # Message2.msg 55 | # ) 56 | 57 | ## Generate services in the 'srv' folder 58 | # add_service_files( 59 | # FILES 60 | # Service1.srv 61 | # Service2.srv 62 | # ) 63 | 64 | ## Generate actions in the 'action' folder 65 | # add_action_files( 66 | # FILES 67 | # Action1.action 68 | # Action2.action 69 | # ) 70 | 71 | ## Generate added messages and services with any dependencies listed here 72 | # generate_messages( 73 | # DEPENDENCIES 74 | # std_msgs # Or other packages containing msgs 75 | # ) 76 | 77 | ################################################ 78 | ## Declare ROS dynamic reconfigure parameters ## 79 | ################################################ 80 | 81 | ## To declare and build dynamic reconfigure parameters within this 82 | ## package, follow these steps: 83 | ## * In the file package.xml: 84 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 85 | ## * In this file (CMakeLists.txt): 86 | ## * add "dynamic_reconfigure" to 87 | ## find_package(catkin REQUIRED COMPONENTS ...) 88 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 89 | ## and list every .cfg file to be processed 90 | 91 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 92 | # generate_dynamic_reconfigure_options( 93 | # cfg/DynReconf1.cfg 94 | # cfg/DynReconf2.cfg 95 | # ) 96 | 97 | ################################### 98 | ## catkin specific configuration ## 99 | ################################### 100 | ## The catkin_package macro generates cmake config files for your package 101 | ## Declare things to be passed to dependent projects 102 | ## INCLUDE_DIRS: uncomment this if you package contains header files 103 | ## LIBRARIES: libraries you create in this project that dependent projects also need 104 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 105 | ## DEPENDS: system dependencies of this project that dependent projects also need 106 | catkin_package( 107 | # INCLUDE_DIRS include 108 | # LIBRARIES marm_description 109 | # CATKIN_DEPENDS roscpp tf urdf xacro 110 | # DEPENDS system_lib 111 | ) 112 | 113 | ########### 114 | ## Build ## 115 | ########### 116 | 117 | ## Specify additional locations of header files 118 | ## Your package locations should be listed before other locations 119 | include_directories( 120 | # include 121 | ${catkin_INCLUDE_DIRS} 122 | ) 123 | 124 | ## Declare a C++ library 125 | # add_library(${PROJECT_NAME} 126 | # src/${PROJECT_NAME}/marm_description.cpp 127 | # ) 128 | 129 | ## Add cmake target dependencies of the library 130 | ## as an example, code may need to be generated before libraries 131 | ## either from message generation or dynamic reconfigure 132 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 133 | 134 | ## Declare a C++ executable 135 | ## With catkin_make all packages are built within a single CMake context 136 | ## The recommended prefix ensures that target names across packages don't collide 137 | # add_executable(${PROJECT_NAME}_node src/marm_description_node.cpp) 138 | 139 | ## Rename C++ executable without prefix 140 | ## The above recommended prefix causes long target names, the following renames the 141 | ## target back to the shorter version for ease of user use 142 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 143 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 144 | 145 | ## Add cmake target dependencies of the executable 146 | ## same as for the library above 147 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 148 | 149 | ## Specify libraries to link a library or executable target against 150 | # target_link_libraries(${PROJECT_NAME}_node 151 | # ${catkin_LIBRARIES} 152 | # ) 153 | 154 | ############# 155 | ## Install ## 156 | ############# 157 | 158 | # all install targets should use catkin DESTINATION variables 159 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 160 | 161 | ## Mark executable scripts (Python etc.) for installation 162 | ## in contrast to setup.py, you can choose the destination 163 | # install(PROGRAMS 164 | # scripts/my_python_script 165 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 166 | # ) 167 | 168 | ## Mark executables and/or libraries for installation 169 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 170 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 171 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 172 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 173 | # ) 174 | 175 | ## Mark cpp header files for installation 176 | # install(DIRECTORY include/${PROJECT_NAME}/ 177 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 178 | # FILES_MATCHING PATTERN "*.h" 179 | # PATTERN ".svn" EXCLUDE 180 | # ) 181 | 182 | ## Mark other files for installation (e.g. launch and bag files, etc.) 183 | # install(FILES 184 | # # myfile1 185 | # # myfile2 186 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 187 | # ) 188 | 189 | ############# 190 | ## Testing ## 191 | ############# 192 | 193 | ## Add gtest based cpp test target and link libraries 194 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_marm_description.cpp) 195 | # if(TARGET ${PROJECT_NAME}-test) 196 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 197 | # endif() 198 | 199 | ## Add folders to be run by python nosetests 200 | # catkin_add_nosetests(test) 201 | -------------------------------------------------------------------------------- /marm_gazebo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(marm_gazebo) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | # add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | gazebo_msgs 12 | gazebo_plugins 13 | gazebo_ros 14 | gazebo_ros_control 15 | ) 16 | 17 | ## System dependencies are found with CMake's conventions 18 | # find_package(Boost REQUIRED COMPONENTS system) 19 | 20 | 21 | ## Uncomment this if the package has a setup.py. This macro ensures 22 | ## modules and global scripts declared therein get installed 23 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 24 | # catkin_python_setup() 25 | 26 | ################################################ 27 | ## Declare ROS messages, services and actions ## 28 | ################################################ 29 | 30 | ## To declare and build messages, services or actions from within this 31 | ## package, follow these steps: 32 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 33 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 34 | ## * In the file package.xml: 35 | ## * add a build_depend tag for "message_generation" 36 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 37 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 38 | ## but can be declared for certainty nonetheless: 39 | ## * add a run_depend tag for "message_runtime" 40 | ## * In this file (CMakeLists.txt): 41 | ## * add "message_generation" and every package in MSG_DEP_SET to 42 | ## find_package(catkin REQUIRED COMPONENTS ...) 43 | ## * add "message_runtime" and every package in MSG_DEP_SET to 44 | ## catkin_package(CATKIN_DEPENDS ...) 45 | ## * uncomment the add_*_files sections below as needed 46 | ## and list every .msg/.srv/.action file to be processed 47 | ## * uncomment the generate_messages entry below 48 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 49 | 50 | ## Generate messages in the 'msg' folder 51 | # add_message_files( 52 | # FILES 53 | # Message1.msg 54 | # Message2.msg 55 | # ) 56 | 57 | ## Generate services in the 'srv' folder 58 | # add_service_files( 59 | # FILES 60 | # Service1.srv 61 | # Service2.srv 62 | # ) 63 | 64 | ## Generate actions in the 'action' folder 65 | # add_action_files( 66 | # FILES 67 | # Action1.action 68 | # Action2.action 69 | # ) 70 | 71 | ## Generate added messages and services with any dependencies listed here 72 | # generate_messages( 73 | # DEPENDENCIES 74 | # gazebo_msgs 75 | # ) 76 | 77 | ################################################ 78 | ## Declare ROS dynamic reconfigure parameters ## 79 | ################################################ 80 | 81 | ## To declare and build dynamic reconfigure parameters within this 82 | ## package, follow these steps: 83 | ## * In the file package.xml: 84 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 85 | ## * In this file (CMakeLists.txt): 86 | ## * add "dynamic_reconfigure" to 87 | ## find_package(catkin REQUIRED COMPONENTS ...) 88 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 89 | ## and list every .cfg file to be processed 90 | 91 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 92 | # generate_dynamic_reconfigure_options( 93 | # cfg/DynReconf1.cfg 94 | # cfg/DynReconf2.cfg 95 | # ) 96 | 97 | ################################### 98 | ## catkin specific configuration ## 99 | ################################### 100 | ## The catkin_package macro generates cmake config files for your package 101 | ## Declare things to be passed to dependent projects 102 | ## INCLUDE_DIRS: uncomment this if you package contains header files 103 | ## LIBRARIES: libraries you create in this project that dependent projects also need 104 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 105 | ## DEPENDS: system dependencies of this project that dependent projects also need 106 | catkin_package( 107 | # INCLUDE_DIRS include 108 | # LIBRARIES marm_gazebo 109 | # CATKIN_DEPENDS gazebo_msgs gazebo_plugins gazebo_ros gazebo_ros_control 110 | # DEPENDS system_lib 111 | ) 112 | 113 | ########### 114 | ## Build ## 115 | ########### 116 | 117 | ## Specify additional locations of header files 118 | ## Your package locations should be listed before other locations 119 | include_directories( 120 | # include 121 | ${catkin_INCLUDE_DIRS} 122 | ) 123 | 124 | ## Declare a C++ library 125 | # add_library(${PROJECT_NAME} 126 | # src/${PROJECT_NAME}/marm_gazebo.cpp 127 | # ) 128 | 129 | ## Add cmake target dependencies of the library 130 | ## as an example, code may need to be generated before libraries 131 | ## either from message generation or dynamic reconfigure 132 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 133 | 134 | ## Declare a C++ executable 135 | ## With catkin_make all packages are built within a single CMake context 136 | ## The recommended prefix ensures that target names across packages don't collide 137 | # add_executable(${PROJECT_NAME}_node src/marm_gazebo_node.cpp) 138 | 139 | ## Rename C++ executable without prefix 140 | ## The above recommended prefix causes long target names, the following renames the 141 | ## target back to the shorter version for ease of user use 142 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 143 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 144 | 145 | ## Add cmake target dependencies of the executable 146 | ## same as for the library above 147 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 148 | 149 | ## Specify libraries to link a library or executable target against 150 | # target_link_libraries(${PROJECT_NAME}_node 151 | # ${catkin_LIBRARIES} 152 | # ) 153 | 154 | ############# 155 | ## Install ## 156 | ############# 157 | 158 | # all install targets should use catkin DESTINATION variables 159 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 160 | 161 | ## Mark executable scripts (Python etc.) for installation 162 | ## in contrast to setup.py, you can choose the destination 163 | # install(PROGRAMS 164 | # scripts/my_python_script 165 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 166 | # ) 167 | 168 | ## Mark executables and/or libraries for installation 169 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 170 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 171 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 172 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 173 | # ) 174 | 175 | ## Mark cpp header files for installation 176 | # install(DIRECTORY include/${PROJECT_NAME}/ 177 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 178 | # FILES_MATCHING PATTERN "*.h" 179 | # PATTERN ".svn" EXCLUDE 180 | # ) 181 | 182 | ## Mark other files for installation (e.g. launch and bag files, etc.) 183 | # install(FILES 184 | # # myfile1 185 | # # myfile2 186 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 187 | # ) 188 | 189 | ############# 190 | ## Testing ## 191 | ############# 192 | 193 | ## Add gtest based cpp test target and link libraries 194 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_marm_gazebo.cpp) 195 | # if(TARGET ${PROJECT_NAME}-test) 196 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 197 | # endif() 198 | 199 | ## Add folders to be run by python nosetests 200 | # catkin_add_nosetests(test) 201 | -------------------------------------------------------------------------------- /marm_planning/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(marm_planning) 3 | 4 | ## Compile as C++11, supported in ROS Kinetic and newer 5 | add_compile_options(-std=c++11) 6 | 7 | ## Find catkin macros and libraries 8 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 9 | ## is used, also find other catkin packages 10 | find_package(catkin REQUIRED COMPONENTS 11 | actionlib 12 | control_msgs 13 | diagnostic_msgs 14 | geometry_msgs 15 | manipulation_msgs 16 | moveit_msgs 17 | moveit_ros_perception 18 | moveit_ros_planning_interface 19 | roscpp 20 | rospy 21 | sensor_msgs 22 | shape_msgs 23 | std_msgs 24 | std_srvs 25 | tf 26 | trajectory_msgs 27 | ) 28 | 29 | ## System dependencies are found with CMake's conventions 30 | find_package(Boost REQUIRED COMPONENTS system) 31 | 32 | 33 | ## Uncomment this if the package has a setup.py. This macro ensures 34 | ## modules and global scripts declared therein get installed 35 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 36 | # catkin_python_setup() 37 | 38 | ################################################ 39 | ## Declare ROS messages, services and actions ## 40 | ################################################ 41 | 42 | ## To declare and build messages, services or actions from within this 43 | ## package, follow these steps: 44 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 45 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 46 | ## * In the file package.xml: 47 | ## * add a build_depend tag for "message_generation" 48 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 49 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 50 | ## but can be declared for certainty nonetheless: 51 | ## * add a run_depend tag for "message_runtime" 52 | ## * In this file (CMakeLists.txt): 53 | ## * add "message_generation" and every package in MSG_DEP_SET to 54 | ## find_package(catkin REQUIRED COMPONENTS ...) 55 | ## * add "message_runtime" and every package in MSG_DEP_SET to 56 | ## catkin_package(CATKIN_DEPENDS ...) 57 | ## * uncomment the add_*_files sections below as needed 58 | ## and list every .msg/.srv/.action file to be processed 59 | ## * uncomment the generate_messages entry below 60 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 61 | 62 | ## Generate messages in the 'msg' folder 63 | # add_message_files( 64 | # FILES 65 | # Message1.msg 66 | # Message2.msg 67 | # ) 68 | 69 | ## Generate services in the 'srv' folder 70 | # add_service_files( 71 | # FILES 72 | # Service1.srv 73 | # Service2.srv 74 | # ) 75 | 76 | ## Generate actions in the 'action' folder 77 | # add_action_files( 78 | # FILES 79 | # Action1.action 80 | # Action2.action 81 | # ) 82 | 83 | ## Generate added messages and services with any dependencies listed here 84 | # generate_messages( 85 | # DEPENDENCIES 86 | # control_msgs# diagnostic_msgs# geometry_msgs# manipulation_msgs# moveit_msgs# moveit_msgs# sensor_msgs# shape_msgs# std_msgs# trajectory_msgs 87 | # ) 88 | 89 | ################################################ 90 | ## Declare ROS dynamic reconfigure parameters ## 91 | ################################################ 92 | 93 | ## To declare and build dynamic reconfigure parameters within this 94 | ## package, follow these steps: 95 | ## * In the file package.xml: 96 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 97 | ## * In this file (CMakeLists.txt): 98 | ## * add "dynamic_reconfigure" to 99 | ## find_package(catkin REQUIRED COMPONENTS ...) 100 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 101 | ## and list every .cfg file to be processed 102 | 103 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 104 | # generate_dynamic_reconfigure_options( 105 | # cfg/DynReconf1.cfg 106 | # cfg/DynReconf2.cfg 107 | # ) 108 | 109 | ################################### 110 | ## catkin specific configuration ## 111 | ################################### 112 | ## The catkin_package macro generates cmake config files for your package 113 | ## Declare things to be passed to dependent projects 114 | ## INCLUDE_DIRS: uncomment this if you package contains header files 115 | ## LIBRARIES: libraries you create in this project that dependent projects also need 116 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 117 | ## DEPENDS: system dependencies of this project that dependent projects also need 118 | catkin_package( 119 | # INCLUDE_DIRS include 120 | # LIBRARIES marm_planning 121 | # CATKIN_DEPENDS actionlib control_msgs diagnostic_msgs geometry_msgs manipulation_msgs moveit_msgs moveit_msgs roscpp rospy sensor_msgs shape_msgs std_msgs std_srvs tf trajectory_msgs 122 | # DEPENDS system_lib 123 | ) 124 | 125 | ########### 126 | ## Build ## 127 | ########### 128 | 129 | ## Specify additional locations of header files 130 | ## Your package locations should be listed before other locations 131 | include_directories( 132 | # include 133 | ${catkin_INCLUDE_DIRS} 134 | ) 135 | 136 | ## Declare a C++ library 137 | # add_library(${PROJECT_NAME} 138 | # src/${PROJECT_NAME}/marm_planning.cpp 139 | # ) 140 | 141 | ## Add cmake target dependencies of the library 142 | ## as an example, code may need to be generated before libraries 143 | ## either from message generation or dynamic reconfigure 144 | # add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 145 | 146 | ## Declare a C++ executable 147 | ## With catkin_make all packages are built within a single CMake context 148 | ## The recommended prefix ensures that target names across packages don't collide 149 | # add_executable(${PROJECT_NAME}_node src/marm_planning_node.cpp) 150 | 151 | ## Rename C++ executable without prefix 152 | ## The above recommended prefix causes long target names, the following renames the 153 | ## target back to the shorter version for ease of user use 154 | ## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" 155 | # set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") 156 | 157 | ## Add cmake target dependencies of the executable 158 | ## same as for the library above 159 | # add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 160 | 161 | ## Specify libraries to link a library or executable target against 162 | # target_link_libraries(${PROJECT_NAME}_node 163 | # ${catkin_LIBRARIES} 164 | # ) 165 | 166 | add_executable(test_random_node src/test_random.cpp) 167 | target_link_libraries(test_random_node ${catkin_LIBRARIES}) 168 | 169 | add_executable(test_custom_node src/test_custom.cpp) 170 | target_link_libraries(test_custom_node ${catkin_LIBRARIES}) 171 | 172 | add_executable(add_collision_objct_node src/add_collision_objct.cpp) 173 | target_link_libraries(add_collision_objct_node ${catkin_LIBRARIES}) 174 | 175 | add_executable(remove_collision_objct_node src/remove_collision_objct.cpp) 176 | target_link_libraries(remove_collision_objct_node ${catkin_LIBRARIES}) 177 | 178 | add_executable(check_collision_node src/check_collision.cpp) 179 | target_link_libraries(check_collision_node ${catkin_LIBRARIES}) 180 | 181 | add_executable(test_cartesian_path_node src/test_cartesian_path.cpp) 182 | target_link_libraries(test_cartesian_path_node ${catkin_LIBRARIES}) 183 | 184 | ############# 185 | ## Install ## 186 | ############# 187 | 188 | # all install targets should use catkin DESTINATION variables 189 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 190 | 191 | ## Mark executable scripts (Python etc.) for installation 192 | ## in contrast to setup.py, you can choose the destination 193 | # install(PROGRAMS 194 | # scripts/my_python_script 195 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 196 | # ) 197 | 198 | ## Mark executables and/or libraries for installation 199 | # install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node 200 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 201 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 202 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 203 | # ) 204 | 205 | ## Mark cpp header files for installation 206 | # install(DIRECTORY include/${PROJECT_NAME}/ 207 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 208 | # FILES_MATCHING PATTERN "*.h" 209 | # PATTERN ".svn" EXCLUDE 210 | # ) 211 | 212 | ## Mark other files for installation (e.g. launch and bag files, etc.) 213 | # install(FILES 214 | # # myfile1 215 | # # myfile2 216 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 217 | # ) 218 | 219 | ############# 220 | ## Testing ## 221 | ############# 222 | 223 | ## Add gtest based cpp test target and link libraries 224 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_marm_planning.cpp) 225 | # if(TARGET ${PROJECT_NAME}-test) 226 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 227 | # endif() 228 | 229 | ## Add folders to be run by python nosetests 230 | # catkin_add_nosetests(test) 231 | -------------------------------------------------------------------------------- /marm_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 | FMTkConfigDefault: 56 | type: geometric::FMT 57 | num_samples: 1000 # number of states that the planner should sample. default: 1000 58 | radius_multiplier: 1.1 # multiplier used for the nearest neighbors search radius. default: 1.1 59 | nearest_k: 1 # use Knearest strategy. default: 1 60 | cache_cc: 1 # use collision checking cache. default: 1 61 | heuristics: 0 # activate cost to go heuristics. default: 0 62 | extended_fmt: 1 # activate the extended FMT*: adding new samples if planner does not finish successfully. default: 1 63 | BFMTkConfigDefault: 64 | type: geometric::BFMT 65 | num_samples: 1000 # number of states that the planner should sample. default: 1000 66 | radius_multiplier: 1.0 # multiplier used for the nearest neighbors search radius. default: 1.0 67 | nearest_k: 1 # use the Knearest strategy. default: 1 68 | balanced: 0 # exploration strategy: balanced true expands one tree every iteration. False will select the tree with lowest maximum cost to go. default: 1 69 | optimality: 1 # termination strategy: optimality true finishes when the best possible path is found. Otherwise, the algorithm will finish when the first feasible path is found. default: 1 70 | heuristics: 1 # activates cost to go heuristics. default: 1 71 | cache_cc: 1 # use the collision checking cache. default: 1 72 | extended_fmt: 1 # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1 73 | PDSTkConfigDefault: 74 | type: geometric::PDST 75 | STRIDEkConfigDefault: 76 | type: geometric::STRIDE 77 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 78 | goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 79 | use_projected_distance: 0 # whether nearest neighbors are computed based on distances in a projection of the state rather distances in the state space itself. default: 0 80 | degree: 16 # desired degree of a node in the Geometric Near-neightbor Access Tree (GNAT). default: 16 81 | max_degree: 18 # max degree of a node in the GNAT. default: 12 82 | min_degree: 12 # min degree of a node in the GNAT. default: 12 83 | max_pts_per_leaf: 6 # max points per leaf in the GNAT. default: 6 84 | estimated_dimension: 0.0 # estimated dimension of the free space. default: 0.0 85 | min_valid_path_fraction: 0.2 # Accept partially valid moves above fraction. default: 0.2 86 | BiTRRTkConfigDefault: 87 | type: geometric::BiTRRT 88 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 89 | temp_change_factor: 0.1 # how much to increase or decrease temp. default: 0.1 90 | init_temperature: 100 # initial temperature. default: 100 91 | frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() 92 | frountier_node_ratio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 93 | cost_threshold: 1e300 # the cost threshold. Any motion cost that is not better will not be expanded. default: inf 94 | LBTRRTkConfigDefault: 95 | type: geometric::LBTRRT 96 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 97 | goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 98 | epsilon: 0.4 # optimality approximation factor. default: 0.4 99 | BiESTkConfigDefault: 100 | type: geometric::BiEST 101 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 102 | ProjESTkConfigDefault: 103 | type: geometric::ProjEST 104 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 105 | goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 106 | LazyPRMkConfigDefault: 107 | type: geometric::LazyPRM 108 | range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() 109 | LazyPRMstarkConfigDefault: 110 | type: geometric::LazyPRMstar 111 | SPARSkConfigDefault: 112 | type: geometric::SPARS 113 | stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 114 | sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 115 | dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 116 | max_failures: 1000 # maximum consecutive failure limit. default: 1000 117 | SPARStwokConfigDefault: 118 | type: geometric::SPARStwo 119 | stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 120 | sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 121 | dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 122 | max_failures: 5000 # maximum consecutive failure limit. default: 5000 123 | arm: 124 | planner_configs: 125 | - SBLkConfigDefault 126 | - ESTkConfigDefault 127 | - LBKPIECEkConfigDefault 128 | - BKPIECEkConfigDefault 129 | - KPIECEkConfigDefault 130 | - RRTkConfigDefault 131 | - RRTConnectkConfigDefault 132 | - RRTstarkConfigDefault 133 | - TRRTkConfigDefault 134 | - PRMkConfigDefault 135 | - PRMstarkConfigDefault 136 | - FMTkConfigDefault 137 | - BFMTkConfigDefault 138 | - PDSTkConfigDefault 139 | - STRIDEkConfigDefault 140 | - BiTRRTkConfigDefault 141 | - LBTRRTkConfigDefault 142 | - BiESTkConfigDefault 143 | - ProjESTkConfigDefault 144 | - LazyPRMkConfigDefault 145 | - LazyPRMstarkConfigDefault 146 | - SPARSkConfigDefault 147 | - SPARStwokConfigDefault 148 | projection_evaluator: joints(joint1,joint2) 149 | longest_valid_segment_fraction: 0.005 150 | gripper: 151 | planner_configs: 152 | - SBLkConfigDefault 153 | - ESTkConfigDefault 154 | - LBKPIECEkConfigDefault 155 | - BKPIECEkConfigDefault 156 | - KPIECEkConfigDefault 157 | - RRTkConfigDefault 158 | - RRTConnectkConfigDefault 159 | - RRTstarkConfigDefault 160 | - TRRTkConfigDefault 161 | - PRMkConfigDefault 162 | - PRMstarkConfigDefault 163 | - FMTkConfigDefault 164 | - BFMTkConfigDefault 165 | - PDSTkConfigDefault 166 | - STRIDEkConfigDefault 167 | - BiTRRTkConfigDefault 168 | - LBTRRTkConfigDefault 169 | - BiESTkConfigDefault 170 | - ProjESTkConfigDefault 171 | - LazyPRMkConfigDefault 172 | - LazyPRMstarkConfigDefault 173 | - SPARSkConfigDefault 174 | - SPARStwokConfigDefault -------------------------------------------------------------------------------- /marm_planning/config/arm_paths.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 0 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /MotionPlanning1/Scene Geometry1 9 | - /MotionPlanning1/Scene Robot1 10 | - /MotionPlanning1/Planning Request1 11 | - /MotionPlanning1/Planned Path1 12 | - /RobotModel1 13 | - /RobotModel1/Links1 14 | - /RobotModel1/Links1/grasping_frame1 15 | Splitter Ratio: 0.519512177 16 | Tree Height: 633 17 | - Class: rviz/Help 18 | Name: Help 19 | - Class: rviz/Views 20 | Expanded: 21 | - /Current View1 22 | Name: Views 23 | Splitter Ratio: 0.5 24 | Visualization Manager: 25 | Class: "" 26 | Displays: 27 | - Alpha: 0.5 28 | Cell Size: 1 29 | Class: rviz/Grid 30 | Color: 119; 119; 122 31 | Enabled: true 32 | Line Style: 33 | Line Width: 0.0299999993 34 | Value: Lines 35 | Name: Grid 36 | Normal Cell Count: 0 37 | Offset: 38 | X: 0 39 | Y: 0 40 | Z: 0 41 | Plane: XY 42 | Plane Cell Count: 10 43 | Reference Frame: 44 | Value: true 45 | - Class: moveit_rviz_plugin/MotionPlanning 46 | Enabled: true 47 | Move Group Namespace: "" 48 | MoveIt_Goal_Tolerance: 0 49 | MoveIt_Planning_Attempts: 10 50 | MoveIt_Planning_Time: 5 51 | MoveIt_Use_Constraint_Aware_IK: true 52 | MoveIt_Warehouse_Host: 127.0.0.1 53 | MoveIt_Warehouse_Port: 33829 54 | MoveIt_Workspace: 55 | Center: 56 | X: 0 57 | Y: 0 58 | Z: 0 59 | Size: 60 | X: 2 61 | Y: 2 62 | Z: 2 63 | Name: MotionPlanning 64 | Planned Path: 65 | Color Enabled: false 66 | Interrupt Display: false 67 | Links: 68 | All Links Enabled: true 69 | Expand Joint Details: false 70 | Expand Link Details: false 71 | Expand Tree: false 72 | Link Tree Style: Links in Alphabetic Order 73 | base_link: 74 | Alpha: 1 75 | Show Axes: false 76 | Show Trail: false 77 | Value: true 78 | bottom_link: 79 | Alpha: 1 80 | Show Axes: false 81 | Show Trail: false 82 | Value: true 83 | grasping_frame: 84 | Alpha: 1 85 | Show Axes: false 86 | Show Trail: false 87 | gripper_finger_link1: 88 | Alpha: 1 89 | Show Axes: false 90 | Show Trail: false 91 | Value: true 92 | gripper_finger_link2: 93 | Alpha: 1 94 | Show Axes: false 95 | Show Trail: false 96 | Value: true 97 | link1: 98 | Alpha: 1 99 | Show Axes: false 100 | Show Trail: false 101 | Value: true 102 | link2: 103 | Alpha: 1 104 | Show Axes: false 105 | Show Trail: false 106 | Value: true 107 | link3: 108 | Alpha: 1 109 | Show Axes: false 110 | Show Trail: false 111 | Value: true 112 | link4: 113 | Alpha: 1 114 | Show Axes: false 115 | Show Trail: false 116 | Value: true 117 | link5: 118 | Alpha: 1 119 | Show Axes: false 120 | Show Trail: false 121 | Value: true 122 | link6: 123 | Alpha: 1 124 | Show Axes: false 125 | Show Trail: false 126 | Value: true 127 | Loop Animation: false 128 | Robot Alpha: 0.5 129 | Robot Color: 150; 50; 150 130 | Show Robot Collision: false 131 | Show Robot Visual: false 132 | Show Trail: false 133 | State Display Time: 0.05 s 134 | Trail Step Size: 1 135 | Trajectory Topic: /move_group/display_planned_path 136 | Planning Metrics: 137 | Payload: 1 138 | Show Joint Torques: false 139 | Show Manipulability: false 140 | Show Manipulability Index: false 141 | Show Weight Limit: false 142 | TextHeight: 0.0799999982 143 | Planning Request: 144 | Colliding Link Color: 255; 0; 0 145 | Goal State Alpha: 1 146 | Goal State Color: 250; 128; 0 147 | Interactive Marker Size: 0 148 | Joint Violation Color: 255; 0; 255 149 | Planning Group: arm 150 | Query Goal State: false 151 | Query Start State: false 152 | Show Workspace: false 153 | Start State Alpha: 1 154 | Start State Color: 0; 255; 0 155 | Planning Scene Topic: /move_group/monitored_planning_scene 156 | Robot Description: robot_description 157 | Scene Geometry: 158 | Scene Alpha: 1 159 | Scene Color: 50; 230; 50 160 | Scene Display Time: 0.200000003 161 | Show Scene Geometry: true 162 | Voxel Coloring: Z-Axis 163 | Voxel Rendering: Occupied Voxels 164 | Scene Robot: 165 | Attached Body Color: 150; 50; 150 166 | Links: 167 | All Links Enabled: true 168 | Expand Joint Details: false 169 | Expand Link Details: false 170 | Expand Tree: false 171 | Link Tree Style: Links in Alphabetic Order 172 | base_link: 173 | Alpha: 1 174 | Show Axes: false 175 | Show Trail: false 176 | Value: true 177 | bottom_link: 178 | Alpha: 1 179 | Show Axes: false 180 | Show Trail: false 181 | Value: true 182 | grasping_frame: 183 | Alpha: 1 184 | Show Axes: false 185 | Show Trail: false 186 | gripper_finger_link1: 187 | Alpha: 1 188 | Show Axes: false 189 | Show Trail: false 190 | Value: true 191 | gripper_finger_link2: 192 | Alpha: 1 193 | Show Axes: false 194 | Show Trail: false 195 | Value: true 196 | link1: 197 | Alpha: 1 198 | Show Axes: false 199 | Show Trail: false 200 | Value: true 201 | link2: 202 | Alpha: 1 203 | Show Axes: false 204 | Show Trail: false 205 | Value: true 206 | link3: 207 | Alpha: 1 208 | Show Axes: false 209 | Show Trail: false 210 | Value: true 211 | link4: 212 | Alpha: 1 213 | Show Axes: false 214 | Show Trail: false 215 | Value: true 216 | link5: 217 | Alpha: 1 218 | Show Axes: false 219 | Show Trail: false 220 | Value: true 221 | link6: 222 | Alpha: 1 223 | Show Axes: false 224 | Show Trail: false 225 | Value: true 226 | Robot Alpha: 0.5 227 | Show Robot Collision: false 228 | Show Robot Visual: false 229 | Value: true 230 | - Alpha: 1 231 | Class: rviz/RobotModel 232 | Collision Enabled: false 233 | Enabled: true 234 | Links: 235 | All Links Enabled: true 236 | Expand Joint Details: false 237 | Expand Link Details: false 238 | Expand Tree: false 239 | Link Tree Style: Links in Alphabetic Order 240 | base_link: 241 | Alpha: 1 242 | Show Axes: false 243 | Show Trail: false 244 | Value: true 245 | bottom_link: 246 | Alpha: 1 247 | Show Axes: false 248 | Show Trail: false 249 | Value: true 250 | grasping_frame: 251 | Alpha: 1 252 | Show Axes: false 253 | Show Trail: true 254 | gripper_finger_link1: 255 | Alpha: 1 256 | Show Axes: false 257 | Show Trail: false 258 | Value: true 259 | gripper_finger_link2: 260 | Alpha: 1 261 | Show Axes: false 262 | Show Trail: false 263 | Value: true 264 | link1: 265 | Alpha: 1 266 | Show Axes: false 267 | Show Trail: false 268 | Value: true 269 | link2: 270 | Alpha: 1 271 | Show Axes: false 272 | Show Trail: false 273 | Value: true 274 | link3: 275 | Alpha: 1 276 | Show Axes: false 277 | Show Trail: false 278 | Value: true 279 | link4: 280 | Alpha: 1 281 | Show Axes: false 282 | Show Trail: false 283 | Value: true 284 | link5: 285 | Alpha: 1 286 | Show Axes: false 287 | Show Trail: false 288 | Value: true 289 | link6: 290 | Alpha: 1 291 | Show Axes: false 292 | Show Trail: false 293 | Value: true 294 | Name: RobotModel 295 | Robot Description: robot_description 296 | TF Prefix: "" 297 | Update Interval: 0 298 | Value: true 299 | Visual Enabled: true 300 | Enabled: true 301 | Global Options: 302 | Background Color: 184; 184; 184 303 | Fixed Frame: base_link 304 | Frame Rate: 30 305 | Name: root 306 | Tools: 307 | - Class: rviz/Interact 308 | Hide Inactive Objects: true 309 | - Class: rviz/MoveCamera 310 | - Class: rviz/Select 311 | Value: true 312 | Views: 313 | Current: 314 | Class: rviz/Orbit 315 | Distance: 1.35690749 316 | Enable Stereo Rendering: 317 | Stereo Eye Separation: 0.0599999987 318 | Stereo Focal Distance: 1 319 | Swap Stereo Eyes: false 320 | Value: false 321 | Focal Point: 322 | X: -0.0540157482 323 | Y: 0.0828352645 324 | Z: 0.243915334 325 | Focal Shape Fixed Size: true 326 | Focal Shape Size: 0.0500000007 327 | Name: Current View 328 | Near Clip Distance: 0.00999999978 329 | Pitch: 0.300397664 330 | Target Frame: base_footprint 331 | Value: Orbit (rviz) 332 | Yaw: 5.85857487 333 | Saved: ~ 334 | Window Geometry: 335 | Displays: 336 | collapsed: false 337 | Height: 768 338 | Help: 339 | collapsed: false 340 | Hide Left Dock: false 341 | Hide Right Dock: true 342 | MotionPlanning: 343 | collapsed: false 344 | MotionPlanning - Slider: 345 | collapsed: false 346 | QMainWindow State: 000000ff00000000fd0000000100000000000001ac000002bafc0200000007fb000000100044006900730070006c0061007900730100000028000002ba000000dd00fffffffb0000000800480065006c00700000000342000000bb0000007600fffffffb0000000a005600690065007700730000000124000000b0000000b000fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670000000139000001900000000000000000fb0000002e004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d00200053006c00690064006500720000000000ffffffff0000004a00fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006700000001670000029b000001cc00ffffff000003c4000002ba00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 347 | Views: 348 | collapsed: false 349 | Width: 1398 350 | X: 291 351 | Y: 53 352 | -------------------------------------------------------------------------------- /marm_planning/config/pick_and_place.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 0 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /MotionPlanning1/Scene Geometry1 9 | - /MotionPlanning1/Scene Robot1 10 | - /MotionPlanning1/Planned Path1 11 | Splitter Ratio: 0.397694528 12 | Tree Height: 724 13 | - Class: rviz/Help 14 | Name: Help 15 | - Class: rviz/Views 16 | Expanded: 17 | - /Current View1 18 | Name: Views 19 | Splitter Ratio: 0.5 20 | Visualization Manager: 21 | Class: "" 22 | Displays: 23 | - Alpha: 0.5 24 | Cell Size: 0.5 25 | Class: rviz/Grid 26 | Color: 0; 0; 0 27 | Enabled: true 28 | Line Style: 29 | Line Width: 0.0299999993 30 | Value: Lines 31 | Name: Grid 32 | Normal Cell Count: 0 33 | Offset: 34 | X: 0 35 | Y: 0 36 | Z: 0 37 | Plane: XY 38 | Plane Cell Count: 20 39 | Reference Frame: 40 | Value: true 41 | - Class: rviz/Marker 42 | Enabled: true 43 | Marker Topic: /target_marker 44 | Name: Marker 45 | Namespaces: 46 | {} 47 | Queue Size: 100 48 | Value: true 49 | - Class: moveit_rviz_plugin/MotionPlanning 50 | Enabled: true 51 | Move Group Namespace: "" 52 | MoveIt_Goal_Tolerance: 0 53 | MoveIt_Planning_Attempts: 10 54 | MoveIt_Planning_Time: 5 55 | MoveIt_Use_Constraint_Aware_IK: true 56 | MoveIt_Warehouse_Host: 127.0.0.1 57 | MoveIt_Warehouse_Port: 33829 58 | MoveIt_Workspace: 59 | Center: 60 | X: 0 61 | Y: 0 62 | Z: 0 63 | Size: 64 | X: 2 65 | Y: 2 66 | Z: 2 67 | Name: MotionPlanning 68 | Planned Path: 69 | Color Enabled: false 70 | Interrupt Display: false 71 | Links: 72 | All Links Enabled: true 73 | Expand Joint Details: false 74 | Expand Link Details: false 75 | Expand Tree: false 76 | Link Tree Style: Links in Alphabetic Order 77 | base_link: 78 | Alpha: 1 79 | Show Axes: false 80 | Show Trail: false 81 | Value: true 82 | bottom_link: 83 | Alpha: 1 84 | Show Axes: false 85 | Show Trail: false 86 | Value: true 87 | grasping_frame: 88 | Alpha: 1 89 | Show Axes: false 90 | Show Trail: false 91 | gripper_finger_link1: 92 | Alpha: 1 93 | Show Axes: false 94 | Show Trail: false 95 | Value: true 96 | gripper_finger_link2: 97 | Alpha: 1 98 | Show Axes: false 99 | Show Trail: false 100 | Value: true 101 | link1: 102 | Alpha: 1 103 | Show Axes: false 104 | Show Trail: false 105 | Value: true 106 | link2: 107 | Alpha: 1 108 | Show Axes: false 109 | Show Trail: false 110 | Value: true 111 | link3: 112 | Alpha: 1 113 | Show Axes: false 114 | Show Trail: false 115 | Value: true 116 | link4: 117 | Alpha: 1 118 | Show Axes: false 119 | Show Trail: false 120 | Value: true 121 | link5: 122 | Alpha: 1 123 | Show Axes: false 124 | Show Trail: false 125 | Value: true 126 | link6: 127 | Alpha: 1 128 | Show Axes: false 129 | Show Trail: false 130 | Value: true 131 | Loop Animation: false 132 | Robot Alpha: 1 133 | Robot Color: 150; 50; 150 134 | Show Robot Collision: false 135 | Show Robot Visual: false 136 | Show Trail: false 137 | State Display Time: 0.05 s 138 | Trail Step Size: 1 139 | Trajectory Topic: /move_group/display_planned_path 140 | Planning Metrics: 141 | Payload: 1 142 | Show Joint Torques: false 143 | Show Manipulability: false 144 | Show Manipulability Index: false 145 | Show Weight Limit: false 146 | TextHeight: 0.0799999982 147 | Planning Request: 148 | Colliding Link Color: 255; 0; 0 149 | Goal State Alpha: 1 150 | Goal State Color: 250; 128; 0 151 | Interactive Marker Size: 0 152 | Joint Violation Color: 255; 0; 255 153 | Planning Group: arm 154 | Query Goal State: false 155 | Query Start State: false 156 | Show Workspace: false 157 | Start State Alpha: 1 158 | Start State Color: 0; 255; 0 159 | Planning Scene Topic: /move_group/monitored_planning_scene 160 | Robot Description: robot_description 161 | Scene Geometry: 162 | Scene Alpha: 1 163 | Scene Color: 50; 230; 50 164 | Scene Display Time: 0.100000001 165 | Show Scene Geometry: true 166 | Voxel Coloring: Z-Axis 167 | Voxel Rendering: Occupied Voxels 168 | Scene Robot: 169 | Attached Body Color: 150; 50; 150 170 | Links: 171 | All Links Enabled: true 172 | Expand Joint Details: false 173 | Expand Link Details: false 174 | Expand Tree: false 175 | Link Tree Style: Links in Alphabetic Order 176 | base_link: 177 | Alpha: 1 178 | Show Axes: false 179 | Show Trail: false 180 | Value: true 181 | bottom_link: 182 | Alpha: 1 183 | Show Axes: false 184 | Show Trail: false 185 | Value: true 186 | grasping_frame: 187 | Alpha: 1 188 | Show Axes: false 189 | Show Trail: false 190 | gripper_finger_link1: 191 | Alpha: 1 192 | Show Axes: false 193 | Show Trail: false 194 | Value: true 195 | gripper_finger_link2: 196 | Alpha: 1 197 | Show Axes: false 198 | Show Trail: false 199 | Value: true 200 | link1: 201 | Alpha: 1 202 | Show Axes: false 203 | Show Trail: false 204 | Value: true 205 | link2: 206 | Alpha: 1 207 | Show Axes: false 208 | Show Trail: false 209 | Value: true 210 | link3: 211 | Alpha: 1 212 | Show Axes: false 213 | Show Trail: false 214 | Value: true 215 | link4: 216 | Alpha: 1 217 | Show Axes: false 218 | Show Trail: false 219 | Value: true 220 | link5: 221 | Alpha: 1 222 | Show Axes: false 223 | Show Trail: false 224 | Value: true 225 | link6: 226 | Alpha: 1 227 | Show Axes: false 228 | Show Trail: false 229 | Value: true 230 | Robot Alpha: 0 231 | Show Robot Collision: false 232 | Show Robot Visual: true 233 | Value: true 234 | - Alpha: 1 235 | Axes Length: 0.200000003 236 | Axes Radius: 0.00999999978 237 | Class: rviz/Pose 238 | Color: 255; 25; 0 239 | Enabled: false 240 | Head Length: 0.300000012 241 | Head Radius: 0.100000001 242 | Name: Gripper Pose 243 | Shaft Length: 1 244 | Shaft Radius: 0.0500000007 245 | Shape: Axes 246 | Topic: /gripper_pose 247 | Unreliable: false 248 | Value: false 249 | - Alpha: 1 250 | Class: rviz/RobotModel 251 | Collision Enabled: false 252 | Enabled: true 253 | Links: 254 | All Links Enabled: true 255 | Expand Joint Details: false 256 | Expand Link Details: false 257 | Expand Tree: false 258 | Link Tree Style: Links in Alphabetic Order 259 | base_link: 260 | Alpha: 1 261 | Show Axes: false 262 | Show Trail: false 263 | Value: true 264 | bottom_link: 265 | Alpha: 1 266 | Show Axes: false 267 | Show Trail: false 268 | Value: true 269 | grasping_frame: 270 | Alpha: 1 271 | Show Axes: false 272 | Show Trail: false 273 | gripper_finger_link1: 274 | Alpha: 1 275 | Show Axes: false 276 | Show Trail: false 277 | Value: true 278 | gripper_finger_link2: 279 | Alpha: 1 280 | Show Axes: false 281 | Show Trail: false 282 | Value: true 283 | link1: 284 | Alpha: 1 285 | Show Axes: false 286 | Show Trail: false 287 | Value: true 288 | link2: 289 | Alpha: 1 290 | Show Axes: false 291 | Show Trail: false 292 | Value: true 293 | link3: 294 | Alpha: 1 295 | Show Axes: false 296 | Show Trail: false 297 | Value: true 298 | link4: 299 | Alpha: 1 300 | Show Axes: false 301 | Show Trail: false 302 | Value: true 303 | link5: 304 | Alpha: 1 305 | Show Axes: false 306 | Show Trail: false 307 | Value: true 308 | link6: 309 | Alpha: 1 310 | Show Axes: false 311 | Show Trail: false 312 | Value: true 313 | Name: RobotModel 314 | Robot Description: robot_description 315 | TF Prefix: "" 316 | Update Interval: 0 317 | Value: true 318 | Visual Enabled: true 319 | Enabled: true 320 | Global Options: 321 | Background Color: 179; 179; 179 322 | Fixed Frame: base_link 323 | Frame Rate: 30 324 | Name: root 325 | Tools: 326 | - Class: rviz/Interact 327 | Hide Inactive Objects: true 328 | - Class: rviz/MoveCamera 329 | - Class: rviz/Select 330 | Value: true 331 | Views: 332 | Current: 333 | Class: rviz/Orbit 334 | Distance: 1.54106736 335 | Enable Stereo Rendering: 336 | Stereo Eye Separation: 0.0599999987 337 | Stereo Focal Distance: 1 338 | Swap Stereo Eyes: false 339 | Value: false 340 | Focal Point: 341 | X: 0.18268007 342 | Y: -0.110009015 343 | Z: 0.296199769 344 | Focal Shape Fixed Size: true 345 | Focal Shape Size: 0.0500000007 346 | Name: Current View 347 | Near Clip Distance: 0.00999999978 348 | Pitch: 0.369798124 349 | Target Frame: base_footprint 350 | Value: Orbit (rviz) 351 | Yaw: 5.76992083 352 | Saved: ~ 353 | Window Geometry: 354 | Displays: 355 | collapsed: false 356 | Height: 859 357 | Help: 358 | collapsed: false 359 | Hide Left Dock: false 360 | Hide Right Dock: false 361 | MotionPlanning: 362 | collapsed: false 363 | MotionPlanning - Slider: 364 | collapsed: false 365 | QMainWindow State: 000000ff00000000fd0000000100000000000001c600000315fc0200000007fb000000100044006900730070006c006100790073010000002800000315000000dd00fffffffb0000000800480065006c00700000000342000000bb0000007600fffffffb0000000a005600690065007700730000000244000000b0000000b000fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e0067000000012e0000017f0000000000000000fb0000002e004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d00200053006c00690064006500720000000000ffffffff0000004a00fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067000000012900000214000001cc00ffffff000003750000031500000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 366 | Views: 367 | collapsed: false 368 | Width: 1345 369 | X: 289 370 | Y: 64 371 | -------------------------------------------------------------------------------- /marm_planning/scripts/moveit_pick_and_place_demo.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | 4 | import rospy, sys 5 | import moveit_commander 6 | from geometry_msgs.msg import PoseStamped, Pose 7 | from moveit_commander import MoveGroupCommander, PlanningSceneInterface 8 | from moveit_msgs.msg import PlanningScene, ObjectColor 9 | from moveit_msgs.msg import Grasp, GripperTranslation, MoveItErrorCodes 10 | 11 | from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint 12 | from tf.transformations import quaternion_from_euler 13 | from copy import deepcopy 14 | 15 | GROUP_NAME_ARM = 'arm' 16 | GROUP_NAME_GRIPPER = 'gripper' 17 | 18 | GRIPPER_FRAME = 'grasping_frame' 19 | 20 | GRIPPER_OPEN = [0.004] 21 | GRIPPER_CLOSED = [0.01] 22 | 23 | REFERENCE_FRAME = 'base_link' 24 | 25 | class MoveItPickAndPlaceDemo: 26 | def __init__(self): 27 | # 初始化move_group的API 28 | moveit_commander.roscpp_initialize(sys.argv) 29 | 30 | # 初始化ROS节点 31 | rospy.init_node('moveit_pick_and_place_demo') 32 | 33 | # 初始化场景对象 34 | scene = PlanningSceneInterface() 35 | 36 | # 创建一个发布场景变化信息的发布者 37 | self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10) 38 | 39 | # 创建一个发布抓取姿态的发布者 40 | self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped, queue_size=10) 41 | 42 | # 创建一个存储物体颜色的字典对象 43 | self.colors = dict() 44 | 45 | # 初始化需要使用move group控制的机械臂中的arm group 46 | arm = MoveGroupCommander(GROUP_NAME_ARM) 47 | 48 | # 初始化需要使用move group控制的机械臂中的gripper group 49 | gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) 50 | 51 | # 获取终端link的名称 52 | end_effector_link = arm.get_end_effector_link() 53 | 54 | # 设置位置(单位：米)和姿态（单位：弧度）的允许误差 55 | arm.set_goal_position_tolerance(0.05) 56 | arm.set_goal_orientation_tolerance(0.1) 57 | 58 | # 当运动规划失败后，允许重新规划 59 | arm.allow_replanning(True) 60 | 61 | # 设置目标位置所使用的参考坐标系 62 | arm.set_pose_reference_frame(REFERENCE_FRAME) 63 | 64 | # 设置每次运动规划的时间限制：5s 65 | arm.set_planning_time(5) 66 | 67 | # 设置pick和place阶段的最大尝试次数 68 | max_pick_attempts = 5 69 | max_place_attempts = 5 70 | rospy.sleep(2) 71 | 72 | # 设置场景物体的名称 73 | table_id = 'table' 74 | box1_id = 'box1' 75 | box2_id = 'box2' 76 | target_id = 'target' 77 | 78 | # 移除场景中之前运行残留的物体 79 | scene.remove_world_object(table_id) 80 | scene.remove_world_object(box1_id) 81 | scene.remove_world_object(box2_id) 82 | scene.remove_world_object(target_id) 83 | 84 | # 移除场景中之前与机器臂绑定的物体 85 | scene.remove_attached_object(GRIPPER_FRAME, target_id) 86 | rospy.sleep(1) 87 | 88 | # 控制机械臂先回到初始化位置 89 | arm.set_named_target('home') 90 | arm.go() 91 | 92 | # 控制夹爪张开 93 | gripper.set_joint_value_target(GRIPPER_OPEN) 94 | gripper.go() 95 | rospy.sleep(1) 96 | 97 | # 设置桌面的高度 98 | table_ground = 0.2 99 | 100 | # 设置table、box1和box2的三维尺寸[长, 宽, 高] 101 | table_size = [0.2, 0.7, 0.01] 102 | box1_size = [0.1, 0.05, 0.05] 103 | box2_size = [0.05, 0.05, 0.15] 104 | 105 | # 将三个物体加入场景当中 106 | table_pose = PoseStamped() 107 | table_pose.header.frame_id = REFERENCE_FRAME 108 | table_pose.pose.position.x = 0.35 109 | table_pose.pose.position.y = 0.0 110 | table_pose.pose.position.z = table_ground + table_size[2] / 2.0 111 | table_pose.pose.orientation.w = 1.0 112 | scene.add_box(table_id, table_pose, table_size) 113 | 114 | box1_pose = PoseStamped() 115 | box1_pose.header.frame_id = REFERENCE_FRAME 116 | box1_pose.pose.position.x = 0.31 117 | box1_pose.pose.position.y = -0.1 118 | box1_pose.pose.position.z = table_ground + table_size[2] + box1_size[2] / 2.0 119 | box1_pose.pose.orientation.w = 1.0 120 | scene.add_box(box1_id, box1_pose, box1_size) 121 | 122 | box2_pose = PoseStamped() 123 | box2_pose.header.frame_id = REFERENCE_FRAME 124 | box2_pose.pose.position.x = 0.29 125 | box2_pose.pose.position.y = 0.13 126 | box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0 127 | box2_pose.pose.orientation.w = 1.0 128 | scene.add_box(box2_id, box2_pose, box2_size) 129 | 130 | # 将桌子设置成红色，两个box设置成橙色 131 | self.setColor(table_id, 0.8, 0, 0, 1.0) 132 | self.setColor(box1_id, 0.8, 0.4, 0, 1.0) 133 | self.setColor(box2_id, 0.8, 0.4, 0, 1.0) 134 | 135 | # 设置目标物体的尺寸 136 | target_size = [0.02, 0.01, 0.12] 137 | 138 | # 设置目标物体的位置，位于桌面之上两个盒子之间 139 | target_pose = PoseStamped() 140 | target_pose.header.frame_id = REFERENCE_FRAME 141 | target_pose.pose.position.x = 0.32 142 | target_pose.pose.position.y = 0.0 143 | target_pose.pose.position.z = table_ground + table_size[2] + target_size[2] / 2.0 144 | target_pose.pose.orientation.w = 1.0 145 | 146 | # 将抓取的目标物体加入场景中 147 | scene.add_box(target_id, target_pose, target_size) 148 | 149 | # 将目标物体设置为黄色 150 | self.setColor(target_id, 0.9, 0.9, 0, 1.0) 151 | 152 | # 将场景中的颜色设置发布 153 | self.sendColors() 154 | 155 | # 设置支持的外观 156 | arm.set_support_surface_name(table_id) 157 | 158 | # 设置一个place阶段需要放置物体的目标位置 159 | place_pose = PoseStamped() 160 | place_pose.header.frame_id = REFERENCE_FRAME 161 | place_pose.pose.position.x = 0.32 162 | place_pose.pose.position.y = -0.2 163 | place_pose.pose.position.z = table_ground + table_size[2] + target_size[2] / 2.0 164 | place_pose.pose.orientation.w = 1.0 165 | 166 | # 将目标位置设置为机器人的抓取目标位置 167 | grasp_pose = target_pose 168 | 169 | # 生成抓取姿态 170 | grasps = self.make_grasps(grasp_pose, [target_id]) 171 | 172 | # 将抓取姿态发布，可以在rviz中显示 173 | for grasp in grasps: 174 | self.gripper_pose_pub.publish(grasp.grasp_pose) 175 | rospy.sleep(0.2) 176 | 177 | # 追踪抓取成功与否，以及抓取的尝试次数 178 | result = None 179 | n_attempts = 0 180 | 181 | # 重复尝试抓取，直道成功或者超多最大尝试次数 182 | while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts: 183 | n_attempts += 1 184 | rospy.loginfo("Pick attempt: " + str(n_attempts)) 185 | result = arm.pick(target_id, grasps) 186 | rospy.sleep(0.2) 187 | 188 | # 如果pick成功，则进入place阶段 189 | if result == MoveItErrorCodes.SUCCESS: 190 | result = None 191 | n_attempts = 0 192 | 193 | # 生成放置姿态 194 | places = self.make_places(place_pose) 195 | 196 | # 重复尝试放置，直道成功或者超多最大尝试次数 197 | while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts: 198 | n_attempts += 1 199 | rospy.loginfo("Place attempt: " + str(n_attempts)) 200 | for place in places: 201 | result = arm.place(target_id, place) 202 | if result == MoveItErrorCodes.SUCCESS: 203 | break 204 | rospy.sleep(0.2) 205 | 206 | if result != MoveItErrorCodes.SUCCESS: 207 | rospy.loginfo("Place operation failed after " + str(n_attempts) + " attempts.") 208 | else: 209 | rospy.loginfo("Pick operation failed after " + str(n_attempts) + " attempts.") 210 | 211 | # 控制机械臂回到初始化位置 212 | arm.set_named_target('home') 213 | arm.go() 214 | 215 | # 控制夹爪回到张开的状态 216 | gripper.set_joint_value_target(GRIPPER_OPEN) 217 | gripper.go() 218 | rospy.sleep(1) 219 | 220 | # 关闭并退出moveit 221 | moveit_commander.roscpp_shutdown() 222 | moveit_commander.os._exit(0) 223 | 224 | # 创建夹爪的姿态数据JointTrajectory 225 | def make_gripper_posture(self, joint_positions): 226 | # 初始化夹爪的关节运动轨迹 227 | t = JointTrajectory() 228 | 229 | # 设置夹爪的关节名称 230 | t.joint_names = ['finger_joint1'] 231 | 232 | # 初始化关节轨迹点 233 | tp = JointTrajectoryPoint() 234 | 235 | # 将输入的关节位置作为一个目标轨迹点 236 | tp.positions = joint_positions 237 | 238 | # 设置夹爪的力度 239 | tp.effort = [1.0] 240 | 241 | # 设置运动时间 242 | tp.time_from_start = rospy.Duration(1.0) 243 | 244 | # 将目标轨迹点加入到运动轨迹中 245 | t.points.append(tp) 246 | 247 | # 返回夹爪的关节运动轨迹 248 | return t 249 | 250 | # 使用给定向量创建夹爪的translation结构 251 | def make_gripper_translation(self, min_dist, desired, vector): 252 | # 初始化translation对象 253 | g = GripperTranslation() 254 | 255 | # 设置方向向量 256 | g.direction.vector.x = vector[0] 257 | g.direction.vector.y = vector[1] 258 | g.direction.vector.z = vector[2] 259 | 260 | # 设置参考坐标系 261 | g.direction.header.frame_id = GRIPPER_FRAME 262 | 263 | # 设置最小和期望的距离 264 | g.min_distance = min_dist 265 | g.desired_distance = desired 266 | 267 | return g 268 | 269 | # 创建一个允许的的抓取姿态列表 270 | def make_grasps(self, initial_pose_stamped, allowed_touch_objects): 271 | # 初始化抓取姿态对象 272 | g = Grasp() 273 | 274 | # 创建夹爪张开、闭合的姿态 275 | g.pre_grasp_posture = self.make_gripper_posture(GRIPPER_OPEN) 276 | g.grasp_posture = self.make_gripper_posture(GRIPPER_CLOSED) 277 | 278 | # 设置期望的夹爪靠近、撤离目标的参数 279 | g.pre_grasp_approach = self.make_gripper_translation(0.01, 0.1, [1.0, 0.0, 0.0]) 280 | g.post_grasp_retreat = self.make_gripper_translation(0.1, 0.15, [0.0, -1.0, 1.0]) 281 | 282 | # 设置抓取姿态 283 | g.grasp_pose = initial_pose_stamped 284 | 285 | # 需要尝试改变姿态的数据列表 286 | pitch_vals = [0, 0.1, -0.1, 0.2, -0.2, 0.3, -0.3] 287 | yaw_vals = [0] 288 | 289 | # 抓取姿态的列表 290 | grasps = [] 291 | 292 | # 改变姿态，生成抓取动作 293 | for y in yaw_vals: 294 | for p in pitch_vals: 295 | # 欧拉角到四元数的转换 296 | q = quaternion_from_euler(0, p, y) 297 | 298 | # 设置抓取的姿态 299 | g.grasp_pose.pose.orientation.x = q[0] 300 | g.grasp_pose.pose.orientation.y = q[1] 301 | g.grasp_pose.pose.orientation.z = q[2] 302 | g.grasp_pose.pose.orientation.w = q[3] 303 | 304 | # 设置抓取的唯一id号 305 | g.id = str(len(grasps)) 306 | 307 | # 设置允许接触的物体 308 | g.allowed_touch_objects = allowed_touch_objects 309 | 310 | # 将本次规划的抓取放入抓取列表中 311 | grasps.append(deepcopy(g)) 312 | 313 | # 返回抓取列表 314 | return grasps 315 | 316 | # 创建一个允许的放置姿态列表 317 | def make_places(self, init_pose): 318 | # 初始化放置抓取物体的位置 319 | place = PoseStamped() 320 | 321 | # 设置放置抓取物体的位置 322 | place = init_pose 323 | 324 | # 定义x方向上用于尝试放置物体的偏移参数 325 | x_vals = [0, 0.005, 0.01, 0.015, -0.005, -0.01, -0.015] 326 | 327 | # 定义y方向上用于尝试放置物体的偏移参数 328 | y_vals = [0, 0.005, 0.01, 0.015, -0.005, -0.01, -0.015] 329 | 330 | pitch_vals = [0] 331 | 332 | # 定义用于尝试放置物体的偏航角参数 333 | yaw_vals = [0] 334 | 335 | # 定义放置物体的姿态列表 336 | places = [] 337 | 338 | # 生成每一个角度和偏移方向上的抓取姿态 339 | for y in yaw_vals: 340 | for p in pitch_vals: 341 | for y in y_vals: 342 | for x in x_vals: 343 | place.pose.position.x = init_pose.pose.position.x + x 344 | place.pose.position.y = init_pose.pose.position.y + y 345 | 346 | # 欧拉角到四元数的转换 347 | q = quaternion_from_euler(0, p, y) 348 | 349 | # 欧拉角到四元数的转换 350 | place.pose.orientation.x = q[0] 351 | place.pose.orientation.y = q[1] 352 | place.pose.orientation.z = q[2] 353 | place.pose.orientation.w = q[3] 354 | 355 | # 将该放置姿态加入列表 356 | places.append(deepcopy(place)) 357 | 358 | # 返回放置物体的姿态列表 359 | return places 360 | 361 | # 设置场景物体的颜色 362 | def setColor(self, name, r, g, b, a = 0.9): 363 | # 初始化moveit颜色对象 364 | color = ObjectColor() 365 | 366 | # 设置颜色值 367 | color.id = name 368 | color.color.r = r 369 | color.color.g = g 370 | color.color.b = b 371 | color.color.a = a 372 | 373 | # 更新颜色字典 374 | self.colors[name] = color 375 | 376 | # 将颜色设置发送并应用到moveit场景当中 377 | def sendColors(self): 378 | # 初始化规划场景对象 379 | p = PlanningScene() 380 | 381 | # 需要设置规划场景是否有差异 382 | p.is_diff = True 383 | 384 | # 从颜色字典中取出颜色设置 385 | for color in self.colors.values(): 386 | p.object_colors.append(color) 387 | 388 | # 发布场景物体颜色设置 389 | self.scene_pub.publish(p) 390 | 391 | if __name__ == "__main__": 392 | MoveItPickAndPlaceDemo() 393 | 394 | 395 | -------------------------------------------------------------------------------- /marm_description/urdf/arm.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 | 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 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | 262 | 263 | 264 | 265 | 266 | 267 | 268 | 269 | 270 | 271 | 272 | 273 | 274 | 275 | 276 | 277 | 278 | 279 | 280 | 281 | 282 | 283 | 284 | 285 | 286 | 287 | 288 | 289 | 290 | 291 | 292 | 293 | 294 | 295 | 296 | 297 | 298 | 299 | 300 | 301 | 302 | 303 | 304 | 305 | 306 | 307 | 308 | 309 | 310 | 311 | 312 | 313 | 314 | 315 | 316 | 317 | 318 | 319 | 320 | 321 | 322 | 323 | 324 | Gazebo/White 325 | 326 | 327 | Gazebo/White 328 | 329 | 330 | Gazebo/Blue 331 | 332 | 333 | Gazebo/White 334 | 335 | 336 | Gazebo/Blue 337 | 338 | 339 | Gazebo/Black 340 | 341 | 342 | Gazebo/White 343 | 344 | 345 | Gazebo/Blue 346 | 347 | 348 | Gazebo/White 349 | 350 | 351 | Gazebo/White 352 | 353 | 354 | 355 | 356 | 357 | transmission_interface/SimpleTransmission 358 | 359 | hardware_interface/PositionJointInterface 360 | 361 | 362 | hardware_interface/PositionJointInterface 363 | 1 364 | 365 | 366 | 367 | 368 | 369 | 370 | 371 | 372 | 373 | 374 | 375 | 376 | 377 | 378 | 379 | /arm 380 | 381 | 382 | 383 | 384 | -------------------------------------------------------------------------------- /marm_moveit_config/launch/moveit.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 84 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /MotionPlanning1 8 | Splitter Ratio: 0.74256 9 | Tree Height: 664 10 | - Class: rviz/Help 11 | Name: Help 12 | - Class: rviz/Views 13 | Expanded: 14 | - /Current View1 15 | Name: Views 16 | Splitter Ratio: 0.5 17 | Visualization Manager: 18 | Class: "" 19 | Displays: 20 | - Alpha: 0.5 21 | Cell Size: 1 22 | Class: rviz/Grid 23 | Color: 160; 160; 164 24 | Enabled: true 25 | Line Style: 26 | Line Width: 0.03 27 | Value: Lines 28 | Name: Grid 29 | Normal Cell Count: 0 30 | Offset: 31 | X: 0 32 | Y: 0 33 | Z: 0 34 | Plane: XY 35 | Plane Cell Count: 10 36 | Reference Frame: 37 | Value: true 38 | - Class: moveit_rviz_plugin/MotionPlanning 39 | Enabled: true 40 | MoveIt_Goal_Tolerance: 0 41 | MoveIt_Planning_Time: 5 42 | MoveIt_Use_Constraint_Aware_IK: true 43 | MoveIt_Warehouse_Host: 127.0.0.1 44 | MoveIt_Warehouse_Port: 33829 45 | Name: MotionPlanning 46 | Planned Path: 47 | Links: 48 | base_bellow_link: 49 | Alpha: 1 50 | Show Axes: false 51 | Show Trail: false 52 | Value: true 53 | base_footprint: 54 | Alpha: 1 55 | Show Axes: false 56 | Show Trail: false 57 | Value: true 58 | base_link: 59 | Alpha: 1 60 | Show Axes: false 61 | Show Trail: false 62 | Value: true 63 | bl_caster_l_wheel_link: 64 | Alpha: 1 65 | Show Axes: false 66 | Show Trail: false 67 | Value: true 68 | bl_caster_r_wheel_link: 69 | Alpha: 1 70 | Show Axes: false 71 | Show Trail: false 72 | Value: true 73 | bl_caster_rotation_link: 74 | Alpha: 1 75 | Show Axes: false 76 | Show Trail: false 77 | Value: true 78 | br_caster_l_wheel_link: 79 | Alpha: 1 80 | Show Axes: false 81 | Show Trail: false 82 | Value: true 83 | br_caster_r_wheel_link: 84 | Alpha: 1 85 | Show Axes: false 86 | Show Trail: false 87 | Value: true 88 | br_caster_rotation_link: 89 | Alpha: 1 90 | Show Axes: false 91 | Show Trail: false 92 | Value: true 93 | double_stereo_link: 94 | Alpha: 1 95 | Show Axes: false 96 | Show Trail: false 97 | Value: true 98 | fl_caster_l_wheel_link: 99 | Alpha: 1 100 | Show Axes: false 101 | Show Trail: false 102 | Value: true 103 | fl_caster_r_wheel_link: 104 | Alpha: 1 105 | Show Axes: false 106 | Show Trail: false 107 | Value: true 108 | fl_caster_rotation_link: 109 | Alpha: 1 110 | Show Axes: false 111 | Show Trail: false 112 | Value: true 113 | fr_caster_l_wheel_link: 114 | Alpha: 1 115 | Show Axes: false 116 | Show Trail: false 117 | Value: true 118 | fr_caster_r_wheel_link: 119 | Alpha: 1 120 | Show Axes: false 121 | Show Trail: false 122 | Value: true 123 | fr_caster_rotation_link: 124 | Alpha: 1 125 | Show Axes: false 126 | Show Trail: false 127 | Value: true 128 | head_mount_kinect_ir_link: 129 | Alpha: 1 130 | Show Axes: false 131 | Show Trail: false 132 | Value: true 133 | head_mount_kinect_rgb_link: 134 | Alpha: 1 135 | Show Axes: false 136 | Show Trail: false 137 | Value: true 138 | head_mount_link: 139 | Alpha: 1 140 | Show Axes: false 141 | Show Trail: false 142 | Value: true 143 | head_mount_prosilica_link: 144 | Alpha: 1 145 | Show Axes: false 146 | Show Trail: false 147 | Value: true 148 | head_pan_link: 149 | Alpha: 1 150 | Show Axes: false 151 | Show Trail: false 152 | Value: true 153 | head_plate_frame: 154 | Alpha: 1 155 | Show Axes: false 156 | Show Trail: false 157 | Value: true 158 | head_tilt_link: 159 | Alpha: 1 160 | Show Axes: false 161 | Show Trail: false 162 | Value: true 163 | l_elbow_flex_link: 164 | Alpha: 1 165 | Show Axes: false 166 | Show Trail: false 167 | Value: true 168 | l_forearm_link: 169 | Alpha: 1 170 | Show Axes: false 171 | Show Trail: false 172 | Value: true 173 | l_forearm_roll_link: 174 | Alpha: 1 175 | Show Axes: false 176 | Show Trail: false 177 | Value: true 178 | l_gripper_l_finger_link: 179 | Alpha: 1 180 | Show Axes: false 181 | Show Trail: false 182 | Value: true 183 | l_gripper_l_finger_tip_link: 184 | Alpha: 1 185 | Show Axes: false 186 | Show Trail: false 187 | Value: true 188 | l_gripper_motor_accelerometer_link: 189 | Alpha: 1 190 | Show Axes: false 191 | Show Trail: false 192 | Value: true 193 | l_gripper_palm_link: 194 | Alpha: 1 195 | Show Axes: false 196 | Show Trail: false 197 | Value: true 198 | l_gripper_r_finger_link: 199 | Alpha: 1 200 | Show Axes: false 201 | Show Trail: false 202 | Value: true 203 | l_gripper_r_finger_tip_link: 204 | Alpha: 1 205 | Show Axes: false 206 | Show Trail: false 207 | Value: true 208 | l_shoulder_lift_link: 209 | Alpha: 1 210 | Show Axes: false 211 | Show Trail: false 212 | Value: true 213 | l_shoulder_pan_link: 214 | Alpha: 1 215 | Show Axes: false 216 | Show Trail: false 217 | Value: true 218 | l_upper_arm_link: 219 | Alpha: 1 220 | Show Axes: false 221 | Show Trail: false 222 | Value: true 223 | l_upper_arm_roll_link: 224 | Alpha: 1 225 | Show Axes: false 226 | Show Trail: false 227 | Value: true 228 | l_wrist_flex_link: 229 | Alpha: 1 230 | Show Axes: false 231 | Show Trail: false 232 | Value: true 233 | l_wrist_roll_link: 234 | Alpha: 1 235 | Show Axes: false 236 | Show Trail: false 237 | Value: true 238 | laser_tilt_mount_link: 239 | Alpha: 1 240 | Show Axes: false 241 | Show Trail: false 242 | Value: true 243 | r_elbow_flex_link: 244 | Alpha: 1 245 | Show Axes: false 246 | Show Trail: false 247 | Value: true 248 | r_forearm_link: 249 | Alpha: 1 250 | Show Axes: false 251 | Show Trail: false 252 | Value: true 253 | r_forearm_roll_link: 254 | Alpha: 1 255 | Show Axes: false 256 | Show Trail: false 257 | Value: true 258 | r_gripper_l_finger_link: 259 | Alpha: 1 260 | Show Axes: false 261 | Show Trail: false 262 | Value: true 263 | r_gripper_l_finger_tip_link: 264 | Alpha: 1 265 | Show Axes: false 266 | Show Trail: false 267 | Value: true 268 | r_gripper_motor_accelerometer_link: 269 | Alpha: 1 270 | Show Axes: false 271 | Show Trail: false 272 | Value: true 273 | r_gripper_palm_link: 274 | Alpha: 1 275 | Show Axes: false 276 | Show Trail: false 277 | Value: true 278 | r_gripper_r_finger_link: 279 | Alpha: 1 280 | Show Axes: false 281 | Show Trail: false 282 | Value: true 283 | r_gripper_r_finger_tip_link: 284 | Alpha: 1 285 | Show Axes: false 286 | Show Trail: false 287 | Value: true 288 | r_shoulder_lift_link: 289 | Alpha: 1 290 | Show Axes: false 291 | Show Trail: false 292 | Value: true 293 | r_shoulder_pan_link: 294 | Alpha: 1 295 | Show Axes: false 296 | Show Trail: false 297 | Value: true 298 | r_upper_arm_link: 299 | Alpha: 1 300 | Show Axes: false 301 | Show Trail: false 302 | Value: true 303 | r_upper_arm_roll_link: 304 | Alpha: 1 305 | Show Axes: false 306 | Show Trail: false 307 | Value: true 308 | r_wrist_flex_link: 309 | Alpha: 1 310 | Show Axes: false 311 | Show Trail: false 312 | Value: true 313 | r_wrist_roll_link: 314 | Alpha: 1 315 | Show Axes: false 316 | Show Trail: false 317 | Value: true 318 | sensor_mount_link: 319 | Alpha: 1 320 | Show Axes: false 321 | Show Trail: false 322 | Value: true 323 | torso_lift_link: 324 | Alpha: 1 325 | Show Axes: false 326 | Show Trail: false 327 | Value: true 328 | Loop Animation: true 329 | Robot Alpha: 0.5 330 | Show Robot Collision: false 331 | Show Robot Visual: true 332 | Show Trail: false 333 | State Display Time: 0.05 s 334 | Trajectory Topic: move_group/display_planned_path 335 | Planning Metrics: 336 | Payload: 1 337 | Show Joint Torques: false 338 | Show Manipulability: false 339 | Show Manipulability Index: false 340 | Show Weight Limit: false 341 | Planning Request: 342 | Colliding Link Color: 255; 0; 0 343 | Goal State Alpha: 1 344 | Goal State Color: 250; 128; 0 345 | Interactive Marker Size: 0 346 | Joint Violation Color: 255; 0; 255 347 | Planning Group: left_arm 348 | Query Goal State: true 349 | Query Start State: false 350 | Show Workspace: false 351 | Start State Alpha: 1 352 | Start State Color: 0; 255; 0 353 | Planning Scene Topic: move_group/monitored_planning_scene 354 | Robot Description: robot_description 355 | Scene Geometry: 356 | Scene Alpha: 1 357 | Scene Color: 50; 230; 50 358 | Scene Display Time: 0.2 359 | Show Scene Geometry: true 360 | Voxel Coloring: Z-Axis 361 | Voxel Rendering: Occupied Voxels 362 | Scene Robot: 363 | Attached Body Color: 150; 50; 150 364 | Links: 365 | base_bellow_link: 366 | Alpha: 1 367 | Show Axes: false 368 | Show Trail: false 369 | Value: true 370 | base_footprint: 371 | Alpha: 1 372 | Show Axes: false 373 | Show Trail: false 374 | Value: true 375 | base_link: 376 | Alpha: 1 377 | Show Axes: false 378 | Show Trail: false 379 | Value: true 380 | bl_caster_l_wheel_link: 381 | Alpha: 1 382 | Show Axes: false 383 | Show Trail: false 384 | Value: true 385 | bl_caster_r_wheel_link: 386 | Alpha: 1 387 | Show Axes: false 388 | Show Trail: false 389 | Value: true 390 | bl_caster_rotation_link: 391 | Alpha: 1 392 | Show Axes: false 393 | Show Trail: false 394 | Value: true 395 | br_caster_l_wheel_link: 396 | Alpha: 1 397 | Show Axes: false 398 | Show Trail: false 399 | Value: true 400 | br_caster_r_wheel_link: 401 | Alpha: 1 402 | Show Axes: false 403 | Show Trail: false 404 | Value: true 405 | br_caster_rotation_link: 406 | Alpha: 1 407 | Show Axes: false 408 | Show Trail: false 409 | Value: true 410 | double_stereo_link: 411 | Alpha: 1 412 | Show Axes: false 413 | Show Trail: false 414 | Value: true 415 | fl_caster_l_wheel_link: 416 | Alpha: 1 417 | Show Axes: false 418 | Show Trail: false 419 | Value: true 420 | fl_caster_r_wheel_link: 421 | Alpha: 1 422 | Show Axes: false 423 | Show Trail: false 424 | Value: true 425 | fl_caster_rotation_link: 426 | Alpha: 1 427 | Show Axes: false 428 | Show Trail: false 429 | Value: true 430 | fr_caster_l_wheel_link: 431 | Alpha: 1 432 | Show Axes: false 433 | Show Trail: false 434 | Value: true 435 | fr_caster_r_wheel_link: 436 | Alpha: 1 437 | Show Axes: false 438 | Show Trail: false 439 | Value: true 440 | fr_caster_rotation_link: 441 | Alpha: 1 442 | Show Axes: false 443 | Show Trail: false 444 | Value: true 445 | head_mount_kinect_ir_link: 446 | Alpha: 1 447 | Show Axes: false 448 | Show Trail: false 449 | Value: true 450 | head_mount_kinect_rgb_link: 451 | Alpha: 1 452 | Show Axes: false 453 | Show Trail: false 454 | Value: true 455 | head_mount_link: 456 | Alpha: 1 457 | Show Axes: false 458 | Show Trail: false 459 | Value: true 460 | head_mount_prosilica_link: 461 | Alpha: 1 462 | Show Axes: false 463 | Show Trail: false 464 | Value: true 465 | head_pan_link: 466 | Alpha: 1 467 | Show Axes: false 468 | Show Trail: false 469 | Value: true 470 | head_plate_frame: 471 | Alpha: 1 472 | Show Axes: false 473 | Show Trail: false 474 | Value: true 475 | head_tilt_link: 476 | Alpha: 1 477 | Show Axes: false 478 | Show Trail: false 479 | Value: true 480 | l_elbow_flex_link: 481 | Alpha: 1 482 | Show Axes: false 483 | Show Trail: false 484 | Value: true 485 | l_forearm_link: 486 | Alpha: 1 487 | Show Axes: false 488 | Show Trail: false 489 | Value: true 490 | l_forearm_roll_link: 491 | Alpha: 1 492 | Show Axes: false 493 | Show Trail: false 494 | Value: true 495 | l_gripper_l_finger_link: 496 | Alpha: 1 497 | Show Axes: false 498 | Show Trail: false 499 | Value: true 500 | l_gripper_l_finger_tip_link: 501 | Alpha: 1 502 | Show Axes: false 503 | Show Trail: false 504 | Value: true 505 | l_gripper_motor_accelerometer_link: 506 | Alpha: 1 507 | Show Axes: false 508 | Show Trail: false 509 | Value: true 510 | l_gripper_palm_link: 511 | Alpha: 1 512 | Show Axes: false 513 | Show Trail: false 514 | Value: true 515 | l_gripper_r_finger_link: 516 | Alpha: 1 517 | Show Axes: false 518 | Show Trail: false 519 | Value: true 520 | l_gripper_r_finger_tip_link: 521 | Alpha: 1 522 | Show Axes: false 523 | Show Trail: false 524 | Value: true 525 | l_shoulder_lift_link: 526 | Alpha: 1 527 | Show Axes: false 528 | Show Trail: false 529 | Value: true 530 | l_shoulder_pan_link: 531 | Alpha: 1 532 | Show Axes: false 533 | Show Trail: false 534 | Value: true 535 | l_upper_arm_link: 536 | Alpha: 1 537 | Show Axes: false 538 | Show Trail: false 539 | Value: true 540 | l_upper_arm_roll_link: 541 | Alpha: 1 542 | Show Axes: false 543 | Show Trail: false 544 | Value: true 545 | l_wrist_flex_link: 546 | Alpha: 1 547 | Show Axes: false 548 | Show Trail: false 549 | Value: true 550 | l_wrist_roll_link: 551 | Alpha: 1 552 | Show Axes: false 553 | Show Trail: false 554 | Value: true 555 | laser_tilt_mount_link: 556 | Alpha: 1 557 | Show Axes: false 558 | Show Trail: false 559 | Value: true 560 | r_elbow_flex_link: 561 | Alpha: 1 562 | Show Axes: false 563 | Show Trail: false 564 | Value: true 565 | r_forearm_link: 566 | Alpha: 1 567 | Show Axes: false 568 | Show Trail: false 569 | Value: true 570 | r_forearm_roll_link: 571 | Alpha: 1 572 | Show Axes: false 573 | Show Trail: false 574 | Value: true 575 | r_gripper_l_finger_link: 576 | Alpha: 1 577 | Show Axes: false 578 | Show Trail: false 579 | Value: true 580 | r_gripper_l_finger_tip_link: 581 | Alpha: 1 582 | Show Axes: false 583 | Show Trail: false 584 | Value: true 585 | r_gripper_motor_accelerometer_link: 586 | Alpha: 1 587 | Show Axes: false 588 | Show Trail: false 589 | Value: true 590 | r_gripper_palm_link: 591 | Alpha: 1 592 | Show Axes: false 593 | Show Trail: false 594 | Value: true 595 | r_gripper_r_finger_link: 596 | Alpha: 1 597 | Show Axes: false 598 | Show Trail: false 599 | Value: true 600 | r_gripper_r_finger_tip_link: 601 | Alpha: 1 602 | Show Axes: false 603 | Show Trail: false 604 | Value: true 605 | r_shoulder_lift_link: 606 | Alpha: 1 607 | Show Axes: false 608 | Show Trail: false 609 | Value: true 610 | r_shoulder_pan_link: 611 | Alpha: 1 612 | Show Axes: false 613 | Show Trail: false 614 | Value: true 615 | r_upper_arm_link: 616 | Alpha: 1 617 | Show Axes: false 618 | Show Trail: false 619 | Value: true 620 | r_upper_arm_roll_link: 621 | Alpha: 1 622 | Show Axes: false 623 | Show Trail: false 624 | Value: true 625 | r_wrist_flex_link: 626 | Alpha: 1 627 | Show Axes: false 628 | Show Trail: false 629 | Value: true 630 | r_wrist_roll_link: 631 | Alpha: 1 632 | Show Axes: false 633 | Show Trail: false 634 | Value: true 635 | sensor_mount_link: 636 | Alpha: 1 637 | Show Axes: false 638 | Show Trail: false 639 | Value: true 640 | torso_lift_link: 641 | Alpha: 1 642 | Show Axes: false 643 | Show Trail: false 644 | Value: true 645 | Robot Alpha: 0.5 646 | Show Scene Robot: true 647 | Value: true 648 | Enabled: true 649 | Global Options: 650 | Background Color: 48; 48; 48 651 | Fixed Frame: /base_link 652 | Name: root 653 | Tools: 654 | - Class: rviz/Interact 655 | Hide Inactive Objects: true 656 | - Class: rviz/MoveCamera 657 | - Class: rviz/Select 658 | Value: true 659 | Views: 660 | Current: 661 | Class: rviz/XYOrbit 662 | Distance: 2.9965 663 | Focal Point: 664 | X: 0.113567 665 | Y: 0.10592 666 | Z: 2.23518e-07 667 | Name: Current View 668 | Near Clip Distance: 0.01 669 | Pitch: 0.509797 670 | Target Frame: /base_link 671 | Value: XYOrbit (rviz) 672 | Yaw: 5.65995 673 | Saved: ~ 674 | Window Geometry: 675 | Displays: 676 | collapsed: false 677 | Height: 1337 678 | Help: 679 | collapsed: false 680 | Hide Left Dock: false 681 | Hide Right Dock: false 682 | Motion Planning: 683 | collapsed: false 684 | QMainWindow State: 000000ff00000000fd0000000100000000000002a2000004bcfc0200000005fb000000100044006900730070006c00610079007301000000410000032d000000dd00fffffffb0000000800480065006c00700000000342000000bb0000007600fffffffb0000000a0056006900650077007300000003b0000000b0000000b000fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000017400ffffff0000047a000004bc00000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 685 | Views: 686 | collapsed: false 687 | Width: 1828 688 | X: 459 689 | Y: -243 690 | --------------------------------------------------------------------------------