From 327427873ac361b48e5a80b4c7c560e316c1ba90 Mon Sep 17 00:00:00 2001 From: TescaF Date: Fri, 26 May 2017 18:02:35 -0400 Subject: [PATCH 1/9] init commit for 7dof jaco --- .../controller/arm_controller_j2s7s300.yaml | 178 ++++++++++++++++++ .../gripper_controller_robotiq_j2s7s300.yaml | 11 ++ hlpr_gazebo/launch/kinova_control.launch | 46 +++++ hlpr_gazebo/launch/vector.launch | 21 ++- hlpr_gazebo/launch/vector_controllers.launch | 67 ++++--- 5 files changed, 293 insertions(+), 30 deletions(-) create mode 100644 hlpr_gazebo/controller/arm_controller_j2s7s300.yaml create mode 100644 hlpr_gazebo/controller/gripper_controller_robotiq_j2s7s300.yaml create mode 100644 hlpr_gazebo/launch/kinova_control.launch diff --git a/hlpr_gazebo/controller/arm_controller_j2s7s300.yaml b/hlpr_gazebo/controller/arm_controller_j2s7s300.yaml new file mode 100644 index 0000000..814e5e4 --- /dev/null +++ b/hlpr_gazebo/controller/arm_controller_j2s7s300.yaml @@ -0,0 +1,178 @@ +j2s7s300: + effort_finger_trajectory_controller: + constraints: + goal_time: 1.0 + j2s7s300_joint_finger_1: + goal: 0.02 + trajectory: 0.05 + j2s7s300_joint_finger_2: + goal: 0.02 + trajectory: 0.05 + j2s7s300_joint_finger_3: + goal: 0.02 + trajectory: 0.05 + stopped_velocity_tolerance: 0.02 + gains: + j2s7s300_joint_finger_1: + d: 0 + i: 0 + i_clamp: 1 + p: 10 + j2s7s300_joint_finger_2: + d: 0 + i: 0 + i_clamp: 1 + p: 10 + j2s7s300_joint_finger_3: + d: 0 + i: 0 + i_clamp: 1 + p: 10 + joints: + - j2s7s300_joint_finger_1 + - j2s7s300_joint_finger_2 + - j2s7s300_joint_finger_3 + type: effort_controllers/JointTrajectoryController + effort_joint_trajectory_controller: + constraints: + goal_time: 1.0 + j2s7s300_joint_1: + goal: 0.02 + trajectory: 0.05 + j2s7s300_joint_2: + goal: 0.02 + trajectory: 0.05 + j2s7s300_joint_3: + goal: 0.02 + trajectory: 0.05 + j2s7s300_joint_4: + goal: 0.02 + trajectory: 0.05 + j2s7s300_joint_5: + goal: 0.02 + trajectory: 0.05 + j2s7s300_joint_6: + goal: 0.02 + trajectory: 0.05 + j2s7s300_joint_7: + goal: 0.02 + trajectory: 0.05 + stopped_velocity_tolerance: 0.02 + gains: + j2s7s300_joint_1: + d: 0 + i: 0 + i_clamp: 10 + p: 5000 + j2s7s300_joint_2: + d: 0 + i: 0 + i_clamp: 10 + p: 5000 + j2s7s300_joint_3: + d: 0 + i: 0 + i_clamp: 10 + p: 5000 + j2s7s300_joint_4: + d: 0 + i: 0 + i_clamp: 10 + p: 500 + j2s7s300_joint_5: + d: 0 + i: 0 + i_clamp: 10 + p: 200 + j2s7s300_joint_6: + d: 0 + i: 0 + i_clamp: 10 + p: 500 + j2s7s300_joint_7: + d: 0 + i: 0 + i_clamp: 10 + p: 500 + joints: + - j2s7s300_joint_1 + - j2s7s300_joint_2 + - j2s7s300_joint_3 + - j2s7s300_joint_4 + - j2s7s300_joint_5 + - j2s7s300_joint_6 + - j2s7s300_joint_7 + type: effort_controllers/JointTrajectoryController + finger_1_position_controller: + joint: j2s7s300_joint_finger_1 + pid: + d: 0 + i: 0 + p: 10 + type: effort_controllers/JointPositionController + finger_2_position_controller: + joint: j2s7s300_joint_finger_2 + pid: + d: 0 + i: 0 + p: 10 + type: effort_controllers/JointPositionController + finger_3_position_controller: + joint: j2s7s300_joint_finger_3 + pid: + d: 0 + i: 0 + p: 10 + type: effort_controllers/JointPositionController + joint_1_position_controller: + joint: j2s7s300_joint_1 + pid: + d: 0 + i: 0 + p: 5000 + type: effort_controllers/JointPositionController + joint_2_position_controller: + joint: j2s7s300_joint_2 + pid: + d: 0 + i: 0 + p: 5000 + type: effort_controllers/JointPositionController + joint_3_position_controller: + joint: j2s7s300_joint_3 + pid: + d: 0 + i: 0 + p: 5000 + type: effort_controllers/JointPositionController + joint_4_position_controller: + joint: j2s7s300_joint_4 + pid: + d: 0 + i: 0 + p: 500 + type: effort_controllers/JointPositionController + joint_5_position_controller: + joint: j2s7s300_joint_5 + pid: + d: 0 + i: 0 + p: 200 + type: effort_controllers/JointPositionController + joint_6_position_controller: + joint: j2s7s300_joint_6 + pid: + d: 0 + i: 0 + p: 500 + type: effort_controllers/JointPositionController + joint_7_position_controller: + joint: j2s7s300_joint_7 + pid: + d: 0 + i: 0 + p: 500 + type: effort_controllers/JointPositionController + joint_state_controller: + publish_rate: 50 + type: joint_state_controller/JointStateController diff --git a/hlpr_gazebo/controller/gripper_controller_robotiq_j2s7s300.yaml b/hlpr_gazebo/controller/gripper_controller_robotiq_j2s7s300.yaml new file mode 100644 index 0000000..e1fcb83 --- /dev/null +++ b/hlpr_gazebo/controller/gripper_controller_robotiq_j2s7s300.yaml @@ -0,0 +1,11 @@ +gripper_controller: + type: position_controllers/JointTrajectoryController + joints: + - j2s7s300_robotiq_85_left_knuckle_joint + constraints: + goal_time: 0.6 + stopped_velocity_tolerance: 0.05 + j2s7s300_robotiq_85_left_knuckle_joint: {trajectory: 0.1, goal: 0.1} + stop_trajectory_duration: 0.5 + state_publish_rate: 25 + action_monitor_rate: 10 diff --git a/hlpr_gazebo/launch/kinova_control.launch b/hlpr_gazebo/launch/kinova_control.launch new file mode 100644 index 0000000..cdb0f59 --- /dev/null +++ b/hlpr_gazebo/launch/kinova_control.launch @@ -0,0 +1,46 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/hlpr_gazebo/launch/vector.launch b/hlpr_gazebo/launch/vector.launch index 59177db..fb62ea7 100644 --- a/hlpr_gazebo/launch/vector.launch +++ b/hlpr_gazebo/launch/vector.launch @@ -1,7 +1,7 @@ - + @@ -13,13 +13,14 @@ - + + @@ -28,17 +29,24 @@ - - - + + + + + + + + + + @@ -48,6 +56,9 @@ + + + diff --git a/hlpr_gazebo/launch/vector_controllers.launch b/hlpr_gazebo/launch/vector_controllers.launch index 5dbc083..ba90706 100644 --- a/hlpr_gazebo/launch/vector_controllers.launch +++ b/hlpr_gazebo/launch/vector_controllers.launch @@ -4,9 +4,10 @@ - - - + + + + @@ -26,35 +27,51 @@ + + + + - - - + + - - + + + + + - - - - - + + + + - - - - + + + - - - + + + + + + + + - - + + + + + + + + @@ -67,6 +84,6 @@ + args="linear_actuator_controller 0.65"/> From 9e1c84f5805cb2bdc421701305475966ba60ca83 Mon Sep 17 00:00:00 2001 From: TescaF Date: Wed, 31 May 2017 20:16:49 -0400 Subject: [PATCH 2/9] changed effort controller to position controller --- .../controller/arm_controller_j2s7s300.yaml | 195 ++---------------- hlpr_gazebo/launch/vector.launch | 15 +- hlpr_gazebo/launch/vector_controllers.launch | 4 +- 3 files changed, 26 insertions(+), 188 deletions(-) diff --git a/hlpr_gazebo/controller/arm_controller_j2s7s300.yaml b/hlpr_gazebo/controller/arm_controller_j2s7s300.yaml index 814e5e4..e7ef4c1 100644 --- a/hlpr_gazebo/controller/arm_controller_j2s7s300.yaml +++ b/hlpr_gazebo/controller/arm_controller_j2s7s300.yaml @@ -1,178 +1,23 @@ j2s7s300: - effort_finger_trajectory_controller: - constraints: - goal_time: 1.0 - j2s7s300_joint_finger_1: - goal: 0.02 - trajectory: 0.05 - j2s7s300_joint_finger_2: - goal: 0.02 - trajectory: 0.05 - j2s7s300_joint_finger_3: - goal: 0.02 - trajectory: 0.05 - stopped_velocity_tolerance: 0.02 - gains: - j2s7s300_joint_finger_1: - d: 0 - i: 0 - i_clamp: 1 - p: 10 - j2s7s300_joint_finger_2: - d: 0 - i: 0 - i_clamp: 1 - p: 10 - j2s7s300_joint_finger_3: - d: 0 - i: 0 - i_clamp: 1 - p: 10 + type: position_controllers/JointTrajectoryController joints: - - j2s7s300_joint_finger_1 - - j2s7s300_joint_finger_2 - - j2s7s300_joint_finger_3 - type: effort_controllers/JointTrajectoryController - effort_joint_trajectory_controller: + - j2s7s300_joint_1 + - j2s7s300_joint_2 + - j2s7s300_joint_3 + - j2s7s300_joint_4 + - j2s7s300_joint_5 + - j2s7s300_joint_6 + - j2s7s300_joint_7 constraints: - goal_time: 1.0 - j2s7s300_joint_1: - goal: 0.02 - trajectory: 0.05 - j2s7s300_joint_2: - goal: 0.02 - trajectory: 0.05 - j2s7s300_joint_3: - goal: 0.02 - trajectory: 0.05 - j2s7s300_joint_4: - goal: 0.02 - trajectory: 0.05 - j2s7s300_joint_5: - goal: 0.02 - trajectory: 0.05 - j2s7s300_joint_6: - goal: 0.02 - trajectory: 0.05 - j2s7s300_joint_7: - goal: 0.02 - trajectory: 0.05 - stopped_velocity_tolerance: 0.02 - gains: - j2s7s300_joint_1: - d: 0 - i: 0 - i_clamp: 10 - p: 5000 - j2s7s300_joint_2: - d: 0 - i: 0 - i_clamp: 10 - p: 5000 - j2s7s300_joint_3: - d: 0 - i: 0 - i_clamp: 10 - p: 5000 - j2s7s300_joint_4: - d: 0 - i: 0 - i_clamp: 10 - p: 500 - j2s7s300_joint_5: - d: 0 - i: 0 - i_clamp: 10 - p: 200 - j2s7s300_joint_6: - d: 0 - i: 0 - i_clamp: 10 - p: 500 - j2s7s300_joint_7: - d: 0 - i: 0 - i_clamp: 10 - p: 500 - joints: - - j2s7s300_joint_1 - - j2s7s300_joint_2 - - j2s7s300_joint_3 - - j2s7s300_joint_4 - - j2s7s300_joint_5 - - j2s7s300_joint_6 - - j2s7s300_joint_7 - type: effort_controllers/JointTrajectoryController - finger_1_position_controller: - joint: j2s7s300_joint_finger_1 - pid: - d: 0 - i: 0 - p: 10 - type: effort_controllers/JointPositionController - finger_2_position_controller: - joint: j2s7s300_joint_finger_2 - pid: - d: 0 - i: 0 - p: 10 - type: effort_controllers/JointPositionController - finger_3_position_controller: - joint: j2s7s300_joint_finger_3 - pid: - d: 0 - i: 0 - p: 10 - type: effort_controllers/JointPositionController - joint_1_position_controller: - joint: j2s7s300_joint_1 - pid: - d: 0 - i: 0 - p: 5000 - type: effort_controllers/JointPositionController - joint_2_position_controller: - joint: j2s7s300_joint_2 - pid: - d: 0 - i: 0 - p: 5000 - type: effort_controllers/JointPositionController - joint_3_position_controller: - joint: j2s7s300_joint_3 - pid: - d: 0 - i: 0 - p: 5000 - type: effort_controllers/JointPositionController - joint_4_position_controller: - joint: j2s7s300_joint_4 - pid: - d: 0 - i: 0 - p: 500 - type: effort_controllers/JointPositionController - joint_5_position_controller: - joint: j2s7s300_joint_5 - pid: - d: 0 - i: 0 - p: 200 - type: effort_controllers/JointPositionController - joint_6_position_controller: - joint: j2s7s300_joint_6 - pid: - d: 0 - i: 0 - p: 500 - type: effort_controllers/JointPositionController - joint_7_position_controller: - joint: j2s7s300_joint_7 - pid: - d: 0 - i: 0 - p: 500 - type: effort_controllers/JointPositionController - joint_state_controller: - publish_rate: 50 - type: joint_state_controller/JointStateController + goal_time: 1.0 + stopped_velocity_tolerance: 0.02 + j2s7s300_joint_1: {trajectory: 0.05, goal: 0.02} + j2s7s300_joint_2: {trajectory: 0.05, goal: 0.02} + j2s7s300_joint_3: {trajectory: 0.05, goal: 0.02} + j2s7s300_joint_4: {trajectory: 0.05, goal: 0.02} + j2s7s300_joint_5: {trajectory: 0.05, goal: 0.02} + j2s7s300_joint_6: {trajectory: 0.05, goal: 0.02} + j2s7s300_joint_7: {trajectory: 0.05, goal: 0.02} + stop_trajectory_duration: 0.5 + state_publish_rate: 50 + action_monitor_rate: 10 diff --git a/hlpr_gazebo/launch/vector.launch b/hlpr_gazebo/launch/vector.launch index fb62ea7..c80ec91 100644 --- a/hlpr_gazebo/launch/vector.launch +++ b/hlpr_gazebo/launch/vector.launch @@ -1,7 +1,7 @@ - + @@ -27,7 +27,9 @@ - + + + @@ -38,15 +40,6 @@ - - - - - - - - - diff --git a/hlpr_gazebo/launch/vector_controllers.launch b/hlpr_gazebo/launch/vector_controllers.launch index ba90706..f5f2a64 100644 --- a/hlpr_gazebo/launch/vector_controllers.launch +++ b/hlpr_gazebo/launch/vector_controllers.launch @@ -60,11 +60,11 @@ - + + args="j2s7s300"> From a0e27d72a2d8192d06fc36d6d1f2e453104e3aa8 Mon Sep 17 00:00:00 2001 From: TescaF Date: Thu, 1 Jun 2017 17:11:22 -0400 Subject: [PATCH 3/9] correct command topic for 7dof arm --- .../controller/arm_controller_j2s7s300.yaml | 20 +++++++++---------- hlpr_gazebo/launch/vector_controllers.launch | 3 ++- hlpr_gazebo/src/vector_control_interface.py | 6 +++++- 3 files changed, 17 insertions(+), 12 deletions(-) diff --git a/hlpr_gazebo/controller/arm_controller_j2s7s300.yaml b/hlpr_gazebo/controller/arm_controller_j2s7s300.yaml index e7ef4c1..28f2074 100644 --- a/hlpr_gazebo/controller/arm_controller_j2s7s300.yaml +++ b/hlpr_gazebo/controller/arm_controller_j2s7s300.yaml @@ -9,15 +9,15 @@ j2s7s300: - j2s7s300_joint_6 - j2s7s300_joint_7 constraints: - goal_time: 1.0 - stopped_velocity_tolerance: 0.02 - j2s7s300_joint_1: {trajectory: 0.05, goal: 0.02} - j2s7s300_joint_2: {trajectory: 0.05, goal: 0.02} - j2s7s300_joint_3: {trajectory: 0.05, goal: 0.02} - j2s7s300_joint_4: {trajectory: 0.05, goal: 0.02} - j2s7s300_joint_5: {trajectory: 0.05, goal: 0.02} - j2s7s300_joint_6: {trajectory: 0.05, goal: 0.02} - j2s7s300_joint_7: {trajectory: 0.05, goal: 0.02} + goal_time: 0.6 + stopped_velocity_tolerance: 0.05 + j2s7s300_joint_1: {trajectory: 0.1, goal: 0.1} + j2s7s300_joint_2: {trajectory: 0.1, goal: 0.1} + j2s7s300_joint_3: {trajectory: 0.1, goal: 0.1} + j2s7s300_joint_4: {trajectory: 0.1, goal: 0.1} + j2s7s300_joint_5: {trajectory: 0.1, goal: 0.1} + j2s7s300_joint_6: {trajectory: 0.1, goal: 0.1} + j2s7s300_joint_7: {trajectory: 0.1, goal: 0.1} stop_trajectory_duration: 0.5 - state_publish_rate: 50 + state_publish_rate: 25 action_monitor_rate: 10 diff --git a/hlpr_gazebo/launch/vector_controllers.launch b/hlpr_gazebo/launch/vector_controllers.launch index f5f2a64..8034629 100644 --- a/hlpr_gazebo/launch/vector_controllers.launch +++ b/hlpr_gazebo/launch/vector_controllers.launch @@ -24,6 +24,7 @@ + @@ -65,7 +66,7 @@ - + diff --git a/hlpr_gazebo/src/vector_control_interface.py b/hlpr_gazebo/src/vector_control_interface.py index 2df79a2..2cbc807 100755 --- a/hlpr_gazebo/src/vector_control_interface.py +++ b/hlpr_gazebo/src/vector_control_interface.py @@ -87,6 +87,7 @@ def __init__(self): # Get flag for one vs. two arms self.two_arms = get_param('/two_arms', False) + self.use_7dof_jaco = get_param('/use_7dof_jaco', False) # Setup pan/tilt sim to real controller topics to simulate the real robot # Needed because ROS controller has a different message than Stanley @@ -100,7 +101,10 @@ def __init__(self): self.gripper_name = get_param('/gripper_controller/joints', '') # Setup the arm controller - self.arm_pub = rospy.Publisher('/vector/right_arm/command', JointTrajectory, queue_size=10) + if self.use_7dof_jaco: + self.arm_pub = rospy.Publisher('/j2s7s300/command', JointTrajectory, queue_size=10) + else: + self.arm_pub = rospy.Publisher('/vector/right_arm/command', JointTrajectory, queue_size=10) # Setup subscribers to listen to the commands self.linact_command_sub = rospy.Subscriber('/vector/linear_actuator_cmd', LinearActuatorCmd, self.linactCallback, queue_size=10) From 41195772a573b8197972a049b389a93069ca2638 Mon Sep 17 00:00:00 2001 From: Vivian Chu Date: Wed, 2 Aug 2017 11:15:41 -0400 Subject: [PATCH 4/9] change so that we use environment variable to set 7dof arm --- hlpr_gazebo/launch/vector.launch | 6 +----- hlpr_gazebo/launch/vector_controllers.launch | 7 +++---- 2 files changed, 4 insertions(+), 9 deletions(-) diff --git a/hlpr_gazebo/launch/vector.launch b/hlpr_gazebo/launch/vector.launch index c80ec91..368bd85 100644 --- a/hlpr_gazebo/launch/vector.launch +++ b/hlpr_gazebo/launch/vector.launch @@ -13,7 +13,6 @@ - @@ -27,9 +26,7 @@ - - - + @@ -37,7 +34,6 @@ - diff --git a/hlpr_gazebo/launch/vector_controllers.launch b/hlpr_gazebo/launch/vector_controllers.launch index 8034629..58bdc4d 100644 --- a/hlpr_gazebo/launch/vector_controllers.launch +++ b/hlpr_gazebo/launch/vector_controllers.launch @@ -7,7 +7,6 @@ - @@ -24,11 +23,11 @@ - + - + @@ -58,7 +57,7 @@ - + From 846f009b7c2a40559f43dfce012f92194617ff1d Mon Sep 17 00:00:00 2001 From: TescaF Date: Tue, 17 Oct 2017 16:29:33 -0400 Subject: [PATCH 5/9] removed prefix from gripper controller --- .../controller/gripper_controller_robotiq_j2s7s300.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/hlpr_gazebo/controller/gripper_controller_robotiq_j2s7s300.yaml b/hlpr_gazebo/controller/gripper_controller_robotiq_j2s7s300.yaml index e1fcb83..fabefc2 100644 --- a/hlpr_gazebo/controller/gripper_controller_robotiq_j2s7s300.yaml +++ b/hlpr_gazebo/controller/gripper_controller_robotiq_j2s7s300.yaml @@ -1,11 +1,11 @@ gripper_controller: type: position_controllers/JointTrajectoryController joints: - - j2s7s300_robotiq_85_left_knuckle_joint + - robotiq_85_left_knuckle_joint constraints: goal_time: 0.6 stopped_velocity_tolerance: 0.05 - j2s7s300_robotiq_85_left_knuckle_joint: {trajectory: 0.1, goal: 0.1} + robotiq_85_left_knuckle_joint: {trajectory: 0.1, goal: 0.1} stop_trajectory_duration: 0.5 state_publish_rate: 25 action_monitor_rate: 10 From c3be694dfe724f8a07d42130b14df10001cd9e00 Mon Sep 17 00:00:00 2001 From: ragtz Date: Wed, 1 Nov 2017 12:46:10 -0500 Subject: [PATCH 6/9] Add poli launch file --- hlpr_gazebo/launch/poli.launch | 6 ++++++ 1 file changed, 6 insertions(+) create mode 100644 hlpr_gazebo/launch/poli.launch diff --git a/hlpr_gazebo/launch/poli.launch b/hlpr_gazebo/launch/poli.launch new file mode 100644 index 0000000..7ed2cf4 --- /dev/null +++ b/hlpr_gazebo/launch/poli.launch @@ -0,0 +1,6 @@ + + + + + + From c4c39ab217686b09bd2d78b403ff7d639e5b2e59 Mon Sep 17 00:00:00 2001 From: Vivian Chu Date: Fri, 17 Nov 2017 18:36:58 -0500 Subject: [PATCH 7/9] add in changes so that the robot is publishing at 100hz like the real robot --- .../controller/arm_controller_j2s7s300.yaml | 2 +- .../gripper_controller_robotiq.yaml | 2 +- .../controller/joint_state_controller.yaml | 2 +- hlpr_gazebo/src/vector_control_interface.py | 28 ++++++++++++++++++- 4 files changed, 30 insertions(+), 4 deletions(-) diff --git a/hlpr_gazebo/controller/arm_controller_j2s7s300.yaml b/hlpr_gazebo/controller/arm_controller_j2s7s300.yaml index 28f2074..8f86b3b 100644 --- a/hlpr_gazebo/controller/arm_controller_j2s7s300.yaml +++ b/hlpr_gazebo/controller/arm_controller_j2s7s300.yaml @@ -19,5 +19,5 @@ j2s7s300: j2s7s300_joint_6: {trajectory: 0.1, goal: 0.1} j2s7s300_joint_7: {trajectory: 0.1, goal: 0.1} stop_trajectory_duration: 0.5 - state_publish_rate: 25 + state_publish_rate: 100 action_monitor_rate: 10 diff --git a/hlpr_gazebo/controller/gripper_controller_robotiq.yaml b/hlpr_gazebo/controller/gripper_controller_robotiq.yaml index ae01f91..704fdff 100644 --- a/hlpr_gazebo/controller/gripper_controller_robotiq.yaml +++ b/hlpr_gazebo/controller/gripper_controller_robotiq.yaml @@ -7,5 +7,5 @@ gripper_controller: stopped_velocity_tolerance: 0.05 right_robotiq_85_left_knuckle_joint: {trajectory: 0.1, goal: 0.1} stop_trajectory_duration: 0.5 - state_publish_rate: 25 + state_publish_rate: 100 action_monitor_rate: 10 diff --git a/hlpr_gazebo/controller/joint_state_controller.yaml b/hlpr_gazebo/controller/joint_state_controller.yaml index 318e3fa..fde781a 100644 --- a/hlpr_gazebo/controller/joint_state_controller.yaml +++ b/hlpr_gazebo/controller/joint_state_controller.yaml @@ -1,3 +1,3 @@ joint_state_controller: type: joint_state_controller/JointStateController - publish_rate: 10 + publish_rate: 100 diff --git a/hlpr_gazebo/src/vector_control_interface.py b/hlpr_gazebo/src/vector_control_interface.py index 2cbc807..191087d 100755 --- a/hlpr_gazebo/src/vector_control_interface.py +++ b/hlpr_gazebo/src/vector_control_interface.py @@ -41,7 +41,9 @@ import rospy import tf +import os import math +import numpy as np from collections import defaultdict from vector_msgs.msg import LinearActuatorCmd, GripperCmd, JacoCartesianVelocityCmd, GripperStat from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint @@ -87,7 +89,7 @@ def __init__(self): # Get flag for one vs. two arms self.two_arms = get_param('/two_arms', False) - self.use_7dof_jaco = get_param('/use_7dof_jaco', False) + self.use_7dof_jaco = os.environ['VECTOR_HAS_KINOVA_7DOF_ARM'] # Setup pan/tilt sim to real controller topics to simulate the real robot # Needed because ROS controller has a different message than Stanley @@ -119,6 +121,12 @@ def __init__(self): self.gripper_joint_state_pub = rospy.Publisher('/vector/right_gripper/joint_states', JointState, queue_size=1) self.gripper_stat_pub = rospy.Publisher('/vector/right_gripper/stat', GripperStat, queue_size=1) + # Setup an additional one for the 7dof arm + if self.use_7dof_jaco: + self.arm_state_pub = rospy.Publisher('/j2s7s300_driver/out/joint_state', JointState, queue_size=1) + self.joint_state_sub = rospy.Subscriber('/joint_states', JointState, self.js_cb, queue_size=1) + self.arm_joint_names = ['j2s7s300_joint_1','j2s7s300_joint_2','j2s7s300_joint_3','j2s7s300_joint_4','j2s7s300_joint_5','j2s7s300_joint_6','j2s7s300_joint_7'] + # Initialize necessary components for TF self.listener = tf.TransformListener() self.trans = None @@ -149,6 +157,24 @@ def __init__(self): rospy.loginfo("Done Init") + def js_cb(self, msg): + # Pull out the joint values related to the arm and publish out + + new_msg = JointState() + new_msg.header = msg.header + # Get index of arm joints + arm_idx = [msg.name.index(x) for x in self.arm_joint_names] + positions = np.array(msg.position)[arm_idx] + effort = np.array(msg.effort)[arm_idx] + velocity = np.array(msg.velocity)[arm_idx] + + # Store the new data + new_msg.name = self.arm_joint_names + new_msg.position = positions + new_msg.effort = effort + new_msg.velocity = velocity + self.arm_state_pub.publish(new_msg) + def jointTrajHelper(self, joint_names, positions): # Setup the joint trajectory From 6841102f99b1fd0bd8d8000d4dcbd51d5c46c2ee Mon Sep 17 00:00:00 2001 From: Vivian Chu Date: Fri, 17 Nov 2017 18:55:56 -0500 Subject: [PATCH 8/9] fix the simulator to not use the moveit launch at startup --- hlpr_gazebo/launch/vector.launch | 7 ++++++- hlpr_gazebo/launch/vector_controllers.launch | 7 +++++-- hlpr_gazebo/src/vector_control_interface.py | 10 ++++++---- 3 files changed, 17 insertions(+), 7 deletions(-) diff --git a/hlpr_gazebo/launch/vector.launch b/hlpr_gazebo/launch/vector.launch index 368bd85..ee62dc2 100644 --- a/hlpr_gazebo/launch/vector.launch +++ b/hlpr_gazebo/launch/vector.launch @@ -6,13 +6,16 @@ + + + @@ -30,10 +33,12 @@ + diff --git a/hlpr_gazebo/launch/vector_controllers.launch b/hlpr_gazebo/launch/vector_controllers.launch index 58bdc4d..ec42124 100644 --- a/hlpr_gazebo/launch/vector_controllers.launch +++ b/hlpr_gazebo/launch/vector_controllers.launch @@ -2,23 +2,26 @@ + + - + diff --git a/hlpr_gazebo/src/vector_control_interface.py b/hlpr_gazebo/src/vector_control_interface.py index 191087d..0b48c2c 100755 --- a/hlpr_gazebo/src/vector_control_interface.py +++ b/hlpr_gazebo/src/vector_control_interface.py @@ -89,7 +89,9 @@ def __init__(self): # Get flag for one vs. two arms self.two_arms = get_param('/two_arms', False) - self.use_7dof_jaco = os.environ['VECTOR_HAS_KINOVA_7DOF_ARM'] + self.use_7dof_jaco = os.getenv('VECTOR_HAS_KINOVA_7DOF_ARM', False) + + rospy.logwarn("Using 7DOF arm: %s" % self.use_7dof_jaco) # Setup pan/tilt sim to real controller topics to simulate the real robot # Needed because ROS controller has a different message than Stanley @@ -103,9 +105,9 @@ def __init__(self): self.gripper_name = get_param('/gripper_controller/joints', '') # Setup the arm controller - if self.use_7dof_jaco: + if self.use_7dof_jaco is True: self.arm_pub = rospy.Publisher('/j2s7s300/command', JointTrajectory, queue_size=10) - else: + else: self.arm_pub = rospy.Publisher('/vector/right_arm/command', JointTrajectory, queue_size=10) # Setup subscribers to listen to the commands @@ -122,7 +124,7 @@ def __init__(self): self.gripper_stat_pub = rospy.Publisher('/vector/right_gripper/stat', GripperStat, queue_size=1) # Setup an additional one for the 7dof arm - if self.use_7dof_jaco: + if self.use_7dof_jaco is True: self.arm_state_pub = rospy.Publisher('/j2s7s300_driver/out/joint_state', JointState, queue_size=1) self.joint_state_sub = rospy.Subscriber('/joint_states', JointState, self.js_cb, queue_size=1) self.arm_joint_names = ['j2s7s300_joint_1','j2s7s300_joint_2','j2s7s300_joint_3','j2s7s300_joint_4','j2s7s300_joint_5','j2s7s300_joint_6','j2s7s300_joint_7'] From c0e7c36269fc72e0c6d10140ac2c2e079ca2f660 Mon Sep 17 00:00:00 2001 From: Angel Andres Daruna Date: Mon, 29 Jan 2018 10:33:25 -0500 Subject: [PATCH 9/9] patch replacing moveit with trak_ik --- hlpr_gazebo/launch/vector.launch | 15 ++-- hlpr_gazebo/launch/vector_controllers.launch | 23 ++--- hlpr_gazebo/src/vector_control_interface.py | 95 ++++++++++++-------- 3 files changed, 68 insertions(+), 65 deletions(-) diff --git a/hlpr_gazebo/launch/vector.launch b/hlpr_gazebo/launch/vector.launch index 368bd85..794ea1f 100644 --- a/hlpr_gazebo/launch/vector.launch +++ b/hlpr_gazebo/launch/vector.launch @@ -6,13 +6,10 @@ - - - - - + + + - @@ -30,10 +27,8 @@ - - - - + + diff --git a/hlpr_gazebo/launch/vector_controllers.launch b/hlpr_gazebo/launch/vector_controllers.launch index 58bdc4d..f01be40 100644 --- a/hlpr_gazebo/launch/vector_controllers.launch +++ b/hlpr_gazebo/launch/vector_controllers.launch @@ -2,22 +2,13 @@ - - - - - - - - - - - - - - - + + + + + + + diff --git a/hlpr_gazebo/src/vector_control_interface.py b/hlpr_gazebo/src/vector_control_interface.py index 2cbc807..cebf32c 100755 --- a/hlpr_gazebo/src/vector_control_interface.py +++ b/hlpr_gazebo/src/vector_control_interface.py @@ -36,7 +36,7 @@ This script takes commands sent through the "real robot" topics and converts them into standard ROS messages for Gazebo to interpret -Note: it currently depends on MoveIt! to perform arm manipulation using the joystick +Note: No longer depends on MoveIt! Uses trak_ik instead """ import rospy @@ -49,8 +49,7 @@ from sensor_msgs.msg import JointState from std_msgs.msg import Float64 from geometry_msgs.msg import Pose, PoseStamped, Point, Quaternion -from moveit_msgs.srv import GetPositionIK -from moveit_msgs.msg import PositionIKRequest +from hlpr_trac_ik.srv import * import dynamixel_msgs.msg def get_param(name, value=None): @@ -86,8 +85,9 @@ def __init__(self): self.tilt_names = get_param('/tilt_sim_controller/joints', '') # Get flag for one vs. two arms - self.two_arms = get_param('/two_arms', False) - self.use_7dof_jaco = get_param('/use_7dof_jaco', False) + self.two_arms = get_param('two_arms', False) + # Gets flag for 6-DoF vs. 7-DoF + self.use_7dof_jaco = get_param('use_7dof_jaco', False) # Setup pan/tilt sim to real controller topics to simulate the real robot # Needed because ROS controller has a different message than Stanley @@ -100,12 +100,22 @@ def __init__(self): self.gripper_pub = rospy.Publisher('/gripper_controller/command', JointTrajectory, queue_size=10) self.gripper_name = get_param('/gripper_controller/joints', '') - # Setup the arm controller - if self.use_7dof_jaco: + # Sets up the arm controller + if self.use_7dof_jaco: self.arm_pub = rospy.Publisher('/j2s7s300/command', JointTrajectory, queue_size=10) - else: + else: self.arm_pub = rospy.Publisher('/vector/right_arm/command', JointTrajectory, queue_size=10) + # Listens to arm joints states for IK + if self.use_7dof_jaco: + self.arm_sub = rospy.Subscriber('/j2s7s300/state', JointTrajectoryControllerState, self.armStateCallback, queue_size=1) + else: + self.arm_sub = rospy.Subscriber('/vector/right_arm/state', JointTrajectoryControllerState, self.armStateCallback, queue_size=1) + + # Initializes member variables to trak the arm joint state + self.arm_joint_state = None + self.arm_joint_names = None + # Setup subscribers to listen to the commands self.linact_command_sub = rospy.Subscriber('/vector/linear_actuator_cmd', LinearActuatorCmd, self.linactCallback, queue_size=10) self.pan_sub = rospy.Subscriber('/pan_controller/command', Float64, self.panCallback, queue_size=1) @@ -134,16 +144,16 @@ def __init__(self): self.gripper_cmd = dict() self.gripper_cmd[self.RIGHT_KEY] = None - # Initialize components for moveit IK service - rospy.logwarn("Waiting for MoveIt! for 10 seconds...") + # Initialize components for Trak IK service + rospy.logwarn("Waiting for trak_ik for 10 seconds...") try: - rospy.wait_for_service('compute_ik', timeout=10.0) # Wait for 10 seconds and assumes we don't want IK - self.compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK) + rospy.wait_for_service('hlpr_trac_ik', timeout=10.0) # Wait for 10 seconds and assumes we don't want IK + self.compute_ik = rospy.ServiceProxy('hlpr_trac_ik', IKHandler) except rospy.ROSException, e: - rospy.logwarn("MoveIt was not loaded and arm teleop will not be available") + rospy.logwarn("IKHanlder Service was not loaded and arm teleop will not be available") self.compute_ik = None else: - rospy.logwarn("MoveIt detected") + rospy.logwarn("IKHandler detected") self.eef_sub = rospy.Subscriber('/vector/right_arm/cartesian_vel_cmd', JacoCartesianVelocityCmd, self.EEFCallback, queue_size=1) rospy.loginfo("Done Init") @@ -261,14 +271,18 @@ def gripperCallback(self, msg, side): jtm = self.jointTrajHelper(self.gripper_name, grip_joint_pos) # Send the command - self.gripper_pub.publish(jtm) + self.gripper_pub.publish(jtm) + + def armStateCallback(self, msg): + # gets the current arm state in joint space (joint angles) + self.arm_joint_state = msg.actual.positions + self.arm_joint_names = msg.joint_names def isCommandAllZero(self, msg): return (msg.x + msg.y + msg.z + msg.theta_x + msg.theta_y + msg.theta_z) == 0 def EEFCallback(self, msg): - # Check if we have EEF positions yet if self.trans == None or self.rot == None: return @@ -301,7 +315,7 @@ def EEFCallback(self, msg): pose['position']['keys'] = ['x','y','z'] pose['rotation']['keys'] = ['theta_x','theta_y','theta_z'] pose['rotation']['speed'] = "0.075" # rotation speed (degrees) - pose['position']['speed'] = "0.15" # position speed (cm) + pose['position']['speed'] = "0.3" # position speed (cm) for val in ['position','rotation']: for i in xrange(len(pose[val]['keys'])): @@ -346,42 +360,45 @@ def EEFCallback(self, msg): def computeIK(self, pose): - - # Create a pose to compute IK for - pose_stamped = PoseStamped() - pose_stamped.header.frame_id = 'base_link' # Hard coded for now - pose_stamped.header.stamp = rospy.Time.now() - pose_stamped.pose = pose + # Create a Trak IK request + ik_request = IKHandlerRequest() + ik_request.goals = [pose] + ik_request.tolerance = [0.01,0.01,0.01,0.01,0.01,0.01,0.01] + ik_request.verbose = False + + # Checks for the current arm state in joint space + if not self.arm_joint_state == None: + ik_request.origin = self.arm_joint_state + else: + rospy.logerror("IK service request failed: Current arm joint state not read") + return None,None - # Create a moveit ik request - ik_request = PositionIKRequest() - ik_request.group_name = 'arm' # Hard coded for now - ik_request.pose_stamped = pose_stamped - ik_request.timeout.secs = 0.1 - ik_request.avoid_collisions = True - + # Makes IK request try: request_value = self.compute_ik(ik_request) - if request_value.error_code.val == -31: - rospy.logwarn("Teleop Arm: No IK Solution") - if request_value.error_code.val == 1: - joint_positions = request_value.solution.joint_state.position[1:7] - joint_names = request_value.solution.joint_state.name[1:7] + if request_value.success: + joint_positions = request_value.poses[0].positions + joint_names = self.arm_joint_names return joint_positions,joint_names else: + rospy.logwarn("Teleop Arm: No IK Solution") return None,None - + except rospy.ServiceException, e: - print "IK service request failed: %s" % e + rospy.logerror("IK service request failed: %s", e) return None,None def updateEEF(self): - + # Selects are type (6-DoF or 7-DoF Jaco) + if self.use_7dof_jaco: + end_link = 'j2s7s300_ee_link' + else: + end_link = 'right_ee_link' rate = rospy.Rate(100.0) # Run at 100hz? # Continuously cycles and updates the EEF using tf if available while not rospy.is_shutdown(): try: - (self.trans,self.rot) = self.listener.lookupTransform('/base_link', 'right_ee_link', rospy.Time(0)) + (self.trans,self.rot) = self.listener.lookupTransform('/linear_actuator_link', end_link, rospy.Time(0)) except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): continue