diff --git a/moma_bringup/launch/components/sensors.launch b/moma_bringup/launch/components/sensors.launch index d0beb1ae3..f1414ebcd 100644 --- a/moma_bringup/launch/components/sensors.launch +++ b/moma_bringup/launch/components/sensors.launch @@ -12,7 +12,8 @@ - + + diff --git a/moma_bringup/launch/panda_real.launch b/moma_bringup/launch/panda_real.launch index 0c7dc4bf7..fc9e753c6 100644 --- a/moma_bringup/launch/panda_real.launch +++ b/moma_bringup/launch/panda_real.launch @@ -5,7 +5,7 @@ - + diff --git a/moma_bringup/scripts/icp_test_calibration.py b/moma_bringup/scripts/icp_test_calibration.py new file mode 100644 index 000000000..c5fcd758e --- /dev/null +++ b/moma_bringup/scripts/icp_test_calibration.py @@ -0,0 +1,82 @@ +import open3d as o3d +import copy +import numpy as np +from scipy.spatial.transform import Rotation as R + +def draw_registration_result_original_color(source, target, transformation): + source_temp = copy.deepcopy(source) + source_temp.transform(transformation) + o3d.visualization.draw_geometries([source_temp, target], + zoom=0.5, + front=[-0.2458, -0.8088, 0.5342], + lookat=[1.7745, 2.2305, 0.9787], + up=[0.3109, -0.5878, -0.7468]) + +print("1. Load two point clouds and show initial pose") +# source = o3d.io.read_point_cloud("/root/data/run_1/rs_435_2.pcd") +# target = o3d.io.read_point_cloud("/root/data/run_1/rs_435_3.pcd") +# source = o3d.io.read_point_cloud("/root/data/run_2/rs_435_2.pcd") +# target = o3d.io.read_point_cloud("/root/data/run_2/rs_435_3.pcd") +source = o3d.io.read_point_cloud("/root/data/run_3/rs_435_2.pcd") +target = o3d.io.read_point_cloud("/root/data/run_3/rs_435_3.pcd") + +# draw initial alignment +translation = [-0.340, -0.096, 0.014] # Example translation +quaternion = [-0.028, 0.063, 0.997, -0.039] # Example quaternion (x, y, z, w) +rotation_matrix = R.from_quat(quaternion).as_matrix() +current_transformation = np.eye(4) +current_transformation[:3, :3] = rotation_matrix +current_transformation[:3, 3] = translation +# current_transformation = np.identity(4) + +draw_registration_result_original_color(source, target, current_transformation) + +# initial_pcd = o3d.geometry.PointCloud(source) # Copy using the constructor +# initial_pcd = initial_pcd.transform(current_transformation) +# draw_registration_result_original_color(source, target, current_transformation) + +# # point to plane ICP +# print("2. Point-to-plane ICP registration is applied on original point") +# print(" clouds to refine the alignment. Distance threshold 0.02.") +# result_icp = o3d.pipelines.registration.registration_icp( +# source, target, 0.02, current_transformation, +# o3d.pipelines.registration.TransformationEstimationPointToPoint()) +# print(result_icp) +# draw_registration_result_original_color(source, target, +# result_icp.transformation) + +print("3. Colored point cloud registration") +voxel_radius = [0.04, 0.02, 0.01, 0.05] +max_iter = [50, 30, 14, 50] +for scale in range(3): + iter = max_iter[scale] + radius = voxel_radius[scale] + print([iter, radius, scale]) + + print("3-1. Downsample with a voxel size %.2f" % radius) + source_down = source.voxel_down_sample(radius) + target_down = target.voxel_down_sample(radius) + + print("3-2. Estimate normal.") + source_down.estimate_normals( + o3d.geometry.KDTreeSearchParamHybrid(radius=radius * 2, max_nn=30)) + target_down.estimate_normals( + o3d.geometry.KDTreeSearchParamHybrid(radius=radius * 2, max_nn=30)) + + print("3-3. Applying colored point cloud registration") + result_icp = o3d.pipelines.registration.registration_colored_icp( + source_down, target_down, radius, current_transformation, + o3d.pipelines.registration.TransformationEstimationForColoredICP(), + o3d.pipelines.registration.ICPConvergenceCriteria(relative_fitness=1e-6, + relative_rmse=1e-6, + max_iteration=iter)) + + # result_icp = o3d.pipelines.registration.registration_icp( + # source_down, target_down, 0.02, current_transformation, + # o3d.pipelines.registration.TransformationEstimationPointToPlane()) + + + current_transformation = result_icp.transformation + print(result_icp) +draw_registration_result_original_color(source, target, + result_icp.transformation) diff --git a/moma_demos/grasp_demo/config/grasp_demo.rviz b/moma_demos/grasp_demo/config/grasp_demo.rviz index 6b2d4e28a..66d1a326e 100644 --- a/moma_demos/grasp_demo/config/grasp_demo.rviz +++ b/moma_demos/grasp_demo/config/grasp_demo.rviz @@ -4,12 +4,9 @@ Panels: Name: Displays Property Tree Widget: Expanded: - - /TF1/Frames1 - - /RobotModel1 - - /Quality Cloud1 - /MarkerArray1/Namespaces1 - Splitter Ratio: 0.545271635055542 - Tree Height: 802 + Splitter Ratio: 0.48490944504737854 + Tree Height: 808 - Class: rviz/Selection Name: Selection - Class: rviz/Tool Properties @@ -56,10 +53,90 @@ Visualization Manager: Reference Frame: Value: true - Class: rviz/TF - Enabled: false + Enabled: true + Filter (blacklist): "" + Filter (whitelist): "" Frame Timeout: 15 Frames: All Enabled: false + fixed_camera_link: + Value: false + panda_EE: + Value: false + panda_K: + Value: false + panda_NE: + Value: false + panda_default_ee: + Value: true + panda_hand: + Value: false + panda_hand_sc: + Value: false + panda_hand_tcp: + Value: false + panda_leftfinger: + Value: false + panda_link0: + Value: false + panda_link0_sc: + Value: false + panda_link1: + Value: false + panda_link1_sc: + Value: false + panda_link2: + Value: false + panda_link2_sc: + Value: false + panda_link3: + Value: false + panda_link3_sc: + Value: false + panda_link4: + Value: false + panda_link4_sc: + Value: false + panda_link5: + Value: false + panda_link5_sc: + Value: false + panda_link6: + Value: false + panda_link6_sc: + Value: false + panda_link7: + Value: false + panda_link7_sc: + Value: false + panda_link8: + Value: true + panda_rightfinger: + Value: false + task: + Value: false + world: + Value: false + wrist_camera_bottom_screw_frame: + Value: false + wrist_camera_color_frame: + Value: false + wrist_camera_color_optical_frame: + Value: false + wrist_camera_depth_frame: + Value: false + wrist_camera_depth_optical_frame: + Value: false + wrist_camera_infra1_frame: + Value: false + wrist_camera_infra1_optical_frame: + Value: false + wrist_camera_infra2_frame: + Value: false + wrist_camera_infra2_optical_frame: + Value: false + wrist_camera_link: + Value: false Marker Alpha: 1 Marker Scale: 0.30000001192092896 Name: TF @@ -67,9 +144,67 @@ Visualization Manager: Show Axes: true Show Names: false Tree: - {} + world: + fixed_camera_link: + {} + panda_link0: + panda_link0_sc: + {} + panda_link1: + panda_link1_sc: + {} + panda_link2: + panda_link2_sc: + {} + panda_link3: + panda_link3_sc: + {} + panda_link4: + panda_link4_sc: + {} + panda_link5: + panda_link5_sc: + {} + panda_link6: + panda_link6_sc: + {} + panda_link7: + panda_link7_sc: + {} + panda_link8: + panda_NE: + panda_EE: + panda_K: + {} + panda_default_ee: + {} + panda_hand: + panda_hand_sc: + {} + panda_hand_tcp: + {} + panda_leftfinger: + {} + panda_rightfinger: + {} + wrist_camera_bottom_screw_frame: + wrist_camera_link: + wrist_camera_color_frame: + wrist_camera_color_optical_frame: + {} + wrist_camera_depth_frame: + wrist_camera_depth_optical_frame: + {} + wrist_camera_infra1_frame: + wrist_camera_infra1_optical_frame: + {} + wrist_camera_infra2_frame: + wrist_camera_infra2_optical_frame: + {} + task: + {} Update Interval: 0 - Value: false + Value: true - Alpha: 1 Class: rviz/RobotModel Collision Enabled: false @@ -345,7 +480,7 @@ Visualization Manager: Color: 255; 255; 255 Color Transformer: AxisColor Decay Time: 0 - Enabled: true + Enabled: false Invert Rainbow: false Max Color: 255; 255; 255 Min Color: 0; 0; 0 @@ -360,7 +495,7 @@ Visualization Manager: Unreliable: false Use Fixed Frame: true Use rainbow: true - Value: true + Value: false - Alpha: 1 Autocompute Intensity Bounds: false Autocompute Value Bounds: @@ -447,6 +582,22 @@ Visualization Manager: roi: true Queue Size: 100 Value: true + - Alpha: 1 + Axes Length: 0.05000000074505806 + Axes Radius: 0.009999999776482582 + Class: rviz/Pose + Color: 255; 25; 0 + Enabled: true + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Name: Pose + Queue Size: 10 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Shape: Axes + Topic: /target + Unreliable: false + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 @@ -475,7 +626,7 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 1.401139497756958 + Distance: 1.2434059381484985 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 @@ -491,9 +642,9 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.7197965979576111 + Pitch: -0.06520330905914307 Target Frame: - Yaw: 3.8698341846466064 + Yaw: 5.464844703674316 Saved: ~ Window Geometry: Displays: diff --git a/moma_demos/grasp_demo/config/grasp_demo.yaml b/moma_demos/grasp_demo/config/grasp_demo.yaml index 01e853323..0fefd76fb 100644 --- a/moma_demos/grasp_demo/config/grasp_demo.yaml +++ b/moma_demos/grasp_demo/config/grasp_demo.yaml @@ -1,8 +1,9 @@ base_frame_id: panda_link0 -ee_grasp_offset_z: -0.04 #-0.04 +ee_grasp_offset_z: -0.04 #-0.02 #-0.04 # More negative is LOWER into the object +# ee_grasp_offset_x: 0.010 task_frame_id: task -table_height: 0.14 # Measured from the base frame specified above +table_height: 0.20 #0.25 #-0.05 #0.14 # Measured from the base frame specified above workspaces: - scan_joints: diff --git a/moma_demos/grasp_demo/launch/grasp_demo.launch b/moma_demos/grasp_demo/launch/grasp_demo.launch index 43efa6b76..7d80db701 100644 --- a/moma_demos/grasp_demo/launch/grasp_demo.launch +++ b/moma_demos/grasp_demo/launch/grasp_demo.launch @@ -8,27 +8,30 @@ - + - - + + + + + - - - - - + + + + + - + - + diff --git a/moma_description/urdf/panda.urdf.xacro b/moma_description/urdf/panda.urdf.xacro index 91fa96e43..6bc0a06f6 100644 --- a/moma_description/urdf/panda.urdf.xacro +++ b/moma_description/urdf/panda.urdf.xacro @@ -7,7 +7,7 @@ - + diff --git a/moma_description/urdf/panda_arm.xacro b/moma_description/urdf/panda_arm.xacro index 1f813864a..963bd3a4c 100644 --- a/moma_description/urdf/panda_arm.xacro +++ b/moma_description/urdf/panda_arm.xacro @@ -13,8 +13,8 @@ hand:=true gazebo:=false use_wrist_camera:=false - wrist_calibration_rpy:='-0.0109077 -1.5469874 2.3727493' - wrist_calibration_xyz:='0.02144228129647578 -0.028987519327164604 0.04877543953719046' + wrist_calibration_rpy:='0.0227369 -1.5550382 2.3388778' + wrist_calibration_xyz:='0.026189514316117823 -0.026817004845920814 0.0464006595138288' wrist_camera_parent_link:='panda_link8' use_fixed_camera:=false fixed_calibration_rpy:='-0.1566281 1.3870703 2.9725147' @@ -175,7 +175,7 @@ - + diff --git a/moma_utils/src/moma_utils/ros/conversions.py b/moma_utils/src/moma_utils/ros/conversions.py index 7a809696e..73f37fe15 100644 --- a/moma_utils/src/moma_utils/ros/conversions.py +++ b/moma_utils/src/moma_utils/ros/conversions.py @@ -4,7 +4,7 @@ import rospy import std_msgs.msg -from moma_utils.transform import Rotation, Transform +from moma_utils.spatial import Rotation, Transform def from_point_msg(msg: geometry_msgs.msg.Point) -> np.array: diff --git a/moveit_configs/panda_moveit_config/.setup_assistant b/moveit_configs/panda_moveit_config/.setup_assistant old mode 100644 new mode 100755 diff --git a/moveit_configs/panda_moveit_config/config/arm.xacro b/moveit_configs/panda_moveit_config/config/arm.xacro deleted file mode 100644 index fef366a6b..000000000 --- a/moveit_configs/panda_moveit_config/config/arm.xacro +++ /dev/null @@ -1,73 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/moveit_configs/panda_moveit_config/config/fake_controllers.yaml b/moveit_configs/panda_moveit_config/config/fake_controllers.yaml index 1ea8dff46..f404323d9 100644 --- a/moveit_configs/panda_moveit_config/config/fake_controllers.yaml +++ b/moveit_configs/panda_moveit_config/config/fake_controllers.yaml @@ -1,22 +1,14 @@ controller_list: - - name: fake_$(arg arm_id)_arm_controller + - name: fake_panda_arm_controller type: $(arg fake_execution_type) joints: - - $(arg arm_id)_joint1 - - $(arg arm_id)_joint2 - - $(arg arm_id)_joint3 - - $(arg arm_id)_joint4 - - $(arg arm_id)_joint5 - - $(arg arm_id)_joint6 - - $(arg arm_id)_joint7 - - - name: fake_$(arg arm_id)_hand_controller - type: $(arg fake_execution_type) - joints: - - $(arg arm_id)_finger_joint1 - + - panda_joint1 + - panda_joint2 + - panda_joint3 + - panda_joint4 + - panda_joint5 + - panda_joint6 + - panda_joint7 initial: # Define initial robot poses per group - - group: $(arg arm_id)_arm - pose: ready - - group: $(arg arm_id)_hand - pose: open + - group: panda_arm + pose: ready \ No newline at end of file diff --git a/moveit_configs/panda_moveit_config/config/gazebo_panda.urdf b/moveit_configs/panda_moveit_config/config/gazebo_panda.urdf new file mode 100644 index 000000000..e7486c4bc --- /dev/null +++ b/moveit_configs/panda_moveit_config/config/gazebo_panda.urdf @@ -0,0 +1,920 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + 30 + + 0.976899239 + + R8G8B8 + 640 + 480 + + + 0.1 + 10 + + + + true + + 0.0 + wrist_camera + /wrist_camera/color/image_raw + /wrist_camera/color/camera_info + /wrist_camera/depth/image_rect_raw + /wrist_camera/depth/camera_info + /wrist_camera/depth/color/points + wrist_camera_depth_optical_frame + 0.1 + 10 + 0 + 0 + 0 + 0 + 0 + + + + / + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + transmission_interface/SimpleTransmission + + hardware_interface/EffortJointInterface + + + hardware_interface/EffortJointInterface + 1 + + + + diff --git a/moveit_configs/panda_moveit_config/config/hand.xacro b/moveit_configs/panda_moveit_config/config/hand.xacro deleted file mode 100644 index 47a06b658..000000000 --- a/moveit_configs/panda_moveit_config/config/hand.xacro +++ /dev/null @@ -1,41 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/moveit_configs/panda_moveit_config/config/joint_limits.yaml b/moveit_configs/panda_moveit_config/config/joint_limits.yaml index 9bdaadd3b..174d7aff5 100644 --- a/moveit_configs/panda_moveit_config/config/joint_limits.yaml +++ b/moveit_configs/panda_moveit_config/config/joint_limits.yaml @@ -7,56 +7,39 @@ default_acceleration_scaling_factor: 0.1 # Specific joint properties can be changed with the keys [max_position, min_position, max_velocity, max_acceleration] # Joint limits can be turned off with [has_velocity_limits, has_acceleration_limits] - -# As MoveIt! does not support jerk limits, the acceleration limits provided here are the highest values that guarantee -# that no jerk limits will be violated. More precisely, applying Euler differentiation in the worst case (from min accel -# to max accel in 1 ms) the acceleration limits are the ones that satisfy -# max_jerk = (max_acceleration - min_acceleration) / 0.001 - joint_limits: - $(arg arm_id)_finger_joint1: + panda_joint1: has_velocity_limits: true - max_velocity: 0.1 + max_velocity: 2.175 has_acceleration_limits: false max_acceleration: 0 - $(arg arm_id)_finger_joint2: + panda_joint2: has_velocity_limits: true - max_velocity: 0.1 + max_velocity: 2.175 has_acceleration_limits: false max_acceleration: 0 - $(arg arm_id)_joint1: - has_velocity_limits: true - max_velocity: 2.1750 - has_acceleration_limits: true - max_acceleration: 3.75 - $(arg arm_id)_joint2: - has_velocity_limits: true - max_velocity: 2.1750 - has_acceleration_limits: true - max_acceleration: 1.875 - $(arg arm_id)_joint3: + panda_joint3: has_velocity_limits: true - max_velocity: 2.1750 - has_acceleration_limits: true - max_acceleration: 2.5 - $(arg arm_id)_joint4: + max_velocity: 2.175 + has_acceleration_limits: false + max_acceleration: 0 + panda_joint4: has_velocity_limits: true - max_velocity: 2.1750 - has_acceleration_limits: true - max_acceleration: 3.125 - $(arg arm_id)_joint5: + max_velocity: 2.175 + has_acceleration_limits: false + max_acceleration: 0 + panda_joint5: has_velocity_limits: true - max_velocity: 2.6100 - has_acceleration_limits: true - max_acceleration: 3.75 - $(arg arm_id)_joint6: + max_velocity: 2.61 + has_acceleration_limits: false + max_acceleration: 0 + panda_joint6: has_velocity_limits: true - max_velocity: 2.6100 - has_acceleration_limits: true - max_acceleration: 5 - $(arg arm_id)_joint7: + max_velocity: 2.61 + has_acceleration_limits: false + max_acceleration: 0 + panda_joint7: has_velocity_limits: true - max_velocity: 2.6100 - has_acceleration_limits: true - max_acceleration: 5 - + max_velocity: 2.61 + has_acceleration_limits: false + max_acceleration: 0 \ No newline at end of file diff --git a/moveit_configs/panda_moveit_config/config/kinematics.yaml b/moveit_configs/panda_moveit_config/config/kinematics.yaml index 0a8c801ac..fac6862bf 100644 --- a/moveit_configs/panda_moveit_config/config/kinematics.yaml +++ b/moveit_configs/panda_moveit_config/config/kinematics.yaml @@ -1,6 +1,7 @@ -$(arg arm_id)_arm: &arm_config +panda_arm: kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin kinematics_solver_search_resolution: 0.005 - kinematics_solver_timeout: 0.05 - -$(arg arm_id)_manipulator: *arm_config + kinematics_solver_timeout: 0.005 + goal_joint_tolerance: 0.0001 + goal_position_tolerance: 0.0001 + goal_orientation_tolerance: 0.001 \ No newline at end of file diff --git a/moveit_configs/panda_moveit_config/config/lerp_planning.yaml b/moveit_configs/panda_moveit_config/config/lerp_planning.yaml deleted file mode 100644 index 9d2eebd53..000000000 --- a/moveit_configs/panda_moveit_config/config/lerp_planning.yaml +++ /dev/null @@ -1 +0,0 @@ -num_steps: 40 diff --git a/moveit_configs/panda_moveit_config/config/ompl_planning.yaml b/moveit_configs/panda_moveit_config/config/ompl_planning.yaml index 32b496edd..b375665c6 100644 --- a/moveit_configs/panda_moveit_config/config/ompl_planning.yaml +++ b/moveit_configs/panda_moveit_config/config/ompl_planning.yaml @@ -51,8 +51,8 @@ planner_configs: temp_change_factor: 2.0 # how much to increase or decrease temp. default: 2.0 min_temperature: 10e-10 # lower limit of temp change. default: 10e-10 init_temperature: 10e-6 # initial temperature. default: 10e-6 - frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() - frountierNodeRatio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + frontier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frontier_node_ratio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 k_constant: 0.0 # value used to normalize expresssion. default: 0.0 set in setup() PRM: type: geometric::PRM @@ -95,8 +95,8 @@ planner_configs: range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() temp_change_factor: 0.1 # how much to increase or decrease temp. default: 0.1 init_temperature: 100 # initial temperature. default: 100 - frountier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() - frountier_node_ratio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 + frontier_threshold: 0.0 # dist new state to nearest neighbor to disqualify as frontier. default: 0.0 set in setup() + frontier_node_ratio: 0.1 # 1/10, or 1 nonfrontier for every 10 frontier. default: 0.1 cost_threshold: 1e300 # the cost threshold. Any motion cost that is not better will not be expanded. default: inf LBTRRT: type: geometric::LBTRRT @@ -127,7 +127,45 @@ planner_configs: sparse_delta_fraction: 0.25 # delta fraction for connection distance. This value represents the visibility range of sparse samples. default: 0.25 dense_delta_fraction: 0.001 # delta fraction for interface detection. default: 0.001 max_failures: 5000 # maximum consecutive failure limit. default: 5000 -$(arg arm_id)_arm: &arm_config + AITstar: + type: geometric::AITstar + use_k_nearest: 1 # whether to use a k-nearest RGG connection model (1) or an r-disc model (0). Default: 1 + rewire_factor: 1.001 # rewire factor of the RGG. Valid values: [1.0:0.01:3.0]. Default: 1.001 + samples_per_batch: 100 # batch size. Valid values: [1:1:1000]. Default: 100 + use_graph_pruning: 1 # enable graph pruning (1) or not (0). Default: 1 + find_approximate_solutions: 0 # track approximate solutions (1) or not (0). Default: 0 + set_max_num_goals: 1 # maximum number of goals sampled from sampleable goal regions. Valid values: [1:1:1000]. Default: 1 + ABITstar: + type: geometric::ABITstar + use_k_nearest: 1 # whether to use a k-nearest RGG connection model (1) or an r-disc model (0). Default: 1 + rewire_factor: 1.001 # rewire factor of the RGG. Valid values: [1.0:0.01:3.0]. Default: 1.001 + samples_per_batch: 100 # batch size. Valid values: [1:1:1000]. Default: 100 + use_graph_pruning: 1 # enable graph pruning (1) or not (0). Default: 1 + prune_threshold_as_fractional_cost_change: 0.1 # fractional change in the solution cost AND problem measure necessary for pruning to occur. Default: 0.1 + delay_rewiring_to_first_solution: 0 # delay (1) or not (0) rewiring until a solution is found. Default: 0 + use_just_in_time_sampling: 0 # delay the generation of samples until they are * necessary. Only works with r-disc connection and path length minimization. Default: 0 + drop_unconnected_samples_on_prune: 0 # drop unconnected samples when pruning, regardless of their heuristic value. Default: 0 + stop_on_each_solution_improvement: 0 # stop the planner each time a solution improvement is found. Useful for debugging. Default: 0 + use_strict_queue_ordering: 0 # sort edges in the queue at the end of the batch (0) or after each rewiring (1). Default: 0 + find_approximate_solutions: 0 # track approximate solutions (1) or not (0). Default: 0 + initial_inflation_factor: 1000000 # inflation factor for the initial search. Valid values: [1.0:0.01:1000000.0]. Default: 1000000 + inflation_scaling_parameter: 10 # scaling parameter for the inflation factor update policy. Valid values: [1.0:0.01:1000000.0]. Default: 0 + truncation_scaling_parameter: 5.0 # scaling parameter for the truncation factor update policy. Valid values: [1.0:0.01:1000000.0]. Default: 0 + BITstar: + type: geometric::BITstar + use_k_nearest: 1 # whether to use a k-nearest RGG connection model (1) or an r-disc model (0). Default: 1 + rewire_factor: 1.001 # rewire factor of the RGG. Valid values: [1.0:0.01:3.0]. Default: 1.001 + samples_per_batch: 100 # batch size. Valid values: [1:1:1000]. Default: 100 + use_graph_pruning: 1 # enable graph pruning (1) or not (0). Default: 1 + prune_threshold_as_fractional_cost_change: 0.1 # fractional change in the solution cost AND problem measure necessary for pruning to occur. Default: 0.1 + delay_rewiring_to_first_solution: 0 # delay (1) or not (0) rewiring until a solution is found. Default: 0 + use_just_in_time_sampling: 0 # delay the generation of samples until they are * necessary. Only works with r-disc connection and path length minimization. Default: 0 + drop_unconnected_samples_on_prune: 0 # drop unconnected samples when pruning, regardless of their heuristic value. Default: 0 + stop_on_each_solution_improvement: 0 # stop the planner each time a solution improvement is found. Useful for debugging. Default: 0 + use_strict_queue_ordering: 0 # sort edges in the queue at the end of the batch (0) or after each rewiring (1). Default: 0 + find_approximate_solutions: 0 # track approximate solutions (1) or not (0). Default: 0 +panda_arm: + default_planner_config: RRTConnect planner_configs: - AnytimePathShortening - SBL @@ -153,32 +191,8 @@ $(arg arm_id)_arm: &arm_config - LazyPRMstar - SPARS - SPARStwo - projection_evaluator: joints($(arg arm_id)_joint1,$(arg arm_id)_joint2) + - AITstar + - ABITstar + - BITstar + projection_evaluator: joints(panda_joint1,panda_joint2) longest_valid_segment_fraction: 0.005 -$(arg arm_id)_manipulator: *arm_config -$(arg arm_id)_hand: - planner_configs: - - AnytimePathShortening - - SBL - - EST - - LBKPIECE - - BKPIECE - - KPIECE - - RRT - - RRTConnect - - RRTstar - - TRRT - - PRM - - PRMstar - - FMT - - BFMT - - PDST - - STRIDE - - BiTRRT - - LBTRRT - - BiEST - - ProjEST - - LazyPRM - - LazyPRMstar - - SPARS - - SPARStwo diff --git a/moveit_configs/panda_moveit_config/config/panda.srdf b/moveit_configs/panda_moveit_config/config/panda.srdf new file mode 100644 index 000000000..250a09bbf --- /dev/null +++ b/moveit_configs/panda_moveit_config/config/panda.srdf @@ -0,0 +1,165 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/moveit_configs/panda_moveit_config/config/panda.srdf.xacro b/moveit_configs/panda_moveit_config/config/panda.srdf.xacro deleted file mode 100644 index 9277c2d86..000000000 --- a/moveit_configs/panda_moveit_config/config/panda.srdf.xacro +++ /dev/null @@ -1,27 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/moveit_configs/panda_moveit_config/config/sensors_3d.yaml b/moveit_configs/panda_moveit_config/config/sensors_3d.yaml new file mode 100644 index 000000000..01538cccd --- /dev/null +++ b/moveit_configs/panda_moveit_config/config/sensors_3d.yaml @@ -0,0 +1,9 @@ +sensors: + - filtered_cloud_topic: filtered_cloud + max_range: 5.0 + max_update_rate: 1.0 + padding_offset: 0.1 + padding_scale: 1.0 + point_cloud_topic: /head_mount_kinect/depth_registered/points + point_subsample: 1 + sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater \ No newline at end of file diff --git a/moveit_configs/panda_moveit_config/config/sensors_kinect_depthmap.yaml b/moveit_configs/panda_moveit_config/config/sensors_kinect_depthmap.yaml deleted file mode 100644 index fc4d7d93b..000000000 --- a/moveit_configs/panda_moveit_config/config/sensors_kinect_depthmap.yaml +++ /dev/null @@ -1,12 +0,0 @@ -sensors: - - sensor_plugin: occupancy_map_monitor/DepthImageOctomapUpdater - image_topic: /camera/depth_registered/image_raw - queue_size: 5 - near_clipping_plane_distance: 0.3 - far_clipping_plane_distance: 5.0 - shadow_threshold: 0.2 - padding_scale: 4.0 - padding_offset: 0.03 - max_update_rate: 1.0 - filtered_cloud_topic: filtered_cloud - ns: kinect diff --git a/moveit_configs/panda_moveit_config/config/sensors_kinect_pointcloud.yaml b/moveit_configs/panda_moveit_config/config/sensors_kinect_pointcloud.yaml deleted file mode 100644 index dd7dbbb9c..000000000 --- a/moveit_configs/panda_moveit_config/config/sensors_kinect_pointcloud.yaml +++ /dev/null @@ -1,10 +0,0 @@ -sensors: - - sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater - point_cloud_topic: /camera/depth_registered/points - max_range: 5.0 - point_subsample: 1 - padding_offset: 0.1 - padding_scale: 1.0 - max_update_rate: 1.0 - filtered_cloud_topic: filtered_cloud - ns: kinect diff --git a/moveit_configs/panda_moveit_config/config/simple_moveit_controllers.yaml b/moveit_configs/panda_moveit_config/config/simple_moveit_controllers.yaml index 19a874be8..4f91cea9b 100644 --- a/moveit_configs/panda_moveit_config/config/simple_moveit_controllers.yaml +++ b/moveit_configs/panda_moveit_config/config/simple_moveit_controllers.yaml @@ -1,19 +1,19 @@ controller_list: - - name: $(arg transmission)_joint_trajectory_controller + - name: position_joint_trajectory_controller action_ns: follow_joint_trajectory type: FollowJointTrajectory default: True joints: - - $(arg arm_id)_joint1 - - $(arg arm_id)_joint2 - - $(arg arm_id)_joint3 - - $(arg arm_id)_joint4 - - $(arg arm_id)_joint5 - - $(arg arm_id)_joint6 - - $(arg arm_id)_joint7 + - panda_joint1 + - panda_joint2 + - panda_joint3 + - panda_joint4 + - panda_joint5 + - panda_joint6 + - panda_joint7 - name: franka_gripper action_ns: gripper_action type: GripperCommand default: True joints: - - $(arg arm_id)_finger_joint1 + - panda_finger_joint1 \ No newline at end of file diff --git a/moveit_configs/panda_moveit_config/config/stomp_planning.yaml b/moveit_configs/panda_moveit_config/config/stomp_planning.yaml index 7084f2fd8..0cd2c8a03 100644 --- a/moveit_configs/panda_moveit_config/config/stomp_planning.yaml +++ b/moveit_configs/panda_moveit_config/config/stomp_planning.yaml @@ -1,37 +1,39 @@ -optimization: - num_timesteps: 60 - num_iterations: 40 - num_iterations_after_valid: 0 - num_rollouts: 30 - max_rollouts: 30 - initialization_method: 1 #[1 : LINEAR_INTERPOLATION, 2 : CUBIC_POLYNOMIAL, 3 : MININUM_CONTROL_COST] - control_cost_weight: 0.0 -task: - noise_generator: - - class: stomp_moveit/NormalDistributionSampling - stddev: [0.05, 0.8, 1.0, 0.8, 0.4, 0.4, 0.4] - cost_functions: - - class: stomp_moveit/CollisionCheck - collision_penalty: 1.0 - cost_weight: 1.0 - kernel_window_percentage: 0.2 - longest_valid_joint_move: 0.05 - noisy_filters: - - class: stomp_moveit/JointLimits - lock_start: True - lock_goal: True - - class: stomp_moveit/MultiTrajectoryVisualization - line_width: 0.02 - rgb: [255, 255, 0] - marker_array_topic: stomp_trajectories - marker_namespace: noisy - update_filters: - - class: stomp_moveit/PolynomialSmoother - poly_order: 6 - - class: stomp_moveit/TrajectoryVisualization - line_width: 0.05 - rgb: [0, 191, 255] - error_rgb: [255, 0, 0] - publish_intermediate: True - marker_topic: stomp_trajectory - marker_namespace: optimized +stomp/panda_arm: + group_name: panda_arm + optimization: + num_timesteps: 60 + num_iterations: 40 + num_iterations_after_valid: 0 + num_rollouts: 30 + max_rollouts: 30 + initialization_method: 1 # [1 : LINEAR_INTERPOLATION, 2 : CUBIC_POLYNOMIAL, 3 : MININUM_CONTROL_COST] + control_cost_weight: 0.0 + task: + noise_generator: + - class: stomp_moveit/NormalDistributionSampling + stddev: [0.05, 0.05, 0.05, 0.05, 0.05, 0.05, 0.05] + cost_functions: + - class: stomp_moveit/CollisionCheck + collision_penalty: 1.0 + cost_weight: 1.0 + kernel_window_percentage: 0.2 + longest_valid_joint_move: 0.05 + noisy_filters: + - class: stomp_moveit/JointLimits + lock_start: True + lock_goal: True + - class: stomp_moveit/MultiTrajectoryVisualization + line_width: 0.02 + rgb: [255, 255, 0] + marker_array_topic: stomp_trajectories + marker_namespace: noisy + update_filters: + - class: stomp_moveit/PolynomialSmoother + poly_order: 6 + - class: stomp_moveit/TrajectoryVisualization + line_width: 0.05 + rgb: [0, 191, 255] + error_rgb: [255, 0, 0] + publish_intermediate: True + marker_topic: stomp_trajectory + marker_namespace: optimized \ No newline at end of file diff --git a/moveit_configs/panda_moveit_config/config/trajopt_planning.yaml b/moveit_configs/panda_moveit_config/config/trajopt_planning.yaml deleted file mode 100644 index 6c6ea4361..000000000 --- a/moveit_configs/panda_moveit_config/config/trajopt_planning.yaml +++ /dev/null @@ -1,58 +0,0 @@ -trajopt_param: - improve_ratio_threshold: 0.25 # minimum ratio true_improve/approx_improve to accept step. [Research Paper] TrueImprove/ModelImprove > c - min_trust_box_size: 1e-4 # if trust region gets any smaller, exit and report convergence. [Research Paper] xtol - min_approx_improve: 1e-4 # if model improves less than this, exit and report convergence - min_approx_improve_frac: -.Inf # if model improves less than this, exit and report convergence - max_iter: 100 # The max number of iterations - trust_shrink_ratio: 0.1 # if improvement is less than improve_ratio_threshold, shrink trust region by this ratio. [Research Paper] tao- - trust_expand_ratio: 1.5 # if improvement is less than improve_ratio_threshold, shrink trust region by this ratio. [Research Paper] tao+ - cnt_tolerance: 1e-4 # after convergence of penalty subproblem, if constraint violation is less than this, we're done. [Research Paper] ctol - max_merit_coeff_increases: 5 # number of times that we jack up penalty coefficient. [Reasearch Paper] Max iteration in PenaltyIteration loop - merit_coeff_increase_ratio: 10 # ratio that we increate coeff each time. [Researcy Paper] Penalty scaling factory (italic kappa) - max_time: .inf # not yet implemented - merit_error_coeff: 10 # initial penalty coefficient. [Research Paper] mu_0 - trust_box_size: 1e-1 # current size of trust region (component-wise). [Research Paper] s - -problem_info: - basic_info: - n_steps: 20 # 2 * steps_per_phase - dt_upper_lim: 2.0 # The upper limit of 1/dt values allowed in the optimization - dt_lower_lim: 100.0 # The lower limit of 1/dt values allowed in the optimization - start_fixed: true # if true, start pose is the current pose at timestep = 0 - # if false, the start pose is the one given by req.start_state - use_time: false # if false, it means the timestep is unit, meaning x1-x0 is the velocity for example - # if true, then cost_infos ro cnt_infos should have TT_USE_TIME for their term_type - convex_solver: 1 # which convex solver to use - # 1 = AUTO_SOLVER, 2 = BPMPD, 3 = OSQP, 4 = QPOASES, 5 = GUROBI - - init_info: - type: 3 # 1 = STATIONARY, 2 = JOINT_INTERPOLATED, 3 = GIVEN_TRAJ - # request.start_state should be provided for JOINT_INTERPOLATED and GIVEN_TRAJ - dt: 0.5 - -joint_pos_term_info: - start_pos: # from req.start_state - name: start_pos - first_timestep: 10 # First time step to which the term is applied. - last_timestep: 10 # Last time step to which the term is applied. - # if start_fixed is trure then we can not set the first_timestep to 0 for start_pos as it is going - # to be ignored and only the current pose would be the constraint at timestep = 0. - term_type: 2 # 1 = TT_COST, 2 = TT_CNT, 3 = TT_USE_TIME - middle_pos: - name: middle_pos - first_timestep: 15 - last_timestep: 15 - term_type: 2 - goal_pos: - name: goal_pos - first_timestep: 19 - last_timestep: 19 - term_type: 2 - goal_tmp: # If the constraint from request does not have any name, for example when using MotionPlanning Display in Rviz, - # goal_tmp is assigned as the name of the constraint. - # In this case: start_fixed = false and start_pos should be applied at timestep 0 - # OR: start_fixed = true and start_pos can be applies at any timestep - name: goal_tmp - first_timestep: 19 # n_steps - 1 - last_timestep: 19 # n_steps - 1 - term_type: 2 diff --git a/moveit_configs/panda_moveit_config/launch/chomp_planning_pipeline.launch.xml b/moveit_configs/panda_moveit_config/launch/chomp_planning_pipeline.launch.xml index 39e645f84..77d7697d4 100644 --- a/moveit_configs/panda_moveit_config/launch/chomp_planning_pipeline.launch.xml +++ b/moveit_configs/panda_moveit_config/launch/chomp_planning_pipeline.launch.xml @@ -3,7 +3,8 @@ - - - diff --git a/moveit_configs/panda_moveit_config/launch/demo.launch b/moveit_configs/panda_moveit_config/launch/demo.launch index 543cb190c..873fb0b43 100644 --- a/moveit_configs/panda_moveit_config/launch/demo.launch +++ b/moveit_configs/panda_moveit_config/launch/demo.launch @@ -1,5 +1,4 @@ - @@ -12,9 +11,6 @@ - - - @@ -22,19 +18,13 @@ - - - - - - - + - - - + + + + + + + + - diff --git a/moveit_configs/panda_moveit_config/launch/demo_chomp.launch b/moveit_configs/panda_moveit_config/launch/demo_chomp.launch deleted file mode 100644 index a3051a809..000000000 --- a/moveit_configs/panda_moveit_config/launch/demo_chomp.launch +++ /dev/null @@ -1,5 +0,0 @@ - - - - - diff --git a/moveit_configs/panda_moveit_config/launch/demo_gazebo.launch b/moveit_configs/panda_moveit_config/launch/demo_gazebo.launch index 0b007e643..0ef8f9540 100644 --- a/moveit_configs/panda_moveit_config/launch/demo_gazebo.launch +++ b/moveit_configs/panda_moveit_config/launch/demo_gazebo.launch @@ -1,27 +1,21 @@ + - - + + - - - + + + + + - - - - - - - - - - + + + - + - - + diff --git a/moveit_configs/panda_moveit_config/launch/demo_lerp.launch b/moveit_configs/panda_moveit_config/launch/demo_lerp.launch deleted file mode 100644 index d5d835541..000000000 --- a/moveit_configs/panda_moveit_config/launch/demo_lerp.launch +++ /dev/null @@ -1,6 +0,0 @@ - - - - - - \ No newline at end of file diff --git a/moveit_configs/panda_moveit_config/launch/demo_stomp.launch b/moveit_configs/panda_moveit_config/launch/demo_stomp.launch deleted file mode 100644 index 0ae05517d..000000000 --- a/moveit_configs/panda_moveit_config/launch/demo_stomp.launch +++ /dev/null @@ -1,5 +0,0 @@ - - - - - diff --git a/moveit_configs/panda_moveit_config/launch/franka_control.launch b/moveit_configs/panda_moveit_config/launch/franka_control.launch deleted file mode 100644 index d3e902333..000000000 --- a/moveit_configs/panda_moveit_config/launch/franka_control.launch +++ /dev/null @@ -1,22 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - diff --git a/moveit_configs/panda_moveit_config/launch/gazebo.launch b/moveit_configs/panda_moveit_config/launch/gazebo.launch index 1dfaf3cda..b0d889a78 100644 --- a/moveit_configs/panda_moveit_config/launch/gazebo.launch +++ b/moveit_configs/panda_moveit_config/launch/gazebo.launch @@ -1,96 +1,34 @@ - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + - - - - - - - - - - - - - - - + + + - - - - - - - - - - [franka_state_controller/joint_states, franka_gripper/joint_states] - - + + - - - - - + + + - + + + + diff --git a/moveit_configs/panda_moveit_config/launch/lerp_planning_pipeline.launch.xml b/moveit_configs/panda_moveit_config/launch/lerp_planning_pipeline.launch.xml deleted file mode 100644 index c1193577e..000000000 --- a/moveit_configs/panda_moveit_config/launch/lerp_planning_pipeline.launch.xml +++ /dev/null @@ -1,22 +0,0 @@ - - - - - - - - - - - - - - - - - diff --git a/moveit_configs/panda_moveit_config/launch/move_group.launch b/moveit_configs/panda_moveit_config/launch/move_group.launch index 7d511f3b4..7aef0b03c 100644 --- a/moveit_configs/panda_moveit_config/launch/move_group.launch +++ b/moveit_configs/panda_moveit_config/launch/move_group.launch @@ -21,12 +21,6 @@ - - - - - - - + - - @@ -57,32 +49,30 @@ - - - - - + + + @@ -101,6 +91,10 @@ + + + diff --git a/moveit_configs/panda_moveit_config/launch/moveit.rviz b/moveit_configs/panda_moveit_config/launch/moveit.rviz index 458f3b9bf..82d21c70c 100644 --- a/moveit_configs/panda_moveit_config/launch/moveit.rviz +++ b/moveit_configs/panda_moveit_config/launch/moveit.rviz @@ -5,9 +5,8 @@ Panels: Property Tree Widget: Expanded: - /MotionPlanning1 - - /MotionPlanning1/Planned Path1 Splitter Ratio: 0.5 - Tree Height: 313 + Tree Height: 226 - Class: rviz/Help Name: Help - Class: rviz/Views @@ -15,10 +14,6 @@ Panels: - /Current View1 Name: Views Splitter Ratio: 0.5 -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 Visualization Manager: Class: "" Displays: @@ -28,7 +23,7 @@ Visualization Manager: Color: 160; 160; 164 Enabled: true Line Style: - Line Width: 0.029999999329447746 + Line Width: 0.03 Value: Lines Name: Grid Normal Cell Count: 0 @@ -40,246 +35,30 @@ Visualization Manager: Plane Cell Count: 10 Reference Frame: Value: true - - Acceleration_Scaling_Factor: 0.1 - Class: moveit_rviz_plugin/MotionPlanning + - Class: moveit_rviz_plugin/MotionPlanning Enabled: true - Move Group Namespace: "" - MoveIt_Allow_Approximate_IK: false - MoveIt_Allow_External_Program: false - MoveIt_Allow_Replanning: false - MoveIt_Allow_Sensor_Positioning: false - MoveIt_Planning_Attempts: 10 - MoveIt_Planning_Time: 5 - MoveIt_Use_Cartesian_Path: false - MoveIt_Use_Constraint_Aware_IK: false - MoveIt_Workspace: - Center: - X: 0 - Y: 0 - Z: 0 - Size: - X: 2 - Y: 2 - Z: 2 Name: MotionPlanning Planned Path: - Color Enabled: false - Interrupt Display: false - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - fixed_camera_bottom_screw_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - fixed_camera_color_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - fixed_camera_color_optical_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - fixed_camera_depth_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - fixed_camera_depth_optical_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - fixed_camera_infra1_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - fixed_camera_infra1_optical_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - fixed_camera_infra2_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - fixed_camera_infra2_optical_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - fixed_camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_hand: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_hand_sc: - Alpha: 1 - Show Axes: false - Show Trail: false - panda_hand_tcp: - Alpha: 1 - Show Axes: false - Show Trail: false - panda_leftfinger: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link0: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link0_sc: - Alpha: 1 - Show Axes: false - Show Trail: false - panda_link1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link1_sc: - Alpha: 1 - Show Axes: false - Show Trail: false - panda_link2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link2_sc: - Alpha: 1 - Show Axes: false - Show Trail: false - panda_link3: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link3_sc: - Alpha: 1 - Show Axes: false - Show Trail: false - panda_link4: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link4_sc: - Alpha: 1 - Show Axes: false - Show Trail: false - panda_link5: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link5_sc: - Alpha: 1 - Show Axes: false - Show Trail: false - panda_link6: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link6_sc: - Alpha: 1 - Show Axes: false - Show Trail: false - panda_link7: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link7_sc: - Alpha: 1 - Show Axes: false - Show Trail: false - panda_link8: - Alpha: 1 - Show Axes: false - Show Trail: false - panda_rightfinger: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - world: - Alpha: 1 - Show Axes: false - Show Trail: false - wrist_camera_bottom_screw_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - wrist_camera_color_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - wrist_camera_color_optical_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - wrist_camera_depth_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - wrist_camera_depth_optical_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - wrist_camera_infra1_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - wrist_camera_infra1_optical_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - wrist_camera_infra2_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - wrist_camera_infra2_optical_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - wrist_camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - Loop Animation: false + Links: ~ + Loop Animation: true Robot Alpha: 0.5 - Robot Color: 150; 50; 150 Show Robot Collision: false Show Robot Visual: true Show Trail: false State Display Time: 0.05 s - Trail Step Size: 1 Trajectory Topic: move_group/display_planned_path - Use Sim Time: false Planning Metrics: Payload: 1 Show Joint Torques: false Show Manipulability: false Show Manipulability Index: false Show Weight Limit: false - TextHeight: 0.07999999821186066 Planning Request: Colliding Link Color: 255; 0; 0 Goal State Alpha: 1 Goal State Color: 250; 128; 0 Interactive Marker Size: 0 Joint Violation Color: 255; 0; 255 - Planning Group: panda_arm Query Goal State: true Query Start State: false Show Workspace: false @@ -289,215 +68,19 @@ Visualization Manager: Robot Description: robot_description Scene Geometry: Scene Alpha: 1 - Scene Color: 50; 230; 50 - Scene Display Time: 0.009999999776482582 Show Scene Geometry: true Voxel Coloring: Z-Axis Voxel Rendering: Occupied Voxels Scene Robot: Attached Body Color: 150; 50; 150 - Links: - All Links Enabled: true - Expand Joint Details: false - Expand Link Details: false - Expand Tree: false - Link Tree Style: Links in Alphabetic Order - fixed_camera_bottom_screw_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - fixed_camera_color_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - fixed_camera_color_optical_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - fixed_camera_depth_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - fixed_camera_depth_optical_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - fixed_camera_infra1_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - fixed_camera_infra1_optical_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - fixed_camera_infra2_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - fixed_camera_infra2_optical_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - fixed_camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_hand: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_hand_sc: - Alpha: 1 - Show Axes: false - Show Trail: false - panda_hand_tcp: - Alpha: 1 - Show Axes: false - Show Trail: false - panda_leftfinger: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link0: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link0_sc: - Alpha: 1 - Show Axes: false - Show Trail: false - panda_link1: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link1_sc: - Alpha: 1 - Show Axes: false - Show Trail: false - panda_link2: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link2_sc: - Alpha: 1 - Show Axes: false - Show Trail: false - panda_link3: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link3_sc: - Alpha: 1 - Show Axes: false - Show Trail: false - panda_link4: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link4_sc: - Alpha: 1 - Show Axes: false - Show Trail: false - panda_link5: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link5_sc: - Alpha: 1 - Show Axes: false - Show Trail: false - panda_link6: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link6_sc: - Alpha: 1 - Show Axes: false - Show Trail: false - panda_link7: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - panda_link7_sc: - Alpha: 1 - Show Axes: false - Show Trail: false - panda_link8: - Alpha: 1 - Show Axes: false - Show Trail: false - panda_rightfinger: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true - world: - Alpha: 1 - Show Axes: false - Show Trail: false - wrist_camera_bottom_screw_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - wrist_camera_color_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - wrist_camera_color_optical_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - wrist_camera_depth_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - wrist_camera_depth_optical_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - wrist_camera_infra1_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - wrist_camera_infra1_optical_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - wrist_camera_infra2_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - wrist_camera_infra2_optical_frame: - Alpha: 1 - Show Axes: false - Show Trail: false - wrist_camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true + Links: ~ Robot Alpha: 0.5 - Show Robot Collision: false - Show Robot Visual: true + Show Scene Robot: true Value: true - Velocity_Scaling_Factor: 0.1 Enabled: true Global Options: Background Color: 48; 48; 48 - Default Light: true Fixed Frame: panda_link0 - Frame Rate: 30 Name: root Tools: - Class: rviz/Interact @@ -508,30 +91,30 @@ Visualization Manager: Views: Current: Class: rviz/Orbit - Distance: 2.617434501647949 + Distance: 2.0 Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 + Stereo Eye Separation: 0.06 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Field of View: 0.75 Focal Point: - X: -0.10000000149011612 + X: -0.1 Y: 0.25 - Z: 0.30000001192092896 + Z: 0.30 Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 + Focal Shape Size: 0.05 Invert Z Axis: false Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.4700000286102295 + Near Clip Distance: 0.01 + Pitch: 0.5 Target Frame: panda_link0 - Yaw: 5.334949493408203 + Yaw: -0.6232355833053589 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1016 + Height: 848 Help: collapsed: false Hide Left Dock: false @@ -540,9 +123,9 @@ Window Geometry: collapsed: false MotionPlanning - Trajectory Slider: collapsed: false - QMainWindow State: 000000ff00000000fd0000000100000000000001f0000003a2fc0200000007fb000000100044006900730070006c006100790073010000003b000001c8000000c700fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a000fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670100000209000001d40000016a00ffffff00000540000003a200000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd0000000100000000000001f0000002f6fc0200000007fb000000100044006900730070006c006100790073010000003d00000173000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006701000001b60000017d0000017d00ffffff00000315000002f600000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Views: collapsed: false - Width: 1846 - X: 74 - Y: 27 + Width: 1291 + X: 454 + Y: 25 diff --git a/moveit_configs/panda_moveit_config/launch/moveit_empty.rviz b/moveit_configs/panda_moveit_config/launch/moveit_empty.rviz deleted file mode 100644 index 9014d11a6..000000000 --- a/moveit_configs/panda_moveit_config/launch/moveit_empty.rviz +++ /dev/null @@ -1,99 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 84 - Name: Displays - Property Tree Widget: - Expanded: ~ - Splitter Ratio: 0.5 - Tree Height: 613 - - Class: rviz/Help - Name: Help - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: true - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: rviz_visual_tools - Name: MarkerArray - Namespaces: - {} - Queue Size: 100 - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Default Light: true - Fixed Frame: panda_link0 - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz_visual_tools/KeyTool - Value: true - Views: - Current: - Class: rviz/Orbit - Distance: 2 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Field of View: 0.75 - Focal Point: - X: -0.10000000149011612 - Y: 0.25 - Z: 0.30000001192092896 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.5 - Target Frame: panda_link0 - Yaw: -0.6232355833053589 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 848 - Help: - collapsed: false - Hide Left Dock: false - Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000100000000000001f0000002f6fc020000000afb000000100044006900730070006c006100790073010000003d000002f6000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006700000001ab000001880000000000000000fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb00000024005200760069007a00560069007300750061006c0054006f006f006c007300470075006900000002f2000000410000000000000000fb0000003c005400720061006a006500630074006f007200790020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff000000000000000000000315000002f600000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Views: - collapsed: false - Width: 1291 - X: 444 - Y: 25 diff --git a/moveit_configs/panda_moveit_config/launch/moveit_rviz.launch b/moveit_configs/panda_moveit_config/launch/moveit_rviz.launch index 89e0db9e4..a4605c037 100644 --- a/moveit_configs/panda_moveit_config/launch/moveit_rviz.launch +++ b/moveit_configs/panda_moveit_config/launch/moveit_rviz.launch @@ -4,13 +4,9 @@ - - - - - - - + + + diff --git a/moveit_configs/panda_moveit_config/launch/moveit_scene.rviz b/moveit_configs/panda_moveit_config/launch/moveit_scene.rviz deleted file mode 100644 index b9e16ad76..000000000 --- a/moveit_configs/panda_moveit_config/launch/moveit_scene.rviz +++ /dev/null @@ -1,138 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 84 - Name: Displays - Property Tree Widget: - Expanded: ~ - Splitter Ratio: 0.5 - Tree Height: 542 - - Class: rviz/Help - Name: Help - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz_visual_tools/RvizVisualToolsGui - Name: RvizVisualToolsGui -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.03 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: true - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: rviz_visual_tools - Name: MarkerArray - Namespaces: - {} - Queue Size: 100 - Value: true - - Class: moveit_rviz_plugin/Trajectory - Color Enabled: false - Enabled: true - Interrupt Display: false - Links: ~ - Loop Animation: false - Name: Trajectory - Robot Alpha: 0.5 - Show Robot Collision: false - Show Robot Visual: true - Show Trail: false - State Display Time: 3x - Trail Step Size: 1 - Trajectory Topic: move_group/display_planned_path - Value: true - - Class: moveit_rviz_plugin/PlanningScene - Enabled: true - Move Group Namespace: "" - Name: PlanningScene - Planning Scene Topic: move_group/monitored_planning_scene - Robot Description: robot_description - Scene Geometry: - Scene Alpha: 0.9 - Show Scene Geometry: true - Voxel Coloring: Z-Axis - Voxel Rendering: Occupied Voxels - Scene Robot: - Attached Body Color: 150; 50; 150 - Links: ~ - Robot Alpha: 1 - Show Robot Collision: false - Show Robot Visual: true - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Default Light: true - Fixed Frame: panda_link0 - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz_visual_tools/KeyTool - Value: true - Views: - Current: - Class: rviz/Orbit - Distance: 2.0 - Enable Stereo Rendering: - Stereo Eye Separation: 0.06 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Field of View: 0.75 - Focal Point: - X: -0.1 - Y: 0.25 - Z: 0.30 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.01 - Pitch: 0.5 - Target Frame: panda_link0 - Yaw: -0.6232355833053589 - Saved: ~ -Window Geometry: - Displays: - collapsed: false - Height: 848 - Help: - collapsed: false - Hide Left Dock: false - Hide Right Dock: false - QMainWindow State: 000000ff00000000fd0000000100000000000001f0000002f6fc0200000009fb000000100044006900730070006c006100790073010000003d000002af000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000010c000000a4000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e006700000001ab000001880000000000000000fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000004100fffffffb00000024005200760069007a00560069007300750061006c0054006f006f006c007300470075006901000002f2000000410000004100ffffff00000315000002f600000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - RvizVisualToolsGui: - collapsed: false - Trajectory - Slider: - collapsed: false - Views: - collapsed: false - Width: 1291 - X: 449 - Y: 25 diff --git a/moveit_configs/panda_moveit_config/launch/ompl-chomp_planning_pipeline.launch.xml b/moveit_configs/panda_moveit_config/launch/ompl-chomp_planning_pipeline.launch.xml index 178d2bef7..bbf263fda 100644 --- a/moveit_configs/panda_moveit_config/launch/ompl-chomp_planning_pipeline.launch.xml +++ b/moveit_configs/panda_moveit_config/launch/ompl-chomp_planning_pipeline.launch.xml @@ -1,19 +1,20 @@ - + - + - + diff --git a/moveit_configs/panda_moveit_config/launch/ompl_planning_pipeline.launch.xml b/moveit_configs/panda_moveit_config/launch/ompl_planning_pipeline.launch.xml index af9d56e19..fb66d53e7 100644 --- a/moveit_configs/panda_moveit_config/launch/ompl_planning_pipeline.launch.xml +++ b/moveit_configs/panda_moveit_config/launch/ompl_planning_pipeline.launch.xml @@ -2,7 +2,8 @@ - - - - - + diff --git a/moveit_configs/panda_moveit_config/launch/planning_context.launch b/moveit_configs/panda_moveit_config/launch/planning_context.launch index 308406433..bbc69d245 100644 --- a/moveit_configs/panda_moveit_config/launch/planning_context.launch +++ b/moveit_configs/panda_moveit_config/launch/planning_context.launch @@ -1,37 +1,26 @@ - - - - + + - + - - - - + - + - - + + - + - + - \ No newline at end of file + diff --git a/moveit_configs/panda_moveit_config/launch/planning_pipeline.launch.xml b/moveit_configs/panda_moveit_config/launch/planning_pipeline.launch.xml index 250e7432c..4b4d0d663 100644 --- a/moveit_configs/panda_moveit_config/launch/planning_pipeline.launch.xml +++ b/moveit_configs/panda_moveit_config/launch/planning_pipeline.launch.xml @@ -4,8 +4,7 @@ It is assumed that all planning pipelines are named XXX_planning_pipeline.launch --> - - + diff --git a/moveit_configs/panda_moveit_config/launch/robot_description.launch b/moveit_configs/panda_moveit_config/launch/robot_description.launch deleted file mode 100644 index 82c0d1ad5..000000000 --- a/moveit_configs/panda_moveit_config/launch/robot_description.launch +++ /dev/null @@ -1,16 +0,0 @@ - - - - - - - - - - - - - - - \ No newline at end of file diff --git a/moveit_configs/panda_moveit_config/launch/ros_controllers.launch b/moveit_configs/panda_moveit_config/launch/ros_controllers.launch index 47806e5ab..46fd66ae7 100644 --- a/moveit_configs/panda_moveit_config/launch/ros_controllers.launch +++ b/moveit_configs/panda_moveit_config/launch/ros_controllers.launch @@ -2,9 +2,10 @@ - + + + + - - diff --git a/moveit_configs/panda_moveit_config/launch/run_benchmark_trajopt.launch b/moveit_configs/panda_moveit_config/launch/run_benchmark_trajopt.launch deleted file mode 100644 index 762841632..000000000 --- a/moveit_configs/panda_moveit_config/launch/run_benchmark_trajopt.launch +++ /dev/null @@ -1,21 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - diff --git a/moveit_configs/panda_moveit_config/launch/sensor_manager.launch.xml b/moveit_configs/panda_moveit_config/launch/sensor_manager.launch.xml index 61db9d9dd..17279ddde 100644 --- a/moveit_configs/panda_moveit_config/launch/sensor_manager.launch.xml +++ b/moveit_configs/panda_moveit_config/launch/sensor_manager.launch.xml @@ -2,13 +2,11 @@ - - - + + - diff --git a/moveit_configs/panda_moveit_config/launch/simple_moveit_controller_manager.launch.xml b/moveit_configs/panda_moveit_config/launch/simple_moveit_controller_manager.launch.xml index 44e6d6c65..5b18aca3b 100644 --- a/moveit_configs/panda_moveit_config/launch/simple_moveit_controller_manager.launch.xml +++ b/moveit_configs/panda_moveit_config/launch/simple_moveit_controller_manager.launch.xml @@ -3,5 +3,6 @@ - + + diff --git a/moveit_configs/panda_moveit_config/launch/stomp_planning_pipeline.launch.xml b/moveit_configs/panda_moveit_config/launch/stomp_planning_pipeline.launch.xml index cbff8df91..39fdb2e40 100644 --- a/moveit_configs/panda_moveit_config/launch/stomp_planning_pipeline.launch.xml +++ b/moveit_configs/panda_moveit_config/launch/stomp_planning_pipeline.launch.xml @@ -1,16 +1,17 @@ - + - - + + @@ -18,17 +19,5 @@ - - - - - - - - - - - - - + diff --git a/moveit_configs/panda_moveit_config/launch/trajopt_planning_pipeline.launch.xml b/moveit_configs/panda_moveit_config/launch/trajopt_planning_pipeline.launch.xml deleted file mode 100644 index ec9ea9b12..000000000 --- a/moveit_configs/panda_moveit_config/launch/trajopt_planning_pipeline.launch.xml +++ /dev/null @@ -1,32 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - -