diff --git a/hlpr_gazebo/controller/arm_controller_j2s7s300.yaml b/hlpr_gazebo/controller/arm_controller_j2s7s300.yaml
new file mode 100644
index 0000000..8f86b3b
--- /dev/null
+++ b/hlpr_gazebo/controller/arm_controller_j2s7s300.yaml
@@ -0,0 +1,23 @@
+j2s7s300:
+ type: position_controllers/JointTrajectoryController
+ joints:
+ - j2s7s300_joint_1
+ - j2s7s300_joint_2
+ - j2s7s300_joint_3
+ - j2s7s300_joint_4
+ - j2s7s300_joint_5
+ - j2s7s300_joint_6
+ - j2s7s300_joint_7
+ constraints:
+ 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: 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/gripper_controller_robotiq_j2s7s300.yaml b/hlpr_gazebo/controller/gripper_controller_robotiq_j2s7s300.yaml
new file mode 100644
index 0000000..fabefc2
--- /dev/null
+++ b/hlpr_gazebo/controller/gripper_controller_robotiq_j2s7s300.yaml
@@ -0,0 +1,11 @@
+gripper_controller:
+ type: position_controllers/JointTrajectoryController
+ joints:
+ - robotiq_85_left_knuckle_joint
+ constraints:
+ goal_time: 0.6
+ stopped_velocity_tolerance: 0.05
+ 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/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/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/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 @@
+
+
+
+
+
+
diff --git a/hlpr_gazebo/launch/vector.launch b/hlpr_gazebo/launch/vector.launch
index 59177db..23f8eaf 100644
--- a/hlpr_gazebo/launch/vector.launch
+++ b/hlpr_gazebo/launch/vector.launch
@@ -6,20 +6,18 @@
-
-
-
-
-
+
+
+
+
-
-
+
@@ -28,15 +26,10 @@
-
-
-
-
-
-
-
+
+
@@ -48,6 +41,9 @@
+
+
+
diff --git a/hlpr_gazebo/launch/vector_controllers.launch b/hlpr_gazebo/launch/vector_controllers.launch
index 5dbc083..a9d2901 100644
--- a/hlpr_gazebo/launch/vector_controllers.launch
+++ b/hlpr_gazebo/launch/vector_controllers.launch
@@ -1,60 +1,68 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
-
-
-
+
+
-
-
+
+
+
+
+
-
-
-
-
-
+
+
+
+
-
-
-
-
+
+
+
-
-
-
+
+
+
+
+
+
+
+
-
-
+
+
+
+
+
+
+
+
@@ -67,6 +75,6 @@
+ args="linear_actuator_controller 0.65"/>
diff --git a/hlpr_gazebo/src/vector_control_interface.py b/hlpr_gazebo/src/vector_control_interface.py
index 2df79a2..a7fe907 100755
--- a/hlpr_gazebo/src/vector_control_interface.py
+++ b/hlpr_gazebo/src/vector_control_interface.py
@@ -36,12 +36,14 @@
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
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
@@ -49,8 +51,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,7 +87,11 @@ 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.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)
+
+ 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
@@ -99,8 +104,21 @@ 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
- self.arm_pub = rospy.Publisher('/vector/right_arm/command', JointTrajectory, queue_size=10)
+ # Sets up the arm controller
+ 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)
+
+ # 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)
@@ -115,6 +133,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 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']
+
# Initialize necessary components for TF
self.listener = tf.TransformListener()
self.trans = None
@@ -130,21 +154,39 @@ 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")
+ 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
@@ -257,14 +299,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
@@ -297,7 +343,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'])):
@@ -342,42 +388,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