Permalink
Browse files

some changes in controllers

  • Loading branch information...
rguzman1 committed Sep 28, 2016
1 parent 51d0d11 commit 1b838ac08065b26a5afd5940e9bf39907e934476
Showing with 104 additions and 15 deletions.
  1. +84 −9 rb1_control/config/rb1_control.yaml
  2. +20 −6 rb1_control/launch/rb1_control.launch
@@ -36,47 +36,122 @@ rb1:
j1_position_controller:
type: effort_controllers/JointPositionController
joint: mico_joint_1
pid: {p: 500.0, i: 20.0, d: 5.0}
pid: {p: 350.0, i: 20.0, d: 5.0, i_clamp: 5.0}
j2_position_controller:
type: effort_controllers/JointPositionController
joint: mico_joint_2
pid: {p: 700.0, i: 30.0, d: 5.0}
pid: {p: 1500.0, i: 250.0, d: 10.0, i_clamp: 300.0}
j3_position_controller:
type: effort_controllers/JointPositionController
joint: mico_joint_3
pid: {p: 800.0, i: 30, d: 0.0}
pid: {p: 700.0, i: 30, d: 10.0, i_clamp: 30.0}
j4_position_controller:
type: effort_controllers/JointPositionController
joint: mico_joint_4
pid: {p: 200.0, i: 20, d: 5.0}
pid: {p: 500.0, i: 20, d: 10.0, i_clamp: 10.0 }
j5_position_controller:
type: effort_controllers/JointPositionController
joint: mico_joint_5
pid: {p: 200.0, i: 15, d: 0.0}
pid: {p: 500.0, i: 15, d: 10.0, i_clamp: 10.0 }
jhand_position_controller:
type: effort_controllers/JointPositionController
joint: mico_joint_6
pid: {p: 200.0, i: 10, d: 1.0}
pid: {p: 500.0, i: 10, d: 5.0, i_clamp: 5.0 }
jf1_position_controller:
type: effort_controllers/JointPositionController
joint: mico_joint_finger_1
pid: {p: 50.0, i: 0.1, d: 0.1}
pid: {p: 100.0, i: 0.1, d: 10.0}
jf2_position_controller:
type: effort_controllers/JointPositionController
joint: mico_joint_finger_2
pid: {p: 50.0, i: 0.1, d: 0.1}
pid: {p: 100.0, i: 0.1, d: 10.0}
jf3_position_controller:
type: effort_controllers/JointPositionController
joint: mico_joint_finger_3
pid: {p: 50.0, i: 0.1, d: 0.1}
pid: {p: 100.0, i: 0.1, d: 10.0}
mobile_base_controller:
type : "diff_drive_controller/DiffDriveController"
left_wheel : 'joint_left_wheel'
right_wheel : 'joint_right_wheel'
publish_rate: 50.0 # default: 50
pose_covariance_diagonal : [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0]
# Wheel separation and diameter. These are both optional.
# diff_drive_controller will attempt to read either one or both from the
# URDF if not specified as a parameter
wheel_separation : 0.433
wheel_radius : 0.0762
# Wheel separation and radius multipliers
wheel_separation_multiplier: 1.0 # default: 1.0
wheel_radius_multiplier : 1.0 # default: 1.0
# Velocity commands timeout [s], default 0.5
cmd_vel_timeout: 0.25
# Base frame_id
base_frame_id: base_footprint #default: base_link
# Velocity and acceleration limits
# Whenever a min_* is unspecified, default to -max_*
linear:
x:
has_velocity_limits : true
max_velocity : 1.75 # m/s
min_velocity : -1.75 # m/s
has_acceleration_limits: true
max_acceleration : 1.75 # m/s^2
min_acceleration : -1.75 # m/s^2
angular:
z:
has_velocity_limits : true
max_velocity : 3.0 # rad/s
has_acceleration_limits: true
max_acceleration : 3.0 # rad/s^2
mico_arm_controller:
type: "effort_controllers/JointTrajectoryController"
joints:
- mico_joint_1
- mico_joint_2
- mico_joint_3
- mico_joint_4
- mico_joint_5
- mico_joint_6
gains: # Required because we're controlling an effort interface
mico_joint_1: {p: 350, d: 20, i: 5, i_clamp: 5}
mico_joint_2: {p: 800, d: 30, i: 20, i_clamp: 20}
mico_joint_3: {p: 700, d: 30, i: 20, i_clamp: 20}
mico_joint_4: {p: 500, d: 20, i: 10, i_clamp: 10}
mico_joint_5: {p: 500, d: 15, i: 10, i_clamp: 10}
mico_joint_6: {p: 500, d: 10, i: 5, i_clamp: 5}
gripper_controller:
# Unfortunately this controller type works only for grippers with a single joint :(
#type: "effort_controllers/GripperActionController"
type: "effort_controllers/JointTrajectoryController"
joints:
- mico_joint_finger_1
- mico_joint_finger_2
- mico_joint_finger_3
gains:
mico_joint_finger_1: {p: 500.0, d: 20, i: 10, i_clamp: 10.0}
mico_joint_finger_2: {p: 500.0, d: 20, i: 10, i_clamp: 10.0}
mico_joint_finger_3: {p: 500.0, d: 20, i: 10, i_clamp: 10.0}
@@ -8,11 +8,10 @@
<node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
output="screen" ns="/rb1" args="--shutdown-timeout 1 --namespace=/rb1
joint_read_state_controller
joint_left_velocity_controller
joint_right_velocity_controller
mobile_base_controller
j1_torso_controller
j1_head_controller
j2_head_controller
j2_head_controller
j1_position_controller
j2_position_controller
j3_position_controller
@@ -21,9 +20,24 @@
jhand_position_controller
jf1_position_controller
jf2_position_controller
jf3_position_controller
"/>
jf3_position_controller"/>
<!--
mico_arm_controller
gripper_controller
-->
<!--
joint_left_velocity_controller
joint_right_velocity_controller
-->
<node pkg="twist_mux" type="twist_mux" name="twist_mux">
<rosparam command="load" file="$(find rb1_base_control)/config/twist_mux.yaml" />
<remap from="cmd_vel_out" to="/rb1/cmd_vel"/>
</node>
<!-- once the joint_state is published, run the robot_state_publisher, the namespace has to be adapted -->
<!-- convert joint states to TF transforms for rviz, etc -->
<node name="robot_state_publisher_base" pkg="robot_state_publisher" type="robot_state_publisher"

0 comments on commit 1b838ac

Please sign in to comment.