def __init__(self, arm):
self._arm = arm
# inputs
self._cuff = Cuff(limb=arm)
self._gripper_position = []
self._max_gripper_position = 0.01
self._toogle = -1
self._recorder = []
self._data_number = 1
self._path = '/home/cloud/code/learn-dmp/data_collector/'
try:
self._gripper = Gripper(arm)
if not (self._gripper.is_calibrated()
or self._gripper.calibrate() == True):
rospy.logerr("({0}_gripper) calibration failed.".format(
self._gripper.name))
raise
self._cuff.register_callback(self._record_action,
'{0}_button_upper'.format(arm))
self._cuff.register_callback(self._open_action,
'{0}_button_lower'.format(arm))
rospy.loginfo("{0} Cuff Control initialized...".format(
self._gripper.name))
except:
self._gripper = None
msg = ("{0} Gripper is not connected to the robot").format(
arm.capitalize())
rospy.logwarn(msg)
def __init__(self, arm, lights=True):
"""
@type arm: str
@param arm: arm of gripper to control
@type lights: bool
@param lights: if lights should activate on cuff grasp
"""
self._arm = arm
# inputs
self._cuff = Cuff(limb=arm)
# connect callback fns to signals
self._lights = None
if lights:
self._lights = Lights()
self._cuff.register_callback(self._light_action,
'{0}_cuff'.format(arm))
try:
self._gripper = Gripper(arm)
if not (self._gripper.is_calibrated() or
self._gripper.calibrate() == True):
rospy.logerr("({0}_gripper) calibration failed.".format(
self._gripper.name))
raise
self._cuff.register_callback(self._close_action,
'{0}_button_upper'.format(arm))
self._cuff.register_callback(self._open_action,
'{0}_button_lower'.format(arm))
rospy.loginfo("{0} Cuff Control initialized...".format(
self._gripper.name))
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, 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)
class RecordMotion(object):
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 clean_shutdown(self):
print("\nExiting example...")
if not self._init_state:
print("Disabling robot...")
# self._rs.disable()
return True
def record(self):
self._cuff.register_callback(self._close_action,
'{0}_button_upper'.format(self._arm))
self._cuff.register_callback(self._open_action,
'{0}_button_lower'.format(self._arm))
print "Registering COntrols"
ok_id = self._navigator.register_callback(
self._record_spot, 'right_button_ok'
) # The circular button in the middle of the navigator.
circle_id = self._navigator.register_callback(
self._record_OK, 'right_button_circle'
) #The button above the OK button, typically with a 'Back' arrow symbol.
show_id = self._navigator.register_callback(
self._record_start, 'right_button_show'
) #The "Rethink Button", is above the OK button, next to back button and typically is labeled with the Rethink logo.
while not rospy.is_shutdown():
rospy.sleep(0.1)
self._navigator.deregister_callback(ok_id)
self._navigator.deregister_callback(circle_id)
self._navigator.deregister_callback(show_id)
def _record_start(self, value):
self.headLight("green")
def _record_OK(self, value):
self.headLight("red")
print "we cool"
def _record_spot(self, value):
time = (datetime.datetime.now() - self.lastButtonPress).seconds
print time
print "!!!!!!!!!!!!!"
if time < 2:
print "time to bail"
return
self.lastButtonPress = datetime.datetime.now()
print "spot record"
self.headLight("red")
self.commandNumber += 1
posts = self.db.Command
joints = {}
names = self._limb.joint_names()
for join in names:
joints.update({join: self._limb.joint_angle(join)})
if joints == self._lastJoin:
return 0
self._lastJoin = joints
post = {
"Name": self.commandName,
"Order": self.commandNumber,
"Action": "Move",
"Joints": joints
}
posts.insert(post)
print post
self.headLight("green")
def _open_action(self, value):
self.headLight("red")
if value and self._gripper.is_ready():
self._gripper.open()
self.commandNumber += 1
posts = self.db.Command
post = {
"Name": self.commandName,
"Order": self.commandNumber,
"Action": "Gripper",
"Open": True
}
posts.insert(post)
self.headLight("green")
def _close_action(self, value):
self.headLight("red")
if value and self._gripper.is_ready():
rospy.logdebug("gripper close triggered")
self._gripper.close()
self.commandNumber += 1
posts = self.db.Command
post = {
"Name": self.commandName,
"Order": self.commandNumber,
"Action": "Gripper",
"Open": False
}
posts.insert(post)
self.headLight("green")
def _light_action(self, value):
if value:
rospy.logdebug("cuff grasp triggered")
else:
rospy.logdebug("cuff release triggered")
if self._lights:
self._set_lights('red', False)
self._set_lights('green', False)
self._set_lights('blue', value)
def _set_lights(self, color, value):
self._lights.set_light_state('head_{0}_light'.format(color),
on=bool(value))
self._lights.set_light_state('{0}_hand_{1}_light'.format(
self._arm, color),
on=bool(value))
def headLight(self, value):
colors = ["red", "blue", "green"]
for color in colors:
self._lights.set_light_state('head_{0}_light'.format(color),
on=bool(value == color))
def finalize(self):
print "ENDING!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!"
class GripperConnect(object):
"""
Connects wrist button presses to gripper open/close commands.
Uses the Navigator callback feature to make callbacks to connected
action functions when the button values change.
"""
def __init__(self, arm, lights=True):
"""
@type arm: str
@param arm: arm of gripper to control
@type lights: bool
@param lights: if lights should activate on cuff grasp
"""
self._arm = arm
# inputs
self._cuff = Cuff(limb=arm)
# connect callback fns to signals
self._lights = None
if lights:
self._lights = Lights()
self._cuff.register_callback(self._light_action,
'{0}_cuff'.format(arm))
try:
self._gripper = Gripper(arm)
if not (self._gripper.is_calibrated() or
self._gripper.calibrate() == True):
rospy.logerr("({0}_gripper) calibration failed.".format(
self._gripper.name))
raise
self._cuff.register_callback(self._close_action,
'{0}_button_upper'.format(arm))
self._cuff.register_callback(self._open_action,
'{0}_button_lower'.format(arm))
rospy.loginfo("{0} Cuff Control initialized...".format(
self._gripper.name))
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 _open_action(self, value):
if value and self._gripper.is_ready():
rospy.logdebug("gripper open triggered")
self._gripper.open()
if self._lights:
self._set_lights('red', False)
self._set_lights('green', True)
def _close_action(self, value):
if value and self._gripper.is_ready():
rospy.logdebug("gripper close triggered")
self._gripper.close()
if self._lights:
self._set_lights('green', False)
self._set_lights('red', True)
def _light_action(self, value):
if value:
rospy.logdebug("cuff grasp triggered")
else:
rospy.logdebug("cuff release triggered")
if self._lights:
self._set_lights('red', False)
self._set_lights('green', False)
self._set_lights('blue', value)
def _set_lights(self, color, value):
self._lights.set_light_state('head_{0}_light'.format(color), on=bool(value))
self._lights.set_light_state('{0}_hand_{1}_light'.format(self._arm, color),
on=bool(value))
def process():
#Location pair 1
'''start = [0.0, 0.5, 0.15]
goal = [0.3, 0.5, 0.15] '''
#Location pair 2
'''start = [-0.4, 0.5, 0.32]
goal = [0.3, 0.5, 0.06] '''
#Location pair 3 - Move to block
start = [-0.4, 0.5, 0.32]
goal = [0.0, 0.5, 0.06]
#Duration of trajectory
T = 2
dt = 0.01
overhead_orientation = Quaternion(x=-0.0249590815779,
y=0.999649402929,
z=0.00737916180073,
w=0.00486450832011)
rospy.init_node('testRecord1')
limb = intera_interface.Limb('right')
right_gripper = Gripper('right_gripper')
moveToNeutral(limb)
right_gripper.open()
pose = Pose(position=Point(x=start[0], y=start[1], z=start[2]),
orientation=overhead_orientation)
#move to start position
try:
angles = limb.ik_request(pose, 'right_hand')
limb.move_to_joint_positions(angles)
except:
print("erron in conversion or jointangles")
rospy.sleep(1)
dx = (goal[0] - start[0]) / (T / dt)
dy = (goal[1] - start[1]) / (T / dt)
dz = (goal[2] - start[2]) / (T / dt)
x = start[0]
y = start[1]
z = start[2]
timeElapsed = 0.0
while timeElapsed <= T:
#print("in loop", dx, " ", dy, " ", dz)
x += dx
y += dy
z += dz
pose = Pose(position=Point(x, y, z), orientation=overhead_orientation)
try:
angles = limb.ik_request(pose, 'right_hand')
limb.move_to_joint_positions(angles)
storeKeyframe(limb, right_gripper.is_gripping())
except:
print("error in conversion or jointangles")
timeElapsed += dt
rospy.sleep(dt)
rospy.sleep(1)
import intera_interface
from intera_interface import Gripper
import intera_external_devices
from geometry_msgs.msg import (
PoseStamped,
Pose,
Point,
Quaternion,
)
import numpy as np
import math
rospy.init_node('dmp_rep')
limb = intera_interface.Limb('right')
right_gripper = Gripper('right_gripper')
_filename = "fs.txt"
_gripperZOffset = 0.15
#Constants
alpha = 3.99 #4.6
K = 2500. #600#9 #25
D = 93.8 #50#6 #10
timesteps = [] #time points, array size= number of recorded data points
dt = 0.045
S = []
learnt_S_F = []
X = []
F = []
timesteps_since_subgoal = 0 # to count c-step elapse for subgoal proposal
episode_num = 0 # incremental episode counter
done = True
reset = False
manager_temp_transition = list() # temp manager transition
# demoEnv inherits robotEnv
env = demoEnv(max_steps=max_ep_len, control_mode='velocity', isPOMDP=True, isGripper=USE_GRIPPER, isCartesian=USE_CARTESIAN, train_indicator=IS_TRAIN)
controller_buffer = DemoReplayBuffer(obs_dim=obs_dim, stt_dim=stt_dim, act_dim=act_dim, aux_dim=aux_dim, size=buffer_size)
manager_buffer = DemoManagerReplayBuffer(obs_dim=obs_dim, stt_dim=stt_dim, act_dim=act_dim, aux_dim=aux_dim, size=buffer_size, seq_len=manager_propose_freq)
obs_shape_list = [(100,100,3), (7), (7), (7), (1), (7), (3)]
summary_manager = SummaryManager(obs_shape_list=obs_shape_list) # manager for rms
# create instances for the arm and gripper
limb = Limb()
gripper = Gripper()
def update_rms(full_stt=None, c_obs=None, aux=None, act=None):
"""Update the mean/stddev of the running mean-std normalizers.
Normalize full-state, color_obs, and auxiliary observation.
Caution on the shape!
"""
summary_manager.s_t0_rms.update(c_obs) # c_obs
summary_manager.s_t1_rms.update(full_stt[:7]) # joint_pos
summary_manager.s_t2_rms.update(full_stt[7:14]) # joint_vel
summary_manager.s_t3_rms.update(full_stt[14:21]) # joint_eff
summary_manager.s_t4_rms.update(full_stt[21:22]) # gripper_position
summary_manager.s_t5_rms.update(full_stt[22:]) # ee_pose
summary_manager.s_t6_rms.update(aux) # aux
summary_manager.a_t_rms.update(act) # ee_pose
class GripperConnect(object):
def __init__(self, arm):
self._arm = arm
# inputs
self._cuff = Cuff(limb=arm)
self._gripper_position = []
self._max_gripper_position = 0.01
self._toogle = -1
self._recorder = []
self._data_number = 1
self._path = '/home/cloud/code/learn-dmp/data_collector/'
try:
self._gripper = Gripper(arm)
if not (self._gripper.is_calibrated()
or self._gripper.calibrate() == True):
rospy.logerr("({0}_gripper) calibration failed.".format(
self._gripper.name))
raise
self._cuff.register_callback(self._record_action,
'{0}_button_upper'.format(arm))
self._cuff.register_callback(self._open_action,
'{0}_button_lower'.format(arm))
rospy.loginfo("{0} Cuff Control initialized...".format(
self._gripper.name))
except:
self._gripper = None
msg = ("{0} Gripper is not connected to the robot").format(
arm.capitalize())
rospy.logwarn(msg)
def _open_action(self, value):
self._position = self._gripper.get_position()
if value and self._gripper.is_ready():
if self._position > self._max_gripper_position:
rospy.logdebug("gripper open triggered")
self._gripper.close()
print 'gripper is closed'
print self._position
else:
rospy.logdebug("gripper close triggered")
self._gripper.open()
print 'gripper is opened'
print self._position
def _record_action(self, value):
if value and self._gripper.is_ready():
self._toogle = (-1) * self._toogle
if self._toogle == 1:
print 'Opening a new file named demo_' + str(self._data_number)
self._recorder = subprocess.Popen([
'rosrun', 'intera_examples', 'joint_recorder.py', '-f',
self._path + 'demo' + str(self._data_number) + '.txt'
],
stdout=subprocess.PIPE)
if self._toogle == -1:
print self._toogle
print 'Close the file'
self._data_number = self._data_number + 1
self._recorder.kill()