# It allows a user to open and close the gripper.
import sys
from blue_interface import BlueInterface
import numpy as np
import consts
if __name__ == '__main__':
blue = BlueInterface(
consts.default_arm, consts.default_address
) #creates object of class KokoInterface at the IP in quotes with the name 'blue'
opened = True
try:
while True:
input(
"Press enter to open/close the gripper. To exit, press <ctrl+c>."
)
if opened:
blue.command_gripper(-1.5, 20.0)
print("Closing...")
else:
blue.command_gripper(0.0, 10.0)
print("Opening...")
opened = not opened
except:
pass
blue.disable_control()
blue.shutdown()
sys.path.append('../../blue_vive_tracking/scripts')
from vive_tracking import ViveTracker
import random
import os
import time
import numpy as np
kk = BlueInterface(
"right", "hekate.cs.berkeley.edu"
) # creates object of class KokoInterface at the IP in quotes with the name 'kk'
kk.disable_control(
) # this turns off any other control currently on the robot (leaves it in gravtiy comp mode)
vv = ViveTracker()
kk.command_gripper(2.05, .1) #This isn't working
if not os.path.isfile('../results/benchmark_joint_angle_list.p'):
print("You should first run 'record_positions.py")
#TODO make this an actual error
joint_angle_list = pickle.load(
open("../results/benchmark_joint_angle_list.p", "rb")
) #uses the pickle function to read the binary file created in record_poses.py
pose_list = pickle.load(open("../results/benchmark_pose_list.p", "rb"))
location_list = [
0 for x in range(len(joint_angle_list))
] #initialize a list of locations where each entry is of the form [joint_angle_position, pose]
for k in range(len(joint_angle_list)):
location_ID = k
# If no argument is passed for replay frequency, play the recording at the rate it was recorded.
if args.frequency == 0:
frequency = record_frequency # In Hertz
else:
frequency = args.frequency
input("Press enter to start replay. To exit, press <ctrl+c>.")
while True:
try:
last_time = 0.0
for i in range(len(joint_angle_list)):
#if len(joint_angle_list[i]) == 7:
blue.set_joint_positions(
np.array(joint_angle_list[i])
) # tell the robot to go to a set of joint angles
#blue.command_gripper(gripper_list[i], 30.0)
if gripper_list[i] > 0:
blue.command_gripper(2, 15.0)
else:
blue.command_gripper(-0.8, 10.0)
sleep_time = 1.0 / frequency - (time.time() - last_time)
if sleep_time > 0:
time.sleep(sleep_time)
last_time = time.time()
except:
print(sys.exc_info()[0])
print("Something went wrong... exiting")
break
input_rot, current_orientation)
## Publishing target pose
pose_target_msg = {
"header": {
"frame_id": "base_link"
},
"pose": {
"position":
dict(zip(("x", "y", "z"), target_position)),
"orientation":
dict(zip(("x", "y", "z", "w"), target_orientation))
}
}
pose_target_publisher.publish(pose_target_msg)
## Gripper stuff
if gripper_closed != mouse.input_button1:
if gripper_closed:
# open gripper
blue.command_gripper(-1.0, 10.0)
else:
# close gripper
blue.command_gripper(2.00, 30.0)
gripper_closed = mouse.input_button1
prev_input_button0 = mouse.input_button0
## Sleep
time.sleep(0.01)
#!/usr/bin/env python3
# A basic example of using BlueInterface for gripper control.
# It allows a user to open and close the gripper.
from blue_interface import BlueInterface
import numpy as np
side = "right"
ip = "127.0.0.1"
blue = BlueInterface(side, ip)
# After calibration, the gripper will be closed
blue.calibrate_gripper()
opened = False
while True:
input("Press enter to open/close the gripper. To exit, press <ctrl+c>.")
if opened:
print("Closing...")
blue.command_gripper(-1.5, 20.0, wait=True)
else:
print("Opening...")
blue.command_gripper(0.0, 10.0, wait=True)
opened = not opened
# Blue Python interface
arm_side = "right"
address = "127.0.0.1"
blue = BlueInterface(arm_side, address)
blue.disable_control()
data = pickle.load( open(filename, "rb")) #uses the pickle function to read the binary file created in record_poses.py
joint_angle_list, _, gripper_list, frequency = data
# If no argument is passed for replay frequency, play the recording at the rate it was recorded.
input("Press enter to start replay. To exit, press <ctrl+c>.")
try:
last_time = 0.0
for i in range (len(joint_angle_list)):
#if len(joint_angle_list[i]) == 7:
blue.set_joint_positions(np.array(joint_angle_list[i])) # tell the robot to go to a set of joint angles
#blue.command_gripper(gripper_list[i], 30.0)
if gripper_list[i] < -0.2:
blue.command_gripper(-1.3, 15.0)
else:
blue.command_gripper(0, 2.0)
sleep_time = 1.0/frequency - (time.time() - last_time)
if sleep_time > 0:
time.sleep(sleep_time)
last_time = time.time()
except:
print (sys.exc_info()[0])
print ("Something went wrong... exiting")
pass
class BlueRobotEnv(object):
def __init__(self, from_pixels=True, height=100, width=100, camera_id=0, control="torque"):
side = "right"
ip = "127.0.0.1"
self.blue = BlueInterface(side, ip)
self.blue.calibrate_gripper()
self._from_pixels = from_pixels
self.control = control
self._frame_skip = frame_skip
self.channels_first=True
self.camera = StreamVideo(height=height, width=width, camera_id=camera_id)
self.camera.start()
# true and normalized action spaces
self._true_action_space = spaces.Box(
low=[-3.3999, -2.2944, -2.6761, -2.2944, -2.6761, -2.2944, -2.6761, 0.0],
high=[2.3412, 0, 2.6761, 0, 2.6761, 0, 2.676, 0.0],
shape=np.zeros(8),
dtype=np.float32
)
self._norm_action_space = spaces.Box(
low=-1.0,
high=1.0,
shape=self._true_action_space.shape,
dtype=np.float32
)
# create observation space
if from_pixels:
shape = [3, height, width] if self.channels_first else [height, width, 3]
self._observation_space = spaces.Box(
low=0, high=255, shape=shape, dtype=np.uint8
)
else:
self._observation_space = _spec_to_box(
self._env.observation_spec().values()
)
self._state_space = _spec_to_box(
self._env.observation_spec().values()
)
self.current_state = None
# set seed
self.seed(seed=task_kwargs.get('random', 1))
def __delete__(self):
self.camera.stop()
@property
def observation_space(self):
return self._observation_space
@property
def state_space(self):
return self._state_space
@property
def action_space(self):
return self._norm_action_space
def step(self,a):
for _ in range(self._frame_skip):
# time_step = self._env.step(action)
# reward += time_step.reward or 0
if control=="torque":
self.blue.set_joint_torques(a[:-1])
self.blue.command_gripper(a[-1], 20.0, wait=True)
else:
self.blue.set_joint_positions(a[:-1], duration=0.1)
self.blue.command_gripper(a[-1], 20.0, wait=True)
obs = self._get_obs()
reward = 0
done = False
return obs, reward, done, None
def reset(self):
init_pos = np.array([0, -2.31, 1.57, -0.75, -1.57, 0, 0])
self.blue.set_joint_positions(init_pos, duration=0.0)
self.blue.calibrate_gripper()
self.blue.command_gripper(0.0, 20.0, wait=False)
obs = self._get_obs()
return obs
def _get_obs(self):
if self._from_pixels:
obs = self.render()
if self._channels_first:
obs = obs.transpose(2, 0, 1).copy()
else:
obs = get_robot_joints()
return obs
def render(self):
return self.camera()
@property
def get_qpos(self):
joint = self.blue.get_joint_positions()
gripper = self.blue.get_gripper_position()
return np.concatenate((joint,gripper),axis=0)
@property
def get_qvel(self):
joint = self.blue.get_joint_velocities()
gripper = 0
return np.concatenate((joint,gripper),axis=0)
def get_robot_joints(self):
return np.concatenate([
self.get_qpos,
self.get_qvel,
])
args = parser.parse_args()
arm = consts.default_arm
address = args.address
port = args.port
filename = args.tape_file
blue = BlueInterface(arm, address, port) #creates object of class KokoInterface at the IP in quotes with the name 'blue'
print("..")
recorded_positions = []
error = 0.5
blue.disable_control()
blue.disable_gripper()
#TODO: make number of positions a command line arg
with open(filename, 'rb') as handle:
recorded_positions = pickle.load(handle)
input("Press enter to start trajectory.")
while True:
for desired_position in recorded_positions:
current_position = (blue.get_gripper_position(), blue.get_joint_positions())
blue.set_joint_positions(np.array(desired_position[1]))
blue.command_gripper(desired_position[0], 2.0, True)
while np.linalg.norm(desired_position[1] - current_position[1]) > error:
current_position = (blue.get_gripper_position(), blue.get_joint_positions())
time.sleep(0.1)
target_position[0] -= 0.4
target_position[2] += 0.3
target_orientation = list(euler2quat(ori)) # w, x, y, z
# target_orientation = target_orientation[1:] + target_orientation[:1]
# Compute IK solution
goal_curr = blue.inverse_kinematics(target_position,
target_orientation)
# Send command to robot
if goal_curr != []:
goal = goal_curr
print("goal: ", goal)
blue.set_joint_positions(goal,
duration=3,
soft_position_control=False)
blue.command_gripper(grab_strength, 10.0, wait=False)
# Wait for system to settle
i += 1
time.sleep(3)
# Direct motor angle mapping approach
else:
if "Right hand" in hands_data.keys():
hand_data = hands_data["Right hand"]
pos = hand_data['palm_position']
ori = [
hand_data['palm_pitch'], hand_data['palm_roll'],
hand_data['palm_yaw']
]
grab_strength = hand_data['grab_strength']