diff --git a/config/panda_control.yaml b/config/panda_control.yaml index 053fbdcfa29d5b13c8753587b6ebc6b1e2179a53..94c10f3c68067aaab3a2d7795cf361f54cb8fad3 100644 --- a/config/panda_control.yaml +++ b/config/panda_control.yaml @@ -39,4 +39,4 @@ - panda_joint7 gains: [ 1, 1, 1, 1, 1, 1, 1 - ] \ No newline at end of file + ] diff --git a/launch/simulation.launch b/launch/simulation.launch index a982c720e046458a10b05d96603481485084859e..0639685f9548bfa2b2b848230f5f1cbecc984675 100644 --- a/launch/simulation.launch +++ b/launch/simulation.launch @@ -1,5 +1,4 @@ <launch> - <param name="robot_description" command="$(find xacro)/xacro $(find franka_description)/robots/panda_arm_hand.urdf.xacro" /> <!-- GAZEBO arguments --> <arg name="paused" default="false" /> @@ -9,6 +8,11 @@ <arg name="debug" default="false" /> <arg name="load_gripper" default="true" /> + <!-- don't include franka_control, but include the controller stuff from there --> + <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find franka_description)/robots/panda_arm_hand.urdf.xacro'" if="$(arg load_gripper)" /> + <param name="robot_description" command="$(find xacro)/xacro --inorder '$(find franka_description)/robots/panda_arm.urdf.xacro'" unless="$(arg load_gripper)" /> + <rosparam command="load" file="$(find franka_control)/config/default_controllers.yaml" /> + <!--launch GAZEBO with own world configuration --> <include file="$(find gazebo_ros)/launch/empty_world.launch"> <!-- <arg name="world_name" value="$(find robotics_assisted_tomography)/worlds/tomography.world"/> --> @@ -19,8 +23,6 @@ <arg name="headless" value="$(arg headless)" /> </include> - <node name="rqt_console" pkg="rqt_console" type="rqt_console" /> - <!-- <node name="rqt_logger_level" pkg="rqt_logger_level" type="rqt_logger_level" /> --> <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model panda" /> @@ -28,7 +30,7 @@ <rosparam file="$(find panda_simulation)/config/panda_control.yaml" command="load" /> <!-- load the controllers --> - <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="joint_state_controller panda_arm_controller" /> + <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="joint_state_controller position_joint_trajectory_controller" /> <node if="$(arg load_gripper)" name="controller_spawner_hand" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="panda_hand_controller" /> @@ -57,14 +59,7 @@ <!-- run custom node for automatic intialization --> <node pkg="panda_simulation" type="robot_state_initializer_node" name="robot_state_initializer_node" /> - <!--<node name="move_group_interface_tutorial" pkg="panda_simulation" type="move_group_interface_tutorial" respawn="false" output="screen"/>--> - <node name="constraint_planner" pkg="panda_simulation" type="constraint_planner" respawn="false" output="screen"/> - <!--<node name="motion_planning_api_tutorial" pkg="panda_simulation" type="motion_planning_api_tutorial" respawn="false" output="screen"> - <rosparam command="load" file="$(find panda_moveit_config)/config/kinematics.yaml"/> - <param name="/planning_plugin" value="ompl_interface/OMPLPlanner"/> - <rosparam command="load" file="$(find panda_moveit_config)/config/ompl_planning.yaml"/> - </node>--> </launch> diff --git a/src/robot_state_initializer.cpp b/src/robot_state_initializer.cpp index 1fce35bb27c14181ccb0243fb23c8099311f5e3b..4c6626d324df001a8563645c605871b40f21483e 100644 --- a/src/robot_state_initializer.cpp +++ b/src/robot_state_initializer.cpp @@ -20,7 +20,7 @@ int main(int argc, char **argv) { ros::Duration(2.0).sleep(); // 1. switch from default joint trajectory controller to custom position controller - std::string panda_arm_controller{"panda_arm_controller"}, panda_hand_controller{"panda_hand_controller"}, + std::string panda_arm_controller{"position_joint_trajectory_controller"}, panda_hand_controller{"panda_hand_controller"}, joint_position_controller{"joint_position_controller"}; std::vector<std::string> stop_controllers{panda_arm_controller, panda_hand_controller}; std::vector<std::string> start_controllers{joint_position_controller}; @@ -72,4 +72,4 @@ int main(int argc, char **argv) { ros::spin(); return 0; -} \ No newline at end of file +}