def put_text(text, bg_color = (255,255,255), text_color = (0,0,0), scale=3, thickness=4):
"""
Write text to Sawyer's display screen
:param text: string- test to displya
:param bg_color: (b,g,r)- background color
:param text_color: (b,g,r)- text color
:param scale: float- font size
:param thickness: float- font thickness
:return:
"""
disp = intera_interface.HeadDisplay()
img = np.zeros(shape=[600, 1024, 3], dtype=np.uint8)
b, g, r = bg_color
img[:,:,0] = b
img[:, :, 1] = g
img[:, :, 2] = r
font = cv2.FONT_HERSHEY_DUPLEX
textsize = cv2.getTextSize(text, font, scale, thickness)[0]
textX = (img.shape[1] - textsize[0]) / 2
textY = (img.shape[0] + textsize[1]) / 2
img = cv2.putText(img, text, (textX, textY), font, scale, text_color, thickness)
cv2.imwrite('temp.png', img)
disp.display_image('temp.png')
os.remove('temp.png')
def displayFace(mode):
"""
Displays a face on the sawyer robot corresponding to a mode:
0: display a face for a Tie
1: display a face for a Win
2: display a face for a Loss
11: display a face for Robot's turn
22: display a face for the Player's turn
"""
_head = intera_interface.Head()
head_display = intera_interface.HeadDisplay()
rospack = rospkg.RosPack()
pkgpath = rospack.get_path('final-project-tic-tac-toe')
if mode == 0:
filepath = pkgpath + "/images/tie" + str(randrange(4)) + ".jpg"
if mode == 1:
filepath = pkgpath + "/images/win" + str(randrange(4)) + ".jpg"
if mode == 2:
filepath = pkgpath + "/images/lose.jpg"
if mode == 11:
filepath = pkgpath + "/images/board.jpg"
if mode == 22:
filepath = pkgpath + "/images/turn" + str(randrange(4)) + ".jpg"
head_display.display_image(filepath)
def talker():
pub = rospy.Publisher('Pickup_object', String, queue_size=10)
rospy.init_node('EMG_Interface', anonymous=True)
rate = rospy.Rate(10) # 10hz
head_display = intera_interface.HeadDisplay()
listener()
head_display.display_image(home_interface[0])
idx = 0
loop = False
global start_time
start_time = time.time()
while not rospy.is_shutdown():
global data_received
global val
#pub.publish("none")
if time.time() - start_time >= timeout:
val = None
if val is not None:
if loop == False:
if val.lower() == "next":
#data_received = True
if idx >= len(home_interface)-1:
idx = 1
else:
idx = idx+1
head_display.display_image(home_interface[idx])
#data_received = False
if val.lower() == "select":
idx1 = 0
#data_received = True
head_display.display_image(selected_image[idx][idx1])
#data_received = False
if idx != 0:
loop = True
else:
rospy.loginfo("Nothing selected")
loop = False
else:
#if time.time() - start_time >= timeout:
# val = None
if val is not None:
if val.lower() == "next":
if idx1 >= len(selected_image[idx])-1:
idx1 = 0
else:
idx1 = idx1+1
head_display.display_image(selected_image[idx][idx1])
if val.lower() == "select":
if idx1 == 0:
for i in range(0,10):
pub.publish(products[idx])
#rate.sleep()
head_display.display_image(home_interface[idx])
loop = False
if idx1 == 1:
data_received =False
head_display.display_image(home_interface[idx])
loop = False
def __init__(self, config=None):
"""Initialize.
See the parent class.
"""
super(FrankaPandaReal, self).__init__(config=config)
if rospy.get_name() == '/unnamed':
rospy.init_node('franka_panda')
assert rospy.get_name() != '/unnamed', 'Must call rospy.init_node().'
tic = time.time()
rospy.loginfo('Initializing robot...')
self._head = intera_interface.Head()
self._display = intera_interface.HeadDisplay()
self._lights = intera_interface.Lights()
self._limb = intera_interface.Limb()
self._joint_names = self._limb.joint_names()
self.cmd = []
self._command_msg = JointCommand()
self._command_msg.names = self._joint_names
self._commanders = dict(velocity=None, torque=None)
try:
self._gripper = intera_interface.Gripper()
self._has_gripper = True
except Exception:
self._has_gripper = False
self._robot_enable = intera_interface.RobotEnable(True)
self._params = intera_interface.RobotParams()
self._motion_planning = self.config.MOTION_PLANNING
if self._motion_planning == 'moveit':
rospy.loginfo('Initializing moveit toolkit...')
moveit_commander.roscpp_initialize(sys.argv)
self._scene = moveit_commander.PlanningSceneInterface()
self._group = moveit_commander.MoveGroupCommander('right_arm')
toc = time.time()
rospy.loginfo('Initialization finished after %.3f seconds.'
% (toc - tic))
def __init__(self, arm, name, start):
uri = "mongodb://*****:*****@sawyer-mongo.documents.azure.com:10255/?ssl=true"
client = MongoClient(uri)
self.db = client.SawyerDB
self.collection = self.db.Commands
self.commandName = name
self.commandNumber = start
rp = RobotParams()
self._lastJoin = {}
self.lastButtonPress = datetime.datetime.now()
self._rs = intera_interface.RobotEnable()
self._init_state = self._rs.state().enabled
print("Enabling robot... ")
self._rs.enable()
self._navigator_io = intera_interface.Navigator()
head_display = intera_interface.HeadDisplay()
head_display.display_image(
"/home/microshak/Pictures/Sawyer_Navigator_Main.png", False, 1.0)
valid_limbs = rp.get_limb_names()
self._arm = rp.get_limb_names()[0]
self._limb = intera_interface.Limb(arm)
if start == None:
limb = intera_interface.Limb("right")
limb.move_to_neutral()
print(self._arm)
# inputs
self._cuff = Cuff(limb='right')
self._navigator = Navigator()
# connect callback fns to signals
self._lights = None
self._lights = Lights()
self._cuff.register_callback(self._light_action,
'{0}_cuff'.format('right'))
try:
self._gripper = Gripper(self._arm)
#if not (self._gripper.is_calibrated() or self._gripper.calibrate() == True):
# rospy.logerr("({0}_gripper) calibration failed.".format(self._gripper.name))
# raise
except:
self._gripper = None
msg = ("{0} Gripper is not connected to the robot."
" Running cuff-light connection only.").format(
arm.capitalize())
rospy.logwarn(msg)
def __init__(self):
rospy.init_node("head_display", anonymous=True)
self.head_display = intera_interface.HeadDisplay()
# Load in camera image to show in display
rospy.Subscriber('/me495/raw_image', Image, self.img_callback)
# listen to button in Sawyer
rospy.Subscriber('io/robot/navigator/state', IODeviceStatus,
self.fun_callback)
# Subscribe to control node
self.state_camera = False
rospy.Subscriber("/me495/command", String, self.cmd_callback)
# Subscribe to camera data
rospy.Subscriber('/camera_driver/image_raw', Image,
self.camera_callback)
# Show in display
self.disp_pub = rospy.Publisher('robot/head_display',
Image,
latch=True,
queue_size=10)
# Load in fun images
self.fun = False
rospack = rospkg.RosPack()
image_path = rospack.get_path('me495_vision') + '/images/'
self.start_img = cv_bridge.CvBridge().cv2_to_imgmsg(
cv2.imread(image_path + 'start.jpg'), encoding="bgr8")
self.fun_img_1 = cv_bridge.CvBridge().cv2_to_imgmsg(
cv2.imread(image_path + 'normal.jpg'), encoding="bgr8")
self.fun_img_2 = cv_bridge.CvBridge().cv2_to_imgmsg(
cv2.imread(image_path + 'blink.jpg'), encoding="bgr8")
r = rospy.Rate(5)
# Show in display
self.useful_image = self.start_img
self.fun_change = False
#self.fun_time = rospy.Time.now()
while not rospy.is_shutdown():
if self.fun:
if self.fun_change == True:
self.disp_pub.publish(self.fun_img_2)
#if (rospy.Time.now() - self.fun_time).to_sec() > 0.2:
self.fun_change = False
else:
self.disp_pub.publish(self.fun_img_1)
else:
self.disp_pub.publish(self.useful_image)
r.sleep()
def __init__(self, Name):
CONNECTION_STRING = "HostName=RobotForman.azure-devices.net;DeviceId=PythonTest;SharedAccessKey=oh9Fj0mAMWJZpNNyeJ+bSecVH3cBQwbzjDnoVmeSV5g="
self.protocol = IoTHubTransportProvider.HTTP
self.client = IoTHubClient(CONNECTION_STRING, self.protocol)
self.client.set_option("messageTimeout", MESSAGE_TIMEOUT)
self.client.set_option("MinimumPollingTime", MINIMUM_POLLING_TIME)
self.client.set_message_callback(receive_message_callback,
RECEIVE_CONTEXT)
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('mid' + str(uuid.uuid4().hex), anonymous=True)
print 'mid' + str(uuid.uuid4().hex)
self.head_display = intera_interface.HeadDisplay()
self.head_display.display_image("/home/microshak/Pictures/Ready.png",
False, 1.0)
self.head = intera_interface.Head()
rp = RobotParams()
valid_limbs = rp.get_limb_names()
robot = moveit_commander.RobotCommander()
rp.max_velocity_scaling_factor = .5
scene = moveit_commander.PlanningSceneInterface()
self.group = moveit_commander.MoveGroupCommander("right_arm")
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
self.light = Lights()
self.headLight("green")
# rs = intera_interface.RobotEnable(CHECK_VERSION)
self.group.clear_pose_targets()
self.endeffector = intera_interface.Gripper("right")
self.uri = "mongodb://*****:*****@sawyer-mongo.documents.azure.com:10255/?ssl=true"
self.Mongoclient = MongoClient(self.uri)
if Name == None:
self.poleIoTHub()
else:
self.completeCommands(Name)
def init_sawyer():
""" basic setup stuff """
rospy.loginfo("initializing sawyer...")
limb = intera_interface.Limb('right')
hd = intera_interface.HeadDisplay()
head = intera_interface.Head()
limb.move_to_neutral(timeout=TIMEOUT)
# TODO: Hardocoded to jon's computer
hd.display_image(
"/home/jon/catkin_ws/src/sawyer_simulator/sawyer_gazebo/share/images/sawyer_sdk_research.png"
)
head.set_pan(0.0)
rospy.loginfo("sawyer initialized")
return limb, hd, head
def main():
"""RSDK Head Display Example:
Displays a given image file or multiple files on the robot's face.
Pass the relative or absolute file path to an image file on your
computer, and the example will read and convert the image using
cv_bridge, sending it to the screen as a standard ROS Image Message.
"""
epilog = """
Notes:
Max screen resolution is 1024x600.
Images are always aligned to the top-left corner.
Image formats are those supported by OpenCv - LoadImage().
"""
arg_fmt = argparse.RawDescriptionHelpFormatter
parser = argparse.ArgumentParser(formatter_class=arg_fmt,
description=main.__doc__,
epilog=epilog)
required = parser.add_argument_group('required arguments')
required.add_argument(
'-f',
'--file',
nargs='+',
help=
'Path to image file to send. Multiple files are separated by a space, eg.: a.png b.png'
)
parser.add_argument(
'-l',
'--loop',
action="store_true",
help='Display images in loop, add argument will display images in loop'
)
parser.add_argument(
'-r',
'--rate',
type=float,
default=1.0,
help='Image display frequency for multiple and looped images.')
args = parser.parse_args()
rospy.init_node("head_display_example", anonymous=True)
head_display = intera_interface.HeadDisplay()
head_display.display_image(args.file, args.loop, args.rate)
def headText(self, q):
t = 'x:{x} y:{y} z:{z} w:{w}'
template = {'x': q.x, 'y': q.y, 'z': q.z, 'w': q.w}
text = t.format(**template)
h = 600
w = 1024
img = np.zeros((h, w, 3), np.uint8)
img.fill(255)
#cv2.putText(img,text, (50,200), cv2.FONT_HERSHEY_SIMPLEX, 6, 100,14)
y0, dy = 150, 120
for i, line in enumerate(text.split(' ')):
y = y0 + i * dy
cv2.putText(img, line, (50, y), cv2.FONT_HERSHEY_SIMPLEX, 4, 2, 10)
cv2.imwrite("/home/microshak/Pictures/head.png", img)
head_display = intera_interface.HeadDisplay()
head_display.display_image("/home/microshak/Pictures/head.png", False,
1.0)
def __init__(self):
self._done = False
self._head = intera_interface.Head()
self._limb = intera_interface.Limb()
self._robot_state = intera_interface.RobotEnable(CHECK_VERSION)
self._init_state = self._robot_state.state().enabled
self._robot_state.enable()
self._display = intera_interface.HeadDisplay()
self._last_angles = self._limb.joint_angles()
self._last_head_pan = self._head.pan()
move_arm_out_of_way_service = rospy.Service('arm/move_away', MoveArm,
self.move_arm_out_of_way)
return_to_last_position = rospy.Service('arm/return_to_position',
MoveArm,
self.return_to_last_position)
move_to_rest = rospy.Service('arm/move_to_rest', MoveArm,
self.move_to_rest)
move_arm_to_rest = rospy.Service('arm/move_arm_to_rest', MoveArm,
self.move_arm_to_rest)
def display_png(img_name, bg_color=(255,255,255)):
"""
Displays a png image from imagaes/display directory on Sawyer's screen. Cannot handle images larger than
1024 x 600.
Good emojis here: https://emojiisland.com/pages/free-download-emoji-icons-png
:param img_name: (string) - name of image to display
:param bg_color: (b, g, r) color to make background
"""
disp = intera_interface.HeadDisplay()
img = np.zeros(shape=[600, 1024, 3], dtype=np.uint8)
b, g, r = bg_color
img[:,:,0] = b
img[:, :, 1] = g
img[:, :, 2] = r
# sloppy way to get paths to display images- don't use but may be useful in other nodes
img_dir = os.path.dirname(os.path.dirname(os.path.dirname(os.path.abspath(__file__))))
img_dir = os.path.join(img_dir, 'images', 'display')
img_path = os.path.join(img_dir, img_name)
pic = cv2.imread(img_path, cv2.IMREAD_UNCHANGED)
alpha = pic[:, :, 3]
h, w = alpha.shape
for x in range(h):
for y in range(w):
if not alpha[x, y]:
pic[x, y] = (b, g, r, 1)
offh = int((img.shape[0] - pic.shape[0]) / 2)
offw = int((img.shape[1] - pic.shape[1]) / 2)
img[offh:(offh + pic.shape[0]), offw:(offw + pic.shape[1]), :] = pic[:, :, :3]
cv2.imwrite('temp.png', img)
disp.display_image('temp.png')
os.remove('temp.png')
def __init__(self):
# Initialize Sawyer
rospy.init_node('Sawyer_comm_node', anonymous=True)
intera_interface.HeadDisplay().display_image('logo.png')
# Publishing topics
suppress_cuff_interaction = rospy.Publisher('/robot/limb/right/suppress_cuff_interaction', Empty, queue_size=1)
self.endpoint_topic = rospy.Publisher('/EndpointInfo', EndpointInfo, queue_size=10)
# Subscribing topics
rospy.Subscriber('/robot/limb/right/endpoint_state', intera_core_msgs.msg.EndpointState, self.forwardEndpointState)
# Lights
self.lights = intera_interface.Lights()
for light in self.lights.list_all_lights():
self.lights.set_light_state(light,False)
# Initialization complete. Spin.
rospy.loginfo('Ready.')
r = rospy.Rate(10)
while not rospy.is_shutdown():
suppress_cuff_interaction.publish()
r.sleep()
def main():
rp = intera_interface.RobotParams()
valid_limbs = rp.get_limb_names()
rospy.init_node('doctor', log_level=rospy.DEBUG)
rs = intera_interface.RobotEnable(CHECK_VERSION)
head_display = intera_interface.HeadDisplay()
head_display.display_image("./head_display_pics/redcross.png", False, 1.0)
init_state = rs.state().enabled
rs.enable()
rospy.logdebug("Closing gripper...")
#raw_input("Press any key to close gripper")
right_gripper = intera_interface.gripper.Gripper('right')
right_gripper.calibrate()
#rospy.sleep(2.0)
right_gripper.open()
#raw_input("Press any key to close gripper")
right_gripper.close()
#rospy.sleep(2.0)
rospy.logdebug("Gripper closed")
doctor = doctor_sawyer(valid_limbs[0])
doctor.find_table()
while (not rospy.is_shutdown()):
action = raw_input("(P)oke, (M)easurement?")
if (action == "P"):
print(doctor.probe(2, 2, 0.02, 0.02))
if (action == "M"):
doctor.start_measure()
if (action == "P1"):
doctor.probe(0, 0, 0.02, 0.02)
rospy.spin()
def __init__(self):
# Initialize Sawyer
rospy.init_node('Sawyer_comm_node', anonymous=True)
intera_interface.HeadDisplay().display_image('logo.png')
# Publishing topics
suppress_cuff_interaction = rospy.Publisher(
'/robot/limb/right/suppress_cuff_interaction', Empty, queue_size=1)
self.position_topic = rospy.Publisher('/position',
JointInfo,
queue_size=1)
self.velocity_topic = rospy.Publisher('/velocity',
JointInfo,
queue_size=1)
self.effort_topic = rospy.Publisher('/effort', JointInfo, queue_size=1)
self.dev_topic = rospy.Publisher('/dev_topic', Int32, queue_size=10)
self.scroll_wheel_button_topic = rospy.Publisher(
'/scroll_wheel_button_topic', Empty, queue_size=10)
self.command_complete_topic = rospy.Publisher('/command_complete',
Empty,
queue_size=1)
self.wheel_delta_topic = rospy.Publisher('/wheel_delta',
Int32,
queue_size=10)
self.clicked = rospy.Publisher('scroll_wheel_pressed',
Bool,
queue_size=10)
self.toggle_completed_topic = rospy.Publisher('/toggle_completed',
Empty,
queue_size=1)
self.highTwo_success_topic = rospy.Publisher('/highTwo_success',
Bool,
queue_size=1)
self.endpoint_topic = rospy.Publisher('/EndpointInfo',
EndpointInfo,
queue_size=10)
self.interaction_control_topic = rospy.Publisher('/InteractionControl',
InteractionControl,
queue_size=10)
self.pickedup_topic = rospy.Publisher('/pickedup', Bool, queue_size=1)
self.pos_orient_topic = rospy.Publisher('/pos_orient',
String,
queue_size=1)
# Subscribing topics
rospy.Subscriber('/audio_duration', Float64, self.rx_audio_duration)
rospy.Subscriber('/robot/joint_states', sensor_msgs.msg.JointState,
self.forwardJointState)
rospy.Subscriber('/GoToJointAngles', GoToJointAngles,
self.cb_GoToJointAngles)
rospy.Subscriber('/wheel_subscription', Bool,
self.cb_WheelSubscription)
rospy.Subscriber('/joint_move', joint_move, self.cb_joint_move)
rospy.Subscriber('/InteractionControl', InteractionControl,
self.cb_interaction)
rospy.Subscriber('/JointAngle', String, self.cb_joint_angle)
rospy.Subscriber('/JointImpedance', JointImpedance, self.cb_impedance)
rospy.Subscriber('/multiple_choice', Bool, self.cb_multiple_choice)
rospy.Subscriber('/highTwo', Bool, self.cb_highTwo)
rospy.Subscriber('/robot/limb/right/endpoint_state',
intera_core_msgs.msg.EndpointState,
self.forwardEndpointState)
rospy.Subscriber('/adjustPoseTo', adjustPoseTo, self.cb_adjustPoseTo)
rospy.Subscriber('/closeGripper', Bool, self.cb_closeGripper)
rospy.Subscriber('/openGripper', Bool, self.cb_openGripper)
rospy.Subscriber('/GoToCartesianPose', GoToCartesianPose,
self.cb_GoToCartesianPose)
rospy.Subscriber('/cuffInteraction', cuffInteraction,
self.cb_cuffInteraction)
rospy.Subscriber('/check_pickup', Bool, self.cb_check_pickup)
rospy.Subscriber('/adjustPoseBy', adjustPoseBy, self.cb_adjustPoseBy)
rospy.Subscriber('/camera', Bool, self.cb_camera)
rospy.Subscriber('/robot/digital_io/right_lower_button/state',
intera_core_msgs.msg.DigitalIOState,
self.cb_cuff_lower)
rospy.Subscriber('/robot/digital_io/right_upper_button/state',
intera_core_msgs.msg.DigitalIOState,
self.cb_cuff_upper)
# Global Vars
self.audio_duration = 0
self.finished = False
self.startPos = 0
self.devMode = False
self.seqArr = []
self.p_command = lambda self: self.pub_num(180 / math.pi * self.limb.
joint_angle(shoulder))
wheel = self.finished
b_less = lambda self: self.limb.joint_angle(shoulder) < point_b[0]
b_more = lambda self: self.limb.joint_angle(shoulder) > point_b[0]
dot_2 = lambda self: self.limb.joint_angle(shoulder) < joint_dot_2[0]
#endpoint = lambda self : self.endpoints_equal(self.limb.endpoint_pose(),dot_3,tol) or self.finished
# Limb
self.limb = LimbPlus()
self.limb.go_to_joint_angles()
# Lights
self.lights = intera_interface.Lights()
for light in self.lights.list_all_lights():
self.lights.set_light_state(light, False)
# Navigator
self.navigator = intera_interface.Navigator()
self.multi_choice_callback_ids = self.BUTTON.copy()
for key in self.multi_choice_callback_ids:
self.multi_choice_callback_ids[key] = -1
self.wheel_callback_id = -1
self.wheel_state = self.navigator.get_wheel_state('right_wheel')
self.navigator.register_callback(self.rx_finished, 'right_button_ok')
self.navigator.register_callback(self.rx_cheat_code, 'head_button_ok')
# Gripper
self.gripper = intera_interface.get_current_gripper_interface()
if isinstance(self.gripper, intera_interface.SimpleClickSmartGripper):
if self.gripper.needs_init():
self.gripper.initialize()
else:
if not (self.gripper.is_calibrated()
or self.gripper.calibrate() == True):
raise
self.open_gripper()
# Cuff
self.cuff = intera_interface.Cuff()
self.cuff_callback_ids = self.CUFF_BUTTON.copy()
for key in self.cuff_callback_ids:
self.cuff_callback_ids[key] = -1
# Initialization complete. Spin.
rospy.loginfo('Ready.')
r = rospy.Rate(10)
while not rospy.is_shutdown():
suppress_cuff_interaction.publish()
r.sleep()
def __init__(self,
config='cfg/sawyer_ctrl_config.yaml',
controlType='EEImpedance',
use_safenet=False,
node_name='sawyer_interface'):
"""
Initialize Sawyer Robot for control
Args:
use_safenet: bool
whether to use the Safenet to constrain ee positions
and velocities
node_name: str
name of the node for ROS
Attributes:
ee_position
ee_orientation
"""
self.log_start_times = []
self.log_end_times = []
self.cmd_start_times = []
self.cmd_end_times = []
# Connect to redis client
# use default port 6379 at local host.
self.config = YamlConfig(config)
self.redisClient = RobotRedis(**self.config['redis'])
self.redisClient.flushall()
# Timing
self.startTime = time.time()
self.endTime = time.time()
self.action_set = False
self.model = Model()
self.ee_name = self.config['sawyer']['end_effector_name']
if config is not None:
if self.config['controller']['interpolator']['type'] == 'linear':
interp_kwargs = {
'max_dx': 0.005,
'ndim': 3,
'controller_freq': 500,
'policy_freq': 20,
'ramp_ratio': 0.2
}
self.interpolator_pos = LinearInterpolator(**interp_kwargs)
self.interpolator_ori = LinearOriInterpolator(**interp_kwargs)
rospy.loginfo(
"Linear interpolator created with params {}".format(
interp_kwargs))
else:
self.interpolator_pos = None
self.interpolator_ori = None
self.interpolator_goal_set = False
start = time.time()
rospy.loginfo('Initializing Sawyer robot interface...')
try:
self._head = iif.Head()
self._display = iif.HeadDisplay()
self._lights = iif.Lights()
self._has_head = True
except:
rospy.logerr('No head found, head functionality disabled')
self._has_head = False
self._limb = iif.Limb(limb="right", synchronous_pub=False)
self._joint_names = self._limb.joint_names()
self.cmd = []
try:
self._gripper = iif.Gripper()
self._has_gripper = True
except:
rospy.logerr('No gripper found, gripper control disabled')
self._has_gripper = False
self._robot_enable = iif.RobotEnable(True)
self._params = iif.RobotParams()
self.blocking = False
# set up internal pybullet simulation for jacobian and mass matrix calcs.
self._clid = pb.connect(pb.DIRECT)
pb.resetSimulation(physicsClientId=self._clid)
# TODO: make this not hard coded
sawyer_urdf_path = self.config['sawyer']['arm']['path']
self._pb_sawyer = pb.loadURDF(
fileName=sawyer_urdf_path,
basePosition=self.config['sawyer']['arm']['pose'],
baseOrientation=pb.getQuaternionFromEuler(
self.config['sawyer']['arm']['orn']),
globalScaling=1.0,
useFixedBase=self.config['sawyer']['arm']['is_static'],
flags=pb.URDF_USE_SELF_COLLISION_EXCLUDE_PARENT
| pb.URDF_USE_INERTIA_FROM_FILE,
physicsClientId=self._clid)
# For pybullet dof
self._motor_joint_positions = self.get_motor_joint_positions()
try:
ns = "ExternalTools/right/PositionKinematicsNode/IKService"
self._iksvc = rospy.ServiceProxy(ns, SolvePositionIK)
rospy.wait_for_service(ns, 5.0)
self._ik_service = True
except:
rospy.logerr('IKService from Intera timed out')
self._ik_service = False
self._joint_names = self.config['sawyer']['limb_joint_names']
self.free_joint_dict = self.get_free_joint_dict()
self.joint_dict = self.get_joint_dict()
self._link_id_dict = self.get_link_dict()
rospy.loginfo('Sawyer initialization finished after {} seconds'.format(
time.time() - start))
# Set desired pose to initial
self.neutral_joint_position = self.config['sawyer'][
'neutral_joint_angles'] #[0, -1.18, 0.00, 2.18, 0.00, 0.57, 3.3161]
self.prev_cmd = np.asarray(self.neutral_joint_position)
self.current_cmd = self.prev_cmd
self.reset_to_neutral()
self.default_control_type = self.config['controller']['selected_type']
self.default_params = self.config['controller']['Real'][
self.default_control_type]
self.redisClient.set(CONTROLLER_CONTROL_TYPE_KEY,
self.default_control_type)
self.redisClient.set(CONTROLLER_CONTROL_PARAMS_KEY,
json.dumps(self.default_params))
self.pb_ee_pos_log = []
self.pb_ee_ori_log = []
self.pb_ee_v_log = []
self.pb_ee_w_log = []
# Set initial redis keys
self._linear_jacobian = None
self._angular_jacobian = None
self._jacobian = None
self._calc_mass_matrix()
self._calc_jacobian()
self.update_redis()
self.update_model()
self.controlType = self.get_control_type()
self.control_dict = self.get_controller_params()
self.controller = self.make_controller_from_redis(
self.controlType, self.control_dict)
self.redisClient.set(ROBOT_CMD_TSTAMP_KEY, time.time())
self.redisClient.set(ROBOT_SET_GRIPPER_CMD_KEY, 0.1)
self.redisClient.set(ROBOT_SET_GRIPPER_CMD_TSTAMP_KEY, time.time())
self.last_cmd_tstamp = self.get_cmd_tstamp()
self.last_gripper_cmd_tstamp = self.get_gripper_cmd_tstamp()
self.prev_gripper_state = self.des_gripper_state
rospy.logdebug('Control Interface initialized')
# TIMING TEST ONLY
self.timelog_start = open('cmd_tstamp.txt', 'w')
self.timelog_end = open('cmd_set_time.txt', 'w')
self.controller_times = []
self.loop_times = []
self.cmd_end_time = []
import tf.transformations as tr
import tf
import rospy
from std_msgs.msg import Bool, Int32, Float64
from geometry_msgs.msg import Pose, Point, Quaternion
from urdf_parser_py.urdf import URDF
from pykdl_utils.kdl_kinematics import KDLKinematics
import PyKDL
import os
os.chdir("/home/irobot/catkin_ws/src/ddpg/scripts")
rospy.init_node('pose_acquisition')
limb = intera_interface.Limb("right")
head_display = intera_interface.HeadDisplay()
head_display.display_image('/home/irobot/Downloads' + "/Cute.png")
cuff = intera_interface.Cuff()
tf_listenser = tf.TransformListener()
endpt_ori = [0.0, 0.0, 0.0, 0.0]
endpt_pos = [0.0, 0.0, 0.0]
position_x = list()
position_y = list()
position_z = list()
orient_x = list()
orient_y = list()
orient_z = list()
orient_w = list()
roll = list()
pitch = list()
def __init__(self):
print "Initializing Node"
rospy.init_node('poirot_rrhost')
print "Enabling Robot"
rs = intera_interface.RobotEnable()
rs.enable()
self.rs1 = intera_interface.RobotEnable()
rospy.Subscriber("spacenav/joy", Joy, self.SpaceNavJoyCallback)
rospy.Subscriber("/robot/digital_io/right_lower_cuff/state",
DigitalIOState, self.CuffCallback)
rospy.Subscriber("/robot/digital_io/right_button_ok/state",
DigitalIOState, self.NavWheelCallback)
rospy.Subscriber("/robot/digital_io/right_button_triangle/state",
DigitalIOState, self.XCallback)
self.DIOpub = rospy.Publisher('/io/comms/io/command',
IOComponentCommand,
queue_size=10)
self.h = IOComponentCommand()
self.h.time = rospy.Time.now()
self.h.op = "set"
self.h.args = "{ \"signals\" : { \"port_sink_0\" : { \"format\" : { \"type\" : \"int\", \"dimensions\" : [ 1] }, \"data\" : [ 0] } } }"
self.nav = intera_interface.Navigator()
#self.nav.register_callback(self.ok_pressed, '_'.join([nav_name, 'button_ok']))
#intera_interface/robot/digital_io/right_lower_cuff/state
# self._valid_limb_names = {'left': 'left',
# 'l': 'left',
# 'right': 'right',
# 'r': 'right'}
self._valid_limb_names = {'right': 'right', 'r': 'right'}
# get information from the SDK
# self._left = intera_interface.Limb('left')
self._right = intera_interface.Limb('right')
#self._l_jnames = self._left.joint_names()
self._r_jnames = self._right.joint_names()
self.kin = sawyer_kinematics('right')
# data initializations
self._jointpos = [0] * 7
self._jointvel = [0] * 7
self._jointtor = [0] * 7
self._ee_pos = [0] * 3
self._ee_or = [0] * 4
self._ee_tw = [0] * 6
self._ee_wr = [0] * 6
self._ee_vel = [0] * 6
self._pijac = [
] #numpy.matrix('0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0')
self._jac = [
] #numpy.matrix('0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0')
self._inertia_mat = [
] #numpy.matrix('0,0,0,0,0,0,0;0,0,0,0,0,0,0;0,0,0,0,0,0,0;0,0,0,0,0,0,0;0,0,0,0,0,0,0;0,0,0,0,0,0,0;0,0,0,0,0,0,0')
self.Jxl_raw = 0
self.Jyl_raw = 0
self.Jzl_raw = 0
self.Jxa_raw = 0
self.Jya_raw = 0
self.Jza_raw = 0
self.leftbutton = 0
self.rightbutton = 0
self.spacenav = []
#self._l_joint_command = dict(zip(self._l_jnames,[0.0]*7))
self._r_joint_command = dict(zip(self._r_jnames, [0.0] * 7))
self.MODE_POSITION = 0
self.MODE_VELOCITY = 1
self.MODE_TORQUE = 2
self._mode = self.MODE_POSITION
self.RMH_delay = .01
# initial joint command is current pose
self.readJointPositions()
#self.setJointCommand('left',self._jointpos[0:7])
self.setJointCommand('right', self._jointpos[0:7])
self.head_display = intera_interface.HeadDisplay()
print('head display!')
self.image = "/home/rachel/ros_ws/src/sawyer_simulator/sawyer_sim_examples/models/cafe_table/materials/textures/Wood_Floor_Dark.jpg"
self.head_display.display_image(self.image, display_rate=100)
print('finished first head display')
# Start background threads
self._running = True
self._t_joints = threading.Thread(target=self.jointspace_worker)
self._t_joints.daemon = True
self._t_joints.start()
self._t_effector = threading.Thread(target=self.endeffector_worker)
self._t_effector.daemon = True
self._t_effector.start()
self._t_command = threading.Thread(target=self.command_worker)
self._t_command.daemon = True
self._t_command.start()
self._t_display = threading.Thread(target=self.display_worker)
self._t_display.daemon = True
self._t_display.start()
self._t_dio = threading.Thread(target=self.dio_worker)
self._t_dio.daemon = True
self._t_dio.start()
def display_image():
global g_image_path
head_display = intera_interface.HeadDisplay()
head_display.display_image(g_image_path)
def main():
# Initialize the node
rospy.init_node('MoveCartesian')
# Initialize interfaces
limb = intera_interface.Limb('right')
head_display = intera_interface.HeadDisplay()
try:
gripper = intera_interface.Gripper('right_gripper')
gripper.calibrate()
nsteps = 5.0 # Increase it for a finer motion
dgripper = gripper.MAX_POSITION / nsteps
except ValueError:
rospy.logerr("Could not detect a gripper")
return
# Set the path for images
folder = str(packages.get_pkg_dir('sawyer_utec')) + '/images/'
# Diplay the UTEC logo in the robot head
head_display.display_image(folder + 'up1.jpg', False, 1.0)
# Move arm to the initial position
limb.move_to_neutral()
jangles_neutral = limb.joint_angles()
if (False): print jangles_neutral
# Open the gripper
gripper.open()
# ===================
# Motion for object 1
# ===================
# PRIMER PISO
# VASO 1 MOD
# pose_init1 = ((0.6, 0.50, 0.03), quat_init)
# pose_final1 = ((0.6, -0.5, 0.02), quat_final)
# VASO 2 MOD
# pose_initial2 = ((0.6, 0.40, 0.03), quat_init)
# pose_final2 = ((0.6, -0.42, 0.02), quat_final)
# VASO 3
# pose_initial3 = ((0.6, 0.32, 0.03), quat_init)
# pose_final3 = ((0.6, -0.34, 0.02), quat_final)
# SEGUNDO PISO
# VASO 4
# pose_initial4 = ((0.7, 0.5, 0.03), quat_init)
# pose_final4 = ((0.6, -0.46, 0.12), quat_final)
# VASO 5
# pose_initial5 = ((0.7, 0.4, 0.03), quat_init)
# pose_final5 = ((0.6, -0.38, 0.12), quat_final)
# VASO 6
# pose_initial = ((0.7, 0.4, 0.03), quat_init)
# pose_final = ((0.6, -0.38, 0.12), quat_final)
# Initial and final poses of the object
quat_init = quaternionFromAxisAngle(180.0, (0.0, 1.0, 0.0))
offset_table_z = -0.02
# vaso 1
pose_initial1 = ((0.6, 0.50, 0.03 - offset_table_z), quat_init)
quat_final = quaternionFromAxisAngle(180.0, (0.0, 1.0, 0.0))
pose_final1 = ((0.6, -0.5, 0.02 - offset_table_z), quat_final)
# Gripper opening (0*dgripper [closed] to nsteps*dgripper [open])
gripper_opening = 3.5 * dgripper
# Offset in z (from the desired position) for pre-grasping
z_pre_grasp = 0.20
pick_place(limb, gripper, pose_initial1, pose_final1, gripper_opening,
z_pre_grasp, jangles_neutral)
# # vaso 2
# quat_init2 = quaternionFromAxisAngle(180.0, (0.0, 1.0, 0.0))
# pose_initial2 = ((0.6, 0.40, 0.03 - offset_table_z), quat_init)
# quat_final2 = quaternionFromAxisAngle(180.0, (0.0, 1.0, 0.0))
# pose_final2 = ((0.6, -0.42, 0.02 - offset_table_z), quat_final)
# # Gripper opening (0*dgripper [closed] to nsteps*dgripper [open])
# gripper_opening = 3.5*dgripper
# # Offset in z (from the desired position) for pre-grasping
# z_pre_grasp = 0.20
# pick_place(limb, gripper, pose_initial2, pose_final2, gripper_opening, z_pre_grasp, jangles_neutral)
# # vaso 3
# quat_init = quaternionFromAxisAngle(180.0, (0.0, 1.0, 0.0))
# pose_initial3 = ((0.6, 0.32, 0.03 - offset_table_z), quat_init)
# quat_final = quaternionFromAxisAngle(180.0, (0.0, 1.0, 0.0))
# pose_final3 = ((0.6, -0.34, 0.02 - offset_table_z), quat_final)
# # Gripper opening (0*dgripper [closed] to nsteps*dgripper [open])
# gripper_opening = 3.5*dgripper
# # Offset in z (from the desired position) for pre-grasping
# z_pre_grasp = 0.20
# pick_place(limb, gripper, pose_initial3, pose_final3, gripper_opening, z_pre_grasp, jangles_neutral)
# # vaso 4
# quat_init = quaternionFromAxisAngle(180.0, (0.0, 1.0, 0.0))
# pose_initial4 = ((0.7, 0.5, 0.03 - offset_table_z), quat_init)
# quat_final = quaternionFromAxisAngle(180.0, (0.0, 1.0, 0.0))
# pose_final4 = ((0.6, -0.46, 0.12 - offset_table_z), quat_final)
# # Gripper opening (0*dgripper [closed] to nsteps*dgripper [open])
# gripper_opening = 3.5*dgripper
# # Offset in z (from the desired position) for pre-grasping
# z_pre_grasp = 0.20
# pick_place(limb, gripper, pose_initial4, pose_final4, gripper_opening, z_pre_grasp, jangles_neutral)
# # vaso 5
# quat_init = quaternionFromAxisAngle(180.0, (0.0, 1.0, 0.0))
# pose_initial5 = ((0.7, 0.4, 0.03 - offset_table_z), quat_init)
# quat_final = quaternionFromAxisAngle(180.0, (0.0, 1.0, 0.0))
# pose_final5 = ((0.6, -0.38, 0.12 - offset_table_z), quat_final)
# # Gripper opening (0*dgripper [closed] to nsteps*dgripper [open])
# gripper_opening = 3.5*dgripper
# # Offset in z (from the desired position) for pre-grasping
# z_pre_grasp = 0.20
# pick_place(limb, gripper, pose_initial5, pose_final5, gripper_opening, z_pre_grasp, jangles_neutral)
# #vaso 6
# quat_init = quaternionFromAxisAngle(180.0, (0.0, 1.0, 0.0))
# pose_initial = ((0.7, 0.32, 0.03 - offset_table_z), quat_init)
# quat_final = quaternionFromAxisAngle(180.0, (0.0, 1.0, 0.0))
# pose_final = ((0.6, -0.42, 0.22 - offset_table_z), quat_final)
# # Gripper opening (0*dgripper [closed] to nsteps*dgripper [open])
# gripper_opening = 3.5*dgripper
# # Offset in z (from the desired position) for pre-grasping
# z_pre_grasp = 0.20
# pick_place(limb, gripper, pose_initial, pose_final, gripper_opening, z_pre_grasp, jangles_neutral)
limb.move_to_neutral()
jangles_neutral = limb.joint_angles()
if (False): print jangles_neutral
# Display another face
head_display.display_image(folder + 'sleep1.png', False, 1.0)
def main():
# Initialize the node
rospy.init_node('DemoVasos')
# Initialize interfaces
limb = intera_interface.Limb('right')
head_display = intera_interface.HeadDisplay()
try:
gripper = intera_interface.Gripper('right_gripper')
gripper.calibrate()
nsteps = 5.0 # Increase it for a finer motion
dgripper = gripper.MAX_POSITION / nsteps
except ValueError:
rospy.logerr("Could not detect a gripper")
return
# Set the path for images
folder = str(packages.get_pkg_dir('sawyer_utec')) + '/images/'
# Diplay the UTEC logo in the robot head
head_display.display_image(folder + 'frente.jpg', False, 1.0)
# Set camera and tag searcher
camera_setup()
tags = tag_searcher()
# Move arm to the initial position
limb.move_to_neutral()
jangles_neutral = limb.joint_angles()
if (False): print jangles_neutral
# Open the gripper
gripper.open()
# Initial and final poses of the object
quat_init = quaternionFromAxisAngle(180.0, (0.0, 1.0, 0.0))
quat_rot = quaternionFromAxisAngle(90.0, (0.0, 0.0, 1.0))
quat_init = quaternionMult(quat_init, quat_rot)
offset_table_z = -0.05
# vaso 1
pose_initial1 = ((0.6, 0.4, 0.03 - offset_table_z), quat_init)
quat_tmp = quaternionFromAxisAngle(180.0, (0.0, 1.0, 0.0))
quat_final = quaternionMult(quat_tmp,
quaternionFromAxisAngle(90.0, (0.0, 0.0, 1.0)))
pose_final1 = ((0.6, -0.5, 0.02 - offset_table_z), quat_final)
# Gripper opening (0*dgripper [closed] to nsteps*dgripper [open])
gripper_opening = 3.5 * dgripper
# Offset in z (from the desired position) for pre-grasping
z_pre_grasp = 0.17
pick_place_autonomous(limb, gripper, pose_initial1, pose_final1,
gripper_opening, z_pre_grasp, jangles_neutral, tags,
head_display)
# vaso 2
quat_init2 = quaternionFromAxisAngle(180.0, (0.0, 1.0, 0.0))
pose_initial2 = ((0.6, 0.40, 0.03 - offset_table_z), quat_init)
quat_tmp = quaternionFromAxisAngle(180.0, (0.0, 1.0, 0.0))
quat_final2 = quaternionMult(
quat_tmp, quaternionFromAxisAngle(90.0, (0.0, 0.0, 1.0)))
# pose_final2 = ((0.6, -0.42, 0.02 - offset_table_z), quat_final2)
pose_final2 = ((0.6, -0.38, 0.02 - offset_table_z), quat_final2)
# Gripper opening (0*dgripper [closed] to nsteps*dgripper [open])
gripper_opening = 3.5 * dgripper
# Offset in z (from the desired position) for pre-grasping
z_pre_grasp = 0.17
pick_place_autonomous(limb, gripper, pose_initial2, pose_final2,
gripper_opening, z_pre_grasp, jangles_neutral, tags,
head_display)
# vaso 3
quat_init = quaternionFromAxisAngle(180.0, (0.0, 1.0, 0.0))
pose_initial3 = ((0.6, 0.40, 0.03 - offset_table_z), quat_init)
quat_tmp = quaternionFromAxisAngle(180.0, (0.0, 1.0, 0.0))
quat_final3 = quaternionMult(
quat_tmp, quaternionFromAxisAngle(90.0, (0.0, 0.0, 1.0)))
# pose_final3 = ((0.6, -0.34, 0.02 - offset_table_z), quat_final3)
pose_final3 = ((0.6, -0.26, 0.02 - offset_table_z), quat_final3)
# Gripper opening (0*dgripper [closed] to nsteps*dgripper [open])
gripper_opening = 3.5 * dgripper
# Offset in z (from the desired position) for pre-grasping
z_pre_grasp = 0.17
pick_place_autonomous(limb, gripper, pose_initial3, pose_final3,
gripper_opening, z_pre_grasp, jangles_neutral, tags,
head_display)
# vaso 4
quat_init = quaternionFromAxisAngle(180.0, (0.0, 1.0, 0.0))
pose_initial4 = ((0.6, 0.40, 0.03 - offset_table_z), quat_init)
quat_tmp = quaternionFromAxisAngle(180.0, (0.0, 1.0, 0.0))
quat_final4 = quaternionMult(
quat_tmp, quaternionFromAxisAngle(90.0, (0.0, 0.0, 1.0)))
# pose_final4 = ((0.6, -0.46, 0.12 - offset_table_z), quat_final4)
pose_final4 = ((0.5, -0.5, 0.02 - offset_table_z), quat_final4)
# Gripper opening (0*dgripper [closed] to nsteps*dgripper [open])
gripper_opening = 3.5 * dgripper
# Offset in z (from the desired position) for pre-grasping
z_pre_grasp = 0.17
pick_place_autonomous(limb, gripper, pose_initial4, pose_final4,
gripper_opening, z_pre_grasp, jangles_neutral, tags,
head_display)
# vaso 5
quat_init = quaternionFromAxisAngle(180.0, (0.0, 1.0, 0.0))
pose_initial5 = ((0.6, 0.40, 0.03 - offset_table_z), quat_init)
quat_tmp = quaternionFromAxisAngle(180.0, (0.0, 1.0, 0.0))
quat_final5 = quaternionMult(
quat_tmp, quaternionFromAxisAngle(90.0, (0.0, 0.0, 1.0)))
#pose_final5 = ((0.6, -0.38, 0.12 - offset_table_z), quat_final5)
pose_final5 = ((0.5, -0.38, 0.02 - offset_table_z), quat_final5)
# Gripper opening (0*dgripper [closed] to nsteps*dgripper [open])
gripper_opening = 3.5 * dgripper
# Offset in z (from the desired position) for pre-grasping
z_pre_grasp = 0.17
pick_place_autonomous(limb, gripper, pose_initial5, pose_final5,
gripper_opening, z_pre_grasp, jangles_neutral, tags,
head_display)
#vaso 6
quat_init = quaternionFromAxisAngle(180.0, (0.0, 1.0, 0.0))
pose_initial = ((0.6, 0.40, 0.03 - offset_table_z), quat_init)
quat_tmp = quaternionFromAxisAngle(180.0, (0.0, 1.0, 0.0))
quat_final6 = quaternionMult(
quat_tmp, quaternionFromAxisAngle(90.0, (0.0, 0.0, 1.0)))
# pose_final = ((0.6, -0.42, 0.22 - offset_table_z), quat_final6)
pose_final = ((0.5, -0.26, 0.02 - offset_table_z), quat_final6)
# Gripper opening (0*dgripper [closed] to nsteps*dgripper [open])
gripper_opening = 3.5 * dgripper
# Offset in z (from the desired position) for pre-grasping
z_pre_grasp = 0.17
pick_place_autonomous(limb, gripper, pose_initial, pose_final,
gripper_opening, z_pre_grasp, jangles_neutral, tags,
head_display)
limb.move_to_neutral()
jangles_neutral = limb.joint_angles()
if (False): print jangles_neutral
# Display another face
head_display.display_image(folder + 'sleep1.png', False, 1.0)
def init_head_dis(self):
head_display = intera_interface.HeadDisplay()
head_display.display_image("bob.jpg")
def __init__(self):
print "Initializing Node"
rospy.init_node('poirot_rrhost')
print "Enabling Robot"
rs = intera_interface.RobotEnable()
rs.enable()
self.rs1 = intera_interface.RobotEnable()
rospy.Subscriber("spacenav/joy", Joy, self.SpaceNavJoyCallback)
rospy.Subscriber("/robot/digital_io/right_lower_cuff/state",
DigitalIOState, self.CuffCallback)
rospy.Subscriber("/robot/digital_io/right_button_ok/state",
DigitalIOState, self.NavWheelCallback)
rospy.Subscriber("/robot/digital_io/right_button_triangle/state",
DigitalIOState, self.XCallback)
rospy.Subscriber("/robot/digital_io/right_lower_button/state",
DigitalIOState, self.GripperCloseCallback)
rospy.Subscriber("/robot/digital_io/right_upper_button/state",
DigitalIOState, self.GripperOpenCallback)
rospy.Subscriber("/pushed1", Bool, self.pushed1callback)
rospy.Subscriber("/pushed2", Bool, self.pushed2callback)
rospy.Subscriber("/pushed3", Bool, self.pushed3callback)
rospy.Subscriber("/pushed4", Bool, self.pushed4callback)
self.DIOpub = rospy.Publisher('/io/comms/io/command',
IOComponentCommand,
queue_size=10)
self.h = IOComponentCommand()
self.h.time = rospy.Time.now()
self.h.op = "set"
self.h.args = "{ \"signals\" : { \"port_sink_0\" : { \"format\" : { \"type\" : \"int\", \"dimensions\" : [ 1] }, \"data\" : [ 0] } } }"
rospy.Subscriber("Robotiq2FGripperRobotInput",
inputMsg.Robotiq2FGripper_robot_input,
self.GripperStatusCallback)
self.gripperpub = rospy.Publisher(
'Robotiq2FGripperRobotOutput',
outputMsg.Robotiq2FGripper_robot_output)
self.grippercommand = outputMsg.Robotiq2FGripper_robot_output()
self.gripperstatus = inputMsg.Robotiq2FGripper_robot_input()
self.grip_des_pos = 444
#self.des_gripper_pos=0
#activate-reset-activate gripper
self.grippercommand.rACT = 1
self.grippercommand.rGTO = 1
self.grippercommand.rSP = 255
self.grippercommand.rFR = 150
self.gripperpub.publish(self.grippercommand)
time.sleep(.5)
self.grippercommand = outputMsg.Robotiq2FGripper_robot_output()
self.grippercommand.rACT = 0
self.gripperpub.publish(self.grippercommand)
time.sleep(.5)
self.grippercommand = outputMsg.Robotiq2FGripper_robot_output()
self.grippercommand.rACT = 1
self.grippercommand.rGTO = 1
self.grippercommand.rSP = 255
self.grippercommand.rFR = 150
self.pb1 = 1
self.pb2 = 1
self.pb3 = 1
self.pb4 = 1
time.sleep(.5)
self.nav = intera_interface.Navigator()
self.head = intera_interface.Head()
self.headangle = 0
self._valid_limb_names = {'right': 'right', 'r': 'right'}
# get information from the SDK
# self._left = intera_interface.Limb('left')
self._right = intera_interface.Limb('right')
#self._l_jnames = self._left.joint_names()
self._r_jnames = self._right.joint_names()
self.kin = sawyer_kinematics('right')
# data initializations
self._jointpos = [0] * 7
self._jointvel = [0] * 7
self._jointtor = [0] * 7
self._ee_pos = [0] * 3
self._ee_or = [0] * 4
self._ee_tw = [0] * 6
self._ee_wr = [0] * 6
self._ee_vel = [0] * 6
self._pijac = [
] #numpy.matrix('0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0')
self._jac = [
] #numpy.matrix('0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0;0,0,0,0,0,0')
self._inertia_mat = [
] #numpy.matrix('0,0,0,0,0,0,0;0,0,0,0,0,0,0;0,0,0,0,0,0,0;0,0,0,0,0,0,0;0,0,0,0,0,0,0;0,0,0,0,0,0,0;0,0,0,0,0,0,0')
self.Jxl_raw = 0
self.Jyl_raw = 0
self.Jzl_raw = 0
self.Jxa_raw = 0
self.Jya_raw = 0
self.Jza_raw = 0
self.leftbutton = 0
self.rightbutton = 0
self.spacenav = []
#self._l_joint_command = dict(zip(self._l_jnames,[0.0]*7))
self._r_joint_command = dict(zip(self._r_jnames, [0.0] * 7))
self.MODE_POSITION = 0
self.MODE_VELOCITY = 1
self.MODE_TORQUE = 2
self._mode = self.MODE_POSITION
self.RMH_delay = .01
# initial joint command is current pose
self.readJointPositions()
#self.setJointCommand('left',self._jointpos[0:7])
self.setJointCommand('right', self._jointpos[0:7])
self.hascollided = 0
self.head_display = intera_interface.HeadDisplay()
print('head display!')
#self.image="/home/rachel/rawhide/rawhide_ws/src/sawyer_simulator/sawyer_sim_examples/models/cafe_table/materials/textures/Wood_Floor_Dark.jpg"
self.image = "/home/rachel/rawhide/rmh_code/puppy.png"
self.head_display.display_image(self.image, display_rate=100)
print('finished first head display')
# Start background threads
self._running = True
self._t_joints = threading.Thread(target=self.jointspace_worker)
self._t_joints.daemon = True
self._t_joints.start()
self._t_effector = threading.Thread(target=self.endeffector_worker)
self._t_effector.daemon = True
self._t_effector.start()
self._t_command = threading.Thread(target=self.command_worker)
self._t_command.daemon = True
self._t_command.start()
self._t_display = threading.Thread(target=self.display_worker)
self._t_display.daemon = True
self._t_display.start()
self._t_dio = threading.Thread(target=self.dio_worker)
self._t_dio.daemon = True
self._t_dio.start()
