def setUpClass(cls): rospy.wait_for_message('/move_group/status', GoalStatusArray) cls.hand_type = rospy.get_param('~test_hand_and_arm_sim/hand_type') cls.scene = rospy.get_param('~test_hand_and_arm_sim/scene') # ur-specific launch files do not accept 'side' param as it is already set # for phantom hands use hand finder try: cls.side = rospy.get_param('~test_hand_and_arm_sim/side') if cls.side == 'right': cls.hand_id = 'rh' elif cls.side == 'left': cls.hand_id = 'lh' except rospy.ROSException: rospy.loginfo("No side param for this test type") cls.hand_id = rospy.get_param('/hand/mapping/0') if cls.hand_id == 'rh': cls.arm_id = 'ra' cls.robot_commander = SrRobotCommander(name="right_arm_and_hand") cls.hand_commander = SrHandCommander(name='right_hand') cls.arm_commander = SrArmCommander(name='right_arm', set_ground=not (cls.scene)) elif cls.hand_id == 'lh': cls.arm_id = 'la' cls.robot_commander = SrRobotCommander(name="left_arm_and_hand") cls.hand_commander = SrHandCommander(name='left_hand') cls.arm_commander = SrArmCommander(name='left_arm', set_ground=not (cls.scene)) rospy.Subscriber('/move_group/monitored_planning_scene', PlanningScene, cls.scene_data_cb) rospy.sleep(10)
def __init__(self): self.bridge = CvBridge() self.hand_commander = SrHandCommander(name="right_hand") # play piano start pos start_play_pos = { "rh_THJ1": 20, "rh_THJ2": 10, "rh_THJ3": 0, "rh_THJ4": 0, "rh_THJ5": 0, "rh_FFJ1": 45, "rh_FFJ2": 80, "rh_FFJ3": 0, "rh_FFJ4": 0, "rh_MFJ1": 45, "rh_MFJ2": 80, "rh_MFJ3": 0, "rh_MFJ4": 0, "rh_RFJ1": 45, "rh_RFJ2": 80, "rh_RFJ3": 0, "rh_RFJ4": 0, "rh_LFJ1": 45, "rh_LFJ2": 80, "rh_LFJ3": 0, "rh_LFJ4": 0, "rh_LFJ5": 0, "rh_WRJ1": -30, "rh_WRJ2": 0 } self.hand_commander.move_to_joint_value_target_unsafe( start_play_pos, 1.5, False, angle_degrees=True) rospy.sleep(1)
class GraspExecution(object): def __init__(self): self.keyboard_pressed = False self.hand_commander = SrHandCommander(name='right_hand') self.grasp_yaml = {} def _open_yaml(self): grasp_config_filename = '/home/user/projects/shadow_robot/base/src/'\ 'sr_interface/sr_example/config/demo_grasps.yaml' with open(grasp_config_filename) as f: self.grasp_yaml = yaml.load(f, Loader=yaml.FullLoader) def _get_input(self): fd = sys.stdin.fileno() old_settings = termios.tcgetattr(fd) try: tty.setraw(sys.stdin.fileno()) ch = sys.stdin.read(1) finally: termios.tcsetattr(fd, termios.TCSADRAIN, old_settings) return ch def run(self): self._open_yaml() while True: input_val = self._get_input() if input_val == "1": self.execute_grasp("open_hand") elif input_val == "2": self.execute_grasp("close_hand") elif input_val == "3": self.execute_grasp("point") elif input_val == "4": self.execute_grasp("2_finger_pinch") elif input_val == "5": self.execute_grasp("3_finger_pinch") elif input_val == "6": self.execute_grasp("parallel_extension") elif input_val == "7": self.execute_grasp("grasp_sphere") if '0x1b' == hex(ord(input_val)): sys.exit(0) def execute_grasp(self, grasp): rospy.loginfo("Grasp {} started.".format(grasp)) open_dict = dict( zip(self.grasp_yaml['joint_names'], self.grasp_yaml['grasps']['open_hand'])) grasp_dict = dict( zip(self.grasp_yaml['joint_names'], self.grasp_yaml['grasps'][grasp])) self.hand_commander.move_to_joint_value_target_unsafe( open_dict, 5.0, True) self.hand_commander.move_to_joint_value_target_unsafe( grasp_dict, 5.0, True) rospy.sleep(2.0) rospy.loginfo("Grasp {} completed.".format(grasp))
def test_hand_finder_init(self): hand_finder = HandFinder() hand_parameters = hand_finder.get_hand_parameters() hand_commander = SrHandCommander( hand_parameters=hand_parameters, hand_serial=hand_parameters.mapping.keys()[0]) self.assertGreater(len(hand_commander.get_joints_position()), 0, "No joints found, init must have failed.")
def test_strip_prefix(self): hand_commander = SrHandCommander() self.assertEqual(hand_commander._strip_prefix("rh_ffj3"), "ffj3", msg="Strip failed") self.assertEqual(hand_commander._strip_prefix("ffj3"), "ffj3", msg="Strip failed")
def setUpClass(cls): cls.hand_type = rospy.get_param('~test_sim/hand_type', 'hand_e') if cls.hand_type not in ('hand_e', 'hand_g'): raise TypeError("The specified hand_type is incorrect.") cls.hand_id = rospy.get_param('~test_sim/hand_id', 'rh') rospy.wait_for_message('/move_group/status', GoalStatusArray) if cls.hand_id == 'rh': cls.hand_commander = SrHandCommander(name='right_hand') elif cls.hand_id == 'lh': cls.hand_commander = SrHandCommander(name='left_hand') else: raise TypeError("The specified hand_id is incorrect.")
def setUpClass(cls): rospy.wait_for_message('/move_group/status', GoalStatusArray) cls.hand_type = 'hand_e' cls.robot_commander = SrRobotCommander(name="two_arms_and_hands") cls.rh_commander = SrHandCommander(name='right_hand') cls.lh_commander = SrHandCommander(name='left_hand') cls.hand_commander = SrHandCommander(name='two_hands') cls.arm_commander = SrArmCommander(name='two_arms', set_ground=False) cls.arm_commander.set_max_velocity_scaling_factor(0.2) cls.robot_commander.set_max_velocity_scaling_factor(0.2) rospy.sleep(3)
def setUpClass(cls): rospy.wait_for_message('/move_group/status', GoalStatusArray) cls.scene = rospy.get_param('~test_hand_and_arm_sim/scene') cls.robot_commander = SrRobotCommander(name="two_arms_and_hands") cls.hand_commander = SrHandCommander(name='two_hands') cls.arm_commander = SrArmCommander(name='two_arms', set_ground=not (cls.scene)) rospy.Subscriber('/move_group/monitored_planning_scene', PlanningScene, cls.scene_data_cb) rospy.sleep(10)
def __init__(self): self.mgi = moveit_commander.MoveGroupCommander("right_hand") self.bridge = CvBridge() self.mgi.set_named_target("open") self.mgi.go() self.hand_commander = SrHandCommander(name="right_hand") # self.hand_commander.move_to_joint_value_target_unsafe(start_pos, 1.2, False, angle_degrees=True) # get next image after finish one pose self.online_once() rospy.spin()
warehouse_states = { 'state_1': { 'joint_0': 0.00, 'joint_1': 0.00 }, 'state_2': { 'joint_0': 0.00, 'joint_1': 0.00 } } """ rospy.init_node("use_exported_states") # Define a hand commander, so we have something to do with the states we extracted. hand_commander = SrHandCommander() # You could use the states directly: hand_commander.move_to_joint_value_target_unsafe(warehouse_states['state_2']) rospy.sleep(2) hand_commander.move_to_joint_value_target_unsafe(warehouse_states['state_1']) rospy.sleep(2) hand_commander.move_to_joint_value_target_unsafe(warehouse_states['state_2']) rospy.sleep(2) # You could translate all the named states in a trajectory: trajectory = [ { 'name': 'state_1', 'interpolate_time': 3.0 },
def __init__(self, name, hand_or_arm="both", side="right", save_target=False): self._save = rospy.ServiceProxy('save_robot_state', SaveState) self._name = name self._save_target = save_target if name is None or name == '': raise ValueError("Cannot save with empty name.") prefix = "" double_error = [] if hand_or_arm == "arm": if side != "bimanual": prefix = "ra" if side == "right" else "la" self._commander = SrArmCommander(side + '_arm') if not self._commander.arm_found(): double_error.append("Group " + side + "_arm not found.") else: self._commander = SrRobotCommander('two_arms') elif hand_or_arm == 'hand': if side != "bimanual": prefix = "rh" if side == "right" else "lh" self._commander = SrHandCommander(side + '_hand') else: self._commander = SrRobotCommander('two_hands') elif hand_or_arm == 'both': if side != "bimanual": self._commander = SrRobotCommander(side + '_arm_and_hand') else: self._commander = SrRobotCommander('two_arms_and_hands') if len(double_error) != 0: raise ValueError(" ".join(double_error)) self._save_hand = (hand_or_arm == "hand" or hand_or_arm == "both") self._save_arm = (hand_or_arm == "arm" or hand_or_arm == "both") self._save_bimanual = (side == 'bimanual') if save_target: rospy.loginfo("Saving targets instead of current values") self._mutex = Lock() self._target_values = dict() if self._save_hand: self._hand_subscriber = rospy.Subscriber( "/" + prefix + "_trajectory_controller/state", JointTrajectoryControllerState, self._target_cb) if self._save_arm: self._arm_subscriber = rospy.Subscriber( "/" + prefix + "_trajectory_controller/state", JointTrajectoryControllerState, self._target_cb) if self._save_bimanual: if self._save_hand: self.r_hand_subscriber = rospy.Subscriber( "/ra_trajectory_controller/state", JointTrajectoryControllerState, self._target_cb) self.l_hand_subscriber = rospy.Subscriber( "/la_trajectory_controller/state", JointTrajectoryControllerState, self._target_cb) if self._save_arm: self.r_arm_subscriber = rospy.Subscriber( "/ra_trajectory_controller/state", JointTrajectoryControllerState, self._target_cb) self.l_arm_subscriber = rospy.Subscriber( "/la_trajectory_controller/state", JointTrajectoryControllerState, self._target_cb) current_dict = {} rospy.loginfo("Getting current") current_dict = self._commander.get_current_state() robot_name = self._commander.get_robot_name() if self._save_target: rospy.loginfo("Getting targets") waiting_for_targets = True while waiting_for_targets and not rospy.is_shutdown(): self._mutex.acquire() waiting_for_targets = False for joint in current_dict: if joint in self._target_values: current_dict[joint] = self._target_values[joint] else: waiting_for_targets = True rospy.loginfo("Still waiting for %s target" % joint) self._mutex.release() if waiting_for_targets: rospy.loginfo(self._target_values) rospy.sleep(1) if rospy.is_shutdown(): exit(0) self.save_state(current_dict, robot_name)
from __future__ import absolute_import import rospy import sys from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint from sr_robot_commander.sr_robot_commander import SrRobotCommander from sr_robot_commander.sr_arm_commander import SrArmCommander from sr_robot_commander.sr_hand_commander import SrHandCommander import geometry_msgs.msg from builtins import input rospy.init_node("hand_arm_waypoints", anonymous=True) # The constructors for the SrArmCommander, SrHandCommander and SrRobotCommander # take a name parameter that should match the group name of the robot to be used. # How to command the hand or arm separately hand_commander = SrHandCommander(name="right_hand") arm_commander = SrArmCommander(name="right_arm") # How to command the arm and hand together robot_commander = SrRobotCommander(name="right_arm_and_hand") arm_commander.set_max_velocity_scaling_factor(0.1) rospy.sleep(1.0) arm_home_joints_goal = { 'ra_shoulder_pan_joint': 0.00, 'ra_elbow_joint': 2.00, 'ra_shoulder_lift_joint': -1.25, 'ra_wrist_1_joint': -0.733, 'ra_wrist_2_joint': 1.5708, 'ra_wrist_3_joint': 0.00 }
if args.side == 'right': joint_prefix = 'rh_' elif args.side == 'left': joint_prefix = 'lh_' else: joint_prefix = 'both' if 'rh_' == joint_prefix: hand_name = 'right_hand' elif 'lh_' == joint_prefix: hand_name = 'left_hand' else: hand_name = 'two_hands' hand_commander = SrHandCommander(name=hand_name) open_hand_no_id = {'FFJ1': 0.0, 'FFJ2': 0.0, 'FFJ3': 0.0, 'FFJ4': 0.0, 'MFJ1': 0.0, 'MFJ2': 0.0, 'MFJ3': 0.0, 'MFJ4': 0.0, 'RFJ1': 0.0, 'RFJ2': 0.0, 'RFJ3': 0.0, 'RFJ4': 0.0, 'LFJ1': 0.0, 'LFJ2': 0.0, 'LFJ3': 0.0, 'LFJ4': 0.0, 'LFJ5': 0.0, 'THJ1': 0.0, 'THJ2': 0.0, 'THJ3': 0.0, 'THJ4': 0.0, 'THJ5': 0.0, 'WRJ1': 0.0, 'WRJ2': 0.0} close_hand_no_id = {'FFJ1': 1.5707, 'FFJ2': 1.5707, 'FFJ3': 1.5707, 'FFJ4': 0.0, 'MFJ1': 1.5707, 'MFJ2': 1.5707, 'MFJ3': 1.5707, 'MFJ4': 0.0, 'RFJ1': 1.5707, 'RFJ2': 1.5707, 'RFJ3': 1.5707, 'RFJ4': 0.0, 'LFJ1': 1.5707, 'LFJ2': 1.5707, 'LFJ3': 1.5707, 'LFJ4': 0.0, 'LFJ5': 0.0, 'WRJ1': 0.0, 'WRJ2': 0.0} close_thumb_no_id = {'THJ1': 0.52, 'THJ2': 0.61, 'THJ3': 0.0, 'THJ4': 1.20, 'THJ5': 0.17}
elif name == "tdc": self._tdc_db.append(x) if len(self._tdc_db) >= 50: self.baseline(name="tdc") else: print("Proper name value not provided! ['electrdes', 'pac0', 'pac1', \ 'pdc', 'tac', 'tdc']") return rospy.init_node("move_test", anonymous=True) context = zmq.Context() socket = context.socket(zmq.PUB) socket.bind("tcp://*:5556") hand_commander = SrHandCommander() arm_commander = SrArmCommander() rate_handle = rospy.Rate(100) #hz # ff = BiotacData() # ff_elect_db = ff._elect_db # ff_angle_mvavg = ff.angle_mvavg # ff_pac1_mvavg = ff.pac1_mvavg # ff_pdc_mvavg = ff.pdc_mvavg # th = snn.BiotacData() time.sleep(0.1)
from __future__ import division import rospy from std_msgs.msg import String, Float32, UInt8 # from sr_robot_commander.sr_arm_commander import SrArmCommander from sr_robot_commander.sr_hand_commander import SrHandCommander from sr_robot_msgs.msg import BiotacAll import time import numpy as np # for: exp(), .shape import numpy.matlib as npm # for: npm.repmat() rospy.init_node("sr_grasp", anonymous=True) # arm_commander = SrArmCommander() hand_commander = SrHandCommander() time.sleep(1) class Slip(object): """docstring for Slip""" def __init__(self): self.last3 = [0,0,0]#,0,0] #,0,0,0,0,0] self.mvavg = 0 self.base = [] #np.matrix( np.zeros((26,1)) ) self.flag = 0 def new(self, x): self.last3.pop() self.last3.insert(0,x) self.update() def update(self): self.mvavg = sum(self.last3)/len(self.last3)
""" Author: Chad Colestock Date: 08/29/2015 Ver: 1.0 """ #import numpy as np import rospy from sr_robot_commander.sr_hand_commander import SrHandCommander from sr_robot_commander.sr_arm_commander import SrArmCommander import roslib; roslib.load_manifest('sr_ronex_examples') from sr_ronex_msgs.msg import GeneralIOState rospy.init_node('Synergy_move_with_pot', anonymous = True) rospy.sleep(1.0) hand_commander = SrHandCommander() arm_commander = SrArmCommander() t = None #told = 0; def callback(data): global t analogue = data.analogue[0]; t = float(int((analogue+1)))/3400 #if t < told: # t = told # told = t; # print (t)
# This example demonstrates how you can send a trajectory created from named poses. from __future__ import absolute_import import rospy from sr_robot_commander.sr_hand_commander import SrHandCommander from sr_utilities.hand_finder import HandFinder rospy.init_node("named_traj_example", anonymous=True) hand_finder = HandFinder() hand_parameters = hand_finder.get_hand_parameters() hand_serial = hand_parameters.mapping.keys()[0] hand_commander = SrHandCommander(hand_parameters=hand_parameters, hand_serial=hand_serial) # Define trajectory. Interpolate time (time to move to each point from previous posture) # must be specified. Pause time is optional. Names are either the default poses defined # in SRDF or are states stored in the warehouse. trajectory = [{ 'name': 'open', 'interpolate_time': 3.0 }, { 'name': 'pack', 'interpolate_time': 3.0, 'pause_time': 2 }, { 'name': 'open', 'interpolate_time': 3.0 }, {
class Teleoperation(): def __init__(self): self.bridge = CvBridge() self.hand_commander = SrHandCommander(name="right_hand") rospy.sleep(1) def online_once(self): while True: img_data = rospy.wait_for_message( "/camera/aligned_depth_to_color/image_raw", Image) rospy.loginfo("Got an image ^_^") try: img = self.bridge.imgmsg_to_cv2(img_data, desired_encoding="passthrough") except CvBridgeError as e: rospy.logerr(e) try: # preproces img = seg_hand_depth(img, 500, 1000, 10, 100, 4, 4, 250, True, 300) img = img.astype(np.float32) img = img / 255. * 2. - 1 n = cv2.resize(img, (0, 0), fx=2, fy=2) n = cv2.normalize(n, n, alpha=0, beta=1, norm_type=cv2.NORM_MINMAX, dtype=cv2.CV_32F) cv2.imshow("segmented human hand", n) cv2.waitKey(1) # get the clipped joints goal = tuple(self.joint_cal(img, isbio=True)) hand_pos = self.hand_commander.get_joints_position() # first finger hand_pos.update({"rh_FFJ3": goal[3]}) hand_pos.update({"rh_FFJ2": goal[4]}) hand_pos.update({"rh_FFJ4": goal[2]}) # middle finger hand_pos.update({"rh_MFJ3": goal[12]}) hand_pos.update({"rh_MFJ2": goal[13]}) # ring finger hand_pos.update({"rh_RFJ3": goal[16]}) hand_pos.update({"rh_RFJ2": goal[17]}) # little finger hand_pos.update({"rh_LFJ3": goal[8]}) hand_pos.update({"rh_LFJ2": goal[9]}) # thumb hand_pos.update({"rh_THJ5": goal[19]}) hand_pos.update({"rh_THJ4": goal[20]}) hand_pos.update({"rh_THJ3": goal[21]}) hand_pos.update({"rh_THJ2": goal[22]}) self.hand_commander.move_to_joint_value_target_unsafe( hand_pos, 0.3, False, angle_degrees=False) #rospy.sleep(0.5) rospy.loginfo("Next one please ---->") except: rospy.loginfo("no images") def joint_cal(self, img, isbio=False): # start = rospy.Time.now().to_sec() # run the model feature = test(model, img) # network_time = rospy.Time.now().to_sec() - start # print("network_time is ", network_time) joint = [0.0, 0.0] joint += feature.tolist() if isbio: joint[5] = 0.3498509706185152 joint[10] = 0.3498509706185152 joint[14] = 0.3498509706185152 joint[18] = 0.3498509706185152 joint[23] = 0.3498509706185152 # joints crop joint[2] = self.clip(joint[2], 0.349, -0.349) joint[3] = self.clip(joint[3], 1.57, 0) joint[4] = self.clip(joint[4], 1.57, 0) joint[5] = self.clip(joint[5], 1.57, 0) joint[6] = self.clip(joint[6], 0.785, 0) joint[7] = self.clip(joint[7], 0.349, -0.349) joint[8] = self.clip(joint[8], 1.57, 0) joint[9] = self.clip(joint[9], 1.57, 0) joint[10] = self.clip(joint[10], 1.57, 0) joint[11] = self.clip(joint[11], 0.349, -0.349) joint[12] = self.clip(joint[12], 1.57, 0) joint[13] = self.clip(joint[13], 1.57, 0) joint[14] = self.clip(joint[14], 1.57, 0) joint[15] = self.clip(joint[15], 0.349, -0.349) joint[16] = self.clip(joint[16], 1.57, 0) joint[17] = self.clip(joint[17], 1.57, 0) joint[18] = self.clip(joint[18], 1.57, 0) joint[19] = self.clip(joint[19], 1.047, -1.047) joint[20] = self.clip(joint[20], 1.222, 0) joint[21] = self.clip(joint[21], 0.209, -0.209) joint[22] = self.clip(joint[22], 0.524, -0.524) joint[23] = self.clip(joint[23], 1.57, 0) return joint def clip(self, x, maxv=None, minv=None): if maxv is not None and x > maxv: x = maxv if minv is not None and x < minv: x = minv return x
def __init__(self): self.bridge = CvBridge() self.hand_commander = SrHandCommander(name="right_hand") rospy.sleep(1)
from sr_robot_commander.sr_arm_commander import SrArmCommander from sr_robot_commander.sr_hand_commander import SrHandCommander from sr_robot_commander.sr_robot_commander import SrRobotCommander if dropdown_commander == 'arm_commander': arm_commander = SrArmCommander(set_ground=False) elif dropdown_commander == 'hand_commander': hand_commander = SrHandCommander(name=side + "_hand") elif dropdown_commander == 'robot_commander': robot_commander = SrRobotCommander(name=side + "_arm_and_hand") elif dropdown_commander == 'hand_h_and_arm': robot_commander = SrRobotCommander(name="right_arm_and_hand", prefix="H1_") else: arm_commander = SrArmCommander(set_ground=False) hand_commander = SrHandCommander(name=side + "_hand")
add_help=True, usage='%(prog)s [-h] --angle_type ANGLE_TYPE', formatter_class=argparse.RawTextHelpFormatter) parser.add_argument("--angle_type", default="radians", help="ANGLE_TYPE should be either degrees or radians") args = parser.parse_args() angle_type = args.angle_type # Use the hand finder to get the hand prefix, to allow this script to be used with either left or right hands hand_finder = HandFinder() hand_parameters = hand_finder.get_hand_parameters() prefix = hand_parameters.mapping.values() hand_serial = hand_parameters.mapping.keys() scale = 1 if angle_type == "degrees": scale = 1 * (180/pi) for i in arange(0, len(prefix)): hand_commander = SrHandCommander(hand_parameters=hand_parameters, hand_serial=hand_serial[i]) print("Joints positions") all_joints_state = hand_commander.get_joints_position() hand_joints_state = { k: (v * scale) for k, v in all_joints_state.items() if k.startswith(prefix[i] + "_")} print("Hand joints position \n " + str(hand_joints_state) + "\n")
""" Allows for a guided process to print out consecutive joint states of the UR10. Useful for building synergy path movements or multistage motions in general. Author: Thomas Colestock Date: 08/29/2015 Ver: 1.0.0 """ import rospy from sr_robot_commander.sr_hand_commander import SrHandCommander rospy.init_node("print_left_joints_position", anonymous=True) hand_commander = SrHandCommander() __author__ = 'Thomas Colestock' print("Joints positions") hand_keyID = ['rh_FFJ1', 'rh_FFJ2', 'rh_FFJ3', 'rh_FFJ4', 'rh_MFJ1', 'rh_MFJ2', 'rh_MFJ3', 'rh_MFJ4', 'rh_RFJ1', 'rh_RFJ2', 'rh_RFJ3', 'rh_RFJ4', 'rh_LFJ1', 'rh_LFJ2', 'rh_LFJ3', 'rh_LFJ4', 'rh_LFJ5', 'rh_THJ1', 'rh_THJ2', 'rh_THJ3', 'rh_THJ4', 'rh_THJ5', 'rh_WRJ1', 'rh_WRJ2'] arm_keyID = ['ra_shoulder_pan_joint', 'ra_shoulder_lift_joint', 'ra_elbow_joint', 'ra_wrist_1_joint', 'ra_wrist_2_joint', 'ra_wrist_3_joint'] filePrefix = raw_input("Input Desired File Name Without Suffix or Spaces (.txt): ") print("Two files will be created: \n" + filePrefix + "_PY.txt\n" + filePrefix + "_MAT.txt\n") print("_PY.txt creates a Python Dictionary Reference and _MAT.txt creates a file easily parsed by MATLAB \n") print("!! REMEMBER TO MOVE FILES TO A NEW LOCATION WHEN FINISHED OR THEY WILL BE OVERWRITTEN !! \n\n")
def __init__(self): self.keyboard_pressed = False self.hand_commander = SrHandCommander(name='right_hand') self.grasp_yaml = {}
#!/usr/bin/env python # Script to move the left hand into store position. import rospy from sr_robot_commander.sr_hand_commander import SrHandCommander rospy.init_node("left_hand_demo", anonymous=False) hand_commander = SrHandCommander(name="left_hand") open_hand = { 'lh_FFJ1': 0.0, 'lh_FFJ2': 0.0, 'lh_FFJ3': 0.0, 'lh_FFJ4': 0.0, 'lh_MFJ1': 0.0, 'lh_MFJ2': 0.0, 'lh_MFJ3': 0.0, 'lh_MFJ4': 0.0, 'lh_RFJ1': 0.0, 'lh_RFJ2': 0.0, 'lh_RFJ3': 0.0, 'lh_RFJ4': 0.0, 'lh_LFJ1': 0.0, 'lh_LFJ2': 0.0, 'lh_LFJ3': 0.0, 'lh_LFJ4': 0.0, 'lh_LFJ5': 0.0, 'lh_THJ1': 0.0, 'lh_THJ2': 0.0,
# It is recommended to run this script in simulation first. from __future__ import absolute_import import rospy from builtins import input from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint from sr_robot_commander.sr_robot_commander import SrRobotCommander from sr_robot_commander.sr_hand_commander import SrHandCommander from sr_robot_commander.sr_arm_commander import SrArmCommander rospy.init_node("left_hand_arm_ef_pos", anonymous=True) # The constructors for the SrArmCommander, SrHandCommander and SrRobotCommander # take a name parameter that should match the group name of the robot to be used. # How to command the hand or arm separately hand_commander = SrHandCommander(name="left_hand") arm_commander = SrArmCommander(name="left_arm") # How to command the arm and hand together robot_commander = SrRobotCommander(name="left_arm_and_hand") arm_commander.set_max_velocity_scaling_factor(0.1) rospy.sleep(2.0) arm_home_joints_goal = { 'la_shoulder_pan_joint': 1.47, 'la_elbow_joint': -1.695, 'la_shoulder_lift_joint': -1.22, 'la_wrist_1_joint': -01.75, 'la_wrist_2_joint': 1.57, 'la_wrist_3_joint': -1.830 }
# Software Foundation version 2 of the License. # # This program is distributed in the hope that it will be useful, but WITHOUT # ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or # FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for # more details. # # You should have received a copy of the GNU General Public License along # with this program. If not, see <http://www.gnu.org/licenses/>. # Reading the tactiles from the hand. from __future__ import absolute_import import rospy from sr_robot_commander.sr_hand_commander import SrHandCommander from sr_utilities.hand_finder import HandFinder rospy.init_node("tactile_reader", anonymous=True) hand_finder = HandFinder() hand_parameters = hand_finder.get_hand_parameters() hand_serial = hand_parameters.mapping.keys()[0] hand_commander = SrHandCommander(hand_parameters=hand_parameters, hand_serial=hand_serial) rospy.sleep(1.0) print("Tactile type: ", hand_commander.get_tactile_type()) print("Tactile state: ", hand_commander.get_tactile_state())
import rospy from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint from sr_robot_commander.sr_hand_commander import SrHandCommander from sr_utilities.hand_finder import HandFinder from numpy import sin, cos, pi, arange rospy.init_node("joint_sine_example", anonymous=True) # handfinder is used to access the hand parameters hand_finder = HandFinder() hand_parameters = hand_finder.get_hand_parameters() prefix = hand_parameters.mapping.values() hand_serial = hand_parameters.mapping.keys()[0] hand_commander = SrHandCommander(hand_parameters=hand_parameters, hand_serial=hand_serial) # cycles per second of sine wave f = 1 # angular frequency, rads/s w = 2 * pi * f # time for motion to complete ts = 20 # specify 2 joints to move joint_names = [prefix[0] + '_FFJ3', prefix[0] + '_RFJ3'] # set max and min joint positions min_pos_J3 = 0.0 max_pos_J3 = pi / 2
# ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or # FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for # more details. # # You should have received a copy of the GNU General Public License along # with this program. If not, see <http://www.gnu.org/licenses/>. # from sr_robot_commander.sr_hand_commander import SrHandCommander import rospy from burn_in_states import warehouse_states from sr_robot_commander.sr_robot_state_exporter import SrRobotStateExporter rospy.init_node("burn_in_test") commander = SrHandCommander() exporter = SrRobotStateExporter(warehouse_states) commander.move_to_named_target(warehouse_states["burn_4_hand"]) test_trajectory = exporter.convert_trajectory([{ 'name': "burn_1_hand", "interpolate_time": 0.5, 'pause_time': 1 }, { 'name': "burn_2_hand", "interpolate_time": 0.5, 'pause_time': 1 }, { 'name': "burn_3_hand", "interpolate_time": 0.5,
# are read and it is detected whether the hand is left or right and if there are tactiles present. # The correct prefix and parameters are then configured. import sys import rospy from sr_robot_commander.sr_hand_commander import SrHandCommander from sr_utilities.hand_finder import HandFinder rospy.init_node("basic_hand_examples", anonymous=True) hand_finder = HandFinder() hand_parameters = hand_finder.get_hand_parameters() hand_serial = hand_parameters.mapping.keys()[0] hand_commander = SrHandCommander(hand_parameters=hand_parameters, hand_serial=hand_serial) hand_mapping = hand_parameters.mapping[hand_serial] # Hand joints are detected joints = hand_finder.get_hand_joints()[hand_mapping] position_values = [0.35, 0.18, 0.38] # Moving to a target determined by the values in position_values. rospy.loginfo("Hand moving to script specified target") position_1 = dict(zip(joints, position_values)) hand_commander.move_to_joint_value_target(position_1) named_target_1 = "pack" rospy.loginfo("Hand moving to named target: " + named_target_1)
#!/usr/bin/env python # Script to move the left hand into store position. import rospy from sr_robot_commander.sr_hand_commander import SrHandCommander rospy.init_node("store_left_hand", anonymous=True) hand_commander = SrHandCommander(name="left_hand") open_hand = {'lh_FFJ1': 0.0, 'lh_FFJ2': 0.0, 'lh_FFJ3': 0.0, 'lh_FFJ4': 0.0, 'lh_MFJ1': 0.0, 'lh_MFJ2': 0.0, 'lh_MFJ3': 0.0, 'lh_MFJ4': 0.0, 'lh_RFJ1': 0.0, 'lh_RFJ2': 0.0, 'lh_RFJ3': 0.0, 'lh_RFJ4': 0.0, 'lh_LFJ1': 0.0, 'lh_LFJ2': 0.0, 'lh_LFJ3': 0.0, 'lh_LFJ4': 0.0, 'lh_LFJ5': 0.0, 'lh_THJ1': 0.0, 'lh_THJ2': 0.0, 'lh_THJ3': 0.0, 'lh_THJ4': 0.0, 'lh_THJ5': 0.0, 'lh_WRJ1': 0.0, 'lh_WRJ2': 0.0} pack_hand_1 = {'lh_FFJ1': 1.5707, 'lh_FFJ2': 1.5707, 'lh_FFJ3': 1.5707, 'lh_FFJ4': 0.0, 'lh_MFJ1': 1.5707, 'lh_MFJ2': 1.5707, 'lh_MFJ3': 1.5707, 'lh_MFJ4': 0.0, 'lh_RFJ1': 1.5707, 'lh_RFJ2': 1.5707, 'lh_RFJ3': 1.5707, 'lh_RFJ4': 0.0, 'lh_LFJ1': 1.5707, 'lh_LFJ2': 1.5707, 'lh_LFJ3': 1.5707, 'lh_LFJ4': 0.0, 'lh_LFJ5': 0.0} pack_hand_2 = {'lh_THJ4': 1.2} pack_hand_3 = {'lh_THJ1': 0.52, 'lh_THJ2': 0.61, 'lh_THJ5': 0.43} # Move hand to open position joint_states = open_hand rospy.loginfo("Moving hand to open position")
rospy.sleep(1) # Do not start at time zero def construct_trajectory_point(posture, duration): trajectory_point = JointTrajectoryPoint() trajectory_point.time_from_start = rospy.Duration.from_sec(float(duration)) for key in joint_trajectory.joint_names: trajectory_point.positions.append(posture[key]) return trajectory_point hand_finder = HandFinder() hand_parameters = hand_finder.get_hand_parameters() hand_serial = hand_parameters.mapping.keys()[0] hand_commander = SrHandCommander(hand_parameters=hand_parameters, hand_serial=hand_serial) hand_mapping = hand_parameters.mapping[hand_serial] # Hand joints are detected joints = hand_finder.get_hand_joints()[hand_mapping] open_hand = {'rh_FFJ1': 0.0, 'rh_FFJ2': 0.0, 'rh_FFJ3': 0.0, 'rh_FFJ4': 0.0, 'rh_MFJ1': 0.0, 'rh_MFJ2': 0.0, 'rh_MFJ3': 0.0, 'rh_MFJ4': 0.0, 'rh_RFJ1': 0.0, 'rh_RFJ2': 0.0, 'rh_RFJ3': 0.0, 'rh_RFJ4': 0.0, 'rh_LFJ1': 0.0, 'rh_LFJ2': 0.0, 'rh_LFJ3': 0.0, 'rh_LFJ4': 0.0, 'rh_LFJ5': 0.0, 'rh_THJ1': 0.0, 'rh_THJ2': 0.0, 'rh_THJ3': 0.0, 'rh_THJ4': 0.0, 'rh_THJ5': 0.0} keys = ['rh_FFJ1', 'rh_FFJ2', 'rh_FFJ3', 'rh_FFJ4', 'rh_LFJ1', 'rh_LFJ2', 'rh_LFJ3', 'rh_LFJ4', 'rh_LFJ5', 'rh_MFJ1', 'rh_MFJ2', 'rh_MFJ3',
import rospy from sr_robot_commander.sr_hand_commander import SrHandCommander from sr_utilities.hand_finder import HandFinder import geometry_msgs.msg import copy rospy.init_node("hand_example_following_finger_traj", anonymous=True) hand_finder = HandFinder() hand_parameters = hand_finder.get_hand_parameters() prefix = hand_parameters.mapping.values()[0] hand_serial = hand_parameters.mapping.keys()[0] # Close all fingers except thumb hand_commander = SrHandCommander() hand_commander.move_to_named_target("fingers_pack_thumb_open") # Change to first finger group group_id = prefix + "_first_finger" hand_commander = SrHandCommander(name=group_id) # Open first finger hand_commander.move_to_named_target("first_finger_open") rospy.sleep(4) rospy.loginfo("\nMoving Index finger following a set of waypoints") waypoints = []
#!/usr/bin/env python # This example demonstrates how to move the left hand and arm through a sequence of joint goals. # At the start and end of the sequence, both the hand and arm will spend 20s in teach mode, # This allows the user to manually move the hand and arm. New goals can be easily generated # using the script 'sr_print_joints_position.py # PLEASE NOTE: move_to_joint_value_target_unsafe is used in this script, so collision checks # between the hand and arm are not made! import rospy from sr_robot_commander.sr_arm_commander import SrArmCommander from sr_robot_commander.sr_hand_commander import SrHandCommander rospy.init_node("basic_hand_arm_example", anonymous=True) hand_commander = SrHandCommander(name="left_hand", prefix="lh") arm_commander = SrArmCommander(name="left_arm", set_ground=False) # sleep for some time during which the arm can be moved around by pushing it # but be careful to get away before the time runs out. You are warned rospy.loginfo("Set arm teach mode ON") arm_commander.set_teach_mode(True) rospy.sleep(20.0) rospy.loginfo("Set arm teach mode OFF") arm_commander.set_teach_mode(False) # Specify goals for hand and arm hand_joints_goal_1 = { 'lh_FFJ1': 0.35, 'lh_FFJ2': 0.0,
#!/usr/bin/env python # Example to demonstrate moving to stored/names targets. Both arm and hand movements executed. # Available named targets can be viewed in MoveIt, on the planning tab. import rospy from sr_robot_commander.sr_arm_commander import SrArmCommander from sr_robot_commander.sr_hand_commander import SrHandCommander rospy.init_node("grab_bottle_example", anonymous=True) hand_commander = SrHandCommander() arm_commander = SrArmCommander() print("Executing moves:") # start a bit higher # arm_commander.move_to_named_target("gamma", True) # approach print("Approach bottle") hand_commander.move_to_named_target("open_bottle", False) arm_commander.move_to_named_target("approach_bottle", True) # grab print("Hold bottle") arm_commander.move_to_named_target("hold_bottle", True) hand_commander.move_to_named_target("hold_bottle", True) # pour print("Pour")