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 test_no_rosbridge():
"""Make sure that importing BlueInterface and instantiating it results in the correct
error when no rosbridge server is running.
(( assumes that no rosbridge server is running at localhost:444 ))
"""
from blue_interface import BlueInterface
try:
error = ConnectionRefusedError
except NameError:
# Python 2.7
error = Exception
with pytest.raises(error):
BlueInterface(side="left", ip="127.0.0.1", port=444)
#!/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
import time
import argparse
from blue_interface import BlueInterface
if __name__ == '__main__':
# load file name
if len(sys.argv) == 2:
filename = sys.argv[1]
else:
filename = "motion.pkl"
# 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)
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,
])
import pickle # will use to save data between program launches
import sys
import numpy as np
sys.path.append('blue_interface')
from blue_interface import BlueInterface # this is the API for the robot
blu = BlueInterface(
"right", "hekate.cs.berkeley.edu"
) # creates object of class KokoInterface at the IP in quotes with the name 'kk'
blu.disable_control(
) # this turns off any other control currently on the robot (leaves it in gravtiy comp mode)
import numpy as np
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
done = False
while not done:
s = raw_input(
"Type the pose number to which you want to return. Type 'd' for done. "
)
if s != 'd':
if int(s) in range(len(joint_angle_list)):
blu.set_joint_positions(np.array(joint_angle_list[int(
s)])) # tell the robot to go to a set of joint angles
print('position set')
else:
print "Please enter a valid pose number between 0 and %d " % (
len(joint_angle_list) - 1)
else:
done = True
#!/usr/bin/env python3
# A basic example of using BlueInterface for joint positions control.
# It allows a user to record four sets of joint positions by manually moving the arm to each
# position and pressing enter. It then plays back a trajectory comprised of the four sets of
# joint positions in an infinite loop.
import sys
from blue_interface import BlueInterface
import numpy as np
import time
if __name__ == '__main__':
arm = "right"
address = "127.0.0.1"
blue = BlueInterface(arm, address)
blue.set_joint_positions(np.zeros(7), duration=3.0)
while True:
pass
# blue.shutdown()
#!/usr/bin/env python3
from blue_interface import BlueInterface
import numpy as np
import time
import transformations as t
import spacemouse
import sys
if __name__ == '__main__':
arm_side = "right"
assert len(sys.argv) == 2
address = sys.argv[1]
# Python interface
blue = BlueInterface(arm_side, address)
home = [
-0.433836, -1.16384, 0.75438465, -1.58150699, -0.05635529, -1.67967716,
-0.13010218
]
home = [0.0, -1.571, 0.0, -1.571, 0.0, -1.571, 0.0]
blue.set_joint_positions( # TODO: this sometimes fights with the IK solver, and makes the robot really jittery
home,
duration=1.0,
# soft_position_control=True
)
position_control_mode = blue._control_mode # super sketchy
# Set up a publisher for our cartesian pose command (a little sketchy)
RBC = blue._RBC
pose_target_publisher = RBC.publisher(
#!/usr/bin/env python3
# A basic example of using BlueInterface for joint positions control.
import numpy as np
from blue_interface import BlueInterface
side = "right"
ip = "127.0.0.1"
blue = BlueInterface(side, ip)
blue.set_joint_positions(np.zeros(7), duration=3.0)
while True:
pass
# When this script terminates (eg via Ctrl+C), the robot will automatically
# stop all controllers and go back to gravity compensation mode
import Leap
from utils.rotations import quat2euler, euler2quat, mat2euler
from utils.leap_listener import SampleListener
import matplotlib.pyplot as plt
parser = argparse.ArgumentParser(description='switch the control mode')
parser.add_argument('--IK',
default=False,
action='store_true',
help='switch to IK-control')
args = parser.parse_args()
side = "right"
ip = "127.0.0.1"
blue = BlueInterface(side, ip)
# Initialize the blue gripper
blue.calibrate_gripper()
# Leap Motion
listener = SampleListener()
controller = Leap.Controller()
target_angles_init = np.array([0.0, -0.85, 1.571, 0, -1.571, -0.2, 0.0])
target_angles_hist = target_angles_init.copy()
i = 0
while True:
hands_data = listener.get_hand(controller)
## IK approach
#!/usr/bin/env python3
# A basic example of using BlueInterface for joint positions control.
from blue_interface import BlueInterface
import numpy as np
import sys
assert len(sys.argv) == 2
side = "right"
ip = sys.argv[1]
blue = BlueInterface(side, ip)
current_joints = blue.get_joint_positions()
blue.set_joint_positions(current_joints, duration=3.0)
while True:
pass
# When this script terminates (eg via Ctrl+C), the robot will automatically
# stop all controllers and go back to gravity compensation mode
import sys
sys.path.append('../../../../../blue_interface')
from blue_interface import BlueInterface
from control_benchmarking import ControllerVis
import time
import matplotlib.pyplot as plt
from sensor_msgs.msg import JointState
from std_msgs.msg import Float64MultiArray
from time import sleep
control_info = ControllerVis()
blu = BlueInterface("right","hekate.cs.berkeley.edu")
blu.disable_control()
sampling_delay = 1.0/125 # seconds, samples at about the same rate as the response is published (use rostopic hz /joint_states)
sample_duration = 2
# Helper functions
def record_rxn():
time_end = time.time() + sample_duration # 3 seconds from now
# Initialize
cmd = []
response = []
while time.time()< time_end:
cmd.append(control_info.get_reference_data())
response.append(control_info.get_response_data())
sleep(sampling_delay)
print "Samples collected: %d" % len(cmd)
import numpy as np #will use to create arrays
import pickle #will use to save data between program launches
import sys
sys.path.append('blue_interface')
from blue_interface import BlueInterface # this is the API for the robot
blu = BlueInterface("right","hekate.cs.berkeley.edu") #creates object of class KokoInterface at the IP in quotes with the name 'blu' # THIS IS THE LINE THAT GETS MY PGORAM STUCK FROM EXITING
blu.disable_control() #this turns off any other control currently on the robot (leaves it in gravtiy comp mode)
joint_angle_list = [] #Initialize the list to hold our joint positions
pose_list = []
done_collecting_positions = False #create a variable to track when we've collected all the desired poses
while not done_collecting_positions:
s = raw_input("Move the robot to a new position and press enter to record or enter d for done and press enter")
if s != "d": #if the user is not done
position = blu.get_joint_positions() #record the pose, this function returns a dictionary object
joint_angle_list.append(position)
pose = blu.get_cartesian_pose()
pose_list.append(pose)
print("Position recorded!")
else:
done_collecting_positions = True #set the variable such that the loop doesn't run again
if len(joint_angle_list)==0:
print('You did not save any positions')
else:
pickle.dump(joint_angle_list, open("../results/benchmark_joint_angle_list.p", "wb")) #uses the pickle function to write a binary file
pickle.dump(pose_list, open("../results/benchmark_pose_list.p", "wb"))
print('Your position list has been saved in the results directory')
#!/usr/bin/env python3
# A basic example of sending Blue a command in cartesian space.
from blue_interface import BlueInterface
import numpy as np
import time
side = "right"
ip = "127.0.0.1"
blue = BlueInterface(side, ip)
# Compute IK solution
target_position = [0.4, 0, 0] # x, y, z
target_orientation = [0.6847088, -0.17378805, -0.69229771,
-0.1472938] # x, y, z, w
target_joint_positions = blue.inverse_kinematics(target_position,
target_orientation)
# Send command to robot
blue.set_joint_positions(target_joint_positions, duration=5)
# Wait for system to settle
time.sleep(5)
# Print results
joint_positions = blue.get_joint_positions()
pose = blue.get_cartesian_pose()
print_aligned = lambda left, right: print("{:30} {}".format(
left, np.round(right, 4)))
print_aligned("Target joint positions: ", target_joint_positions)
#!/usr/bin/python3
import pickle
import sys
import numpy as np
import time
import argparse
from blue_interface import BlueInterface
if __name__ == '__main__':
# Blue Python interface
arm_side = "right"
address = "127.0.0.1"
blue = BlueInterface(arm_side, address)
blue.disable_control()
parser.add_argument('--frequency',
default=consts.default_frequency,
type=int,
help='Record at a custom frequency (Hz)')
args = parser.parse_args()
filename = args.record_file
arm = consts.default_arm
address = args.address
port = args.port
frequency = args.frequency # In Hertz
#blue = BlueInterface("left","10.42.0.1") # creates object of class KokoInterface at the IP in quotes with the name 'blue'
blue = BlueInterface(
arm, address, port
) #creates object of class KokoInterface at the IP in quotes with the name 'blue'
blue.disable_control(
) #this turns off any other control currently on the robot (leaves it in gravtiy comp mode)
joint_angle_list = [] #Initialize the list to hold our joint positions
pose_list = []
gripper_list = []
input(
"Press enter to start recording. To finish recording press <ctrl+c>.")
try:
last_time = 0.0
while True:
position = blue.get_joint_positions(
help='Port that the ros web host was started on')
parser.add_argument('--frequency',
default=0,
type=float,
help='Replay the recording at a custom frequency (Hz)')
args = parser.parse_args()
filename = args.record_file
arm = consts.default_arm
address = args.address
port = args.port
#blue = BlueInterface("left","10.42.0.1") # creates object of class KokoInterface at the IP in quotes with the name 'blue'
blue = BlueInterface(
arm, address, port
) #creates object of class KokoInterface at the IP in quotes with the name 'blue'
# This turns off any other control currently on the robot (leaves it in gravtiy comp mode)
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, record_frequency = data
# 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
#!/usr/bin/env python3
# A basic example of reading information about Blue's current state.
import time
import numpy as np
from blue_interface import BlueInterface
side = "right"
ip = "127.0.0.1"
blue = BlueInterface(side, ip)
def print_aligned(left, right):
print("{:30} {}".format(left, np.round(right, 4)))
while True:
print_aligned("End Effector Position:", blue.get_cartesian_pose()["position"])
print_aligned("End Effector Orientation:", blue.get_cartesian_pose()["orientation"])
print_aligned("Joint Positions:", blue.get_joint_positions())
print_aligned("Joint Velocities:", blue.get_joint_velocities())
print_aligned("Joint Torques:", blue.get_joint_torques())
print_aligned("Gripper Position:", blue.get_gripper_position())
print_aligned("Gripper Effort:", blue.get_gripper_effort())
print("=" * 30)
time.sleep(0.5)
#!/usr/bin/env python3
# A basic example of using BlueInterface for gripper control.
# 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
import argparse
import consts
if __name__ == '__main__':
parser = argparse.ArgumentParser(description='Replay a blue arm through a recorded set of positions.')
parser.add_argument('--tape_file', default = 'tape_trajectory.pickle', type=str, help='Saves a set of positions to this file')
parser.add_argument('--address', default=consts.default_address, type=str, help='Address of the host computer')
parser.add_argument('--port', default=consts.default_port, type=int, help='Port that the ros web host was started on')
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:
import pickle # will use to save data between program launches
import sys
sys.path.append('blue_interface')
from blue_interface import BlueInterface # this is the API for the robot
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 = [
import time
import sys
from blue_interface import BlueInterface # this is the API for the robot
if __name__ == '__main__':
# motion file name
if len(sys.argv) == 2:
filename = sys.argv[1]
else:
filename = "motion.pkl"
frequency = 100 # In Hertz
# Blue Python interface
arm_side = "right"
address = "127.0.0.1"
blue = BlueInterface(arm_side, address)
blue.disable_control(
) #this turns off any other control currently on the robot (leaves it in gravtiy comp mode)
joint_angle_list = [] #Initialize the list to hold our joint positions
pose_list = []
gripper_list = []
input(
"Press enter to start recording. To finish recording press <ctrl+c>.")
try:
last_time = 0.0
while True:
position = blue.get_joint_positions(
) #record the pose, this function returns a dictionary object
joint_angle_list.append(position)
