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):
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, 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 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