diff --git a/nextage_moveit_config/config/NextageOpen.srdf b/nextage_moveit_config/config/NextageOpen.srdf index fee3cda4..2c1b82d5 100644 --- a/nextage_moveit_config/config/NextageOpen.srdf +++ b/nextage_moveit_config/config/NextageOpen.srdf @@ -46,9 +46,18 @@ - - - + + + + + + + + + + + + diff --git a/nextage_moveit_config/config/controllers.yaml b/nextage_moveit_config/config/controllers.yaml index 2b35b732..6c222da6 100644 --- a/nextage_moveit_config/config/controllers.yaml +++ b/nextage_moveit_config/config/controllers.yaml @@ -20,6 +20,22 @@ controller_list: - LARM_JOINT3 - LARM_JOINT4 - LARM_JOINT5 + - name: botharms_controller + action_ns: follow_joint_trajectory_action + default: true + joints: + - LARM_JOINT0 + - LARM_JOINT1 + - LARM_JOINT2 + - LARM_JOINT3 + - LARM_JOINT4 + - LARM_JOINT5 + - RARM_JOINT0 + - RARM_JOINT1 + - RARM_JOINT2 + - RARM_JOINT3 + - RARM_JOINT4 + - RARM_JOINT5 - name: head_controller action_ns: follow_joint_trajectory_action joints: diff --git a/nextage_moveit_config/config/kinematics_ikfast.yaml b/nextage_moveit_config/config/kinematics_ikfast.yaml index bf5bd0b9..91e6ad69 100644 --- a/nextage_moveit_config/config/kinematics_ikfast.yaml +++ b/nextage_moveit_config/config/kinematics_ikfast.yaml @@ -8,6 +8,11 @@ left_arm: kinematics_solver_search_resolution: 0.005 kinematics_solver_timeout: 0.005 kinematics_solver_attempts: 3 +botharms: + kinematics_solver: nextage_right_arm_kinematics/IKFastKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 + kinematics_solver_attempts: 3 head: kinematics_solver: nextage_right_arm_kinematics/IKFastKinematicsPlugin kinematics_solver_search_resolution: 0.005 diff --git a/nextage_moveit_config/config/kinematics_kdl.yaml b/nextage_moveit_config/config/kinematics_kdl.yaml index 57a6e9b2..4b2a1081 100644 --- a/nextage_moveit_config/config/kinematics_kdl.yaml +++ b/nextage_moveit_config/config/kinematics_kdl.yaml @@ -8,6 +8,11 @@ left_arm: kinematics_solver_search_resolution: 0.005 kinematics_solver_timeout: 0.005 kinematics_solver_attempts: 3 +botharms: + kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin + kinematics_solver_search_resolution: 0.005 + kinematics_solver_timeout: 0.005 + kinematics_solver_attempts: 3 head: kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin kinematics_solver_search_resolution: 0.005 diff --git a/nextage_moveit_config/config/ompl_planning.yaml b/nextage_moveit_config/config/ompl_planning.yaml index 1c7d6eb9..f3f22316 100644 --- a/nextage_moveit_config/config/ompl_planning.yaml +++ b/nextage_moveit_config/config/ompl_planning.yaml @@ -64,7 +64,7 @@ botharms: - TRRTkConfigDefault - PRMkConfigDefault - PRMstarkConfigDefault - projection_evaluator: joints(LARM_JOINT0,LARM_JOINT1) + projection_evaluator: joints(LARM_JOINT0,RARM_JOINT0) longest_valid_segment_fraction: 0.05 head: planner_configs: