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")
