diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..fa83498 --- /dev/null +++ b/.gitignore @@ -0,0 +1,3 @@ +# IDE +.vscode +*.code-workspace diff --git a/.setup_assistant b/.setup_assistant index 76ca6b1..395596c 100644 --- a/.setup_assistant +++ b/.setup_assistant @@ -3,8 +3,8 @@ moveit_setup_assistant_config: package: franka_description relative_path: robots/panda_arm.urdf.xacro SRDF: - relative_path: config/panda_arm.srdf.xacro + relative_path: config/panda.srdf CONFIG: - author_name: Mike Lautman - author_email: mike@picknik.ai - generated_timestamp: 1519152260 \ No newline at end of file + author_name: Tyler Weaver + author_email: tyler@picknik.ai + generated_timestamp: 1608134967 \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index 27189ee..675c9ac 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.4) +cmake_minimum_required(VERSION 3.1.3) project(panda_moveit_config) find_package(catkin REQUIRED) diff --git a/config/cartesian_limits.yaml b/config/cartesian_limits.yaml new file mode 100644 index 0000000..7df72f6 --- /dev/null +++ b/config/cartesian_limits.yaml @@ -0,0 +1,5 @@ +cartesian_limits: + max_trans_vel: 1 + max_trans_acc: 2.25 + max_trans_dec: -5 + max_rot_vel: 1.57 diff --git a/config/chomp_planning.yaml b/config/chomp_planning.yaml index a082293..75258e5 100644 --- a/config/chomp_planning.yaml +++ b/config/chomp_planning.yaml @@ -4,25 +4,15 @@ max_iterations_after_collision_free: 5 smoothness_cost_weight: 0.1 obstacle_cost_weight: 1.0 learning_rate: 0.01 -animate_path: true -add_randomness: false smoothness_cost_velocity: 0.0 smoothness_cost_acceleration: 1.0 smoothness_cost_jerk: 0.0 -hmc_discretization: 0.01 -hmc_stochasticity: 0.01 -hmc_annealing_factor: 0.99 -use_hamiltonian_monte_carlo: false -ridge_factor: 0.0 +ridge_factor: 0.01 use_pseudo_inverse: false pseudo_inverse_ridge_factor: 1e-4 -animate_endeffector: false -animate_endeffector_segment: "panda_rightfinger" joint_update_limit: 0.1 collision_clearence: 0.2 collision_threshold: 0.07 -random_jump_amount: 1.0 use_stochastic_descent: true -enable_failure_recovery: false -max_recovery_attempts: 5 -trajectory_initialization_method: "quintic-spline" +enable_failure_recovery: true +max_recovery_attempts: 5 \ No newline at end of file diff --git a/config/fake_controllers.yaml b/config/fake_controllers.yaml index 111ff4c..0502653 100644 --- a/config/fake_controllers.yaml +++ b/config/fake_controllers.yaml @@ -11,9 +11,8 @@ controller_list: - name: fake_hand_controller joints: - panda_finger_joint1 - - panda_finger_joint2 -initial: +initial: # Define initial robot poses. - group: panda_arm pose: ready - group: hand - pose: open + pose: open \ No newline at end of file diff --git a/config/joint_limits.yaml b/config/joint_limits.yaml index a2f08ed..c3472cc 100644 --- a/config/joint_limits.yaml +++ b/config/joint_limits.yaml @@ -1,55 +1,55 @@ # joint_limits.yaml allows the dynamics properties specified in the URDF to be overwritten or augmented as needed -# 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 +# For beginners, we downscale velocity and acceleration limits. +# You can always specify higher scaling factors (<= 1.0) in your motion requests. # Increase the values below to 1.0 to always move at maximum speed. +default_velocity_scaling_factor: 0.1 +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] joint_limits: + panda_finger_joint1: + has_velocity_limits: true + max_velocity: 0.2 + has_acceleration_limits: false + max_acceleration: 0 + panda_finger_joint2: + has_velocity_limits: true + max_velocity: 0.2 + has_acceleration_limits: false + max_acceleration: 0 panda_joint1: has_velocity_limits: true - max_velocity: 2.1750 - has_acceleration_limits: true - max_acceleration: 3.75 + max_velocity: 2.175 + has_acceleration_limits: false + max_acceleration: 0 panda_joint2: has_velocity_limits: true - max_velocity: 2.1750 - has_acceleration_limits: true - max_acceleration: 1.875 + max_velocity: 2.175 + has_acceleration_limits: false + max_acceleration: 0 panda_joint3: has_velocity_limits: true - max_velocity: 2.1750 - has_acceleration_limits: true - max_acceleration: 2.5 + 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 + 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 + 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 - panda_joint7: - has_velocity_limits: true - max_velocity: 2.6100 - has_acceleration_limits: true - max_acceleration: 5 - panda_finger_joint1: - has_velocity_limits: true - max_velocity: 0.1 + max_velocity: 2.61 has_acceleration_limits: false max_acceleration: 0 - panda_finger_joint2: + panda_joint7: has_velocity_limits: true - max_velocity: 0.1 + max_velocity: 2.61 has_acceleration_limits: false - max_acceleration: 0 + max_acceleration: 0 \ No newline at end of file diff --git a/config/kinematics.yaml b/config/kinematics.yaml index e721385..60737fa 100644 --- a/config/kinematics.yaml +++ b/config/kinematics.yaml @@ -1,4 +1,4 @@ panda_arm: kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin kinematics_solver_search_resolution: 0.005 - kinematics_solver_timeout: 0.05 + kinematics_solver_timeout: 0.005 \ No newline at end of file diff --git a/config/ompl_planning.yaml b/config/ompl_planning.yaml index 491e829..6f8d2f7 100644 --- a/config/ompl_planning.yaml +++ b/config/ompl_planning.yaml @@ -1,42 +1,49 @@ planner_configs: - SBLkConfigDefault: + AnytimePathShortening: + type: geometric::AnytimePathShortening + shortcut: true # Attempt to shortcut all new solution paths + hybridize: true # Compute hybrid solution trajectories + max_hybrid_paths: 24 # Number of hybrid paths generated per iteration + num_planners: 4 # The number of default planners to use for planning + planners: "" # A comma-separated list of planner types (e.g., "PRM,EST,RRTConnect"Optionally, planner parameters can be passed to change the default:"PRM[max_nearest_neighbors=5],EST[goal_bias=.5],RRT[range=10. goal_bias=.1]" + SBL: type: geometric::SBL range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - ESTkConfigDefault: + EST: type: geometric::EST range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0 setup() goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 - LBKPIECEkConfigDefault: + LBKPIECE: type: geometric::LBKPIECE range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 - BKPIECEkConfigDefault: + BKPIECE: type: geometric::BKPIECE range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 - KPIECEkConfigDefault: + KPIECE: type: geometric::KPIECE range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 border_fraction: 0.9 # Fraction of time focused on boarder default: 0.9 (0.0,1.] failed_expansion_score_factor: 0.5 # When extending motion fails, scale score by factor. default: 0.5 min_valid_path_fraction: 0.5 # Accept partially valid moves above fraction. default: 0.5 - RRTkConfigDefault: + RRT: type: geometric::RRT range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 - RRTConnectkConfigDefault: + RRTConnect: type: geometric::RRTConnect range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - RRTstarkConfigDefault: + RRTstar: type: geometric::RRTstar range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 delay_collision_checking: 1 # Stop collision checking as soon as C-free parent found. default 1 - TRRTkConfigDefault: + TRRT: type: geometric::TRRT range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() goal_bias: 0.05 # When close to goal select goal, with this probability? default: 0.05 @@ -47,12 +54,12 @@ planner_configs: 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 k_constant: 0.0 # value used to normalize expresssion. default: 0.0 set in setup() - PRMkConfigDefault: + PRM: type: geometric::PRM max_nearest_neighbors: 10 # use k nearest neighbors. default: 10 - PRMstarkConfigDefault: + PRMstar: type: geometric::PRMstar - FMTkConfigDefault: + FMT: type: geometric::FMT num_samples: 1000 # number of states that the planner should sample. default: 1000 radius_multiplier: 1.1 # multiplier used for the nearest neighbors search radius. default: 1.1 @@ -60,7 +67,7 @@ planner_configs: cache_cc: 1 # use collision checking cache. default: 1 heuristics: 0 # activate cost to go heuristics. default: 0 extended_fmt: 1 # activate the extended FMT*: adding new samples if planner does not finish successfully. default: 1 - BFMTkConfigDefault: + BFMT: type: geometric::BFMT num_samples: 1000 # number of states that the planner should sample. default: 1000 radius_multiplier: 1.0 # multiplier used for the nearest neighbors search radius. default: 1.0 @@ -70,9 +77,9 @@ planner_configs: heuristics: 1 # activates cost to go heuristics. default: 1 cache_cc: 1 # use the collision checking cache. default: 1 extended_fmt: 1 # Activates the extended FMT*: adding new samples if planner does not finish successfully. default: 1 - PDSTkConfigDefault: + PDST: type: geometric::PDST - STRIDEkConfigDefault: + STRIDE: type: geometric::STRIDE range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 @@ -83,7 +90,7 @@ planner_configs: max_pts_per_leaf: 6 # max points per leaf in the GNAT. default: 6 estimated_dimension: 0.0 # estimated dimension of the free space. default: 0.0 min_valid_path_fraction: 0.2 # Accept partially valid moves above fraction. default: 0.2 - BiTRRTkConfigDefault: + BiTRRT: type: geometric::BiTRRT 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 @@ -91,113 +98,84 @@ planner_configs: 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 cost_threshold: 1e300 # the cost threshold. Any motion cost that is not better will not be expanded. default: inf - LBTRRTkConfigDefault: + LBTRRT: type: geometric::LBTRRT range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 epsilon: 0.4 # optimality approximation factor. default: 0.4 - BiESTkConfigDefault: + BiEST: type: geometric::BiEST range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - ProjESTkConfigDefault: + ProjEST: type: geometric::ProjEST range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() goal_bias: 0.05 # When close to goal select goal, with this probability. default: 0.05 - LazyPRMkConfigDefault: + LazyPRM: type: geometric::LazyPRM range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup() - LazyPRMstarkConfigDefault: + LazyPRMstar: type: geometric::LazyPRMstar - SPARSkConfigDefault: + SPARS: type: geometric::SPARS stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 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: 1000 # maximum consecutive failure limit. default: 1000 - SPARStwokConfigDefault: + SPARStwo: type: geometric::SPARStwo stretch_factor: 3.0 # roadmap spanner stretch factor. multiplicative upper bound on path quality. It does not make sense to make this parameter more than 3. default: 3.0 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 - TrajOptDefault: - type: geometric::TrajOpt - panda_arm: planner_configs: - - SBLkConfigDefault - - ESTkConfigDefault - - LBKPIECEkConfigDefault - - BKPIECEkConfigDefault - - KPIECEkConfigDefault - - RRTkConfigDefault - - RRTConnectkConfigDefault - - RRTstarkConfigDefault - - TRRTkConfigDefault - - PRMkConfigDefault - - PRMstarkConfigDefault - - FMTkConfigDefault - - BFMTkConfigDefault - - PDSTkConfigDefault - - STRIDEkConfigDefault - - BiTRRTkConfigDefault - - LBTRRTkConfigDefault - - BiESTkConfigDefault - - ProjESTkConfigDefault - - LazyPRMkConfigDefault - - LazyPRMstarkConfigDefault - - SPARSkConfigDefault - - SPARStwokConfigDefault - - TrajOptDefault -panda_arm_hand: - planner_configs: - - SBLkConfigDefault - - ESTkConfigDefault - - LBKPIECEkConfigDefault - - BKPIECEkConfigDefault - - KPIECEkConfigDefault - - RRTkConfigDefault - - RRTConnectkConfigDefault - - RRTstarkConfigDefault - - TRRTkConfigDefault - - PRMkConfigDefault - - PRMstarkConfigDefault - - FMTkConfigDefault - - BFMTkConfigDefault - - PDSTkConfigDefault - - STRIDEkConfigDefault - - BiTRRTkConfigDefault - - LBTRRTkConfigDefault - - BiESTkConfigDefault - - ProjESTkConfigDefault - - LazyPRMkConfigDefault - - LazyPRMstarkConfigDefault - - SPARSkConfigDefault - - SPARStwokConfigDefault - - TrajOptDefault + - AnytimePathShortening + - SBL + - EST + - LBKPIECE + - BKPIECE + - KPIECE + - RRT + - RRTConnect + - RRTstar + - TRRT + - PRM + - PRMstar + - FMT + - BFMT + - PDST + - STRIDE + - BiTRRT + - LBTRRT + - BiEST + - ProjEST + - LazyPRM + - LazyPRMstar + - SPARS + - SPARStwo hand: planner_configs: - - SBLkConfigDefault - - ESTkConfigDefault - - LBKPIECEkConfigDefault - - BKPIECEkConfigDefault - - KPIECEkConfigDefault - - RRTkConfigDefault - - RRTConnectkConfigDefault - - RRTstarkConfigDefault - - TRRTkConfigDefault - - PRMkConfigDefault - - PRMstarkConfigDefault - - FMTkConfigDefault - - BFMTkConfigDefault - - PDSTkConfigDefault - - STRIDEkConfigDefault - - BiTRRTkConfigDefault - - LBTRRTkConfigDefault - - BiESTkConfigDefault - - ProjESTkConfigDefault - - LazyPRMkConfigDefault - - LazyPRMstarkConfigDefault - - SPARSkConfigDefault - - SPARStwokConfigDefault - - TrajOptDefault + - 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/config/panda.srdf b/config/panda.srdf new file mode 100644 index 0000000..9cd47c0 --- /dev/null +++ b/config/panda.srdf @@ -0,0 +1,86 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/config/panda_arm.xacro b/config/panda_arm.xacro index 4a4e9f2..62733dd 100644 --- a/config/panda_arm.xacro +++ b/config/panda_arm.xacro @@ -11,31 +11,22 @@ - - - - - - - + + + + + + + - - - - - - - - - - - - - - - - + + + + + + + @@ -57,6 +48,8 @@ + + diff --git a/config/ros_controllers.yaml b/config/ros_controllers.yaml new file mode 100644 index 0000000..0698869 --- /dev/null +++ b/config/ros_controllers.yaml @@ -0,0 +1,73 @@ +# Simulation settings for using moveit_sim_controllers +moveit_sim_hw_interface: + joint_model_group: panda_arm + joint_model_group_pose: ready +# Settings for ros_control_boilerplate control loop +generic_hw_control_loop: + loop_hz: 300 + cycle_time_error_threshold: 0.01 +# Settings for ros_control hardware interface +hardware_interface: + joints: + - panda_joint1 + - panda_joint2 + - panda_joint3 + - panda_joint4 + - panda_joint5 + - panda_joint6 + - panda_joint7 + - panda_finger_joint1 + sim_control_mode: 1 # 0: position, 1: velocity +# Publish all joint states +# Creates the /joint_states topic necessary in ROS +joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 50 +controller_list: + [] +arm_position_controller: + type: position_controllers/JointPositionController + joints: + - panda_joint1 + - panda_joint2 + - panda_joint3 + - panda_joint4 + - panda_joint5 + - panda_joint6 + - panda_joint7 + gains: + panda_joint1: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + panda_joint2: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + panda_joint3: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + panda_joint4: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + panda_joint5: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + panda_joint6: + p: 100 + d: 1 + i: 1 + i_clamp: 1 + panda_joint7: + p: 100 + d: 1 + i: 1 + i_clamp: 1 \ No newline at end of file diff --git a/config/sensors_3d.yaml b/config/sensors_3d.yaml new file mode 100644 index 0000000..d2955dc --- /dev/null +++ b/config/sensors_3d.yaml @@ -0,0 +1,3 @@ +# The name of this file shouldn't be changed, or else the Setup Assistant won't detect it +sensors: + - {} \ No newline at end of file diff --git a/launch/chomp_planning_pipeline.launch.xml b/launch/chomp_planning_pipeline.launch.xml index 0fb092f..a4d5525 100644 --- a/launch/chomp_planning_pipeline.launch.xml +++ b/launch/chomp_planning_pipeline.launch.xml @@ -1,17 +1,20 @@ - + + + - + diff --git a/launch/demo.launch b/launch/demo.launch index 2ebb63b..2417e62 100644 --- a/launch/demo.launch +++ b/launch/demo.launch @@ -1,5 +1,8 @@ + + + @@ -7,33 +10,32 @@ - + + + - - - - - + - - - [/move_group/fake_controller_joint_states] + + [move_group/fake_controller_joint_states] + + + [move_group/fake_controller_joint_states] - @@ -44,12 +46,14 @@ - + + - - + + + diff --git a/launch/demo_gazebo.launch b/launch/demo_gazebo.launch new file mode 100644 index 0000000..9ae0830 --- /dev/null +++ b/launch/demo_gazebo.launch @@ -0,0 +1,70 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + [move_group/fake_controller_joint_states] + [/joint_states] + + + [move_group/fake_controller_joint_states] + [/joint_states] + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/gazebo.launch b/launch/gazebo.launch new file mode 100644 index 0000000..71d37ef --- /dev/null +++ b/launch/gazebo.launch @@ -0,0 +1,22 @@ + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/gdb_settings.gdb b/launch/gdb_settings.gdb new file mode 100644 index 0000000..ad44f4f --- /dev/null +++ b/launch/gdb_settings.gdb @@ -0,0 +1 @@ +b robot_poses_widget.cpp:520 \ No newline at end of file diff --git a/launch/joystick_control.launch b/launch/joystick_control.launch index f741735..9411f6e 100644 --- a/launch/joystick_control.launch +++ b/launch/joystick_control.launch @@ -13,5 +13,5 @@ - + diff --git a/launch/move_group.launch b/launch/move_group.launch index f5f9e7d..44c126e 100644 --- a/launch/move_group.launch +++ b/launch/move_group.launch @@ -1,18 +1,10 @@ - - - - - - - - + value="gdb -x $(find panda_moveit_config)/launch/gdb_settings.gdb --ex run --args" /> @@ -20,12 +12,25 @@ + + + + + + + + + + + + + @@ -52,30 +57,15 @@ + + - - - - - - - diff --git a/launch/moveit_rviz.launch b/launch/moveit_rviz.launch index 016e5dc..bc71677 100644 --- a/launch/moveit_rviz.launch +++ b/launch/moveit_rviz.launch @@ -2,15 +2,15 @@ - + + - - + + + - - + diff --git a/launch/ompl_planning_pipeline.launch.xml b/launch/ompl_planning_pipeline.launch.xml index de9a542..649a253 100644 --- a/launch/ompl_planning_pipeline.launch.xml +++ b/launch/ompl_planning_pipeline.launch.xml @@ -1,18 +1,19 @@ - + - - + + diff --git a/launch/panda_control_moveit_rviz.launch b/launch/panda_control_moveit_rviz.launch index ddfdcc9..bc2e281 100644 --- a/launch/panda_control_moveit_rviz.launch +++ b/launch/panda_control_moveit_rviz.launch @@ -7,7 +7,7 @@ - + @@ -16,5 +16,7 @@ - + + + diff --git a/launch/panda_moveit_controller_manager.launch.xml b/launch/panda_moveit_controller_manager.launch.xml index ea4c0ad..5536e61 100644 --- a/launch/panda_moveit_controller_manager.launch.xml +++ b/launch/panda_moveit_controller_manager.launch.xml @@ -1,7 +1,10 @@ - - + + + - - - + + + + diff --git a/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml b/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml new file mode 100644 index 0000000..ac6272e --- /dev/null +++ b/launch/pilz_industrial_motion_planner_planning_pipeline.launch.xml @@ -0,0 +1,28 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/launch/planning_context.launch b/launch/planning_context.launch index 3485d67..05f9c6e 100644 --- a/launch/planning_context.launch +++ b/launch/planning_context.launch @@ -1,14 +1,13 @@ - - + - + diff --git a/launch/planning_pipeline.launch.xml b/launch/planning_pipeline.launch.xml index ac6838d..64ec360 100644 --- a/launch/planning_pipeline.launch.xml +++ b/launch/planning_pipeline.launch.xml @@ -1,7 +1,7 @@ - + diff --git a/launch/ros_controllers.launch b/launch/ros_controllers.launch new file mode 100644 index 0000000..f272954 --- /dev/null +++ b/launch/ros_controllers.launch @@ -0,0 +1,11 @@ + + + + + + + + + + diff --git a/launch/run_benchmark_ompl.launch b/launch/run_benchmark_ompl.launch index 6bf8ba2..0252481 100644 --- a/launch/run_benchmark_ompl.launch +++ b/launch/run_benchmark_ompl.launch @@ -11,11 +11,10 @@ - + - diff --git a/launch/sensor_manager.launch.xml b/launch/sensor_manager.launch.xml index 67aa2fd..dbbbd95 100644 --- a/launch/sensor_manager.launch.xml +++ b/launch/sensor_manager.launch.xml @@ -2,13 +2,12 @@ - - + + - - + diff --git a/launch/setup_assistant.launch b/launch/setup_assistant.launch index 95d88d7..70ae6ff 100644 --- a/launch/setup_assistant.launch +++ b/launch/setup_assistant.launch @@ -7,7 +7,7 @@ - diff --git a/launch/test.rviz b/launch/test.rviz new file mode 100644 index 0000000..e298d66 --- /dev/null +++ b/launch/test.rviz @@ -0,0 +1,262 @@ +Panels: + - Class: rviz/Displays + Help Height: 84 + Name: Displays + Property Tree Widget: + Expanded: + - /MarkerArray1 + - /PlanningScene1/Scene Robot1 + Splitter Ratio: 0.7425600290298462 + Tree Height: 649 + - 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.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 + - Class: moveit_rviz_plugin/Trajectory + Color Enabled: false + Enabled: true + 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 + panda_link0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link8: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Loop Animation: false + Name: Trajectory + Robot Alpha: 0.5 + Robot Color: 150; 50; 150 + Robot Description: robot_description + 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 + 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.8999999761581421 + Scene Color: 50; 230; 50 + Scene Display Time: 0.20000000298023224 + 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 + panda_link0: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link1: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link2: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link3: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link4: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link5: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link6: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link7: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + panda_link8: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Robot Alpha: 1 + Show Robot Collision: false + Show Robot Visual: true + Value: true + - 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 + 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/XYOrbit + Distance: 2.827594041824341 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Field of View: 0.7853981852531433 + Focal Point: + X: 0.11356700211763382 + Y: 0.10592000186443329 + Z: 2.2351800055275817e-07 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.47020357847213745 + Target Frame: panda_link0 + Yaw: 6.06496000289917 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 995 + Help: + collapsed: false + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000001000000000000017800000389fc020000000efb000000100044006900730070006c006100790073010000003d0000031a000000c900fffffffb0000000800480065006c00700000000342000000bb0000006e00fffffffb0000000a00560069006500770073000000026d000000b5000000a400fffffffb0000000c00430061006d00650072006100000002ff000001610000000000000000fb0000001e004d006f00740069006f006e00200050006c0061006e006e0069006e00670100000374000001890000000000000000fb0000002e004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d00200053006c0069006400650072010000026f000000480000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e00670000000294000002410000000000000000fb00000024005200760069007a00560069007300750061006c0054006f006f006c0073004700750069010000035d000000690000004100fffffffb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000034f000001fb0000000000000000fb000000120020002d00200053006c006900640065007201000002df000000480000000000000000fb0000001c004d006f00740069006f006e0050006c0061006e006e0069006e0067010000032d000001fb0000000000000000fb00000044004d006f00740069006f006e0050006c0061006e006e0069006e00670020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb000000280020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000000000000000fb0000003c005400720061006a006500630074006f007200790020002d0020005400720061006a006500630074006f0072007900200053006c00690064006500720000000000ffffffff0000001600000016000006020000038900000001000000020000000100000002fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + RvizVisualToolsGui: + collapsed: false + Trajectory - Trajectory Slider: + collapsed: false + Views: + collapsed: false + Width: 1920 + X: 1920 + Y: 456 diff --git a/launch/trajectory_execution.launch.xml b/launch/trajectory_execution.launch.xml index b6c03c9..e74530b 100644 --- a/launch/trajectory_execution.launch.xml +++ b/launch/trajectory_execution.launch.xml @@ -1,8 +1,8 @@ - + - + @@ -12,9 +12,9 @@ - + - + diff --git a/launch/warehouse.launch b/launch/warehouse.launch index a4a1374..7f020e1 100644 --- a/launch/warehouse.launch +++ b/launch/warehouse.launch @@ -1,9 +1,9 @@ - + - + diff --git a/launch/warehouse_settings.launch.xml b/launch/warehouse_settings.launch.xml index 772c922..e473b08 100644 --- a/launch/warehouse_settings.launch.xml +++ b/launch/warehouse_settings.launch.xml @@ -1,14 +1,16 @@ - + - - + + - - + + + + diff --git a/package.xml b/package.xml index f7b834d..742e18c 100644 --- a/package.xml +++ b/package.xml @@ -1,11 +1,12 @@ + panda_moveit_config 0.7.5 - An automatically generated package with all the configuration and launch files for using the panda with the MoveIt! Motion Planning Framework + An automatically generated package with all the configuration and launch files for using the panda with the MoveIt Motion Planning Framework - Mike Lautman - Mike Lautman + Tyler Weaver + Tyler Weaver BSD @@ -15,18 +16,22 @@ catkin - franka_description moveit_ros_move_group + moveit_simple_controller_manager moveit_fake_controller_manager moveit_kinematics moveit_planners_ompl moveit_ros_visualization + moveit_visual_tools + moveit_setup_assistant joint_state_publisher joint_state_publisher_gui robot_state_publisher + tf2_ros xacro - topic_tools - + + franka_description + +