├── rviz.png ├── sample.png ├── 5 dof rviz.png ├── 5 dof gazebo.png ├── README.md ├── 5 DOF ARM ├── autoarm_gazebo │ ├── config │ │ ├── joint_state_controllers.yaml │ │ ├── joint_trajectory_controllers.yaml │ │ └── joint_position_controllers.yaml │ ├── launch │ │ ├── autoarm_with_joint_position_controllers.launch │ │ ├── autoarm_joint_trajectory_controllers.launch │ │ ├── autoarm_joint_position_controllers.launch │ │ ├── autoarm_joint_state_controllers.launch │ │ └── autoarm_in_empty_world.launch │ ├── package.xml │ └── CMakeLists.txt ├── autoarm_moveit_config │ ├── launch │ │ ├── autoarm_moveit_sensor_manager.launch.xml │ │ ├── planning_pipeline.launch.xml │ │ ├── fake_moveit_controller_manager.launch.xml │ │ ├── warehouse.launch │ │ ├── setup_assistant.launch │ │ ├── warehouse_settings.launch.xml │ │ ├── joystick_control.launch │ │ ├── sensor_manager.launch.xml │ │ ├── autoarm_moveit_controller_manager.launch.xml │ │ ├── moveit_rviz.launch │ │ ├── default_warehouse_db.launch │ │ ├── run_benchmark_ompl.launch │ │ ├── ompl_planning_pipeline.launch.xml │ │ ├── trajectory_execution.launch.xml │ │ ├── planning_context.launch │ │ ├── demo.launch │ │ ├── move_group.launch │ │ ├── move_group.launch~ │ │ └── moveit.rviz │ ├── config │ │ ├── kinematics.yaml │ │ ├── sensors_rgbd.yaml │ │ ├── fake_controllers.yaml │ │ ├── controllers.yaml │ │ ├── joint_limits.yaml │ │ ├── ompl_planning.yaml │ │ └── autoarm.srdf │ ├── .setup_assistant │ ├── CMakeLists.txt │ └── package.xml ├── autoarm_moveit │ ├── launch │ │ ├── autoarm_grasp_pose_generator.launch │ │ ├── autoarm_gazebo.launch │ │ └── autoarm_gazebo_with_rviz.launch │ ├── config │ │ └── grasp_data.yaml │ ├── src │ │ ├── test_random.cpp │ │ └── test_custom.cpp │ ├── package.xml │ ├── nodes │ │ └── joint_state_muxer │ ├── CMakeLists.txt │ └── scripts │ │ ├── pick_and_place.py │ │ └── pick_and_place.py~ └── autoarm_description │ ├── launch │ ├── lata_rviz.launch │ └── autoarm_rviz.launch │ ├── package.xml │ ├── config │ └── urdf.rviz │ ├── CMakeLists.txt │ └── urdf │ ├── autoarm.xacro │ └── autoarm.xacro~ ├── .gitattributes └── .gitignore /rviz.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Dhivin/5-DOF-ARM-IN-ROS/HEAD/rviz.png -------------------------------------------------------------------------------- /sample.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Dhivin/5-DOF-ARM-IN-ROS/HEAD/sample.png -------------------------------------------------------------------------------- /5 dof rviz.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Dhivin/5-DOF-ARM-IN-ROS/HEAD/5 dof rviz.png -------------------------------------------------------------------------------- /5 dof gazebo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Dhivin/5-DOF-ARM-IN-ROS/HEAD/5 dof gazebo.png -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # 5-DOF-ARM-IN-ROS 2 | 5 DOF ARM 3 | ![Alt text](sample.png?raw=true "Optional Title") 4 | ![Alt text](rviz.png?raw=true "Optional Title") 5 | -------------------------------------------------------------------------------- /5 DOF ARM/autoarm_gazebo/config/joint_state_controllers.yaml: -------------------------------------------------------------------------------- 1 | autoarm: 2 | joint_state_controller: 3 | type: joint_state_controller/JointStateController 4 | publish_rate: 50 5 | -------------------------------------------------------------------------------- /5 DOF ARM/autoarm_moveit_config/launch/autoarm_moveit_sensor_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /5 DOF ARM/autoarm_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.005 5 | kinematics_solver_attempts: 3 -------------------------------------------------------------------------------- /5 DOF ARM/autoarm_moveit_config/.setup_assistant: -------------------------------------------------------------------------------- 1 | moveit_setup_assistant_config: 2 | URDF: 3 | package: autoarm_description 4 | relative_path: urdf/autoarm.xacro 5 | SRDF: 6 | relative_path: config/autoarm.srdf 7 | CONFIG: 8 | generated_timestamp: 1462263276 -------------------------------------------------------------------------------- /5 DOF ARM/autoarm_moveit_config/config/sensors_rgbd.yaml: -------------------------------------------------------------------------------- 1 | sensors: 2 | - sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater 3 | point_cloud_topic: /rgbd_camera/depth/points 4 | max_range: 10 5 | padding_offset: 0.01 6 | padding_scale: 1.0 7 | point_subsample: 1 8 | filtered_cloud_topic: output_cloud 9 | 10 | -------------------------------------------------------------------------------- /5 DOF ARM/autoarm_moveit/launch/autoarm_grasp_pose_generator.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /5 DOF ARM/autoarm_moveit_config/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(autoarm_moveit_config) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | catkin_package() 7 | 8 | install(DIRECTORY launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 9 | install(DIRECTORY config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 10 | -------------------------------------------------------------------------------- /5 DOF ARM/autoarm_gazebo/launch/autoarm_with_joint_position_controllers.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /5 DOF ARM/autoarm_moveit_config/launch/planning_pipeline.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /5 DOF ARM/autoarm_moveit_config/config/fake_controllers.yaml: -------------------------------------------------------------------------------- 1 | controller_list: 2 | - name: fake_arm_controller 3 | joints: 4 | - shoulder_pan_joint 5 | - shoulder_pitch_joint 6 | - elbow_roll_joint 7 | - elbow_pitch_joint 8 | - wrist_roll_joint 9 | - wrist_pitch_joint 10 | - gripper_roll_joint 11 | - name: fake_gripper_controller 12 | joints: 13 | - finger_joint1 14 | - finger_joint2 -------------------------------------------------------------------------------- /.gitattributes: -------------------------------------------------------------------------------- 1 | # Auto detect text files and perform LF normalization 2 | * text=auto 3 | 4 | # Custom for Visual Studio 5 | *.cs diff=csharp 6 | 7 | # Standard to msysgit 8 | *.doc diff=astextplain 9 | *.DOC diff=astextplain 10 | *.docx diff=astextplain 11 | *.DOCX diff=astextplain 12 | *.dot diff=astextplain 13 | *.DOT diff=astextplain 14 | *.pdf diff=astextplain 15 | *.PDF diff=astextplain 16 | *.rtf diff=astextplain 17 | *.RTF diff=astextplain 18 | -------------------------------------------------------------------------------- /5 DOF ARM/autoarm_gazebo/launch/autoarm_joint_trajectory_controllers.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /5 DOF ARM/autoarm_moveit_config/launch/fake_moveit_controller_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /5 DOF ARM/autoarm_description/launch/lata_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /5 DOF ARM/autoarm_moveit/launch/autoarm_gazebo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /5 DOF ARM/autoarm_moveit/config/grasp_data.yaml: -------------------------------------------------------------------------------- 1 | base_link: 'base_link' 2 | 3 | gripper: 4 | end_effector_name: 'gripper' 5 | 6 | joints : ['finger_joint1', 'finger_joint2'] 7 | 8 | pregrasp_posture : [0.0, 0.0] 9 | pregrasp_time_from_start : 4.0 10 | 11 | grasp_posture : [1.0, 1.0] 12 | grasp_time_from_start : 4.0 13 | 14 | # Desired pose from end effector to grasp [x, y, z] + [R, P, Y] 15 | grasp_pose_to_eef : [0.0, 0.0, 0.0] 16 | grasp_pose_to_eef_rotation : [0.0, 0.0, 0.0] 17 | 18 | end_effector_parent_link: 'gripper_roll_link' 19 | -------------------------------------------------------------------------------- /5 DOF ARM/autoarm_description/launch/autoarm_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /5 DOF ARM/autoarm_moveit_config/launch/warehouse.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /5 DOF ARM/autoarm_moveit_config/launch/setup_assistant.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /5 DOF ARM/autoarm_moveit/src/test_random.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | int main(int argc, char **argv) 4 | { 5 | ros::init(argc, argv, "move_group_interface_demo", ros::init_options::AnonymousName); 6 | 7 | ros::AsyncSpinner spinner(1); 8 | spinner.start(); 9 | 10 | // this connects to a running instance of the move_group node 11 | move_group_interface::MoveGroup group("arm"); 12 | 13 | // specify that our target will be a random one 14 | group.setRandomTarget(); 15 | 16 | // plan the motion and then move the group to the sampled target 17 | group.move(); 18 | 19 | ros::shutdown(); 20 | return 0; 21 | } 22 | -------------------------------------------------------------------------------- /5 DOF ARM/autoarm_gazebo/launch/autoarm_joint_position_controllers.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /5 DOF ARM/autoarm_moveit_config/config/controllers.yaml: -------------------------------------------------------------------------------- 1 | controller_manager_ns: controller_manager 2 | controller_list: 3 | - name: autoarm/arm_controller 4 | action_ns: follow_joint_trajectory 5 | type: FollowJointTrajectory 6 | default: true 7 | joints: 8 | - shoulder_pan_joint 9 | - shoulder_pitch_joint 10 | - elbow_roll_joint 11 | - elbow_pitch_joint 12 | - wrist_roll_joint 13 | - wrist_pitch_joint 14 | - gripper_roll_joint 15 | - name: autoarm/gripper_controller 16 | action_ns: follow_joint_trajectory 17 | type: FollowJointTrajectory 18 | default: true 19 | joints: 20 | - finger_joint1 21 | - finger_joint2 22 | -------------------------------------------------------------------------------- /5 DOF ARM/autoarm_gazebo/launch/autoarm_joint_state_controllers.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 12 | 13 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /5 DOF ARM/autoarm_moveit_config/launch/warehouse_settings.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /5 DOF ARM/autoarm_moveit_config/launch/joystick_control.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /5 DOF ARM/autoarm_moveit_config/launch/sensor_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /5 DOF ARM/autoarm_moveit_config/launch/autoarm_moveit_controller_manager.launch.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /5 DOF ARM/autoarm_moveit_config/launch/moveit_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /5 DOF ARM/autoarm_moveit_config/launch/default_warehouse_db.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /5 DOF ARM/autoarm_moveit/src/test_custom.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | int main(int argc, char **argv) 4 | { 5 | ros::init(argc, argv, "move_group_interface_demo", ros::init_options::AnonymousName); 6 | 7 | ros::AsyncSpinner spinner(1); 8 | spinner.start(); 9 | 10 | moveit::planning_interface::MoveGroup group("arm"); 11 | 12 | geometry_msgs::Pose target_pose1; 13 | target_pose1.orientation.w = 0.726282; 14 | target_pose1.orientation.x= 4.04423e-07; 15 | target_pose1.orientation.y = -0.687396; 16 | target_pose1.orientation.z = 4.81813e-07; 17 | 18 | target_pose1.position.x = 0.0261186; 19 | target_pose1.position.y = 4.50972e-07; 20 | target_pose1.position.z = 0.573659; 21 | group.setPoseTarget(target_pose1); 22 | 23 | group.move(); 24 | 25 | ros::shutdown(); 26 | return 0; 27 | } 28 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Windows image file caches 2 | Thumbs.db 3 | ehthumbs.db 4 | 5 | # Folder config file 6 | Desktop.ini 7 | 8 | # Recycle Bin used on file shares 9 | $RECYCLE.BIN/ 10 | 11 | # Windows Installer files 12 | *.cab 13 | *.msi 14 | *.msm 15 | *.msp 16 | 17 | # Windows shortcuts 18 | *.lnk 19 | 20 | # ========================= 21 | # Operating System Files 22 | # ========================= 23 | 24 | # OSX 25 | # ========================= 26 | 27 | .DS_Store 28 | .AppleDouble 29 | .LSOverride 30 | 31 | # Thumbnails 32 | ._* 33 | 34 | # Files that might appear in the root of a volume 35 | .DocumentRevisions-V100 36 | .fseventsd 37 | .Spotlight-V100 38 | .TemporaryItems 39 | .Trashes 40 | .VolumeIcon.icns 41 | 42 | # Directories potentially created on remote AFP share 43 | .AppleDB 44 | .AppleDesktop 45 | Network Trash Folder 46 | Temporary Items 47 | .apdisk 48 | -------------------------------------------------------------------------------- /5 DOF ARM/autoarm_moveit/launch/autoarm_gazebo_with_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 13 | [/autoarm/joint_states] 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /5 DOF ARM/autoarm_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 | -------------------------------------------------------------------------------- /5 DOF ARM/autoarm_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 | -------------------------------------------------------------------------------- /5 DOF ARM/autoarm_gazebo/launch/autoarm_in_empty_world.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /5 DOF ARM/autoarm_moveit_config/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | autoarm_moveit_config 4 | 0.2.0 5 | 6 | An automatically generated package with all the configuration and launch files for using the autoarm with the MoveIt Motion Planning Framework 7 | 8 | wit 9 | 10 | BSD 11 | 12 | http://moveit.ros.org/ 13 | https://github.com/ros-planning/moveit_setup_assistant/issues 14 | https://github.com/ros-planning/moveit_setup_assistant 15 | 16 | moveit_ros_move_group 17 | moveit_planners_ompl 18 | moveit_ros_visualization 19 | joint_state_publisher 20 | robot_state_publisher 21 | xacro 22 | autoarm_description 23 | autoarm_description 24 | 25 | 26 | catkin 27 | 28 | 29 | -------------------------------------------------------------------------------- /5 DOF ARM/autoarm_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 | -------------------------------------------------------------------------------- /5 DOF ARM/autoarm_gazebo/config/joint_trajectory_controllers.yaml: -------------------------------------------------------------------------------- 1 | autoarm: 2 | arm_controller: 3 | type: "position_controllers/JointTrajectoryController" 4 | joints: 5 | - shoulder_pan_joint 6 | - shoulder_pitch_joint 7 | - elbow_roll_joint 8 | - elbow_pitch_joint 9 | - wrist_roll_joint 10 | - wrist_pitch_joint 11 | - gripper_roll_joint 12 | 13 | gains: 14 | shoulder_pan_joint: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0} 15 | shoulder_pitch_joint: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0} 16 | elbow_roll_joint: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0} 17 | elbow_pitch_joint: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0} 18 | wrist_roll_joint: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0} 19 | wrist_pitch_joint: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0} 20 | gripper_roll_joint: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0} 21 | 22 | 23 | gripper_controller: 24 | type: "position_controllers/JointTrajectoryController" 25 | joints: 26 | - finger_joint1 27 | - finger_joint2 28 | gains: 29 | finger_joint1: {p: 50.0, d: 1.0, i: 0.01, i_clamp: 1.0} 30 | finger_joint2: {p: 50.0, d: 1.0, i: 0.01, i_clamp: 1.0} 31 | 32 | -------------------------------------------------------------------------------- /5 DOF ARM/autoarm_gazebo/config/joint_position_controllers.yaml: -------------------------------------------------------------------------------- 1 | autoarm: 2 | # Position Controllers --------------------------------------- 3 | joint1_position_controller: 4 | type: position_controllers/JointPositionController 5 | joint: shoulder_pan_joint 6 | pid: {p: 100.0, i: 0.01, d: 10.0} 7 | joint2_position_controller: 8 | type: position_controllers/JointPositionController 9 | joint: shoulder_pitch_joint 10 | pid: {p: 100.0, i: 0.01, d: 10.0} 11 | joint3_position_controller: 12 | type: position_controllers/JointPositionController 13 | joint: elbow_roll_joint 14 | pid: {p: 100.0, i: 0.01, d: 10.0} 15 | joint4_position_controller: 16 | type: position_controllers/JointPositionController 17 | joint: elbow_pitch_joint 18 | pid: {p: 100.0, i: 0.01, d: 10.0} 19 | joint5_position_controller: 20 | type: position_controllers/JointPositionController 21 | joint: wrist_roll_joint 22 | pid: {p: 100.0, i: 0.01, d: 10.0} 23 | joint6_position_controller: 24 | type: position_controllers/JointPositionController 25 | joint: wrist_pitch_joint 26 | pid: {p: 100.0, i: 0.01, d: 10.0} 27 | joint7_position_controller: 28 | type: position_controllers/JointPositionController 29 | joint: gripper_roll_joint 30 | pid: {p: 100.0, i: 0.01, d: 10.0} 31 | -------------------------------------------------------------------------------- /5 DOF ARM/autoarm_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 | -------------------------------------------------------------------------------- /5 DOF ARM/autoarm_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 | elbow_pitch_joint: 6 | has_velocity_limits: true 7 | max_velocity: 1 8 | has_acceleration_limits: false 9 | max_acceleration: 0 10 | elbow_roll_joint: 11 | has_velocity_limits: true 12 | max_velocity: 1 13 | has_acceleration_limits: false 14 | max_acceleration: 0 15 | finger_joint1: 16 | has_velocity_limits: true 17 | max_velocity: 1 18 | has_acceleration_limits: false 19 | max_acceleration: 0 20 | finger_joint2: 21 | has_velocity_limits: true 22 | max_velocity: 1 23 | has_acceleration_limits: false 24 | max_acceleration: 0 25 | gripper_roll_joint: 26 | has_velocity_limits: true 27 | max_velocity: 1 28 | has_acceleration_limits: false 29 | max_acceleration: 0 30 | shoulder_pan_joint: 31 | has_velocity_limits: true 32 | max_velocity: 1 33 | has_acceleration_limits: false 34 | max_acceleration: 0 35 | shoulder_pitch_joint: 36 | has_velocity_limits: true 37 | max_velocity: 1 38 | has_acceleration_limits: false 39 | max_acceleration: 0 40 | wrist_pitch_joint: 41 | has_velocity_limits: true 42 | max_velocity: 1 43 | has_acceleration_limits: false 44 | max_acceleration: 0 45 | wrist_roll_joint: 46 | has_velocity_limits: true 47 | max_velocity: 1 48 | has_acceleration_limits: false 49 | max_acceleration: 0 -------------------------------------------------------------------------------- /5 DOF ARM/autoarm_gazebo/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | autoarm_gazebo 4 | 0.0.0 5 | The autoarm_gazebo package 6 | 7 | 8 | 9 | 10 | wit 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /5 DOF ARM/autoarm_moveit/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | autoarm_moveit 4 | 0.0.0 5 | The autoarm_moveit package 6 | 7 | 8 | 9 | 10 | wit 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /5 DOF ARM/autoarm_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | autoarm_description 4 | 0.0.0 5 | The autoarm_description package 6 | 7 | 8 | 9 | 10 | wit 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | -------------------------------------------------------------------------------- /5 DOF ARM/autoarm_moveit_config/launch/demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | [/move_group/fake_controller_joint_states] 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /5 DOF ARM/autoarm_moveit_config/launch/move_group.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | -------------------------------------------------------------------------------- /5 DOF ARM/autoarm_moveit_config/launch/move_group.launch~: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | -------------------------------------------------------------------------------- /5 DOF ARM/autoarm_moveit/nodes/joint_state_muxer: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | import rospy 3 | from sensor_msgs.msg import JointState 4 | from urdf_parser_py.urdf import URDF 5 | from math import pi 6 | 7 | def get_param(name, value=None): 8 | private = "~%s" % name 9 | if rospy.has_param(private): 10 | return rospy.get_param(private) 11 | elif rospy.has_param(name): 12 | return rospy.get_param(name) 13 | else: 14 | return value 15 | 16 | class JointStateMuxer(): 17 | 18 | def __init__(self): 19 | 20 | robot = URDF().from_parameter_server() 21 | self.free_joints = {} 22 | self.dependent_joints = get_param("dependent_joints", {}) 23 | 24 | for joint in robot.joints: 25 | jtype = joint.joint_type 26 | if jtype == 'fixed': 27 | continue 28 | name = joint.name 29 | if jtype == 'continuous' or joint.limit is None: 30 | minval = -pi 31 | maxval = pi 32 | else: 33 | minval = joint.limit.lower 34 | maxval = joint.limit.upper 35 | 36 | if joint.mimic is not None: 37 | entry = {'parent': joint.mimic.joint_name } 38 | if joint.mimic.multiplier is not None: 39 | entry['factor'] = joint.mimic.multiplier 40 | if joint.mimic.offset is not None: 41 | entry['offset'] = joint.mimic.offset 42 | self.dependent_joints[name] = entry 43 | continue 44 | elif name in self.dependent_joints: 45 | continue 46 | elif minval > 0 or maxval < 0: 47 | zeroval = (maxval + minval)/2 48 | else: 49 | zeroval = 0 50 | 51 | joint = {'min':minval, 'max':maxval, 'zero':zeroval, 'position':zeroval, 'velocity':0} 52 | self.free_joints[name] = joint 53 | 54 | source_list = get_param("source_list", []) 55 | self.sources = [] 56 | for source in source_list: 57 | self.sources.append(rospy.Subscriber(source, JointState, self.source_cb)) 58 | 59 | self.pub = rospy.Publisher('joint_states', JointState, queue_size=100) 60 | 61 | def source_cb(self, msg): 62 | for i in range(len(msg.name)): 63 | name = msg.name[i] 64 | if name in self.free_joints: 65 | joint = self.free_joints[name] 66 | joint['position'] = msg.position[i] 67 | if len(msg.velocity) > i: 68 | joint['velocity'] = msg.velocity[i] 69 | else: 70 | joint['velocity'] = 0.0 71 | 72 | 73 | def loop(self): 74 | hz = get_param("rate", 50) 75 | r = rospy.Rate(hz) 76 | 77 | while not rospy.is_shutdown(): 78 | msg = JointState() 79 | msg.header.stamp = rospy.Time.now() 80 | 81 | for (name, joint) in self.free_joints.items(): 82 | msg.name.append(str(name)) 83 | msg.position.append(joint['position']) 84 | msg.velocity.append(joint['velocity']) 85 | 86 | for (name, param) in self.dependent_joints.items(): 87 | parent = param['parent'] 88 | baseval = self.free_joints[parent]['position'] 89 | value = baseval * param.get('factor', 1) 90 | 91 | msg.name.append(str(name)) 92 | msg.position.append(value) 93 | msg.velocity.append(0.0) 94 | 95 | self.pub.publish(msg) 96 | 97 | r.sleep() 98 | 99 | if __name__ == '__main__': 100 | try: 101 | rospy.init_node('joint_state_muxer') 102 | jsp = JointStateMuxer() 103 | jsp.loop() 104 | 105 | except rospy.ROSInterruptException: 106 | pass 107 | -------------------------------------------------------------------------------- /5 DOF ARM/autoarm_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 | arm: 56 | planner_configs: 57 | - SBLkConfigDefault 58 | - ESTkConfigDefault 59 | - LBKPIECEkConfigDefault 60 | - BKPIECEkConfigDefault 61 | - KPIECEkConfigDefault 62 | - RRTkConfigDefault 63 | - RRTConnectkConfigDefault 64 | - RRTstarkConfigDefault 65 | - TRRTkConfigDefault 66 | - PRMkConfigDefault 67 | - PRMstarkConfigDefault 68 | projection_evaluator: joints(shoulder_pan_joint,shoulder_pitch_joint) 69 | longest_valid_segment_fraction: 0.05 70 | gripper: 71 | planner_configs: 72 | - SBLkConfigDefault 73 | - ESTkConfigDefault 74 | - LBKPIECEkConfigDefault 75 | - BKPIECEkConfigDefault 76 | - KPIECEkConfigDefault 77 | - RRTkConfigDefault 78 | - RRTConnectkConfigDefault 79 | - RRTstarkConfigDefault 80 | - TRRTkConfigDefault 81 | - PRMkConfigDefault 82 | - PRMstarkConfigDefault 83 | projection_evaluator: joints(finger_joint1,finger_joint2) 84 | longest_valid_segment_fraction: 0.05 -------------------------------------------------------------------------------- /5 DOF ARM/autoarm_description/config/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: 330 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.588679 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.03 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 | elbow_pitch_link: 72 | Alpha: 1 73 | Show Axes: false 74 | Show Trail: false 75 | Value: true 76 | elbow_roll_link: 77 | Alpha: 1 78 | Show Axes: false 79 | Show Trail: false 80 | Value: true 81 | gripper_finger_link: 82 | Alpha: 1 83 | Show Axes: false 84 | Show Trail: false 85 | Value: true 86 | gripper_roll_link: 87 | Alpha: 1 88 | Show Axes: false 89 | Show Trail: false 90 | Value: true 91 | r_gripper_aft_link: 92 | Alpha: 1 93 | Show Axes: false 94 | Show Trail: false 95 | Value: true 96 | shoulder_pan_link: 97 | Alpha: 1 98 | Show Axes: false 99 | Show Trail: false 100 | Value: true 101 | shoulder_pitch_link: 102 | Alpha: 1 103 | Show Axes: false 104 | Show Trail: false 105 | Value: true 106 | wrist_pitch_link: 107 | Alpha: 1 108 | Show Axes: false 109 | Show Trail: false 110 | Value: true 111 | wrist_roll_link: 112 | Alpha: 1 113 | Show Axes: false 114 | Show Trail: false 115 | Value: true 116 | Name: RobotModel 117 | Robot Description: robot_description 118 | TF Prefix: "" 119 | Update Interval: 0 120 | Value: true 121 | Visual Enabled: true 122 | - Class: rviz/TF 123 | Enabled: false 124 | Frame Timeout: 15 125 | Frames: 126 | All Enabled: true 127 | Marker Scale: 1 128 | Name: TF 129 | Show Arrows: true 130 | Show Axes: true 131 | Show Names: true 132 | Tree: 133 | {} 134 | Update Interval: 0 135 | Value: false 136 | Enabled: true 137 | Global Options: 138 | Background Color: 48; 48; 48 139 | Fixed Frame: base_link 140 | Frame Rate: 30 141 | Name: root 142 | Tools: 143 | - Class: rviz/Interact 144 | Hide Inactive Objects: true 145 | - Class: rviz/MoveCamera 146 | - Class: rviz/Select 147 | - Class: rviz/FocusCamera 148 | - Class: rviz/Measure 149 | - Class: rviz/SetInitialPose 150 | Topic: /initialpose 151 | - Class: rviz/SetGoal 152 | Topic: /move_base_simple/goal 153 | - Class: rviz/PublishPoint 154 | Single click: true 155 | Topic: /clicked_point 156 | Value: true 157 | Views: 158 | Current: 159 | Class: rviz/Orbit 160 | Distance: 1.12009 161 | Enable Stereo Rendering: 162 | Stereo Eye Separation: 0.06 163 | Stereo Focal Distance: 1 164 | Swap Stereo Eyes: false 165 | Value: false 166 | Focal Point: 167 | X: -0.138732 168 | Y: -0.0432205 169 | Z: 0.292796 170 | Name: Current View 171 | Near Clip Distance: 0.01 172 | Pitch: 0.230398 173 | Target Frame: 174 | Value: Orbit (rviz) 175 | Yaw: 0.425398 176 | Saved: ~ 177 | Window Geometry: 178 | Displays: 179 | collapsed: false 180 | Height: 611 181 | Hide Left Dock: false 182 | Hide Right Dock: false 183 | QMainWindow State: 000000ff00000000fd00000004000000000000013c000001d9fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000001d9000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000001d9fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000028000001d9000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004c00000003efc0100000002fb0000000800540069006d00650100000000000004c0000002f600fffffffb0000000800540069006d0065010000000000000450000000000000000000000269000001d900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 184 | Selection: 185 | collapsed: false 186 | Time: 187 | collapsed: false 188 | Tool Properties: 189 | collapsed: false 190 | Views: 191 | collapsed: false 192 | Width: 1216 193 | X: 43 194 | Y: 14 195 | -------------------------------------------------------------------------------- /5 DOF ARM/autoarm_gazebo/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(autoarm_gazebo) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED) 8 | 9 | ## System dependencies are found with CMake's conventions 10 | # find_package(Boost REQUIRED COMPONENTS system) 11 | 12 | 13 | ## Uncomment this if the package has a setup.py. This macro ensures 14 | ## modules and global scripts declared therein get installed 15 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 16 | # catkin_python_setup() 17 | 18 | ################################################ 19 | ## Declare ROS messages, services and actions ## 20 | ################################################ 21 | 22 | ## To declare and build messages, services or actions from within this 23 | ## package, follow these steps: 24 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 25 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 26 | ## * In the file package.xml: 27 | ## * add a build_depend tag for "message_generation" 28 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 29 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 30 | ## but can be declared for certainty nonetheless: 31 | ## * add a run_depend tag for "message_runtime" 32 | ## * In this file (CMakeLists.txt): 33 | ## * add "message_generation" and every package in MSG_DEP_SET to 34 | ## find_package(catkin REQUIRED COMPONENTS ...) 35 | ## * add "message_runtime" and every package in MSG_DEP_SET to 36 | ## catkin_package(CATKIN_DEPENDS ...) 37 | ## * uncomment the add_*_files sections below as needed 38 | ## and list every .msg/.srv/.action file to be processed 39 | ## * uncomment the generate_messages entry below 40 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 41 | 42 | ## Generate messages in the 'msg' folder 43 | # add_message_files( 44 | # FILES 45 | # Message1.msg 46 | # Message2.msg 47 | # ) 48 | 49 | ## Generate services in the 'srv' folder 50 | # add_service_files( 51 | # FILES 52 | # Service1.srv 53 | # Service2.srv 54 | # ) 55 | 56 | ## Generate actions in the 'action' folder 57 | # add_action_files( 58 | # FILES 59 | # Action1.action 60 | # Action2.action 61 | # ) 62 | 63 | ## Generate added messages and services with any dependencies listed here 64 | # generate_messages( 65 | # DEPENDENCIES 66 | # std_msgs # Or other packages containing msgs 67 | # ) 68 | 69 | ################################################ 70 | ## Declare ROS dynamic reconfigure parameters ## 71 | ################################################ 72 | 73 | ## To declare and build dynamic reconfigure parameters within this 74 | ## package, follow these steps: 75 | ## * In the file package.xml: 76 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 77 | ## * In this file (CMakeLists.txt): 78 | ## * add "dynamic_reconfigure" to 79 | ## find_package(catkin REQUIRED COMPONENTS ...) 80 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 81 | ## and list every .cfg file to be processed 82 | 83 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 84 | # generate_dynamic_reconfigure_options( 85 | # cfg/DynReconf1.cfg 86 | # cfg/DynReconf2.cfg 87 | # ) 88 | 89 | ################################### 90 | ## catkin specific configuration ## 91 | ################################### 92 | ## The catkin_package macro generates cmake config files for your package 93 | ## Declare things to be passed to dependent projects 94 | ## INCLUDE_DIRS: uncomment this if you package contains header files 95 | ## LIBRARIES: libraries you create in this project that dependent projects also need 96 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 97 | ## DEPENDS: system dependencies of this project that dependent projects also need 98 | catkin_package( 99 | # INCLUDE_DIRS include 100 | # LIBRARIES autoarm_gazebo 101 | # CATKIN_DEPENDS other_catkin_pkg 102 | # DEPENDS system_lib 103 | ) 104 | 105 | ########### 106 | ## Build ## 107 | ########### 108 | 109 | ## Specify additional locations of header files 110 | ## Your package locations should be listed before other locations 111 | # include_directories(include) 112 | 113 | ## Declare a C++ library 114 | # add_library(autoarm_gazebo 115 | # src/${PROJECT_NAME}/autoarm_gazebo.cpp 116 | # ) 117 | 118 | ## Add cmake target dependencies of the library 119 | ## as an example, code may need to be generated before libraries 120 | ## either from message generation or dynamic reconfigure 121 | # add_dependencies(autoarm_gazebo ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 122 | 123 | ## Declare a C++ executable 124 | # add_executable(autoarm_gazebo_node src/autoarm_gazebo_node.cpp) 125 | 126 | ## Add cmake target dependencies of the executable 127 | ## same as for the library above 128 | # add_dependencies(autoarm_gazebo_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 129 | 130 | ## Specify libraries to link a library or executable target against 131 | # target_link_libraries(autoarm_gazebo_node 132 | # ${catkin_LIBRARIES} 133 | # ) 134 | 135 | ############# 136 | ## Install ## 137 | ############# 138 | 139 | # all install targets should use catkin DESTINATION variables 140 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 141 | 142 | ## Mark executable scripts (Python etc.) for installation 143 | ## in contrast to setup.py, you can choose the destination 144 | # install(PROGRAMS 145 | # scripts/my_python_script 146 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 147 | # ) 148 | 149 | ## Mark executables and/or libraries for installation 150 | # install(TARGETS autoarm_gazebo autoarm_gazebo_node 151 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 152 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 153 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 154 | # ) 155 | 156 | ## Mark cpp header files for installation 157 | # install(DIRECTORY include/${PROJECT_NAME}/ 158 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 159 | # FILES_MATCHING PATTERN "*.h" 160 | # PATTERN ".svn" EXCLUDE 161 | # ) 162 | 163 | ## Mark other files for installation (e.g. launch and bag files, etc.) 164 | # install(FILES 165 | # # myfile1 166 | # # myfile2 167 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 168 | # ) 169 | 170 | ############# 171 | ## Testing ## 172 | ############# 173 | 174 | ## Add gtest based cpp test target and link libraries 175 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_autoarm_gazebo.cpp) 176 | # if(TARGET ${PROJECT_NAME}-test) 177 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 178 | # endif() 179 | 180 | ## Add folders to be run by python nosetests 181 | # catkin_add_nosetests(test) 182 | -------------------------------------------------------------------------------- /5 DOF ARM/autoarm_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(autoarm_description) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED) 8 | 9 | ## System dependencies are found with CMake's conventions 10 | # find_package(Boost REQUIRED COMPONENTS system) 11 | 12 | 13 | ## Uncomment this if the package has a setup.py. This macro ensures 14 | ## modules and global scripts declared therein get installed 15 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 16 | # catkin_python_setup() 17 | 18 | ################################################ 19 | ## Declare ROS messages, services and actions ## 20 | ################################################ 21 | 22 | ## To declare and build messages, services or actions from within this 23 | ## package, follow these steps: 24 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 25 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 26 | ## * In the file package.xml: 27 | ## * add a build_depend tag for "message_generation" 28 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 29 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 30 | ## but can be declared for certainty nonetheless: 31 | ## * add a run_depend tag for "message_runtime" 32 | ## * In this file (CMakeLists.txt): 33 | ## * add "message_generation" and every package in MSG_DEP_SET to 34 | ## find_package(catkin REQUIRED COMPONENTS ...) 35 | ## * add "message_runtime" and every package in MSG_DEP_SET to 36 | ## catkin_package(CATKIN_DEPENDS ...) 37 | ## * uncomment the add_*_files sections below as needed 38 | ## and list every .msg/.srv/.action file to be processed 39 | ## * uncomment the generate_messages entry below 40 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 41 | 42 | ## Generate messages in the 'msg' folder 43 | # add_message_files( 44 | # FILES 45 | # Message1.msg 46 | # Message2.msg 47 | # ) 48 | 49 | ## Generate services in the 'srv' folder 50 | # add_service_files( 51 | # FILES 52 | # Service1.srv 53 | # Service2.srv 54 | # ) 55 | 56 | ## Generate actions in the 'action' folder 57 | # add_action_files( 58 | # FILES 59 | # Action1.action 60 | # Action2.action 61 | # ) 62 | 63 | ## Generate added messages and services with any dependencies listed here 64 | # generate_messages( 65 | # DEPENDENCIES 66 | # std_msgs # Or other packages containing msgs 67 | # ) 68 | 69 | ################################################ 70 | ## Declare ROS dynamic reconfigure parameters ## 71 | ################################################ 72 | 73 | ## To declare and build dynamic reconfigure parameters within this 74 | ## package, follow these steps: 75 | ## * In the file package.xml: 76 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 77 | ## * In this file (CMakeLists.txt): 78 | ## * add "dynamic_reconfigure" to 79 | ## find_package(catkin REQUIRED COMPONENTS ...) 80 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 81 | ## and list every .cfg file to be processed 82 | 83 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 84 | # generate_dynamic_reconfigure_options( 85 | # cfg/DynReconf1.cfg 86 | # cfg/DynReconf2.cfg 87 | # ) 88 | 89 | ################################### 90 | ## catkin specific configuration ## 91 | ################################### 92 | ## The catkin_package macro generates cmake config files for your package 93 | ## Declare things to be passed to dependent projects 94 | ## INCLUDE_DIRS: uncomment this if you package contains header files 95 | ## LIBRARIES: libraries you create in this project that dependent projects also need 96 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 97 | ## DEPENDS: system dependencies of this project that dependent projects also need 98 | catkin_package( 99 | # INCLUDE_DIRS include 100 | # LIBRARIES autoarm_description 101 | # CATKIN_DEPENDS other_catkin_pkg 102 | # DEPENDS system_lib 103 | ) 104 | 105 | ########### 106 | ## Build ## 107 | ########### 108 | 109 | ## Specify additional locations of header files 110 | ## Your package locations should be listed before other locations 111 | # include_directories(include) 112 | 113 | ## Declare a C++ library 114 | # add_library(autoarm_description 115 | # src/${PROJECT_NAME}/autoarm_description.cpp 116 | # ) 117 | 118 | ## Add cmake target dependencies of the library 119 | ## as an example, code may need to be generated before libraries 120 | ## either from message generation or dynamic reconfigure 121 | # add_dependencies(autoarm_description ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 122 | 123 | ## Declare a C++ executable 124 | # add_executable(autoarm_description_node src/autoarm_description_node.cpp) 125 | 126 | ## Add cmake target dependencies of the executable 127 | ## same as for the library above 128 | # add_dependencies(autoarm_description_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 129 | 130 | ## Specify libraries to link a library or executable target against 131 | # target_link_libraries(autoarm_description_node 132 | # ${catkin_LIBRARIES} 133 | # ) 134 | 135 | ############# 136 | ## Install ## 137 | ############# 138 | 139 | # all install targets should use catkin DESTINATION variables 140 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 141 | 142 | ## Mark executable scripts (Python etc.) for installation 143 | ## in contrast to setup.py, you can choose the destination 144 | # install(PROGRAMS 145 | # scripts/my_python_script 146 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 147 | # ) 148 | 149 | ## Mark executables and/or libraries for installation 150 | # install(TARGETS autoarm_description autoarm_description_node 151 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 152 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 153 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 154 | # ) 155 | 156 | ## Mark cpp header files for installation 157 | # install(DIRECTORY include/${PROJECT_NAME}/ 158 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 159 | # FILES_MATCHING PATTERN "*.h" 160 | # PATTERN ".svn" EXCLUDE 161 | # ) 162 | 163 | ## Mark other files for installation (e.g. launch and bag files, etc.) 164 | # install(FILES 165 | # # myfile1 166 | # # myfile2 167 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 168 | # ) 169 | 170 | ############# 171 | ## Testing ## 172 | ############# 173 | 174 | ## Add gtest based cpp test target and link libraries 175 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_autoarm_description.cpp) 176 | # if(TARGET ${PROJECT_NAME}-test) 177 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 178 | # endif() 179 | 180 | ## Add folders to be run by python nosetests 181 | # catkin_add_nosetests(test) 182 | -------------------------------------------------------------------------------- /5 DOF ARM/autoarm_moveit/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(autoarm_moveit) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED 8 | cmake_modules 9 | moveit_core 10 | moveit_ros_planning_interface 11 | roscpp 12 | std_msgs 13 | ) 14 | 15 | ## System dependencies are found with CMake's conventions 16 | find_package(Boost REQUIRED COMPONENTS system) 17 | 18 | 19 | ## Uncomment this if the package has a setup.py. This macro ensures 20 | ## modules and global scripts declared therein get installed 21 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 22 | # catkin_python_setup() 23 | 24 | ################################################ 25 | ## Declare ROS messages, services and actions ## 26 | ################################################ 27 | 28 | ## To declare and build messages, services or actions from within this 29 | ## package, follow these steps: 30 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 31 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 32 | ## * In the file package.xml: 33 | ## * add a build_depend tag for "message_generation" 34 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 35 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 36 | ## but can be declared for certainty nonetheless: 37 | ## * add a run_depend tag for "message_runtime" 38 | ## * In this file (CMakeLists.txt): 39 | ## * add "message_generation" and every package in MSG_DEP_SET to 40 | ## find_package(catkin REQUIRED COMPONENTS ...) 41 | ## * add "message_runtime" and every package in MSG_DEP_SET to 42 | ## catkin_package(CATKIN_DEPENDS ...) 43 | ## * uncomment the add_*_files sections below as needed 44 | ## and list every .msg/.srv/.action file to be processed 45 | ## * uncomment the generate_messages entry below 46 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 47 | 48 | ## Generate messages in the 'msg' folder 49 | # add_message_files( 50 | # FILES 51 | # Message1.msg 52 | # Message2.msg 53 | # ) 54 | 55 | ## Generate services in the 'srv' folder 56 | # add_service_files( 57 | # FILES 58 | # Service1.srv 59 | # Service2.srv 60 | # ) 61 | 62 | ## Generate actions in the 'action' folder 63 | # add_action_files( 64 | # FILES 65 | # Action1.action 66 | # Action2.action 67 | # ) 68 | 69 | ## Generate added messages and services with any dependencies listed here 70 | # generate_messages( 71 | # DEPENDENCIES 72 | # std_msgs # Or other packages containing msgs 73 | # ) 74 | 75 | ################################################ 76 | ## Declare ROS dynamic reconfigure parameters ## 77 | ################################################ 78 | 79 | ## To declare and build dynamic reconfigure parameters within this 80 | ## package, follow these steps: 81 | ## * In the file package.xml: 82 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 83 | ## * In this file (CMakeLists.txt): 84 | ## * add "dynamic_reconfigure" to 85 | ## find_package(catkin REQUIRED COMPONENTS ...) 86 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 87 | ## and list every .cfg file to be processed 88 | 89 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 90 | # generate_dynamic_reconfigure_options( 91 | # cfg/DynReconf1.cfg 92 | # cfg/DynReconf2.cfg 93 | # ) 94 | 95 | ################################### 96 | ## catkin specific configuration ## 97 | ################################### 98 | ## The catkin_package macro generates cmake config files for your package 99 | ## Declare things to be passed to dependent projects 100 | ## INCLUDE_DIRS: uncomment this if you package contains header files 101 | ## LIBRARIES: libraries you create in this project that dependent projects also need 102 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 103 | ## DEPENDS: system dependencies of this project that dependent projects also need 104 | catkin_package( 105 | # INCLUDE_DIRS include 106 | # LIBRARIES autoarm_moveit 107 | # CATKIN_DEPENDS other_catkin_pkg 108 | # DEPENDS system_lib 109 | ) 110 | 111 | ########### 112 | ## Build ## 113 | ########### 114 | 115 | ## Specify additional locations of header files 116 | ## Your package locations should be listed before other locations 117 | # include_directories(include) 118 | include_directories( 119 | ${catkin_INCLUDE_DIRS} 120 | ) 121 | 122 | ## Declare a C++ library 123 | # add_library(autoarm_moveit 124 | # src/${PROJECT_NAME}/autoarm_moveit.cpp 125 | # ) 126 | 127 | ## Add cmake target dependencies of the library 128 | ## as an example, code may need to be generated before libraries 129 | ## either from message generation or dynamic reconfigure 130 | # add_dependencies(autoarm_moveit ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 131 | 132 | add_executable(test_random_node src/test_random.cpp) 133 | add_dependencies(test_random_node autoarm_moveit_generate_messages_cpp) 134 | target_link_libraries(test_random_node 135 | ${catkin_LIBRARIES} 136 | ) 137 | 138 | add_executable(test_custom_node src/test_custom.cpp) 139 | add_dependencies(test_custom_node autoarm_moveit_generate_messages_cpp) 140 | target_link_libraries(test_custom_node 141 | ${catkin_LIBRARIES} 142 | ) 143 | 144 | ############# 145 | ## Install ## 146 | ############# 147 | 148 | # all install targets should use catkin DESTINATION variables 149 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 150 | 151 | ## Mark executable scripts (Python etc.) for installation 152 | ## in contrast to setup.py, you can choose the destination 153 | # install(PROGRAMS 154 | # scripts/my_python_script 155 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 156 | # ) 157 | 158 | ## Mark executables and/or libraries for installation 159 | # install(TARGETS autoarm_moveit autoarm_moveit_node 160 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 161 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 162 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 163 | # ) 164 | 165 | ## Mark cpp header files for installation 166 | # install(DIRECTORY include/${PROJECT_NAME}/ 167 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 168 | # FILES_MATCHING PATTERN "*.h" 169 | # PATTERN ".svn" EXCLUDE 170 | # ) 171 | 172 | ## Mark other files for installation (e.g. launch and bag files, etc.) 173 | # install(FILES 174 | # # myfile1 175 | # # myfile2 176 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 177 | # ) 178 | 179 | ############# 180 | ## Testing ## 181 | ############# 182 | 183 | ## Add gtest based cpp test target and link libraries 184 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_autoarm_moveit.cpp) 185 | # if(TARGET ${PROJECT_NAME}-test) 186 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 187 | # endif() 188 | 189 | ## Add folders to be run by python nosetests 190 | # catkin_add_nosetests(test) 191 | -------------------------------------------------------------------------------- /5 DOF ARM/autoarm_moveit_config/config/autoarm.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 | 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 | -------------------------------------------------------------------------------- /5 DOF ARM/autoarm_moveit/scripts/pick_and_place.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import sys 3 | import copy 4 | import numpy 5 | 6 | import rospy 7 | 8 | from actionlib import SimpleActionClient, GoalStatus 9 | 10 | from moveit_commander import RobotCommander, PlanningSceneInterface 11 | from moveit_commander import roscpp_initialize, roscpp_shutdown 12 | 13 | from tf.transformations import quaternion_from_euler 14 | 15 | from geometry_msgs.msg import Pose, PoseStamped, PoseArray, Quaternion 16 | from moveit_msgs.msg import PickupAction, PickupGoal 17 | from moveit_msgs.msg import PlaceAction, PlaceGoal 18 | from moveit_msgs.msg import PlaceLocation 19 | from moveit_msgs.msg import MoveItErrorCodes 20 | 21 | from autoarm_poses.msg import GenerateGraspPosesAction 22 | from autoarm_poses.msg import GenerateGraspPosesGoal 23 | from autoarm_poses.msg import GraspPoseOptions 24 | 25 | moveit_error_dict = {} 26 | for name in MoveItErrorCodes.__dict__.keys(): 27 | if not name[:1] == '_': 28 | code = MoveItErrorCodes.__dict__[name] 29 | moveit_error_dict[code] = name 30 | 31 | class Pick_Place: 32 | 33 | def __init__(self): 34 | 35 | self._table_object_name = rospy.get_param('~table_object_name', 'Grasp_Table') 36 | self._grasp_object_name = rospy.get_param('~grasp_object_name', 'Grasp_Object') 37 | 38 | self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.01) 39 | 40 | self._arm_group = rospy.get_param('~arm', 'arm') 41 | self._gripper_group = rospy.get_param('~gripper', 'gripper') 42 | 43 | self._approach_retreat_desired_dist = rospy.get_param('~approach_retreat_desired_dist', 0.2) 44 | self._approach_retreat_min_dist = rospy.get_param('~approach_retreat_min_dist', 0.1) 45 | 46 | # Create (debugging) publishers: 47 | self._grasps_pub = rospy.Publisher('grasps', PoseArray, queue_size=1, latch=True) 48 | self._places_pub = rospy.Publisher('places', PoseArray, queue_size=1, latch=True) 49 | 50 | # Create planning scene and robot commander: 51 | self._scene = PlanningSceneInterface() 52 | self._robot = RobotCommander() 53 | 54 | rospy.sleep(1.0) 55 | 56 | self._scene.remove_world_object(self._table_object_name) 57 | self._scene.remove_world_object(self._grasp_object_name) 58 | 59 | self._pose_table = self._add_table(self._table_object_name) 60 | self._pose_coke_can = self._add_grasp_block_(self._grasp_object_name) 61 | 62 | rospy.sleep(1.0) 63 | 64 | self._pose_place = Pose() 65 | 66 | self._pose_place.position.x = self._pose_coke_can.position.x - 1 67 | self._pose_place.position.y = self._pose_coke_can.position.y - 0.06 68 | self._pose_place.position.z = self._pose_coke_can.position.z 69 | 70 | self._pose_place.orientation = Quaternion(*quaternion_from_euler(0.0, 0.0, 0.0)) 71 | 72 | self._arm = self._robot.get_group(self._arm_group) 73 | self._gripper = self._robot.get_group(self._gripper_group) 74 | 75 | self._grasps_ac = SimpleActionClient('/autoarm_grasp_pose_server/generate_grasp_poses', GenerateGraspPosesAction) 76 | if not self._grasps_ac.wait_for_server(rospy.Duration(5.0)): 77 | rospy.logerr('Grasp generator action client not available!') 78 | rospy.signal_shutdown('Grasp generator action client not available!') 79 | return 80 | 81 | # Create move group 'pickup' action client: 82 | self._pickup_ac = SimpleActionClient('/pickup', PickupAction) 83 | if not self._pickup_ac.wait_for_server(rospy.Duration(5.0)): 84 | rospy.logerr('Pick up action client not available!') 85 | rospy.signal_shutdown('Pick up action client not available!') 86 | return 87 | 88 | # Create move group 'place' action client: 89 | self._place_ac = SimpleActionClient('/place', PlaceAction) 90 | if not self._place_ac.wait_for_server(rospy.Duration(5.0)): 91 | rospy.logerr('Place action client not available!') 92 | rospy.signal_shutdown('Place action client not available!') 93 | return 94 | 95 | # Pick Coke can object: 96 | while not self._pickup(self._arm_group, self._grasp_object_name, self._grasp_object_width): 97 | rospy.logwarn('Pick up failed! Retrying ...') 98 | rospy.sleep(1.0) 99 | 100 | rospy.loginfo('Pick up successfully') 101 | 102 | # Place Coke can object on another place on the support surface (table): 103 | while not self._place(self._arm_group, self._grasp_object_name, self._pose_place): 104 | rospy.logwarn('Place failed! Retrying ...') 105 | rospy.sleep(1.0) 106 | 107 | rospy.loginfo('Place successfully') 108 | 109 | def __del__(self): 110 | # Clean the scene: 111 | self._scene.remove_world_object(self._grasp_object_name) 112 | self._scene.remove_world_object(self._table_object_name) 113 | 114 | def _add_table(self, name): 115 | p = PoseStamped() 116 | p.header.frame_id = self._robot.get_planning_frame() 117 | p.header.stamp = rospy.Time.now() 118 | 119 | p.pose.position.x = 0.45 120 | p.pose.position.y = 0.0 121 | p.pose.position.z = 0.22 122 | 123 | q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0)) 124 | p.pose.orientation = Quaternion(*q) 125 | 126 | # Table size from ~/.gazebo/models/table/model.sdf, using the values 127 | # for the surface link. 128 | self._scene.add_box(name, p, (0.5, 0.4, 0.02)) 129 | 130 | return p.pose 131 | 132 | def _add_grasp_block_(self, name): 133 | p = PoseStamped() 134 | p.header.frame_id = self._robot.get_planning_frame() 135 | p.header.stamp = rospy.Time.now() 136 | 137 | p.pose.position.x = 0.25 138 | p.pose.position.y = 0.05 139 | p.pose.position.z = 0.32 140 | 141 | q = quaternion_from_euler(0.0, 0.0, 0.0) 142 | p.pose.orientation = Quaternion(*q) 143 | 144 | # Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae, 145 | # using the measure tape tool from meshlab. 146 | # The box is the bounding box of the coke cylinder. 147 | # The values are taken from the cylinder base diameter and height. 148 | self._scene.add_box(name, p, (0.03, 0.03, 0.09)) 149 | 150 | return p.pose 151 | 152 | def _generate_grasps(self, pose, width): 153 | """ 154 | Generate grasps by using the grasp generator generate action; based on 155 | server_test.py example on moveit_simple_grasps pkg. 156 | """ 157 | 158 | # Create goal: 159 | goal = GenerateGraspPosesGoal() 160 | 161 | goal.pose = pose 162 | goal.width = width 163 | 164 | options = GraspPoseOptions() 165 | # simple_graps.cpp doesn't implement GRASP_AXIS_Z! 166 | #options.grasp_axis = GraspGeneratorOptions.GRASP_AXIS_Z 167 | options.grasp_direction = GraspPoseOptions.GRASP_DIRECTION_UP 168 | options.grasp_rotation = GraspPoseOptions.GRASP_ROTATION_FULL 169 | 170 | # @todo disabled because it works better with the default options 171 | #goal.options.append(options) 172 | 173 | # Send goal and wait for result: 174 | state = self._grasps_ac.send_goal_and_wait(goal) 175 | if state != GoalStatus.SUCCEEDED: 176 | rospy.logerr('Grasp goal failed!: %s' % self._grasps_ac.get_goal_status_text()) 177 | return None 178 | 179 | grasps = self._grasps_ac.get_result().grasps 180 | 181 | # Publish grasps (for debugging/visualization purposes): 182 | self._publish_grasps(grasps) 183 | 184 | return grasps 185 | 186 | def _generate_places(self, target): 187 | """ 188 | Generate places (place locations), based on 189 | https://github.com/davetcoleman/baxter_cpp/blob/hydro-devel/ 190 | baxter_pick_place/src/block_pick_place.cpp 191 | """ 192 | 193 | # Generate places: 194 | places = [] 195 | now = rospy.Time.now() 196 | for angle in numpy.arange(0.0, numpy.deg2rad(360.0), numpy.deg2rad(1.0)): 197 | # Create place location: 198 | place = PlaceLocation() 199 | 200 | place.place_pose.header.stamp = now 201 | place.place_pose.header.frame_id = self._robot.get_planning_frame() 202 | 203 | # Set target position: 204 | place.place_pose.pose = copy.deepcopy(target) 205 | 206 | # Generate orientation (wrt Z axis): 207 | q = quaternion_from_euler(0.0, 0.0, angle ) 208 | place.place_pose.pose.orientation = Quaternion(*q) 209 | 210 | # Generate pre place approach: 211 | place.pre_place_approach.desired_distance = self._approach_retreat_desired_dist 212 | place.pre_place_approach.min_distance = self._approach_retreat_min_dist 213 | 214 | place.pre_place_approach.direction.header.stamp = now 215 | place.pre_place_approach.direction.header.frame_id = self._robot.get_planning_frame() 216 | 217 | place.pre_place_approach.direction.vector.x = 0 218 | place.pre_place_approach.direction.vector.y = 0 219 | place.pre_place_approach.direction.vector.z = 0.2 220 | 221 | # Generate post place approach: 222 | place.post_place_retreat.direction.header.stamp = now 223 | place.post_place_retreat.direction.header.frame_id = self._robot.get_planning_frame() 224 | 225 | place.post_place_retreat.desired_distance = self._approach_retreat_desired_dist 226 | place.post_place_retreat.min_distance = self._approach_retreat_min_dist 227 | 228 | place.post_place_retreat.direction.vector.x = 0 229 | place.post_place_retreat.direction.vector.y = 0 230 | place.post_place_retreat.direction.vector.z = 0.2 231 | 232 | # Add place: 233 | places.append(place) 234 | 235 | # Publish places (for debugging/visualization purposes): 236 | self._publish_places(places) 237 | 238 | return places 239 | 240 | def _create_pickup_goal(self, group, target, grasps): 241 | """ 242 | Create a MoveIt! PickupGoal 243 | """ 244 | 245 | # Create goal: 246 | goal = PickupGoal() 247 | 248 | goal.group_name = group 249 | goal.target_name = target 250 | 251 | goal.possible_grasps.extend(grasps) 252 | 253 | goal.allowed_touch_objects.append(target) 254 | 255 | goal.support_surface_name = self._table_object_name 256 | 257 | # Configure goal planning options: 258 | goal.allowed_planning_time = 7.0 259 | 260 | goal.planning_options.planning_scene_diff.is_diff = True 261 | goal.planning_options.planning_scene_diff.robot_state.is_diff = True 262 | goal.planning_options.plan_only = False 263 | goal.planning_options.replan = True 264 | goal.planning_options.replan_attempts = 20 265 | 266 | return goal 267 | 268 | def _create_place_goal(self, group, target, places): 269 | """ 270 | Create a MoveIt! PlaceGoal 271 | """ 272 | 273 | # Create goal: 274 | goal = PlaceGoal() 275 | 276 | goal.group_name = group 277 | goal.attached_object_name = target 278 | 279 | goal.place_locations.extend(places) 280 | 281 | # Configure goal planning options: 282 | goal.allowed_planning_time = 7.0 283 | 284 | goal.planning_options.planning_scene_diff.is_diff = True 285 | goal.planning_options.planning_scene_diff.robot_state.is_diff = True 286 | goal.planning_options.plan_only = False 287 | goal.planning_options.replan = True 288 | goal.planning_options.replan_attempts = 20 289 | 290 | return goal 291 | 292 | def _pickup(self, group, target, width): 293 | """ 294 | Pick up a target using the planning group 295 | """ 296 | 297 | # Obtain possible grasps from the grasp generator server: 298 | grasps = self._generate_grasps(self._pose_coke_can, width) 299 | 300 | # Create and send Pickup goal: 301 | goal = self._create_pickup_goal(group, target, grasps) 302 | 303 | state = self._pickup_ac.send_goal_and_wait(goal) 304 | if state != GoalStatus.SUCCEEDED: 305 | rospy.logerr('Pick up goal failed!: %s' % self._pickup_ac.get_goal_status_text()) 306 | return None 307 | 308 | result = self._pickup_ac.get_result() 309 | 310 | # Check for error: 311 | err = result.error_code.val 312 | if err != MoveItErrorCodes.SUCCESS: 313 | rospy.logwarn('Group %s cannot pick up target %s!: %s' % (group, target, str(moveit_error_dict[err]))) 314 | 315 | return False 316 | 317 | return True 318 | 319 | def _place(self, group, target, place): 320 | """ 321 | Place a target using the planning group 322 | """ 323 | 324 | # Obtain possible places: 325 | places = self._generate_places(place) 326 | 327 | # Create and send Place goal: 328 | goal = self._create_place_goal(group, target, places) 329 | 330 | state = self._place_ac.send_goal_and_wait(goal) 331 | if state != GoalStatus.SUCCEEDED: 332 | rospy.logerr('Place goal failed!: %s' % self._place_ac.get_goal_status_text()) 333 | return None 334 | 335 | result = self._place_ac.get_result() 336 | 337 | # Check for error: 338 | err = result.error_code.val 339 | if err != MoveItErrorCodes.SUCCESS: 340 | rospy.logwarn('Group %s cannot place target %s!: %s' % (group, target, str(moveit_error_dict[err]))) 341 | 342 | return False 343 | 344 | return True 345 | 346 | def _publish_grasps(self, grasps): 347 | """ 348 | Publish grasps as poses, using a PoseArray message 349 | """ 350 | 351 | if self._grasps_pub.get_num_connections() > 0: 352 | msg = PoseArray() 353 | msg.header.frame_id = self._robot.get_planning_frame() 354 | msg.header.stamp = rospy.Time.now() 355 | 356 | for grasp in grasps: 357 | p = grasp.grasp_pose.pose 358 | 359 | msg.poses.append(Pose(p.position, p.orientation)) 360 | 361 | self._grasps_pub.publish(msg) 362 | 363 | def _publish_places(self, places): 364 | """ 365 | Publish places as poses, using a PoseArray message 366 | """ 367 | 368 | if self._places_pub.get_num_connections() > 0: 369 | msg = PoseArray() 370 | msg.header.frame_id = self._robot.get_planning_frame() 371 | msg.header.stamp = rospy.Time.now() 372 | 373 | for place in places: 374 | msg.poses.append(place.place_pose.pose) 375 | 376 | self._places_pub.publish(msg) 377 | 378 | def main(): 379 | p = Pick_Place() 380 | rospy.spin() 381 | 382 | if __name__ == '__main__': 383 | roscpp_initialize(sys.argv) 384 | rospy.init_node('pick_and_place') 385 | 386 | main() 387 | 388 | roscpp_shutdown() 389 | -------------------------------------------------------------------------------- /5 DOF ARM/autoarm_moveit/scripts/pick_and_place.py~: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import sys 3 | import copy 4 | import numpy 5 | 6 | import rospy 7 | 8 | from actionlib import SimpleActionClient, GoalStatus 9 | 10 | from moveit_commander import RobotCommander, PlanningSceneInterface 11 | from moveit_commander import roscpp_initialize, roscpp_shutdown 12 | 13 | from tf.transformations import quaternion_from_euler 14 | 15 | from geometry_msgs.msg import Pose, PoseStamped, PoseArray, Quaternion 16 | from moveit_msgs.msg import PickupAction, PickupGoal 17 | from moveit_msgs.msg import PlaceAction, PlaceGoal 18 | from moveit_msgs.msg import PlaceLocation 19 | from moveit_msgs.msg import MoveItErrorCodes 20 | 21 | from autoarm_poses.msg import GenerateGraspPosesAction 22 | from autoarm_poses.msg import GenerateGraspPosesGoal 23 | from autoarm_poses.msg import GraspPoseOptions 24 | 25 | moveit_error_dict = {} 26 | for name in MoveItErrorCodes.__dict__.keys(): 27 | if not name[:1] == '_': 28 | code = MoveItErrorCodes.__dict__[name] 29 | moveit_error_dict[code] = name 30 | 31 | class Pick_Place: 32 | 33 | def __init__(self): 34 | 35 | self._table_object_name = rospy.get_param('~table_object_name', 'Grasp_Table') 36 | self._grasp_object_name = rospy.get_param('~grasp_object_name', 'Grasp_Object') 37 | 38 | self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.01) 39 | 40 | self._arm_group = rospy.get_param('~arm', 'arm') 41 | self._gripper_group = rospy.get_param('~gripper', 'gripper') 42 | 43 | self._approach_retreat_desired_dist = rospy.get_param('~approach_retreat_desired_dist', 0.2) 44 | self._approach_retreat_min_dist = rospy.get_param('~approach_retreat_min_dist', 0.1) 45 | 46 | # Create (debugging) publishers: 47 | self._grasps_pub = rospy.Publisher('grasps', PoseArray, queue_size=1, latch=True) 48 | self._places_pub = rospy.Publisher('places', PoseArray, queue_size=1, latch=True) 49 | 50 | # Create planning scene and robot commander: 51 | self._scene = PlanningSceneInterface() 52 | self._robot = RobotCommander() 53 | 54 | rospy.sleep(1.0) 55 | 56 | self._scene.remove_world_object(self._table_object_name) 57 | self._scene.remove_world_object(self._grasp_object_name) 58 | 59 | self._pose_table = self._add_table(self._table_object_name) 60 | self._pose_coke_can = self._add_grasp_block_(self._grasp_object_name) 61 | 62 | rospy.sleep(1.0) 63 | 64 | self._pose_place = Pose() 65 | 66 | self._pose_place.position.x = self._pose_coke_can.position.x - 1 67 | self._pose_place.position.y = self._pose_coke_can.position.y - 0.06 68 | self._pose_place.position.z = self._pose_coke_can.position.z 69 | 70 | self._pose_place.orientation = Quaternion(*quaternion_from_euler(0.0, 0.0, 0.0)) 71 | 72 | self._arm = self._robot.get_group(self._arm_group) 73 | self._gripper = self._robot.get_group(self._gripper_group) 74 | 75 | self._grasps_ac = SimpleActionClient('/autoarm_grasp_pose_server/generate_grasp_poses', GenerateGraspPosesAction) 76 | if not self._grasps_ac.wait_for_server(rospy.Duration(5.0)): 77 | rospy.logerr('Grasp generator action client not available!') 78 | rospy.signal_shutdown('Grasp generator action client not available!') 79 | return 80 | 81 | # Create move group 'pickup' action client: 82 | self._pickup_ac = SimpleActionClient('/pickup', PickupAction) 83 | if not self._pickup_ac.wait_for_server(rospy.Duration(5.0)): 84 | rospy.logerr('Pick up action client not available!') 85 | rospy.signal_shutdown('Pick up action client not available!') 86 | return 87 | 88 | # Create move group 'place' action client: 89 | self._place_ac = SimpleActionClient('/place', PlaceAction) 90 | if not self._place_ac.wait_for_server(rospy.Duration(5.0)): 91 | rospy.logerr('Place action client not available!') 92 | rospy.signal_shutdown('Place action client not available!') 93 | return 94 | 95 | # Pick Coke can object: 96 | while not self._pickup(self._arm_group, self._grasp_object_name, self._grasp_object_width): 97 | rospy.logwarn('Pick up failed! Retrying ...') 98 | rospy.sleep(1.0) 99 | 100 | rospy.loginfo('Pick up successfully') 101 | 102 | # Place Coke can object on another place on the support surface (table): 103 | while not self._place(self._arm_group, self._grasp_object_name, self._pose_place): 104 | rospy.logwarn('Place failed! Retrying ...') 105 | rospy.sleep(1.0) 106 | 107 | rospy.loginfo('Place successfully') 108 | 109 | def __del__(self): 110 | # Clean the scene: 111 | self._scene.remove_world_object(self._grasp_object_name) 112 | self._scene.remove_world_object(self._table_object_name) 113 | 114 | def _add_table(self, name): 115 | p = PoseStamped() 116 | p.header.frame_id = self._robot.get_planning_frame() 117 | p.header.stamp = rospy.Time.now() 118 | 119 | p.pose.position.x = 0.45 120 | p.pose.position.y = 0.0 121 | p.pose.position.z = 0.22 122 | 123 | q = quaternion_from_euler(0.0, 0.0, numpy.deg2rad(90.0)) 124 | p.pose.orientation = Quaternion(*q) 125 | 126 | # Table size from ~/.gazebo/models/table/model.sdf, using the values 127 | # for the surface link. 128 | self._scene.add_box(name, p, (0.5, 0.4, 0.02)) 129 | 130 | return p.pose 131 | 132 | def _add_grasp_block_(self, name): 133 | p = PoseStamped() 134 | p.header.frame_id = self._robot.get_planning_frame() 135 | p.header.stamp = rospy.Time.now() 136 | 137 | p.pose.position.x = 0.25 138 | p.pose.position.y = 0.05 139 | p.pose.position.z = 0.32 140 | 141 | q = quaternion_from_euler(0.0, 0.0, 0.0) 142 | p.pose.orientation = Quaternion(*q) 143 | 144 | # Coke can size from ~/.gazebo/models/coke_can/meshes/coke_can.dae, 145 | # using the measure tape tool from meshlab. 146 | # The box is the bounding box of the coke cylinder. 147 | # The values are taken from the cylinder base diameter and height. 148 | self._scene.add_box(name, p, (0.03, 0.03, 0.09)) 149 | 150 | return p.pose 151 | 152 | def _generate_grasps(self, pose, width): 153 | """ 154 | Generate grasps by using the grasp generator generate action; based on 155 | server_test.py example on moveit_simple_grasps pkg. 156 | """ 157 | 158 | # Create goal: 159 | goal = GenerateGraspPosesGoal() 160 | 161 | goal.pose = pose 162 | goal.width = width 163 | 164 | options = GraspPoseOptions() 165 | # simple_graps.cpp doesn't implement GRASP_AXIS_Z! 166 | #options.grasp_axis = GraspGeneratorOptions.GRASP_AXIS_Z 167 | options.grasp_direction = GraspPoseOptions.GRASP_DIRECTION_UP 168 | options.grasp_rotation = GraspPoseOptions.GRASP_ROTATION_FULL 169 | 170 | # @todo disabled because it works better with the default options 171 | #goal.options.append(options) 172 | 173 | # Send goal and wait for result: 174 | state = self._grasps_ac.send_goal_and_wait(goal) 175 | if state != GoalStatus.SUCCEEDED: 176 | rospy.logerr('Grasp goal failed!: %s' % self._grasps_ac.get_goal_status_text()) 177 | return None 178 | 179 | grasps = self._grasps_ac.get_result().grasps 180 | 181 | # Publish grasps (for debugging/visualization purposes): 182 | self._publish_grasps(grasps) 183 | 184 | return grasps 185 | 186 | def _generate_places(self, target): 187 | """ 188 | Generate places (place locations), based on 189 | https://github.com/davetcoleman/baxter_cpp/blob/hydro-devel/ 190 | baxter_pick_place/src/block_pick_place.cpp 191 | """ 192 | 193 | # Generate places: 194 | places = [] 195 | now = rospy.Time.now() 196 | for angle in numpy.arange(0.0, numpy.deg2rad(360.0), numpy.deg2rad(1.0)): 197 | # Create place location: 198 | place = PlaceLocation() 199 | 200 | place.place_pose.header.stamp = now 201 | place.place_pose.header.frame_id = self._robot.get_planning_frame() 202 | 203 | # Set target position: 204 | place.place_pose.pose = copy.deepcopy(target) 205 | 206 | # Generate orientation (wrt Z axis): 207 | q = quaternion_from_euler(0.0, 0.0, angle ) 208 | place.place_pose.pose.orientation = Quaternion(*q) 209 | 210 | # Generate pre place approach: 211 | place.pre_place_approach.desired_distance = self._approach_retreat_desired_dist 212 | place.pre_place_approach.min_distance = self._approach_retreat_min_dist 213 | 214 | place.pre_place_approach.direction.header.stamp = now 215 | place.pre_place_approach.direction.header.frame_id = self._robot.get_planning_frame() 216 | 217 | place.pre_place_approach.direction.vector.x = 0 218 | place.pre_place_approach.direction.vector.y = 0 219 | place.pre_place_approach.direction.vector.z = 0.2 220 | 221 | # Generate post place approach: 222 | place.post_place_retreat.direction.header.stamp = now 223 | place.post_place_retreat.direction.header.frame_id = self._robot.get_planning_frame() 224 | 225 | place.post_place_retreat.desired_distance = self._approach_retreat_desired_dist 226 | place.post_place_retreat.min_distance = self._approach_retreat_min_dist 227 | 228 | place.post_place_retreat.direction.vector.x = 0 229 | place.post_place_retreat.direction.vector.y = 0 230 | place.post_place_retreat.direction.vector.z = 0.2 231 | 232 | # Add place: 233 | places.append(place) 234 | 235 | # Publish places (for debugging/visualization purposes): 236 | self._publish_places(places) 237 | 238 | return places 239 | 240 | def _create_pickup_goal(self, group, target, grasps): 241 | """ 242 | Create a MoveIt! PickupGoal 243 | """ 244 | 245 | # Create goal: 246 | goal = PickupGoal() 247 | 248 | goal.group_name = group 249 | goal.target_name = target 250 | 251 | goal.possible_grasps.extend(grasps) 252 | 253 | goal.allowed_touch_objects.append(target) 254 | 255 | goal.support_surface_name = self._table_object_name 256 | 257 | # Configure goal planning options: 258 | goal.allowed_planning_time = 7.0 259 | 260 | goal.planning_options.planning_scene_diff.is_diff = True 261 | goal.planning_options.planning_scene_diff.robot_state.is_diff = True 262 | goal.planning_options.plan_only = False 263 | goal.planning_options.replan = True 264 | goal.planning_options.replan_attempts = 20 265 | 266 | return goal 267 | 268 | def _create_place_goal(self, group, target, places): 269 | """ 270 | Create a MoveIt! PlaceGoal 271 | """ 272 | 273 | # Create goal: 274 | goal = PlaceGoal() 275 | 276 | goal.group_name = group 277 | goal.attached_object_name = target 278 | 279 | goal.place_locations.extend(places) 280 | 281 | # Configure goal planning options: 282 | goal.allowed_planning_time = 7.0 283 | 284 | goal.planning_options.planning_scene_diff.is_diff = True 285 | goal.planning_options.planning_scene_diff.robot_state.is_diff = True 286 | goal.planning_options.plan_only = False 287 | goal.planning_options.replan = True 288 | goal.planning_options.replan_attempts = 20 289 | 290 | return goal 291 | 292 | def _pickup(self, group, target, width): 293 | """ 294 | Pick up a target using the planning group 295 | """ 296 | 297 | # Obtain possible grasps from the grasp generator server: 298 | grasps = self._generate_grasps(self._pose_coke_can, width) 299 | 300 | # Create and send Pickup goal: 301 | goal = self._create_pickup_goal(group, target, grasps) 302 | 303 | state = self._pickup_ac.send_goal_and_wait(goal) 304 | if state != GoalStatus.SUCCEEDED: 305 | rospy.logerr('Pick up goal failed!: %s' % self._pickup_ac.get_goal_status_text()) 306 | return None 307 | 308 | result = self._pickup_ac.get_result() 309 | 310 | # Check for error: 311 | err = result.error_code.val 312 | if err != MoveItErrorCodes.SUCCESS: 313 | rospy.logwarn('Group %s cannot pick up target %s!: %s' % (group, target, str(moveit_error_dict[err]))) 314 | 315 | return False 316 | 317 | return True 318 | 319 | def _place(self, group, target, place): 320 | """ 321 | Place a target using the planning group 322 | """ 323 | 324 | # Obtain possible places: 325 | places = self._generate_places(place) 326 | 327 | # Create and send Place goal: 328 | goal = self._create_place_goal(group, target, places) 329 | 330 | state = self._place_ac.send_goal_and_wait(goal) 331 | if state != GoalStatus.SUCCEEDED: 332 | rospy.logerr('Place goal failed!: %s' % self._place_ac.get_goal_status_text()) 333 | return None 334 | 335 | result = self._place_ac.get_result() 336 | 337 | # Check for error: 338 | err = result.error_code.val 339 | if err != MoveItErrorCodes.SUCCESS: 340 | rospy.logwarn('Group %s cannot place target %s!: %s' % (group, target, str(moveit_error_dict[err]))) 341 | 342 | return False 343 | 344 | return True 345 | 346 | def _publish_grasps(self, grasps): 347 | """ 348 | Publish grasps as poses, using a PoseArray message 349 | """ 350 | 351 | if self._grasps_pub.get_num_connections() > 0: 352 | msg = PoseArray() 353 | msg.header.frame_id = self._robot.get_planning_frame() 354 | msg.header.stamp = rospy.Time.now() 355 | 356 | for grasp in grasps: 357 | p = grasp.grasp_pose.pose 358 | 359 | msg.poses.append(Pose(p.position, p.orientation)) 360 | 361 | self._grasps_pub.publish(msg) 362 | 363 | def _publish_places(self, places): 364 | """ 365 | Publish places as poses, using a PoseArray message 366 | """ 367 | 368 | if self._places_pub.get_num_connections() > 0: 369 | msg = PoseArray() 370 | msg.header.frame_id = self._robot.get_planning_frame() 371 | msg.header.stamp = rospy.Time.now() 372 | 373 | for place in places: 374 | msg.poses.append(place.place_pose.pose) 375 | 376 | self._places_pub.publish(msg) 377 | 378 | def main(): 379 | p = Pick_Place() 380 | rospy.spin() 381 | 382 | if __name__ == '__main__': 383 | roscpp_initialize(sys.argv) 384 | rospy.init_node('pick_and_place') 385 | 386 | main() 387 | 388 | roscpp_shutdown() 389 | -------------------------------------------------------------------------------- /5 DOF ARM/autoarm_description/urdf/autoarm.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 | transmission_interface/SimpleTransmission 77 | 78 | PositionJointInterface 79 | 80 | 81 | PositionJointInterface 82 | 1 83 | 84 | 85 | 86 | 87 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | > 111 | 112 | 113 | 114 | Gazebo/White 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | Gazebo/White 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 | Gazebo/Red 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 | Gazebo/White 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 | Gazebo/Black 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 | Gazebo/Red 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 | Gazebo/Black 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 | Gazebo/White 323 | 324 | 325 | 326 | 327 | 328 | 329 | 330 | 331 | 332 | 333 | 334 | 335 | 336 | 337 | 338 | 339 | 340 | 341 | 342 | 343 | 344 | 345 | 346 | 347 | 348 | 349 | 350 | 351 | 352 | 353 | Gazebo/Red 354 | 355 | 356 | 357 | 358 | 359 | 360 | 361 | 362 | 363 | 367 | 368 | 369 | 370 | 371 | 372 | 373 | 374 | 375 | 376 | 377 | 378 | 379 | 380 | 381 | 382 | 383 | Gazebo/White 384 | 385 | 386 | 387 | 388 | 389 | 390 | 391 | 392 | 393 | 394 | 395 | 396 | 397 | 398 | 399 | 400 | 401 | 402 | 403 | 404 | 405 | 406 | 407 | 408 | 409 | 410 | Gazebo/White 411 | 412 | 413 | 414 | 415 | 416 | 417 | 418 | 419 | 420 | 421 | 422 | 423 | 424 | 425 | 426 | 427 | 428 | 429 | 430 | 431 | 432 | 433 | 434 | 435 | 436 | 437 | 438 | 439 | 440 | 441 | 442 | 443 | 444 | 445 | 446 | 447 | 448 | 449 | 450 | 451 | 452 | 453 | 454 | 455 | 456 | 457 | 458 | 459 | 460 | /autoarm 461 | 462 | 463 | 464 | 465 | -------------------------------------------------------------------------------- /5 DOF ARM/autoarm_description/urdf/autoarm.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 | transmission_interface/SimpleTransmission 77 | 78 | PositionJointInterface 79 | 80 | 81 | PositionJointInterface 82 | 1 83 | 84 | 85 | 86 | 87 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | > 111 | 112 | 113 | 114 | Gazebo/White 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | Gazebo/White 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 | Gazebo/Red 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 | Gazebo/White 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 | Gazebo/Black 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 | Gazebo/Red 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 | Gazebo/Black 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 | Gazebo/White 323 | 324 | 325 | 326 | 327 | 328 | 329 | 330 | 331 | 332 | 333 | 334 | 335 | 336 | 337 | 338 | 339 | 340 | 341 | 342 | 343 | 344 | 345 | 346 | 347 | 348 | 349 | 350 | 351 | 352 | 353 | Gazebo/Red 354 | 355 | 356 | 357 | 358 | 359 | 360 | 361 | 362 | 363 | 367 | 368 | 369 | 370 | 371 | 372 | 373 | 374 | 375 | 376 | 377 | 378 | 379 | 380 | 381 | 382 | 383 | Gazebo/White 384 | 385 | 386 | 387 | 388 | 389 | 390 | 391 | 392 | 393 | 394 | 395 | 396 | 397 | 398 | 399 | 400 | 401 | 402 | 403 | 404 | 405 | 406 | 407 | 408 | 409 | 410 | Gazebo/White 411 | 412 | 413 | 414 | 415 | 416 | 417 | 418 | 419 | 420 | 421 | 422 | 423 | 424 | 425 | 426 | 427 | 428 | 429 | 430 | 431 | 432 | 433 | 434 | 435 | 436 | 437 | 438 | 439 | 440 | 441 | 442 | 443 | 444 | 445 | 446 | 447 | 448 | 449 | 450 | 451 | 452 | 453 | 454 | 455 | 456 | 457 | 458 | 459 | 460 | /autoarm 461 | 462 | 463 | 464 | 465 | -------------------------------------------------------------------------------- /5 DOF ARM/autoarm_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 | --------------------------------------------------------------------------------