def cartesian():
rospy.init_node("move_to_position")
limb = intera_interface.Limb('right')
limb.move_to_neutral()
cars = []
for i in range(4):
print("Enter point %d" % (i + 1))
done = False
while not done:
c = intera_external_devices.getch()
if c in ['\x1b', '\x03']:
return None
elif c == 'p':
done = True
current_pose = limb.endpoint_pose()
cars.append([current_pose['position'].x, current_pose['position'].y])
print('Point %d: (%.2f, %.2f)' % (i + 1, current_pose['position'].x, current_pose['position'].y))
cars = np.array(cars)
X_min = cars[:, 0].min()
X_max = cars[:, 0].max()
Y_min = cars[:, 1].min()
Y_max = cars[:, 1].max()
return [[X_min, X_max], [Y_max, Y_min]]
def collect_goal_image(self, ind=0):
savedir = self.recording_dir + '/goalimage'
if not os.path.exists(savedir):
os.makedirs(savedir)
done = False
print("Press g to take goalimage!")
while not done and not rospy.is_shutdown():
c = intera_external_devices.getch()
if c:
# catch Esc or ctrl-c
if c in ['\x1b', '\x03']:
done = True
rospy.signal_shutdown("Example finished.")
if c == 'g':
print 'taking goalimage'
imagemain = self.recorder.ltob.img_cropped
cv2.imwrite( savedir+ "/goal_main{}.png".format(ind),
imagemain, [cv2.IMWRITE_PNG_STRATEGY_DEFAULT, 1])
state = self.get_endeffector_pos()
with open(savedir + '/goalim{}.pkl'.format(ind), 'wb') as f:
cPickle.dump({'main': imagemain, 'state': state}, f)
break
else:
print 'wrong key!'
print 'place object in different location!'
pdb.set_trace()
def move_eef(self):
action_size = 0.05
rate = rospy.Rate(100)
action = np.array([0.0, 0.0, 0.0])
print('Move the end-effector and press d for done')
print('''\r **Ready. Do one of the following.
8 : go forwards in x
2 : go backwards in x
6 : go forwards in y
4 : go backwards in y
+ : go forwards in z
- : go backwards in z
h : double speed
l : half speed
d : finish moving
esc : quit
''')
while not rospy.is_shutdown():
c = intera_external_devices.getch()
if c:
if (c in ['8']):
action = np.array([1.0, 0.0, 0.0])
elif (c in ['2']):
action = np.array([-1.0, 0.0, 0.0])
elif (c in ['6']):
action = np.array([0.0, 1.0, 0.0])
elif (c in ['4']):
action = np.array([0.0, -1.0, 0.0])
elif (c in ['+']):
action = np.array([0.0, 0.0, 1.0])
elif (c in ['-']):
action = np.array([0.0, 0.0, -1.0])
elif (c in ['5']):
action = np.array([0.0, 0.0, 0.0])
elif (c in ['h']):
action = np.array([0.0, 0.0, 0.0])
action_size = 2 * action_size
elif (c in ['l']):
action = np.array([0.0, 0.0, 0.0])
action_size = action_size / 2
elif c in ['\x1b', '\x03', 'f']:
action = np.array([0.0, 0.0, 0.0])
rospy.signal_shutdown("Shutting Down...")
elif c in ['d']:
rospy.loginfo('Done Moving')
rospy.set_param('manual_arm_move', False)
print(self._right_arm.endpoint_pose())
return
joint_vels = self.get_control(action, max_action=action_size)
# Send contgrol to robot
self._right_arm.set_joint_velocities(joint_vels)
rate.sleep()
return
def map_keyboard(head):
done = False
while not done and not rospy.is_shutdown():
c = intera_external_devices.getch()
if c:
#catch Esc or ctrl-c
if c in ['\x1b', '\x03']:
done = True
rospy.signal_shutdown("Exiting.")
elif c == 'a':
head.move_head(np.pi / 180.0)
elif c == 'd':
head.move_head(-np.pi / 180.0)
elif c == 'q':
head.move_head(5 * np.pi / 180.0)
elif c == 'e':
head.move_head(-5 * np.pi / 180.0)
elif c == 'r':
head.set_head(0.0)
else:
print_bindings()
def main():
print('Get marker pixel position ...')
pixel_markers = pixel()
print('Marker pixel = ', pixel_markers)
print('Get marker catersian position ...')
cartesian_markers = cartesian()
print('Marker cartesian = ', cartesian_markers)
if cartesian_markers:
print("Save calib info? [y/n]")
done = False
while not done:
c = intera_external_devices.getch()
if c == 'n':
done = True
elif c == 'y':
calib_info = {'pixel': pixel_markers, 'cartesian': cartesian_markers}
with open(CALIB_INFO, 'w') as f:
json.dump(calib_info, f)
done = True
print('Saved calib info at ' + CALIB_INFO)
def map_keyboard(side):
limb = intera_interface.Limb(side)
try:
gripper = intera_interface.Gripper(side)
except:
has_gripper = False
rospy.logerr("Could not initalize the gripper.")
else:
has_gripper = True
joints = limb.joint_names()
def set_j(limb, joint_name, delta):
current_position = limb.joint_angle(joint_name)
joint_command = {joint_name: current_position + delta}
limb.set_joint_positions(joint_command)
def set_g(action):
if has_gripper:
if action == "close":
gripper.close()
elif action == "open":
gripper.open()
elif action == "calibrate":
gripper.calibrate()
bindings = {
'1': (set_j, [limb, joints[0], 0.1], joints[0] + " increase"),
'q': (set_j, [limb, joints[0], -0.1], joints[0] + " decrease"),
'2': (set_j, [limb, joints[1], 0.1], joints[1] + " increase"),
'w': (set_j, [limb, joints[1], -0.1], joints[1] + " decrease"),
'3': (set_j, [limb, joints[2], 0.1], joints[2] + " increase"),
'e': (set_j, [limb, joints[2], -0.1], joints[2] + " decrease"),
'4': (set_j, [limb, joints[3], 0.1], joints[3] + " increase"),
'r': (set_j, [limb, joints[3], -0.1], joints[3] + " decrease"),
'5': (set_j, [limb, joints[4], 0.1], joints[4] + " increase"),
't': (set_j, [limb, joints[4], -0.1], joints[4] + " decrease"),
'6': (set_j, [limb, joints[5], 0.1], joints[5] + " increase"),
'y': (set_j, [limb, joints[5], -0.1], joints[5] + " decrease"),
'7': (set_j, [limb, joints[6], 0.1], joints[6] + " increase"),
'u': (set_j, [limb, joints[6], -0.1], joints[6] + " decrease"),
'8': (set_g, "close", side + " gripper close"),
'i': (set_g, "open", side + " gripper open"),
'9': (set_g, "calibrate", side + " gripper calibrate")
}
done = False
print("Controlling joints. Press ? for help, Esc to quit.")
while not done and not rospy.is_shutdown():
c = intera_external_devices.getch()
if c:
#catch Esc or ctrl-c
if c in ['\x1b', '\x03']:
done = True
rospy.signal_shutdown("Example finished.")
elif c in bindings:
cmd = bindings[c]
if c == '8' or c == 'i' or c == '9':
cmd[0](cmd[1])
print("command: %s" % (cmd[2], ))
else:
#expand binding to something like "set_j(right, 'j0', 0.1)"
cmd[0](*cmd[1])
print("command: %s" % (cmd[2], ))
else:
print("key bindings: ")
print(" Esc: Quit")
print(" ?: Help")
for key, val in sorted(bindings.items(),
key=lambda x: x[1][2]):
print(" %s: %s" % (key, val[2]))
def main():
parser = argparse.ArgumentParser()
parser.add_argument(
"-d", "--log_dir", type=str,
default='../logs',
help="Path where the trajectories produced are stored")
parser.add_argument(
"-l", "--load_dir", type=str,
default='../../assets/rl',
help="Path where the policy networks are loaded")
parser.add_argument(
"-m", "--method_idx", type=int,
default=0,
help="The index of the method to use")
parser.add_argument(
"-r", "--randomisation_idx", type=int,
default=0,
help="The index of the randomisation regime to use")
args = parser.parse_args(rospy.myargv()[1:])
log_dir = args.log_dir
load_dir = args.load_dir
method_idx = args.method_idx
randomisation_idx = args.randomisation_idx
rospy.init_node('reach_control', anonymous=True)
# Parameters
horizon = 50
repeats = 0
goal_no = 1
action_dim=3
internal_state_dim=14
internal_action_dim=7
params_dim=49
env_name='SawyerReach'
# Setting up Sawyer
print('Setting up the robot...')
Right_arm = intera_interface.limb.Limb("right")
print("Getting robot state... ")
_rs = intera_interface.RobotEnable(CHECK_VERSION)
_init_state = _rs.state().enabled
print("Enabling robot... ")
_rs.enable()
print("Robot ready.")
# Setup controller and observation maker
observation_generator = Observation(Right_arm, reset_goal_pos= True, predefined_goal='goal_{}'.format(goal_no))
controller = Control(Right_arm)
while not rospy.is_shutdown():
state_dim = 6
##### SETTING UP THE POLICY #########
method = ['Single', 'LSTM', 'EPI', 'UPOSI'][method_idx]
if(method == 'Single'):
alg_idx = 1
elif(method == 'LSTM'):
alg_idx = 2
elif(method == 'UPOSI'):
alg_idx = 3
osi_l = 5
CAT_INTERNAL = True
if CAT_INTERNAL:
osi_input_dim = osi_l*(state_dim+action_dim+internal_state_dim+internal_action_dim)
else:
osi_input_dim = osi_l * (state_dim + action_dim)
state_dim+=params_dim
elif(method == 'EPI'):
alg_idx = 4
embed_dim = 10
traj_l = 10
NO_RESET = True
embed_input_dim = traj_l*(state_dim+action_dim)
ori_state_dim = state_dim
state_dim += embed_dim
else:
continue
alg_name = ['sac', 'td3', 'lstm_td3', 'uposi_td3', 'epi_td3'][alg_idx]
randomisation_type = ['reach_no-randomisation', 'reach_full-randomisation', \
'reach_force-randomisation', 'reach_force-&-noise-randomisation'][randomisation_idx]
number_random_params = [0, 14, 1, 3][randomisation_idx]
folder_path = load_dir +'/' +method + '/' + alg_name + '/model/'
path = folder_path + env_name + str(
number_random_params) + '_' + alg_name
policy = load(path=path, alg=alg_name, state_dim=state_dim,
action_dim=action_dim)
if method == 'UPOSI':
osi_model = load_model(model_name='osi', path=path, input_dim = osi_input_dim, output_dim=params_dim )
elif method == 'EPI':
embed_model = load_model(model_name='embedding', path=path, input_dim = embed_input_dim, output_dim = embed_dim )
embed_model.cuda()
epi_policy_path = folder_path + env_name + str(number_random_params) + '_' + 'epi_ppo_epi_policy'
epi_policy = load(path=epi_policy_path, alg='ppo', state_dim=ori_state_dim, action_dim=action_dim )
if (alg_name == 'lstm_td3'):
# Initialize hidden state for lstm, (hidden, cell), each is (layer, batch, dim)
hidden_out = (torch.zeros([1, 1, 512], dtype=torch.float).cuda(), \
torch.zeros([1, 1, 512],
dtype=torch.float).cuda())
last_action = np.array([0, 0, 0])
######################################
log_save_name = '{}_{}_{}_{}'.format(method, alg_name, randomisation_type, number_random_params)
done = False
while not done and not rospy.is_shutdown():
if (observation_generator.goal_pos_in_base is not None ):
sys.stdout.write('\r **Observations ready**.')
sys.stdout.flush()
done = True
else:
sys.stdout.write('\rWaiting for observation initialisation...')
sys.stdout.flush()
controller.go_to_start()
done = False
while not done and not rospy.is_shutdown():
sys.stdout.write('\r **Ready. Press any key to start, f to quit**.')
sys.stdout.flush()
c = intera_external_devices.getch()
if c in ['\x1b', '\x03', 'f']:
rospy.signal_shutdown("Shutting Down...")
return
elif c:
done = True
#Initialise the trajectory logger
log_path = '{}/reaching/{}/goal_{}/trajectory_log_{}.csv'.format(log_dir,log_save_name,goal_no,repeats)
log_path = os.path.join(os.path.dirname(os.path.realpath(__file__)),
log_path)
log_list = ["step", "time",
"cmd_eef_vx", "cmd_eef_vy", "cmd_eef_vz",
"eef_x", "eef_y", "eef_z",
"eef_vx", "eef_vy", "eef_vz",
"goal_x","goal_y","goal_z",
"obs_0", "obs_1", "obs_2",
"obs_3", "obs_4", "obs_5"
]
logger = Logger(log_list, log_path)
# Set control rate
rate = rospy.Rate(10)
# Time and iteration count
i = 0
start = rospy.Time.now()
if (alg_name == 'uposi_td3') or (alg_name == 'epi_td3'):
epi_traj = []
while not rospy.is_shutdown():
# Grab observations
elapsed = rospy.Time.now() - start
observation = observation_generator.get_observation()
c_observation = observation_generator.get_characterisation_observation()
##### Pre-action choosing operations
if (alg_name == 'uposi_td3'):
if len(epi_traj)>=osi_l:
osi_input = stack_data(epi_traj, osi_l)
pre_params = osi_model(osi_input).detach().numpy()
else:
zero_osi_input = np.zeros(osi_input_dim)
pre_params = osi_model(zero_osi_input).detach().numpy()
params_state = np.concatenate((pre_params, observation))
elif (alg_name == 'epi_td3'):
# epi rollout first for each episode;
if len(epi_traj) < traj_l:
a = epi_policy.get_action(observation)
action = np.clip(a, -epi_policy.action_range, epi_policy.action_range)
s_a_r = np.concatenate((observation, action, [0])) # state, action, reward; no reward in reality
epi_traj.append(s_a_r)
if(len(epi_traj)>= traj_l):
state_action_in_traj = np.array(epi_traj)[:, :-1] # remove the rewards
embedding = embed_model(state_action_in_traj.reshape(-1))
embedding = embedding.detach().cpu().numpy()
if NO_RESET:
observation = observation # last observation
else:
# reset the env here and get new observation
rospy.signal_shutdown('Cannot reset the environment in the real world')
sys.exit(1)
observation=np.concatenate((observation, embedding))
###############
#### CHOOSING THE ACTION #####
if (alg_name == 'lstm_td3'):
hidden_in = hidden_out
action, hidden_out=policy.get_action(observation, last_action, hidden_in, noise_scale=0.0)
last_action = action
elif (alg_name == 'uposi_td3'):
# using internal state or not
if CAT_INTERNAL:
internal_state = np.concatenate((controller.get_joints_angle(), np.array(controller.get_joints_vel())))
full_state = np.concatenate([observation, internal_state])
else:
full_state = observation
action = policy.get_action(params_state, noise_scale=0.0)
elif (alg_name == 'epi_td3') and len(epi_traj) < traj_l: # action already given by EPI policy
pass
else:
action = policy.get_action(observation, noise_scale=0.0)
###############################
if (np.isnan(action).any()):
rospy.signal_shutdown('NAN in action')
sys.exit(1)
# joint_vels is the joint velocity COMMAND - the current joint velocities can be queried through controller.joint_vels
joint_vels = controller.get_control(action)
controller._right_arm.set_joint_velocities(joint_vels)
if (alg_name == 'uposi_td3'):
if CAT_INTERNAL:
target_joint_action = controller.joint_array_from_joint_dict(joint_vels)
print(action.shape)#
print(target_joint_action.shape)
full_action = np.concatenate([action, target_joint_action])
else:
full_action = action
epi_traj.append(np.concatenate((full_state, full_action)))
## Format the observations for logging and log
elapsed_sec = elapsed.to_sec()
eef_pos_in_base = c_observation['eef_pos_in_base']
eef_vel_in_base = c_observation['eef_vel_in_base']
goal_pos_in_base = c_observation['goal_pos_in_base']
action_in_base = controller.base_rot_in_eef.dot(action)
logger.log(i, elapsed_sec,
action_in_base[0], action_in_base[1], action_in_base[2],
eef_pos_in_base[0], eef_pos_in_base[1], eef_pos_in_base[2],
eef_vel_in_base[0], eef_vel_in_base[1], eef_vel_in_base[2],
goal_pos_in_base[0], goal_pos_in_base[1], goal_pos_in_base[2],
observation[0], observation[1], observation[2],
observation[3], observation[4], observation[5]
)
i += 1
if (i >= horizon):
break
rate.sleep()
controller.set_neutral()
done = False
def _finish_message():
print('''Episode finished, press:
f or esc to quit
r to restart
g to change the goal position''')
_finish_message()
while not done and not rospy.is_shutdown():
c = intera_external_devices.getch()
if c:
if c in ['\x1b', '\x03', 'f']:
done = True
rospy.signal_shutdown("Shutting Down...")
elif c in ['g']:
repeats = 0
goal_no +=1
observation_generator.reset(specific_goal ='goal_{}'.format(goal_no))
done = True
elif c in ['r']:
repeats+=1
done = True
else:
_finish_message()
def main():
parser = argparse.ArgumentParser()
parser.add_argument(
"-p",
"--log_dir",
type=str,
default='../logs/manual_slide',
help=
"Path to the parent directory where the produced trajectories are stored"
)
args = parser.parse_args(rospy.myargv()[1:])
log_dir = args.log_path
rospy.init_node('reach_characterisation', anonymous=True)
default_value = 0.1
a = default_value
repeats = 0
# Setting up Sawyer
print('Setting up the robot...')
Right_arm = intera_interface.limb.Limb("right")
print("Getting robot state... ")
_rs = intera_interface.RobotEnable(CHECK_VERSION)
_init_state = _rs.state().enabled
print("Enabling robot... ")
_rs.enable()
print("Robot ready.")
Right_arm.set_joint_position_speed(speed=0.1)
# Setup controller and observation maker
observation_generator = Observation(Right_arm)
controller = Control(Right_arm)
while not rospy.is_shutdown():
done = False
while not done and not rospy.is_shutdown():
if (observation_generator.operational is not None):
sys.stdout.write('\r **Observations ready**.')
sys.stdout.flush()
done = True
else:
sys.stdout.write('\rWaiting for observation initialisation...')
sys.stdout.flush()
controller.go_to_start()
done = False
while not done and not rospy.is_shutdown():
sys.stdout.write(
'\r **Ready. Press any key to start, f to quit**.')
sys.stdout.flush()
c = intera_external_devices.getch()
if c in ['\x1b', '\x03', 'f']:
rospy.signal_shutdown("Shutting Down...")
return
elif c:
done = True
log_path = '{}/goal1/trajectory_log_{}.csv'.format(log_dir, repeats)
log_path = os.path.join(os.path.dirname(os.path.realpath(__file__)),
log_path)
log_list = [
"step",
"time",
"cmd_j5",
"cmd_j6",
"obj_x",
"obj_y",
"obj_z",
"sin_z",
"cos_z",
"obj_vx",
"obj_vy",
"obj_vz",
"z_angle",
"obj_orn_x",
"obj_orn_y",
"obj_orn_z",
"obj_orn_w",
"rot_1",
"rot_2",
"rot_3",
"rot_4",
"rot_5",
"rot_6",
"rot_7",
"rot_8",
"rot_9",
"fallen_object",
"a_j5",
"a_j6",
"v_j5",
"v_j6",
]
logger = Logger(log_list, log_path)
# Set control rate
rate = rospy.Rate(10)
# Time and iteration count
i = 0
start = rospy.Time.now()
while not rospy.is_shutdown():
# Grab observations
elapsed = rospy.Time.now() - start
observation = observation_generator.get_c_observation()
# Set the action
action = [a, a]
joint_vels = controller.get_control(action)
controller._right_arm.set_joint_velocities(joint_vels)
## Format the observations for logging and log
elapsed_sec = elapsed.to_sec()
logger.log(i, elapsed_sec, action[0], action[1], observation[0],
observation[1], observation[2], observation[3],
observation[4], observation[5], observation[6],
observation[7], observation[8], observation[9],
observation[10], observation[11], observation[12],
observation[13], observation[14], observation[15],
observation[16], observation[17], observation[18],
observation[19], observation[20], observation[21],
observation[22], observation[23], observation[24],
observation[25], observation[26])
if (elapsed_sec > 2.):
break
i += 1
rate.sleep()
done = False
def _finish_message():
print('''Episode finished, press:
f or esc to quit
r to restart
g to change the goal position''')
_finish_message()
while not done and not rospy.is_shutdown():
c = intera_external_devices.getch()
if c:
if c in ['\x1b', '\x03', 'f']:
done = True
rospy.signal_shutdown("Shutting Down...")
controller.set_slide_neutral()
elif c in ['r']:
controller.set_slide_neutral()
repeats += 1
done = True
else:
_finish_message()
def map_keyboard(limb):
# initialize interfaces
print("Getting robot state...")
rs = intera_interface.RobotEnable(CHECK_VERSION)
init_state = rs.state()
gripper = None
original_deadzone = None
def clean_shutdown():
if gripper and original_deadzone:
gripper.set_dead_zone(original_deadzone)
print("Exiting example.")
try:
gripper = intera_interface.Gripper(limb + '_gripper')
except (ValueError, OSError) as e:
rospy.logerr(
"Could not detect an electric gripper attached to the robot.")
clean_shutdown()
return
rospy.on_shutdown(clean_shutdown)
def offset_position(offset_pos):
cmd_pos = max(
min(gripper.get_position() + offset_pos, gripper.MAX_POSITION),
gripper.MIN_POSITION)
gripper.set_position(cmd_pos)
print(("commanded position set to {0} m".format(cmd_pos)))
def update_velocity(offset_vel):
cmd_speed = max(
min(gripper.get_cmd_velocity() + offset_vel, gripper.MAX_VELOCITY),
gripper.MIN_VELOCITY)
gripper.set_cmd_velocity(cmd_speed)
print(("commanded velocity set to {0} m/s".format(cmd_speed)))
original_deadzone = gripper.get_dead_zone()
# WARNING: setting the deadzone below this can cause oscillations in
# the gripper position. However, setting the deadzone to this
# value is required to achieve the incremental commands in this example
gripper.set_dead_zone(0.001)
rospy.loginfo("Gripper deadzone set to {}".format(gripper.get_dead_zone()))
num_steps = 8.0
percent_delta = 1.0 / num_steps
velocity_increment = (gripper.MAX_VELOCITY -
gripper.MIN_VELOCITY) * percent_delta
position_increment = (gripper.MAX_POSITION -
gripper.MIN_POSITION) * percent_delta
bindings = {
# key: (function, args, description)
'r': (gripper.reboot, [], "reboot"),
'c': (gripper.calibrate, [], "calibrate"),
'q': (gripper.close, [], "close"),
'o': (gripper.open, [], "open"),
'+': (update_velocity, [velocity_increment],
"increase velocity by {0}%".format(percent_delta * 100)),
'-': (update_velocity, [-velocity_increment],
"decrease velocity by {0}%".format(percent_delta * 100)),
's': (gripper.stop, [], "stop"),
'u': (offset_position, [-position_increment],
"decrease position by {0}%".format(percent_delta * 100)),
'i': (offset_position, [position_increment],
"increase position by {0}%".format(percent_delta * 100)),
}
done = False
rospy.loginfo("Enabling robot...")
rs.enable()
print("Controlling grippers. Press ? for help, Esc to quit.")
while not done and not rospy.is_shutdown():
c = intera_external_devices.getch()
if c:
if c in ['\x1b', '\x03']:
done = True
elif c in bindings:
cmd = bindings[c]
print(("command: {0}".format(cmd[2])))
cmd[0](*cmd[1])
else:
print("key bindings: ")
print(" Esc: Quit")
print(" ?: Help")
for key, val in sorted(list(bindings.items()),
key=lambda x: x[1][2]):
print((" %s: %s" % (key, val[2])))
# force shutdown call if caught by key handler
rospy.signal_shutdown("Example finished.")
def map_keyboard(side, redisInstance):
min_q = [-3.0503, -3.8095, -3.0426, -3.0439, -2.9761, -2.9761, -4.7124]
max_q = [3.0503, 2.2736, 3.0426, 3.0439, 2.9761, 2.9761, 4.7124]
home_q = [
-0.11243359375, -1.15601367188, -0.00805859375, 2.16251171875,
0.0014658203125, 0.47171875, 3.31875488281
]
float_q = home_q
limb = intera_interface.Limb(side)
joints = limb.joint_names()
def set_j(limb, joints, q):
joint_command = {
joints[0]: q[0],
joints[1]: q[1],
joints[2]: q[2],
joints[3]: q[3],
joints[4]: q[4],
joints[5]: q[5],
joints[6]: q[6]
}
limb.set_joint_positions(joint_command)
done = False
print("Controlling joints. Press ? for help, Esc to quit.")
while not done and not rospy.is_shutdown():
# Read q from REDIS
flag = False
redis_angles = redisInstance.get('cs225a::robot::sawyer::sensors::q')
if redis_angles is not None: # Check if read from redis correctly
q = redis_angles.split(" ")
if len(q) is 7: # Check if 7 forces read
for i in range(0, 7):
angle_float = float(q[i])
if math.isnan(angle_float) or angle_float < min_q[
i] or angle_float > max_q[i]:
flag = True
else:
float_q[i] = angle_float
else:
flag = True
else:
flag = True
if flag:
q = home_q
# Write q to Robot
set_j(limb, joints, float_q)
print q
# Check for escape characters
c = intera_external_devices.getch()
if c:
#catch Esc or ctrl-c
if c in ['\x1b', '\x03']:
done = True
rospy.signal_shutdown("Example finished.")
def map_keyboard(side):
limb = intera_interface.Limb(side)
print(limb.tip_state("right_hand_camera"))
try:
gripper = intera_interface.Gripper(side + '_gripper')
except:
has_gripper = False
rospy.loginfo("The electric gripper is not detected on the robot.")
else:
has_gripper = True
joints = limb.joint_names()
increment = 0.1
def set_j(limb, joint_name, delta):
current_position = limb.joint_angle(joint_name)
joint_command = {joint_name: current_position + delta}
limb.set_joint_positions(joint_command)
def set_g(action):
if has_gripper:
if action == "close":
gripper.close()
elif action == "open":
gripper.open()
elif action == "calibrate":
gripper.calibrate()
def set_bindings():
global bindings
bindings = {
'1': (set_j, [limb, joints[0],
increment], joints[0] + " increase"),
'q': (set_j, [limb, joints[0],
-increment], joints[0] + " decrease"),
'2': (set_j, [limb, joints[1],
increment], joints[1] + " increase"),
'w': (set_j, [limb, joints[1],
-increment], joints[1] + " decrease"),
'3': (set_j, [limb, joints[2],
increment], joints[2] + " increase"),
'e': (set_j, [limb, joints[2],
-increment], joints[2] + " decrease"),
'4': (set_j, [limb, joints[3],
increment], joints[3] + " increase"),
'r': (set_j, [limb, joints[3],
-increment], joints[3] + " decrease"),
'5': (set_j, [limb, joints[4],
increment], joints[4] + " increase"),
't': (set_j, [limb, joints[4],
-increment], joints[4] + " decrease"),
'6': (set_j, [limb, joints[5],
increment], joints[5] + " increase"),
'y': (set_j, [limb, joints[5],
-increment], joints[5] + " decrease"),
'7': (set_j, [limb, joints[6],
increment], joints[6] + " increase"),
'u': (set_j, [limb, joints[6],
-increment], joints[6] + " decrease")
}
if has_gripper:
bindings.update({
'8': (set_g, "close", side + " gripper close"),
'i': (set_g, "open", side + " gripper open"),
'9': (set_g, "calibrate", side + " gripper calibrate")
})
set_bindings()
done = False
print("Controlling joints. Press ? for help, Esc to quit.")
while not done and not rospy.is_shutdown():
c = intera_external_devices.getch()
#global bindings
if c:
#catch Esc or ctrl-c
if c in ['\x1b', '\x03']:
done = True
rospy.signal_shutdown("Example finished.")
elif c in bindings:
cmd = bindings[c]
if c == '8' or c == 'i' or c == '9':
cmd[0](cmd[1])
print("command: %s" % (cmd[2], ))
else:
#expand binding to something like "set_j(right, 'j0', 0.1)"
cmd[0](*cmd[1])
print("command: %s" % (cmd[2], ))
elif c == 'n':
increment_val = "Increment value (0.0 : 0.4) : "
increment = constrain(float(raw_input(increment_val).lower()),
0.0, 0.4)
set_bindings()
else:
print("key bindings: ")
print(" Esc: Quit")
print(" ?: Help")
print(" n: Change increment value")
for key, val in sorted(bindings.items(),
key=lambda x: x[1][2]):
print(" %s: %s" % (key, val[2]))
def map_keyboard(side):
limb = intera_interface.Limb(side)
tip_name = 'right_hand'
try:
gripper = intera_interface.Gripper(side + '_gripper')
except:
has_gripper = False
rospy.loginfo("The electric gripper is not detected on the robot.")
else:
has_gripper = True
joints = limb.joint_names()
def set_j(limb, joint_name, delta):
current_position = limb.joint_angle(joint_name)
joint_command = {joint_name: current_position + delta}
limb.set_joint_positions(joint_command)
def set_l(limb, cartesian_axis, distance):
# traj_options = TrajectoryOptions()
# traj_options.interpolation_type = 'CARTESIAN'
# traj = MotionTrajectory(trajectory_options = traj_options, limb = limb)
# wpt_opts = MotionWaypointOptions(max_joint_speed_ratio=0.2,
# max_joint_accel=0.2)
# waypoint = MotionWaypoint(options = wpt_opts.to_msg(), limb = limb)
endpoint_state = limb.tip_state(tip_name)
if endpoint_state is None:
rospy.logerr('Endpoint state not found with tip name %s', tip_name)
return None
pose = endpoint_state.pose
# print ('*********')
# print (pose.position)
if cartesian_axis == 'x':
pose.position.x += distance
elif cartesian_axis == 'y':
pose.position.y += distance
elif cartesian_axis == 'z':
pose.position.z += distance
# poseStamped = PoseStamped()
# poseStamped.pose = pose
# print (pose.position)
print('*********')
joints_command = limb.ik_request(pose)
limb.set_joint_positions(joints_command)
print(limb.has_collided())
print("*****")
# print ("*****")
# joint_angles = limb.joint_ordered_angles()
# waypoint.set_cartesian_pose(poseStamped, tip_name, joint_angles)
# traj.append_waypoint(waypoint.to_msg())
# result = traj.send_trajectory()
# if result is None:
# rospy.logerr('Trajectory FAILED to send')
#
# if result.result:
# rospy.loginfo('Motion controller successfully finished the trajectory!')
# else:
# rospy.logerr('Motion controller failed to complete the trajectory with error %s',
# result.errorId)
# traj_options.interpolation_type = 'JOINT'
# joint_angles = limb.joint_ordered_angles()
# waypoint.set_cartesian_pose(poseStamped, tip_name, joint_angles)
# traj.append_waypoint(waypoint.to_msg())
# traj.send_trajectory()
def set_g(action):
if has_gripper:
if action == "close":
gripper.close()
elif action == "open":
gripper.open()
elif action == "calibrate":
gripper.calibrate()
bindings = {
'1': (set_j, [limb, joints[0], 0.1], joints[0] + " increase"),
'q': (set_j, [limb, joints[0], -0.1], joints[0] + " decrease"),
'2': (set_j, [limb, joints[1], 0.1], joints[1] + " increase"),
'w': (set_j, [limb, joints[1], -0.1], joints[1] + " decrease"),
'3': (set_j, [limb, joints[2], 0.1], joints[2] + " increase"),
'e': (set_j, [limb, joints[2], -0.1], joints[2] + " decrease"),
'4': (set_j, [limb, joints[3], 0.1], joints[3] + " increase"),
'r': (set_j, [limb, joints[3], -0.1], joints[3] + " decrease"),
'5': (set_j, [limb, joints[4], 0.1], joints[4] + " increase"),
't': (set_j, [limb, joints[4], -0.1], joints[4] + " decrease"),
'6': (set_j, [limb, joints[5], 0.1], joints[5] + " increase"),
'y': (set_j, [limb, joints[5], -0.1], joints[5] + " decrease"),
'7': (set_j, [limb, joints[6], 0.1], joints[6] + " increase"),
'u': (set_j, [limb, joints[6], -0.1], joints[6] + " decrease"),
'a': (set_l, [limb, "x", +0.01], " x" + " increase"),
'z': (set_l, [limb, "x", -0.01], " x" + " decrease"),
's': (set_l, [limb, "y", +0.01], " y" + " increase"),
'x': (set_l, [limb, "y", -0.01], " y" + " decrease"),
'd': (set_l, [limb, "z", +0.01], " z" + " increase"),
'c': (set_l, [limb, "z", -0.01], " z" + " decrease"),
}
if has_gripper:
bindings.update({
'8': (set_g, "close", side + " gripper close"),
'i': (set_g, "open", side + " gripper open"),
'9': (set_g, "calibrate", side + " gripper calibrate")
})
done = False
print("Controlling joints. Press ? for help, Esc to quit.")
time_tamp = None
time_label = None
while not done and not rospy.is_shutdown():
c = intera_external_devices.getch()
if c:
#catch Esc or ctrl-c
if c in ['\x1b', '\x03']:
done = True
rospy.signal_shutdown("Example finished.")
elif c == '\\':
set_speed = input('请输入设定的速度，按下Enter确认。\n')
limb.set_joint_position_speed(set_speed)
elif c in bindings:
cmd = bindings[c]
if c == '8' or c == 'i' or c == '9':
cmd[0](cmd[1])
print("command: %s" % (cmd[2], ))
else:
#expand binding to something like "set_j(right, 'j0', 0.1)"
cmd[0](*cmd[1])
print("command: %s" % (cmd[2], ))
else:
print("key bindings: ")
print(" Esc: Quit")
print(" ?: Help")
for key, val in sorted(bindings.items(),
key=lambda x: x[1][2]):
print(" %s: %s" % (key, val[2]))
time_tamp = rospy.get_time()
else:
if (time_tamp is not None):
if time_tamp != time_label:
if (rospy.get_time() - time_tamp) > 1:
print('当前姿态：')
print(limb.tip_state(tip_name).pose)
# print (limb.tip_state(tip_name).pose.position)
# print (limb.tip_state(tip_name).pose.quaternion)
# print (limb.endpoint_pose())
# print (limb.endpoint_pose()['position'])
# print (limb.endpoint_pose()['orientation'])
print('当前关节角：')
print(limb.joint_angles())
print('\n')
time_label = time_tamp
def map_keyboard(side):
limb = intera_interface.Limb(side)
try:
gripper = intera_interface.Gripper(side + '_gripper')
except:
has_gripper = False
rospy.loginfo("The electric gripper is not detected on the robot.")
else:
has_gripper = True
joints = limb.joint_names()
def set_j(limb, joint_name, delta):
current_position = limb.joint_angle(joint_name)
joint_command = {joint_name: current_position + delta}
limb.set_joint_positions(joint_command)
def set_g(action):
if has_gripper:
if action == "close":
gripper.close()
elif action == "open":
gripper.open()
elif action == "calibrate":
gripper.calibrate()
bindings = {
'1': (set_j, [limb, joints[0], 0.1], joints[0] + " increase"),
'q': (set_j, [limb, joints[0], -0.1], joints[0] + " decrease"),
'2': (set_j, [limb, joints[1], 0.1], joints[1] + " increase"),
'w': (set_j, [limb, joints[1], -0.1], joints[1] + " decrease"),
'3': (set_j, [limb, joints[2], 0.1], joints[2] + " increase"),
'e': (set_j, [limb, joints[2], -0.1], joints[2] + " decrease"),
'4': (set_j, [limb, joints[3], 0.1], joints[3] + " increase"),
'r': (set_j, [limb, joints[3], -0.1], joints[3] + " decrease"),
'5': (set_j, [limb, joints[4], 0.1], joints[4] + " increase"),
't': (set_j, [limb, joints[4], -0.1], joints[4] + " decrease"),
'6': (set_j, [limb, joints[5], 0.1], joints[5] + " increase"),
'y': (set_j, [limb, joints[5], -0.1], joints[5] + " decrease"),
'7': (set_j, [limb, joints[6], 0.1], joints[6] + " increase"),
'u': (set_j, [limb, joints[6], -0.1], joints[6] + " decrease")
}
if has_gripper:
bindings.update({
'8': (set_g, "close", side + " gripper close"),
'i': (set_g, "open", side + " gripper open"),
'9': (set_g, "calibrate", side + " gripper calibrate")
})
#user set joint angles
theta = {}
theta['right_s0'] = np.float32(raw_input("Enter joint1: "))
theta['right_s1'] = np.float32(raw_input("Enter joint2: "))
theta['right_e0'] = np.float32(raw_input("Enter joint3: "))
theta['right_e1'] = np.float32(raw_input("Enter joint4: "))
theta['right_w0'] = np.float32(raw_input("Enter joint5: "))
theta['right_w1'] = np.float32(raw_input("Enter joint6: "))
theta['right_w2'] = np.float32(raw_input("Enter joint7: "))
while (limb.joint_angle(joints[0]) - theta['right_s0'] < 0.1
or limb.joint_angle(joints[1]) - theta['right_s1'] < 0.1
or limb.joint_angle(joints[2]) - theta['right_e0'] < 0.1
or limb.joint_angle(joints[3]) - theta['right_e1'] < 0.1
or limb.joint_angle(joints[4]) - theta['right_w0'] < 0.1
or limb.joint_angle(joints[5]) - theta['right_w1'] < 0.1
or limb.joint_angle(joints[6]) - theta['right_w2'] < 0.1):
limb.set_joint_positions(theta)
done = False
print("Controlling joints. Press ? for help, Esc to quit.")
while not done and not rospy.is_shutdown():
c = intera_external_devices.getch()
if c:
#catch Esc or ctrl-c
if c in ['\x1b', '\x03']:
done = True
rospy.signal_shutdown("Example finished.")
elif c in bindings:
cmd = bindings[c]
if c == '8' or c == 'i' or c == '9':
cmd[0](cmd[1])
print("command: %s" % (cmd[2], ))
else:
#expand binding to something like "set_j(right, 'j0', 0.1)"
cmd[0](*cmd[1])
print("command: %s" % (cmd[2], ))
else:
print("key bindings: ")
print(" Esc: Quit")
print(" ?: Help")
for key, val in sorted(bindings.items(),
key=lambda x: x[1][2]):
print(" %s: %s" % (key, val[2]))
def main():
parser = argparse.ArgumentParser()
parser.add_argument(
"-p", "--log_dir", type=str,
default='../logs/test/manual_push',
help="Path to the parent directory where the produced trajectories are stored")
args = parser.parse_args(rospy.myargv()[1:])
log_dir = args.log_path
rospy.init_node('push_control', anonymous=True)
default_value = 0.1
a = default_value
repeats = 0
goal_no = 1
# Reset publisher for the topic resetting the observations (used in order to change goals)
reset_publisher = rospy.Publisher('pushing/reset', Bool, queue_size=30)
print('Setting up the robot...')
# Controller
controller = Control()
while not rospy.is_shutdown():
# Only continue with the script if the observations are ready
done = False
while not done and not rospy.is_shutdown():
observation, c_observation = controller.get_observations()
if (observation is not None and c_observation is not None):
sys.stdout.write('\r **Observations ready**.')
sys.stdout.flush()
done = True
else:
sys.stdout.write('\rWaiting for observation initialisation...')
sys.stdout.flush()
controller.go_to_start()
# Ask the user when to start the control loop
done = False
while not done and not rospy.is_shutdown():
sys.stdout.write('\r **Ready. Press any key to start, f to quit**.')
sys.stdout.flush()
c = intera_external_devices.getch()
if c in ['\x1b', '\x03', 'f']:
rospy.signal_shutdown("Shutting Down...")
return
elif c:
done = True
log_path = '{}/goal{}/trajectory_log_{}.csv'.format(log_dir, goal_no,repeats)
log_path = os.path.join(os.path.dirname(os.path.realpath(__file__)),
log_path)
log_list = ["step", "time",
"cmd_eef_vx", "cmd_eef_vy",
"goal_x", "goal_y", "goal_z",
"eef_x", "eef_y", "eef_z",
"eef_vx", "eef_vy", "eef_vz",
"object_x", "object_y", "object_z",
"object_vx", "object_vy", "object_vz",
"z_angle",
"obs_0", "obs_1", "obs_2", "obs_3", "obs_4", "obs_5", "obs_6", "obs_7", "obs_8", "obs_9",
]
logger = Logger(log_list, log_path)
# Set control rate
rate = rospy.Rate(10)
# Time and iteration count
i = 0
start = rospy.Time.now()
while not rospy.is_shutdown():
# Record the time and the current observations
elapsed = rospy.Time.now() - start
observation, c_observation = controller.get_observations()
# Set the action
action = [a, 0.0]
joint_vels = controller.get_control(action)
controller._right_arm.set_joint_velocities(joint_vels)
# Log observations
elapsed_sec = elapsed.to_sec()
action_in_base = controller.base_rot_in_eef.dot(np.concatenate([action, [0.0]]))[:2]
goal_pos_in_base, eef_pos_in_base, object_pos_in_base, \
eef_vel_in_base, object_vel_in_base, z_angle = _convert_message_to_array(controller, c_observation)
logger.log(i, elapsed_sec,
action_in_base[0], action_in_base[1],
goal_pos_in_base[0], goal_pos_in_base[1], goal_pos_in_base[2],
eef_pos_in_base[0], eef_pos_in_base[1], eef_pos_in_base[2],
eef_vel_in_base[0], eef_vel_in_base[1], eef_vel_in_base[2],
object_pos_in_base[0], object_pos_in_base[1], object_pos_in_base[2],
object_vel_in_base[0], object_vel_in_base[1], object_vel_in_base[2],
z_angle,
observation[0], observation[1], observation[2],
observation[3], observation[4], observation[5],
observation[6], observation[7], observation[8], observation[9]
)
## Check that trajectory not stopped
c = intera_external_devices.getch()
if c in ['\x1b', '\x03']:
rospy.signal_shutdown("Shutting Down...")
# Change the action if above 2.5 sec
if (elapsed_sec >2.5):
a = 0.0
if(elapsed_sec > 3.0):
a = default_value
break
i += 1
rate.sleep()
controller.set_neutral()
done = False
print('Episode finished, press f or esc to quit or r to restart, q to change goal position')
while not done and not rospy.is_shutdown():
c = intera_external_devices.getch()
if c in ['\x1b', '\x03', 'f']:
done = True
rospy.signal_shutdown("Shutting Down...")
elif c in ['r']:
repeats += 1
done = True
elif c in ['q']:
# publish reset
reset_message = Bool()
reset_message.data = True
reset_publisher.publish(reset_message)
# controller reset
repeats = 0
goal_no += 1
controller.reset()
done = True
def map_keyboard(limb):
# initialize interfaces
print("Getting robot state...")
rs = intera_interface.RobotEnable(CHECK_VERSION)
init_state = rs.state()
try:
gripper = intera_interface.Gripper(limb)
except ValueError:
rospy.logerr("Could not detect a gripper attached to the robot.")
return
def clean_shutdown():
print("Exiting example.")
rospy.on_shutdown(clean_shutdown)
def offset_position(offset):
current = gripper.get_position()
gripper.set_position(current + offset)
def offset_holding(offset):
current = gripper.get_force()
gripper.set_holding_force(current + offset)
num_steps = 10.0
thirty_percent_velocity = 0.3 * (
gripper.MAX_VELOCITY - gripper.MIN_VELOCITY) + gripper.MIN_VELOCITY
bindings = {
# key: (function, args, description)
'r': (gripper.reboot, [], "reboot"),
'c': (gripper.calibrate, [], "calibrate"),
'q': (gripper.close, [], "close"),
'o': (gripper.open, [], "open"),
'+':
(gripper.set_velocity, [gripper.MAX_VELOCITY], "set 100% velocity"),
'-':
(gripper.set_velocity, [thirty_percent_velocity], "set 30% velocity"),
's': (gripper.stop, [], "stop"),
'h': (offset_holding, [-(gripper.MAX_FORCE / num_steps)],
"decrease holding force"),
'j': (offset_holding, [gripper.MAX_FORCE / num_steps],
"increase holding force"),
'u': (offset_position, [-(gripper.MAX_POSITION / num_steps)],
"decrease position"),
'i': (offset_position, [gripper.MAX_POSITION / num_steps],
"increase position"),
}
done = False
rospy.loginfo("Enabling robot...")
rs.enable()
print("Controlling grippers. Press ? for help, Esc to quit.")
while not done and not rospy.is_shutdown():
c = intera_external_devices.getch()
if c:
if c in ['\x1b', '\x03']:
done = True
elif c in bindings:
cmd = bindings[c]
cmd[0](*cmd[1])
print("command: %s" % (cmd[2], ))
else:
print("key bindings: ")
print(" Esc: Quit")
print(" ?: Help")
for key, val in sorted(bindings.items(),
key=lambda x: x[1][2]):
print(" %s: %s" % (key, val[2]))
# force shutdown call if caught by key handler
rospy.signal_shutdown("Example finished.")
def main():
parser = argparse.ArgumentParser()
parser.add_argument(
"-p",
"--log_path",
type=str,
default='../logs/internal_dynamics_calibration/trajectories.pckl',
help="Path where the trajectories produced are stored")
args = parser.parse_args(rospy.myargv()[1:])
log_path = args.log_path
rospy.init_node('calibrate_internal_dynamics', anonymous=True)
# Controller
controller = Control()
done = False
print('Ready. Press any key to start.')
while not done and not rospy.is_shutdown():
c = intera_external_devices.getch()
if c:
done = True
# Set control rate
rate = rospy.Rate(10)
trajectories_gathered = []
# Time and iteration count
for policy_id, policy_params in enumerate(_POLICIES):
if (rospy.is_shutdown()):
break
trajectories_gathered.append([])
controller.set_neutral()
time.sleep(2)
controller.set_policy_type(policy_params['policy_type'])
controller.set_period_amplitude(policy_params['period'],
policy_params['amplitude'])
controller.set_line_policy(policy_params['line_policy'])
steps = policy_params['steps']
start = rospy.Time.now()
for i in range(steps):
if (rospy.is_shutdown()):
break
elapsed = rospy.Time.now() - start
# Get observation
eef_pose_in_base = controller._right_arm.endpoint_pose()
eef_pos_in_base = np.array([
eef_pose_in_base['position'].x,
eef_pose_in_base['position'].y,
eef_pose_in_base['position'].z,
])
eef_quat_in_base = np.array([
eef_pose_in_base['orientation'].x,
eef_pose_in_base['orientation'].y,
eef_pose_in_base['orientation'].z,
eef_pose_in_base['orientation'].w,
])
eef_rot_in_base = T.quat2mat(eef_quat_in_base)
eef_vel_in_eef = controller._right_arm.endpoint_velocity()
eef_vel_in_eef = np.array([
eef_vel_in_eef['linear'].x,
eef_vel_in_eef['linear'].y,
eef_vel_in_eef['linear'].z,
])
eef_vel_in_base = eef_rot_in_base.dot(eef_vel_in_eef)
obs_joint_pos = controller._right_arm.joint_angles()
obs_joint_pos = np.array([
obs_joint_pos['right_j0'], obs_joint_pos['right_j1'],
obs_joint_pos['right_j2'], obs_joint_pos['right_j3'],
obs_joint_pos['right_j4'], obs_joint_pos['right_j5'],
obs_joint_pos['right_j6']
])
obs_joint_vels = controller._right_arm.joint_velocities()
obs_joint_vels = np.array([
obs_joint_vels['right_j0'],
obs_joint_vels['right_j1'],
obs_joint_vels['right_j2'],
obs_joint_vels['right_j3'],
obs_joint_vels['right_j4'],
obs_joint_vels['right_j5'],
obs_joint_vels['right_j6'],
])
# Find action
action = controller.get_policy(elapsed.to_sec(),
theshold_time=steps * (5. / 60.))
# Get control
if (controller.policy_type == 'joint'):
joint_vels = dict([('right_j{}'.format(idx), action[idx])
for idx in range(7)])
else:
joint_vels = controller.get_control(action)
# Send contgrol to robot
controller._right_arm.set_joint_velocities(joint_vels)
# Record
trajectories_gathered[-1].append([
eef_pos_in_base, eef_vel_in_base, obs_joint_pos, obs_joint_vels
])
rate.sleep()
#pickle results
log_path = os.path.join(os.path.dirname(os.path.realpath(__file__)),
log_path)
pickle.dump(trajectories_gathered, open(os.path.abspath(log_path), 'wb'))
rospy.signal_shutdown('Done')
