Skip to content

Commit

Permalink
Merge pull request #17 from PickNikRobotics/update-servo-and-planning…
Browse files Browse the repository at this point in the history
…-params

Update servo and planning params
  • Loading branch information
dyackzan authored Jan 5, 2024
2 parents 1cc3be4 + 926c218 commit ba60001
Show file tree
Hide file tree
Showing 2 changed files with 11 additions and 3 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -40,7 +40,7 @@ incoming_command_timeout: 0.1 # Stop servoing if X seconds elapse without a new
## Configure handling of singularities and joint limits
lower_singularity_threshold: 30.0 # Start decelerating when the condition number hits this (close to singularity)
hard_stop_singularity_threshold: 50.0 # Stop when the condition number hits this
joint_limit_margin: 0.1 # added as a buffer to joint limits [radians]. If moving quickly, make this larger.
joint_limit_margins: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] # added as a buffer to joint limits [radians]. If moving quickly, make this larger.

## Topic names
cartesian_command_in_topic: ~/delta_twist_cmds # Topic for incoming Cartesian twist commands
Expand Down
12 changes: 10 additions & 2 deletions src/kinova_gen3_base_config/config/moveit/ompl_planning.yaml
Original file line number Diff line number Diff line change
@@ -1,6 +1,6 @@
planning_plugins:
- ompl_interface/OMPLPlanner
# The order of the elements in the adapter corresponds to the order they are processed by the motion planning pipeline
# The order of the elements in the adapter corresponds to the order they are processed by the motion planning pipeline.
request_adapters:
- default_planning_request_adapters/ResolveConstraintFrames
- default_planning_request_adapters/ValidateWorkspaceBounds
Expand All @@ -12,6 +12,13 @@ response_adapters:
- default_planning_response_adapters/DisplayMotionPath

planner_configs:
APSConfigDefault:
type: geometric::AnytimePathShortening
shortcut: 1 # Attempt to shortcut all new solution paths
hybridize: 1 # Compute hybrid solution trajectories
max_hybrid_paths: 32 # Number of hybrid paths generated per iteration
num_planners: 16 # The number of default planners to use for planning
planners: "RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect,RRTConnect" # 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]"
SBLkConfigDefault:
type: geometric::SBL
range: 0.0 # Max motion added to tree. ==> maxDistance_ default: 0.0, if 0.0, set on setup()
Expand Down Expand Up @@ -68,6 +75,7 @@ planner_configs:
manipulator:
default_planner_config: RRTConnectkConfigDefault
planner_configs:
- APSConfigDefault
- SBLkConfigDefault
- ESTkConfigDefault
- LBKPIECEkConfigDefault
Expand All @@ -80,7 +88,7 @@ manipulator:
- PRMkConfigDefault
- PRMstarkConfigDefault
enforce_constrained_state_space: true
#Note: commenting the following line lets moveit chose RRTConnect as default planner rather than LBKPIECE
##Note: commenting the following line lets moveit choose RRTConnect as default planner rather than LBKPIECE
projection_evaluator: joints(joint_1,joint_2)
longest_valid_segment_fraction: 0.01
gripper:
Expand Down

0 comments on commit ba60001

Please sign in to comment.