Skip to content
Snippets Groups Projects
Commit 25208979 authored by ipa-fxm's avatar ipa-fxm
Browse files

updates for latest gazebo under hydro

parent 757b3995
Branches
Tags
No related merge requests found
......@@ -2,11 +2,12 @@
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<gazebo>
<plugin name="gazebo_ros_controller_manager" filename="libgazebo_ros_controller_manager.so">
<alwaysOn>true</alwaysOn>
<updateRate>1000.0</updateRate>
<plugin name="ros_control" filename="libgazebo_ros_control.so">
<!--robotNamespace>/</robotNamespace-->
<!--robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType-->
</plugin>
<!--
<plugin name="gazebo_ros_power_monitor_controller" filename="libgazebo_ros_power_monitor.so">
<alwaysOn>true</alwaysOn>
<updateRate>1.0</updateRate>
......@@ -19,6 +20,7 @@
<dischargeVoltage>15.52</dischargeVoltage>
<chargeVoltage>16.41</chargeVoltage>
</plugin>
-->
</gazebo>
</robot>
arm_controller:
type: robot_mechanism_controllers/JointTrajectoryActionController
type: effort_controllers/JointTrajectoryController
topic: "test"
joints:
- shoulder_pan_joint
- shoulder_lift_joint
- elbow_joint
- wrist_1_joint
- wrist_2_joint
- wrist_3_joint
- shoulder_pan_joint
- shoulder_lift_joint
- elbow_joint
- wrist_1_joint
- wrist_2_joint
- wrist_3_joint
gains:
shoulder_pan_joint:
p: 2000.0
d: 0.0
shoulder_lift_joint:
p: 2000.0
d: 0.0
elbow_joint:
p: 2000.0
d: 0.0
wrist_1_joint:
p: 2020.0
d: 0.0
wrist_2_joint:
p: 2030.0
d: 0.0
wrist_3_joint:
p: 2050.0
d: 0.0
arm_joint_trajectory_action_node:
joints:
- shoulder_pan_joint
- shoulder_lift_joint
- elbow_joint
- wrist_1_joint
- wrist_2_joint
- wrist_3_joint
constraints:
shoulder_pan_joint: {p: 1000.0, i: 0.0, d: 0.0, i_clamp: 0.0, i_clamp_min: 0.0, i_clamp_max: 0.0}
shoulder_lift_joint: {p: 1000.0, i: 0.0, d: 0.0, i_clamp: 0.0, i_clamp_min: 0.0, i_clamp_max: 0.0}
elbow_joint: {p: 1000.0, i: 0.0, d: 0.0, i_clamp: 0.0, i_clamp_min: 0.0, i_clamp_max: 0.0}
wrist_1_joint: {p: 1000.0, i: 0.0, d: 0.0, i_clamp: 0.0, i_clamp_min: 0.0, i_clamp_max: 0.0}
wrist_2_joint: {p: 1000.0, i: 0.0, d: 0.0, i_clamp: 0.0, i_clamp_min: 0.0, i_clamp_max: 0.0}
wrist_3_joint: {p: 1000.0, i: 0.0, d: 0.0, i_clamp: 0.0, i_clamp_min: 0.0, i_clamp_max: 0.0}
constraints:
goal_time: 0.6
shoulder_pan_joint:
goal: 0.05
shoulder_lift_joint:
goal: 0.05
elbow_joint:
goal: 0.05
wrist_1_joint:
goal: 0.05
wrist_2_joint:
goal: 0.05
wrist_3_joint:
goal: 0.05
stopped_velocity_tolerance: 0.5
shoulder_pan_joint: {trajectory: 0.1, goal: 0.1}
shoulder_lift_joint: {trajectory: 0.1, goal: 0.1}
elbow_joint: {trajectory: 0.1, goal: 0.1}
wrist_1_joint: {trajectory: 0.1, goal: 0.1}
wrist_2_joint: {trajectory: 0.1, goal: 0.1}
wrist_3_joint: {trajectory: 0.1, goal: 0.1}
stop_trajectory_duration: 0.5
state_publish_rate: 25
action_monitor_rate: 10
arm_controller:
type: robot_mechanism_controllers/JointTrajectoryActionController
type: effort_controllers/JointTrajectoryController
topic: "test"
joints:
- shoulder_pan_joint
- shoulder_lift_joint
- elbow_joint
- wrist_1_joint
- wrist_2_joint
- wrist_3_joint
- shoulder_pan_joint
- shoulder_lift_joint
- elbow_joint
- wrist_1_joint
- wrist_2_joint
- wrist_3_joint
gains:
shoulder_pan_joint:
p: 2000.0
d: 0.0
shoulder_lift_joint:
p: 2000.0
d: 0.0
elbow_joint:
p: 2000.0
d: 0.0
wrist_1_joint:
p: 2020.0
d: 0.0
wrist_2_joint:
p: 2030.0
d: 0.0
wrist_3_joint:
p: 2050.0
d: 0.0
arm_joint_trajectory_action_node:
joints:
- shoulder_pan_joint
- shoulder_lift_joint
- elbow_joint
- wrist_1_joint
- wrist_2_joint
- wrist_3_joint
constraints:
shoulder_pan_joint: {p: 1000.0, i: 0.0, d: 0.0, i_clamp: 0.0, i_clamp_min: 0.0, i_clamp_max: 0.0}
shoulder_lift_joint: {p: 1000.0, i: 0.0, d: 0.0, i_clamp: 0.0, i_clamp_min: 0.0, i_clamp_max: 0.0}
elbow_joint: {p: 1000.0, i: 0.0, d: 0.0, i_clamp: 0.0, i_clamp_min: 0.0, i_clamp_max: 0.0}
wrist_1_joint: {p: 1000.0, i: 0.0, d: 0.0, i_clamp: 0.0, i_clamp_min: 0.0, i_clamp_max: 0.0}
wrist_2_joint: {p: 1000.0, i: 0.0, d: 0.0, i_clamp: 0.0, i_clamp_min: 0.0, i_clamp_max: 0.0}
wrist_3_joint: {p: 1000.0, i: 0.0, d: 0.0, i_clamp: 0.0, i_clamp_min: 0.0, i_clamp_max: 0.0}
constraints:
goal_time: 0.6
shoulder_pan_joint:
goal: 0.05
shoulder_lift_joint:
goal: 0.05
elbow_joint:
goal: 0.05
wrist_1_joint:
goal: 0.05
wrist_2_joint:
goal: 0.05
wrist_3_joint:
goal: 0.05
stopped_velocity_tolerance: 0.5
shoulder_pan_joint: {trajectory: 0.1, goal: 0.1}
shoulder_lift_joint: {trajectory: 0.1, goal: 0.1}
elbow_joint: {trajectory: 0.1, goal: 0.1}
wrist_1_joint: {trajectory: 0.1, goal: 0.1}
wrist_2_joint: {trajectory: 0.1, goal: 0.1}
wrist_3_joint: {trajectory: 0.1, goal: 0.1}
stop_trajectory_duration: 0.5
state_publish_rate: 25
action_monitor_rate: 10
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 50
<?xml version="1.0"?>
<launch>
<!-- Robot state publisher -->
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
<param name="publish_frequency" type="double" value="50.0" />
<param name="tf_prefix" type="string" value="" />
</node>
<!-- Diagnostics -->
<node pkg="pr2_mechanism_diagnostics" type="pr2_mechanism_diagnostics" name="pr2_mechanism_diagnostics" />
<!-- Fake Calibration -->
<node pkg="rostopic" type="rostopic" name="fake_joint_calibration"
args="pub /calibrated std_msgs/Bool true" />
<!-- joint_state_controller -->
<rosparam file="$(find ur_gazebo)/controller/joint_state_controller.yaml" command="load"/>
<node name="joint_state_controller_spawner" pkg="controller_manager" type="spawner" args="joint_state_controller" />
</launch>
......@@ -2,26 +2,19 @@
<launch>
<!-- startup simulated world -->
<include file="$(find gazebo_worlds)/launch/empty_world.launch" />
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" default="worlds/empty.world"/>
</include>
<!-- send frida urdf to param server -->
<param name="robot_description" command="$(find xacro)/xacro.py '$(find ur_description)/urdf/ur10_robot.urdf.xacro'" />
<!-- push robot_description to factory and spawn robot in gazebo -->
<node name="spawn_gazebo_model" pkg="gazebo" type="spawn_model" args="-urdf -param robot_description -model robot -z 0.1" respawn="false" output="screen" />
<node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model robot -z 0.1" respawn="false" output="screen" />
<!-- Controller Manager -->
<include file="$(find pr2_controller_manager)/controller_manager.launch" />
<!-- Fake Calibration -->
<node pkg="rostopic" type="rostopic" name="fake_joint_calibration" args="pub /calibrated std_msgs/Bool true" />
<include file="$(find ur_gazebo)/launch/controller_utils.launch"/>
<rosparam file="$(find ur_gazebo)/controller/arm_controller_ur10.yaml" command="load"/>
<node name="arm_controller_spawner" pkg="pr2_controller_manager" type="spawner" args="arm_controller" />
<group ns="arm_controller">
<node name="arm_joint_trajectory_action_node" pkg="joint_trajectory_action" type="joint_trajectory_action" />
</group>
<node name="arm_controller_spawner" pkg="controller_manager" type="spawner" args="arm_controller" />
</launch>
......@@ -2,26 +2,19 @@
<launch>
<!-- startup simulated world -->
<include file="$(find gazebo_worlds)/launch/empty_world.launch" />
<include file="$(find gazebo_ros)/launch/empty_world.launch">
<arg name="world_name" default="worlds/empty.world"/>
</include>
<!-- send frida urdf to param server -->
<param name="robot_description" command="$(find xacro)/xacro.py '$(find ur_description)/urdf/ur5_robot.urdf.xacro'" />
<!-- push robot_description to factory and spawn robot in gazebo -->
<node name="spawn_gazebo_model" pkg="gazebo" type="spawn_model" args="-urdf -param robot_description -model robot -z 0.1" respawn="false" output="screen" />
<node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model" args="-urdf -param robot_description -model robot -z 0.1" respawn="false" output="screen" />
<!-- Controller Manager -->
<include file="$(find pr2_controller_manager)/controller_manager.launch" />
<!-- Fake Calibration -->
<node pkg="rostopic" type="rostopic" name="fake_joint_calibration" args="pub /calibrated std_msgs/Bool true" />
<include file="$(find ur_gazebo)/launch/controller_utils.launch"/>
<rosparam file="$(find ur_gazebo)/controller/arm_controller_ur5.yaml" command="load"/>
<node name="arm_controller_spawner" pkg="pr2_controller_manager" type="spawner" args="arm_controller" />
<group ns="arm_controller">
<node name="arm_joint_trajectory_action_node" pkg="joint_trajectory_action" type="joint_trajectory_action" />
</group>
<node name="arm_controller_spawner" pkg="controller_manager" type="spawner" args="arm_controller" />
</launch>
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment