def __init__(self, simulation=True):
"""Initializes various aspects of the Fetch.
TODOs: get things working, also use `simulation` flag to change ROS
topic names if necessary (especially for the cameras!). UPDATE: actually
I don't think this is necessary now, they have the same topic names.
"""
rospy.init_node("fetch")
self.arm = Arm()
self.arm_joints = ArmJoints()
self.base = Base()
self.camera = RGBD()
self.head = Head()
self.gripper = Gripper(self.camera)
self.torso = Torso()
self.joint_reader = JointStateReader()
# Tucked arm starting joint angle configuration
self.names = ArmJoints().names()
self.tucked = [1.3200, 1.3999, -0.1998, 1.7199, 0.0, 1.6600, 0.0]
self.tucked_list = [(x, y) for (x, y) in zip(self.names, self.tucked)]
# Initial (x,y,yaw) position of the robot wrt map origin. We keep this
# fixed so that we can reset to this position as needed. The HSR's
# `omni_base.pose` (i.e., the start pose) returns (x,y,yaw) where yaw is
# the rotation about that axis (intuitively, the z axis). For the base,
# `base.odom` supplies both `position` and `orientation` attributes.
start = copy.deepcopy(self.base.odom.position)
yaw = Base._yaw_from_quaternion(self.base.odom.orientation)
self.start_pose = np.array([start.x, start.y, yaw])
self.TURN_SPEED = 0.3
self.num_restarts = 0
def check_legal(self, x):
rel_x1, rel_y1, rel_x2, rel_y2, rel_x3, rel_y3, grasp_ratio, cw1, ch1 = x
settings[0]['do_gui'] = self.do_gui
kitchen = Kitchen2D(**settings[0])
gripper = Gripper(kitchen, (5, 8), 0)
cup = ks.make_cup(kitchen, (0, 0), 0, cw1, ch1, 0.5)
spoon = ks.make_spoon(kitchen, (5, 10), 0, 0.2, 3, 1.)
gripper.set_grasped(spoon, grasp_ratio, (5, 10), 0)
dposa1, dposa2 = gripper.get_scoop_init_end_pose(
cup, (rel_x1, rel_y1), (rel_x3, rel_y3))
gripper.set_grasped(spoon, grasp_ratio, dposa1[:2], dposa1[2])
g2 = gripper.simulate_itself()
collision = g2.check_point_collision(dposa1[:2], dposa1[2])
if collision:
return False
collision = g2.check_point_collision(dposa2[:2], dposa2[2])
if collision:
return False
self.kitchen = kitchen
self.gripper = gripper
self.cup = cup
self.spoon = spoon
return True
def __init__(self, simulation=True):
"""Initializes various aspects of the Fetch."""
rospy.init_node("fetch")
rospy.loginfo("initializing the Fetch...")
self.arm = Arm()
self.arm_joints = ArmJoints()
self.base = Base()
self.camera = RGBD()
self.head = Head()
self.gripper = Gripper(self.camera)
self.torso = Torso()
self.joint_reader = JointStateReader()
# Tucked arm starting joint angle configuration
self.names = ArmJoints().names()
self.tucked = [1.3200, 1.3999, -0.1998, 1.7199, 0.0, 1.6600, 0.0]
self.tucked_list = [(x,y) for (x,y) in zip(self.names, self.tucked)]
# Initial (x,y,yaw) position of the robot wrt map origin. We keep this
# fixed so that we can reset to this position as needed. The HSR's
# `omni_base.pose` (i.e., the start pose) returns (x,y,yaw) where yaw is
# the rotation about that axis (intuitively, the z axis). For the base,
# `base.odom` supplies both `position` and `orientation` attributes.
start = copy.deepcopy(self.base.odom.position)
yaw = Base._yaw_from_quaternion(self.base.odom.orientation)
self.start_pose = np.array([start.x, start.y, yaw])
rospy.loginfo("...finished initialization!")
def __init__(self, service='/ur_driver/set_io'):
self.gripper = Gripper(service)
rospy.init_node('gripper_node', anonymous=True)
self.subscriber = rospy.Subscriber('/gripper/command',
String,
self.callback,
queue_size=1)
self.time_wait = 0
print('Created gripper_node')
rospy.spin()
def __init__ (self):
rospy.Subscriber("/head_camera/rgb/image_raw",Image,self.RGBImageCallback)
rospy.Subscriber("/head_camera/depth_registered/image_raw",Image,self.DepthImageCallback)
rospy.Subscriber("/ar_pose_marker",AlvarMarkers,self.GetArPosesCallBack)
self.bridge = CvBridge()
self.Arm = Arm()
self.Gripper = Gripper()
self.Head = Head()
self.PoseProcessing = PoseProcessing()
def __init__(self):
# create a RobotCommander
self.robot = RobotCommander()
# create a PlanningSceneInterface object
self.scene = PlanningSceneInterface()
# create arm
self.arm = Arm("manipulator")
# create gripper
self.gripper = Gripper()
def __init__(self):
# PyGame Initialization
pygame.display.init()
self.clock = pygame.time.Clock()
self.gui = Gui()
self.gripper = Gripper()
self.plan = []
self.cur_plan = []
self.cur_action = ''
self.target_name = None
self.beput_name = None
self.action_status = True #to check if the current action is done
self.plan_status = True #to check if the plan is done
def __init__(self):
""" Initializes the environment. """
# the state space (=observation space) are all possible depth images of the kinect camera
if (self.flattenImage):
self.observation_space = spaces.Box(
low=0,
high=255,
shape=[self.imageHeight * self.imageWidth],
dtype=np.uint8)
else:
self.observation_space = spaces.Box(
low=0,
high=255,
shape=[self.imageHeight, self.imageWidth],
dtype=np.uint8)
boundaries_r = [self.gripperRadiusMin, self.gripperRadiusMax]
boundaries_phi = [0, np.pi]
low = np.array([boundaries_r[0], boundaries_phi[0]])
high = np.array([boundaries_r[1], boundaries_phi[1]])
self.action_space = spaces.Box(low=low, high=high, dtype=np.float32)
self.reward_range = (-np.inf, np.inf)
self.seed()
# create object for box (object to pick up)
self.box = ObjectToPickUp(length=self.boxLength,
width=self.boxWidth,
height=self.boxHeight,
gripperRadius=self.gripperRadiusMax)
# create object for kinect
self.kinect = Kinect(self.imageWidth,
self.imageHeight,
x=0.0,
y=0.0,
z=1.0)
# create object for gripper
self.gripper = Gripper(self.gripperDistance,
self.gripperWidth,
self.gripperHeight,
r=0.0,
phi=0.0)
def make_body(kitchen, name, pose, args):
'''
A wrapper to create Box2d bodies based on their name, pose and size.
'''
if 'gripper' in name:
from gripper import Gripper
body = Gripper(kitchen, init_pos=pose[:2], init_angle=pose[2])
elif 'cup' in name:
body = make_cup(kitchen, pose[:2], pose[2], *args[name])
elif 'spoon' in name:
body = make_spoon(kitchen, pose[:2], pose[2], *args[name])
elif 'stir' in name:
body = make_stirrer(kitchen, pose[:2], pose[2], *args[name])
elif 'block' in name:
body = make_block(kitchen, pose[:2], pose[2], *args[name])
else:
raise NotImplementedError(name)
body.name = name
return body
def start(self):
self.server = ServerInterface()
# self.pixhawk = connect('/dev/cu.usbmodem1', baud = 115200, wait_ready=True) # for on mac via USB
# self.pixhawk = connect('/dev/ttyS0', baud = 57600, wait_ready=True) # for on the raspberry PI via telem2
# # self.pixhawk = connect('/dev/tty.usbserial-DA00BL49', baud = 57600) # telem radio on mac
# # self.pixhawk = connect('/dev/tty.SLAB_USBtoUART', baud = 57600) # telem radio on mac
# self.pixhawk.wait_ready(timeout=60)
# self.pixhawk.commands.download()
#self._log('Connected to pixhawk.')
#self._prev_pixhawk_mode = ''
self._prev_command = ''
#self._arming_window_start = 0
self._server_connect_timer = time.time()
#self.current_action = 'idle'
config_loaded = self._load_config() # load info about the uid and auth
online = True # TODO: verify internet connection
self.gripper = Gripper(18) # set up the gripper
self.button = Button(2) # set up the button
self.button.when_pressed = self.gripper.open
self.button.when_released = self.gripper.close
return config_loaded and online
def check_legal(self, x):
grasp_ratio, rel_x, rel_y, dangle, cw1, ch1, cw2, ch2 = x
dangle *= np.sign(rel_x)
settings[0]['do_gui'] = self.do_gui
kitchen = Kitchen2D(**settings[0])
gripper = Gripper(kitchen, (5, 8), 0)
cup1 = ks.make_cup(kitchen, (0, 0), 0, cw1, ch1, 0.5)
cup2 = ks.make_cup(kitchen, (-15, 0), 0, cw2, ch2, 0.5)
gripper.set_grasped(cup2, grasp_ratio, (-15, 0), 0)
gripper.set_position((rel_x, rel_y), 0)
if not kitchen.planning:
g2 = gripper.simulate_itself()
_, collision = g2.check_path_collision((rel_x, rel_y), 0,
(rel_x, rel_y), dangle)
if collision:
return False
self.kitchen = kitchen
self.gripper = gripper
self.cup1 = cup1
self.cup2 = cup2
return True
def main():
gripper = Gripper()
rospy.init_node('gripper_node', anonymous=True)
rate = rospy.Rate(500)
cmds = JointCommands()
# time_interval = 0.0
# vel_ref = np.array([0.0, 0.0])
pos_ref = [0.0, 0.0]
stiffness = [1.0, 1.0]
while not rospy.is_shutdown():
# vel_ref[0] = -4.0 * np.sin(time_interval)
# vel_ref[1] = 0.0
# time_interval += 2e-3
# cmds.vel_sat = vel_ref
cmds.pos_ref = pos_ref
cmds.stiffness = stiffness
gripper.gripperPub.publish(cmds)
joint_cmds = JointCommands()
time_interval = 0.0
pos_ref = np.array([0.0, 0.0])
vel_sat = np.array([3.0, 3.0])
tau_sat = np.array([5.0, 5.0])
stiffness = np.array([0.1, 0.1])
freq_anti_alias = 500.0
while not rospy.is_shutdown():
time_interval += 2e-3
pos_ref[0] = -1.0 * np.sin(time_interval)
joint_cmds.pos_ref = pos_ref
joint_cmds.vel_sat = vel_sat
joint_cmds.tau_sat = tau_sat
joint_cmds.stiffness = stiffness
joint_cmds.freq_anti_alias = freq_anti_alias
gripper.gripperPub.publish(joint_cmds)
rospy.loginfo("pos_ref[0]: {}".format(joint_cmds.pos_ref[0]))
rate.sleep()
def check_legal(self, x):
# for now, check always returns true
# n_stirs are always 5, ignore it for now
rel_pos1_x, rel_pos1_y, rel_pos2_x, rel_pos2_y, cup_w, cup_h, grasp_ratio = x
# creates the Box2D world objects to execute this action
kitchen = Kitchen2D(**settings[0])
gripper = Gripper(kitchen, (5, 8), 0)
cup = ks.make_cup(kitchen, (0, 0), 0, cup_w, cup_h, 0.5)
stirrer = ks.make_stirrer(kitchen, (0, 3.5), 0., 0.2, 5., 0.5)
gripper.set_grasped(stirrer, 0.8, (10, 10), 0)
# stirrer = ks.make_stirrer(kitchen, (0.5,1.0), 0, 0.2, 5., 0.5)
# print gripper.set_grasped(stirrer, 0.8, (0.5,1.0), 0)
dposa1, dposa2 = gripper.get_stir_init_end_pose(
cup, (rel_pos1_x, rel_pos1_y), (rel_pos2_x, rel_pos2_y))
# print gripper.set_grasped(stirrer, grasp_ratio, dposa1[:2], dposa1[2])
# seems that it almost never has collision
g2 = gripper.simulate_itself()
collision = g2.check_point_collision(
dposa1[:2], dposa1[2]) and g2.check_point_collision(
dposa2[:2], dposa2[2])
if collision:
return False
# print {"rel_pos1_x": rel_pos1_x, "rel_pos1_y": rel_pos1_y, "rel_pos2_x" : rel_pos2_x, \
# "rel_pos2_y": rel_pos2_y, "cup_w": cup_w, "cup_h": cup_h, "grasp_ratio": grasp_ratio, \
# "dposa1": dposa1, "dposa2": dposa2}
self.kitchen = kitchen
self.gripper = gripper
self.cup = cup
self.stirrer = stirrer
return True
def createScene(rootNode):
"""This is my first scene"""
MainHeader(rootNode, gravity=[0.0, -981.0, 0.0], plugins=["SoftRobots"])
ContactHeader(rootNode,
alarmDistance=4,
contactDistance=3,
frictionCoef=0.08)
Gripper(rootNode)
Floor(rootNode,
color=[1.0, 0.0, 0.0],
translation=[0.0, -160.0, 0.0],
isAStaticObject=True)
Cube(rootNode,
uniformScale=20.0,
color=[1.0, 1.0, 0.0],
totalMass=0.03,
volume=20,
inertiaMatrix=[1000.0, 0.0, 0.0, 0.0, 1000.0, 0.0, 0.0, 0.0, 1000.0],
translation=[0.0, -130.0, 10.0])
return rootNode
#Q2 = [-5.773678247128622, -1.7172182242022913, 2.291447162628174, -2.1666935125934046, -1.5579717795001429, -0.2839697043048304]
#Q2 = [-5.80492484966387, -1.7727258841144007, 2.351362943649292, -2.23500901857485, -1.5296443144427698, -0.3075040022479456]
Q2 = [-5.802346740161077, -1.727706257496969, 2.3336830139160156, -2.251301113759176, -1.537558380757467, -0.30245906511415654]
#Q2 = (-5.790848229323522, -1.6641319433795374, 2.2818641662597656, -2.133308235798971, -1.57269794145693, -0.289194409047262)
#Q2 = [-5.807982299719946, -1.7263277212726038, 2.412261724472046, -2.3110459486590784, -1.5310810248004358, -0.2967436949359339]
Q3 = [i-j for i, j in zip(Q1, Q2)]
Q5 = [-5.233350698147909, -2.263735596333639, 2.0414814949035645, -1.8042591253863733, -1.5754278341876429, -1.077151123677389]
Q6 = [-3.9884613196002405, -1.7695258299456995, 1.7130355834960938, -1.5220916906939905, -1.5590489546405237, -1.6418374220477503]
# moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_group_python_interface_tutorial',
anonymous=True)
grip = Gripper()
grip.open()
# robot = moveit_commander.RobotCommander()
# scene = moveit_commander.PlanningSceneInterface()
# group_name = "manipulator"
# group = moveit_commander.MoveGroupCommander(group_name)
# display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
# moveit_msgs.msg.DisplayTrajectory,
# queue_size=20)
JOINT_NAMES = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']
client = actionlib.SimpleActionClient('follow_joint_trajectory', FollowJointTrajectoryAction)
client.wait_for_server()
printc(repr(eval(eval_str.get())))
eval_str.set("")
robot = None
ip = "192.168.65.129"
capturing = True
last_error = ""
recording = False
frame = None
last_frame_time = None
new_frame = False
spheres = {}
gripper = Gripper(2, 1, virtual=True)
gripper.activate()
vid_cap_thread = threading.Thread(target=vid_cap_thread_func, args=(0, ))
vid_cap_thread.start()
trajectories = {14: [], 36: []}
last_known_location = {}
# Set up GUI
window = tk.Tk() # Makes main window
window.wm_title("Tracking")
window.config()
# Graphics window
imageFrame = tk.Frame(window, width=600, height=500)
def main():
gripper = Gripper()
rospy.init_node('close', anonymous=True)
gripper.close()
def startMission():
vrep.simxFinish(-1)
clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5)
while clientID <= -1:
clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5)
if clientID != -1:
print('Successful !!!')
else:
print('connection not successful')
#sys.exit('could not connect')
print('connected to remote api server')
#vrep.simxStartSimulation(clientID,vrep.simx_opmode_oneshot)
sens = ProxSensor(clientID)
lineSensors = IRSensors(clientID)
imu = IMU(clientID)
regulator = pidControl(0.006, 0.001, 0.001)
cameraAreaControl = relle(10000, 10500, 0.03)
cameraPosControl = relle(124, 126, 0.005)
car = Robot(clientID, 0.04)
camera = Cam(clientID)
imuFilter = compliment()
gripper = Gripper(clientID)
step = 1
velocity = 0.1
crater1 = np.array([3, 4])
crater2 = np.array([3, 7])
def moveDir(vel, pos):
dt = 0
edgeS = False #машинка на склоне
edgeS2 = False #машинка проехала склон
position = 1
while (position < pos):
ts = vrep.simxGetLastCmdTime(clientID)
camera.getCameraImage()
sign, x, y, area = camera.trackObject('red')
cameraPosControl.control(y)
if area > 120 and area < 140:
position = 3
elif area > 180 and area < 200:
position = 4
elif area > 260 and area < 300:
position = 5
elif area > 450 and area < 550:
position = 6
elif area > 1300 and area < 1800:
position = 7
elif area > 2400 and area < 3000:
position = 8
if (sign):
U1 = cameraPosControl.U
else:
U1 = 0.02
car.move(-U1 - vel, +U1 - vel, 0, dt)
print(area, position)
#if(position>=pos and y<128 and y>122):
# break
#sens.updateAllSensors()
#regulator.pid(sens.lsVal-0.4,dt)
#imu.upgrateIMU()
#imuFilter.filter(imu.accelData,imu.gyroData,dt)
#if(imuFilter.roll>0.2 and not lineSensors.getGreenColorSignal()):
# U=0
# if not edgeS2 and not edgeS:
# edgeS=True
#else:
# #U=regulator.U
# U=0
#if(imuFilter.roll<0.2 and edgeS):
# edgeS2=True
# car.position=0
# edgeS=False
#if edgeS:
# car.position=0
#car.move(-vel-U,-vel+U,0,dt)
dt = (vrep.simxGetLastCmdTime(clientID) - ts) / 1000
car.stop()
car.position = 0
def moveWhile(color):
dt = 0.001
lineSensors.updateLineSensors()
lineSensors.getBlackColorSignal()
lineSensors.getGreenColorSignal()
signal = False
while (not signal):
ts = vrep.simxGetLastCmdTime(clientID)
lineSensors.getBlackColorSignal()
lineSensors.getGreenColorSignal()
if color == 'green':
signal = lineSensors.greenSignal
elif color == 'black':
signal = lineSensors.blackSignal
#print(lineSensors.blackSignal )
car.move(-velocity, -velocity, 0, dt)
dt = (vrep.simxGetLastCmdTime(clientID) - ts) / 1000
time.sleep(2)
car.stop()
def getBall():
dt = 0.001
camera.setCameraPosition(0, 0)
while True:
ts = vrep.simxGetLastCmdTime(clientID)
camera.getCameraImage()
sign, x, y, area = camera.trackObject('blue')
cameraAreaControl.control(area)
cameraPosControl.control(y)
if (sign):
U1 = cameraPosControl.U
U2 = cameraAreaControl.U
#print(y)
else:
U1 = 0.02
U2 = 0
car.move(-U2 - U1, -U2 + U1, 0, dt)
#print(area)
if (area > 6700 and y < 126 and y > 122):
car.stop()
gripper.giveBall()
break
camera.showStream()
dt = (vrep.simxGetLastCmdTime(clientID) - ts) / 1000
def searchDir(color):
camera.setCameraPosition(30, 0.8)
dt = 0.001
while True:
ts = vrep.simxGetLastCmdTime(clientID)
camera.getCameraImage()
sign, x, y, area = camera.trackObject(color)
cameraAreaControl.control(area)
cameraPosControl.control(y)
if (sign):
U1 = cameraPosControl.U
U2 = cameraAreaControl.U
else:
U1 = 0.02
U2 = 0
car.move(-U2 - U1, -U2 + U1, 0, dt)
#print(area)
if (area > 1000 and y < 128 and y > 122):
car.move(-velocity, -velocity, 0, dt)
break
camera.showStream()
dt = (vrep.simxGetLastCmdTime(clientID) - ts) / 1000
def colibrate():
camera.setCameraPosition(30, 0.8)
dt = 0.001
while True:
ts = vrep.simxGetLastCmdTime(clientID)
camera.getCameraImage()
sign, x, y, area = camera.trackObject('red')
cameraAreaControl.control(area)
cameraPosControl.control(y)
if (sign):
U1 = cameraPosControl.U
U2 = cameraAreaControl.U
else:
U1 = 0.02
U2 = 0
car.move(-2 * U2 - U1, -2 * U2 + U1, 0, dt)
#print(area)
if (y < 128 and y > 122):
car.stop()
camera.destroyAllStream()
break
camera.showStream()
dt = (vrep.simxGetLastCmdTime(clientID) - ts) / 1000
def makeOperation(pos):
colibrate()
moveDir(velocity, pos)
car.rotate(80)
moveWhile('black')
getBall()
if pos > 5:
searchDir('blue')
searchDir('green')
moveWhile('green')
gripper.brokeBall()
makeOperation(crater1[1])
makeOperation(crater2[1])
car.stop()
camera.destroyAllStream()
time.sleep(1)
vrep.simxFinish(clientID)
import time
from gripper import Gripper
from sensor_system import SensorSystem
gripper = Gripper()
sensors = SensorSystem()
gripper.move_clockwise()
print("moving")
while True:
if gripper.check_dir() == 0:
if sensors.detect_bottom():
print("bottom")
gripper.stop()
time.sleep(2)
if sensors.detect_ball():
gripper.move_counter_clockwise()
print("ball caught")
if sensors.detect_max_pos_d():
print("maxd")
gripper.move_counter_clockwise()
else:
if sensors.detect_max_pos_u():
print("maxu")
gripper.stop()
time.sleep(3)
gripper.move_clockwise()
print("moving")
time.sleep(0.01)
import rospy
import time
from robot_arm import RobotArm
from ROSInterface import ROSInterface
from gripper import Gripper
from headController import FetchHeadController
from gripInterface import GripInterface
from originInterface import OriginInterface
if __name__ == "__main__":
# Setup clients
Ros = ROSInterface()
Arm = RobotArm()
GripRos = GripInterface()
GripHand = Gripper()
HeadTilt = FetchHeadController()
OriginROS = OriginInterface()
HeadTilt.look_at(0.7, 0, 0.7, "base_link")
print("Yes I looked")
while 1:
try:
Ros.Subscriber()
GripRos.GripSubscriber()
break
except:
rospy.loginfo('waiting for matlab')
while not rospy.is_shutdown():
Ros.PPublisher()
def move(c, args):
"""If you want to let the cloth settle, just run `c.update()` beforehand.
Careful, changing width/height will add more points but not make it stable;
the cloth 'collapses' ... need to investigate code?
For tensioning, wherever tension it by default has z-coordinate of 0,
because we assume a tool has pinched it at that point.
"""
grip = Gripper(cloth=c)
start_t = time.time()
# Will put this in a separate class soon. Need a 'pin' and then we can pull.
circlex = 300
circley = 300
c.pin_position(circlex, circley)
tensioner = c.tensioners[0]
if args.viz_tool == 'matplotlib':
if not args.norender:
# Use `plt.ion()` for interactive plots, requires `plt.pause(...)` later.
nrows, ncols = 1, 2
fig = plt.figure(figsize=(12 * ncols, 12 * nrows))
ax1 = fig.add_subplot(1, 2, 1)
ax2 = fig.add_subplot(1, 2, 2, projection='3d')
plt.ion()
plt.tight_layout()
cid = fig.canvas.mpl_connect('button_press_event', mouse.clicked)
rid = fig.canvas.mpl_connect('button_release_event',
mouse.released)
mid = fig.canvas.mpl_connect('motion_notify_event', mouse.moved)
for i in range(args.num_sim_iters):
if i % 10 == 0:
elapsed_time = (time.time() - start_t) / 60.0
print("Iteration {}, minutes: {:.1f}".format(i, elapsed_time))
z_vals = [p.z for p in c.shapepts]
print(" average z: {}".format(np.mean(z_vals)))
print(" median z: {}".format(np.median(z_vals)))
if not args.norender:
ax1.cla()
ax2.cla()
pull(i, tensioner)
# ----------------------------------------------------------------------
# Re-insert the points, with appropriate colors. 2D AND 3D together.
# ----------------------------------------------------------------------
if not args.norender:
pts = np.array([[p.x, p.y, p.z] for p in c.normalpts])
cpts = np.array([[p.x, p.y, p.z] for p in c.shapepts])
if len(pts) > 0:
ax1.scatter(pts[:, 0], pts[:, 1], c='g')
ax2.scatter(pts[:, 0], pts[:, 1], pts[:, 2], c='g')
if len(cpts) > 0:
ax1.scatter(cpts[:, 0], cpts[:, 1], c='b')
ax2.scatter(cpts[:, 0], cpts[:, 1], cpts[:, 2], c='b')
ax2.set_zlim([0, 300]) # only for visualization purposes
plt.pause(0.001)
# ----------------------------------------------------------------------
# Updates (+5 extra) to allow cloth to respond to environment. Think of
# it as like a 'frame skip' parameter.
for _ in range(args.updates_per_move):
c.simulate()
if not args.norender:
fig.canvas.mpl_disconnect(cid)
fig.canvas.mpl_disconnect(mid)
fig.canvas.mpl_disconnect(rid)
elif args.viz_tool == 'pyopengl':
# Note: 1/60 ~ 0.016 so we might as well try this way ...
target_fps = 60
clock = pygame.time.Clock()
dt = 1.0 / float(target_fps)
# Sequence of update then draw commands
for i in range(args.num_sim_iters):
if not get_input(): break
if i % 10 == 0:
elapsed_time = (time.time() - start_t) / 60.0
print("Iteration {}, minutes: {:.1f}".format(i, elapsed_time))
pull(i, tensioner)
for _ in range(args.updates_per_move):
c.simulate()
# Draw
draw(c)
clock.tick(target_fps)
pygame.quit()
elapsed_time = (time.time() - start_t) / 60.0
print("Total time, minutes: {:.2f}".format(elapsed_time))
def __init__(self, context):
super(SimpleGUI, self).__init__(context)
self.setObjectName('SimpleGUI')
self._widget = QWidget()
self._widget.setFixedSize(600, 600)
self._sound_client = SoundClient()
rospy.Subscriber('robotsound', SoundRequest, self.sound_cb)
QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10))
self.sound_sig.connect(self.sound_sig_cb)
large_box = QtGui.QVBoxLayout()
# Textbox to enter words for PR2 to say
sound_textbox = QtGui.QLineEdit("Squirtle Squirtle") # Default Text
sound_textbox.setFixedWidth(450)
self.marker_publisher = rospy.Publisher('visualization_marker', Marker)
# Set a handler on the textbox to retrieve the text when button clicked
self.sound_textbox = sound_textbox
button_box = QtGui.QHBoxLayout()
button_box.addItem(QtGui.QSpacerItem(15, 20))
button_box.addWidget(self.create_button('Speak', self.command_cb))
button_box.addWidget(sound_textbox)
button_box.addStretch(1)
large_box.addLayout(button_box)
speech_box = QtGui.QHBoxLayout()
speech_box.addItem(QtGui.QSpacerItem(15, 20))
self.speech_label = QtGui.QLabel('Robot has not spoken yet')
palette = QtGui.QPalette()
palette.setColor(QtGui.QPalette.Foreground, QtCore.Qt.blue)
self.speech_label.setPalette(palette)
speech_box.addWidget(self.speech_label)
large_box.addLayout(speech_box)
large_box.addStretch(1)
#large_box.addItem(QtGui.QSpacerItem(50,20))
# Buttons to move the PR2's head
up_head = Head(Head.UP, self)
head_box = QtGui.QVBoxLayout()
up_head_box = QtGui.QHBoxLayout()
up_head_button = self.create_button('^', up_head.create_closure())
down_head = Head(Head.DOWN, self)
down_head_box = QtGui.QHBoxLayout()
down_head_button = self.create_button('v', down_head.create_closure())
right_head = Head(Head.RIGHT, self)
right_head_button = self.create_button('>',
right_head.create_closure())
left_head = Head(Head.LEFT, self)
left_head_button = self.create_button('<', left_head.create_closure())
left_right_head_box = QtGui.QHBoxLayout()
up_head_box.addItem(QtGui.QSpacerItem(235, 20))
up_head_box.addWidget(up_head_button)
up_head_box.addItem(QtGui.QSpacerItem(275, 20))
left_right_head_box.addItem(QtGui.QSpacerItem(160, 20))
left_right_head_box.addWidget(left_head_button)
left_right_head_box.addItem(QtGui.QSpacerItem(60, 20))
left_right_head_box.addWidget(right_head_button)
left_right_head_box.addItem(QtGui.QSpacerItem(225, 20))
down_head_box.addItem(QtGui.QSpacerItem(235, 20))
down_head_box.addWidget(down_head_button)
down_head_box.addItem(QtGui.QSpacerItem(275, 20))
head_box.addLayout(up_head_box)
head_box.addLayout(left_right_head_box)
head_box.addLayout(down_head_box)
large_box.addLayout(head_box)
# Buttons to move the grippers
gripper = Gripper(Gripper.RIGHT, Gripper.OPEN, self)
right_gripper = self.create_button('Right gripper',
gripper.create_closure())
gripper = Gripper(Gripper.LEFT, Gripper.OPEN, self)
left_gripper = self.create_button('Left gripper',
gripper.create_closure())
large_box.addItem(QtGui.QSpacerItem(100, 250))
gripper_box = QtGui.QHBoxLayout()
gripper_box.addItem(QtGui.QSpacerItem(75, 20))
gripper_box.addWidget(left_gripper)
gripper_box.addItem(QtGui.QSpacerItem(450, 20))
gripper_box.addWidget(right_gripper)
gripper_box.addItem(QtGui.QSpacerItem(75, 20))
large_box.addLayout(gripper_box)
# Buttons to move the base
base_box = QtGui.QVBoxLayout()
large_box.addItem(QtGui.QSpacerItem(100, 100))
forward_base_box = QtGui.QHBoxLayout()
forward_base = Base(Base.FORWARD, self)
forward_base_button = self.create_button('move forward',
forward_base.create_closure())
forward_base_box.addItem(QtGui.QSpacerItem(400, 20))
forward_base_box.addWidget(forward_base_button)
forward_base_box.addItem(QtGui.QSpacerItem(400, 20))
base_box.addLayout(forward_base_box)
left_right_base_box = QtGui.QHBoxLayout()
left_base = Base(Base.LEFT, self)
left_base_button = self.create_button('move left',
left_base.create_closure())
right_base = Base(Base.RIGHT, self)
right_base_button = self.create_button('move right',
right_base.create_closure())
left_right_base_box.addItem(QtGui.QSpacerItem(300, 20))
left_right_base_box.addWidget(left_base_button)
left_right_base_box.addItem(QtGui.QSpacerItem(50, 20))
left_right_base_box.addWidget(right_base_button)
left_right_base_box.addItem(QtGui.QSpacerItem(300, 20))
base_box.addLayout(left_right_base_box)
backward_base_box = QtGui.QHBoxLayout()
backward_base = Base(Base.BACKWARD, self)
backward_base_button = self.create_button(
'move backward', backward_base.create_closure())
backward_base_box.addItem(QtGui.QSpacerItem(400, 20))
backward_base_box.addWidget(backward_base_button)
backward_base_box.addItem(QtGui.QSpacerItem(400, 20))
base_box.addLayout(backward_base_box)
large_box.addLayout(base_box)
turn_base_box = QtGui.QHBoxLayout()
counter_base = Base(Base.COUNTER, self)
counter_base_button = self.create_button('\\\n -->',
counter_base.create_closure())
clockwise_base = Base(Base.CLOCKWISE, self)
clockwise_base_button = self.create_button(
' /\n<--', clockwise_base.create_closure())
turn_base_box.addItem(QtGui.QSpacerItem(75, 20))
turn_base_box.addWidget(counter_base_button)
turn_base_box.addItem(QtGui.QSpacerItem(225, 20))
turn_base_box.addWidget(clockwise_base_button)
turn_base_box.addItem(QtGui.QSpacerItem(100, 20))
large_box.addLayout(turn_base_box)
self._widget.setObjectName('SimpleGUI')
self._widget.setLayout(large_box)
context.add_widget(self._widget)
self._widget.setStyleSheet(
"QWidget { image: url(%s) }" %
(str(os.path.dirname(os.path.realpath(__file__))) +
"/../../rosie_background.jpg"))
def __init__(self, mp):
# add mp to the hub
self.mp = mp
self.queue = {
"bound.rightFront.signalA":0,
"bound.leftFront.signalA":0,
"bound.rightRear.signalA":0,
"bound.leftRear.signalA":0,
"bound.rightFront.signalB":0,
"bound.leftFront.signalB":0,
"bound.rightRear.signalB":0,
"bound.leftRear.signalB":0,
"vis.sick.localScanData":0,
"vis.forwardObstacleDist":0,
"vis.forwardObstacle.x":0,
"vis.forwardObstacle.y":0,
"loc.rawOdometry.x":0,
"loc.rawOdometry.y":0,
"loc.rawOdometry.h":0,
"loc.rawOdometry.v":0,
"loc.rawOdometry.w":0,
"loc.rawOdometryWGyro.x":0,
"loc.rawOdometryWGyro.y":0,
"loc.rawOdometryWGyro.h":0,
"loc.rawOdometryWGyro.v":0,
"loc.rawOdometryWGyro.w":0,
"loc.actual.x":0,
"loc.actual.y":0,
"manip.botGotPot":0
}
# initialize the ir cameras to colect signals
# codes are for signalA, signalB
# NOTE: there are many other data elements that can be included here
# probe id 1513: bound.rightFront.signalA type float length 4
# probe id 1514: bound.rightFront.signalB type float length 4
# probe id 1563: bound.leftFront.signalA type float length 4
# probe id 1564: bound.leftFront.signalB type float length 4
# probe id 1613: bound.leftRear.signalA type float length 4
# probe id 1614: bound.leftRear.signalB type float length 4
# probe id 1662: bound.rightRear.signalA type float length 4
# probe id 1663: bound.rightRear.signalB type float length 4
# self.rightFrontA = IrCamera("bound.rightFront.signalA",1513,self.mp)
# self.leftFrontA = IrCamera("bound.leftFront.signalA",1563,self.mp)
# self.rightBackA = IrCamera("bound.rightRear.signalA",1613,self.mp)
# self.leftBackA = IrCamera("bound.leftRear.signalA",1662,self.mp)
# self.rightFrontB = IrCamera("bound.rightFront.signalB",1514,self.mp)
# self.leftFrontB = IrCamera("bound.leftFront.signalB",1564,self.mp)
# self.rightBackB = IrCamera("bound.rightRear.signalB",1614,self.mp)
# self.leftBackB = IrCamera("bound.leftRear.signalB",1663,self.mp)
self.rightFront = IrCamera(("bound.rightFront.signalA","bound.rightFront.signalB"),(1513,1514),self.mp)
self.leftFront = IrCamera(("bound.leftFront.signalA","bound.leftFront.signalB"),(1563,1564),self.mp)
self.rightRear = IrCamera(("bound.rightRear.signalA","bound.rightRear.signalB"),(1613,1614),self.mp)
self.leftRear = IrCamera(("bound.leftRear.signalA","bound.leftRear.signalB"),(1662,1663),self.mp)
# initialize the lidar camera
# codes: start laser scan collection, collect raw data
# probe id 3275: behaviors.interactiveSickRange.run type bool length 1 <-- this starts the lidar data collection
# probe id 2866: vis.sick.globalScanData type tlv length 4372
# probe id 2867: vis.sick.localScanData type tlv length 4372
# probe id 2868: vis.sick.localScanData_EVERY_TICK type tlv length 4372
# self.lidar = LidarCamera("vis.sick.localScanData",3275,self.mp)
# initialize the nearest obstacle detection of the preprocessed laser data
# codes give: x, y, and distance
# probe id 2928: vis.forwardObstacle.x type float length 4
# probe id 2929: vis.forwardObstacle.y type float length 4
# probe id 2930: vis.forwardObstacleDist type float length 4
self.forwardObstacleDist = LidarCameraNearestObstacle("vis.forwardObstacleDist",2930,self.mp)
self.forwardObstacleX = LidarCameraNearestObstacle("vis.forwardObstacle.x",2928,self.mp)
self.forwardObstacleY = LidarCameraNearestObstacle("vis.forwardObstacle.y",2929,self.mp)
# initialize odometery
# probe id 2438: loc.actual.x type float length 4
# probe id 2439: loc.actual.y type float length 4
# probe id 2456: loc.rawOdometry.x type float length 4
# probe id 2457: loc.rawOdometry.y type float length 4
# probe id 2458: loc.rawOdometry.h type float length 4
# probe id 2459: loc.rawOdometry.v type float length 4
# probe id 2460: loc.rawOdometry.w type float length 4
# probe id 2461: loc.rawOdometry.l type float length 4
# probe id 2462: loc.rawOdometry.r type float length 4
# probe id 2463: loc.rawOdometryWGyro.x type float length 4
# probe id 2464: loc.rawOdometryWGyro.y type float length 4
# probe id 2465: loc.rawOdometryWGyro.h type float length 4
# probe id 2466: loc.rawOdometryWGyro.v type float length 4
# probe id 2467: loc.rawOdometryWGyro.w type float length 4
# probe id 2472: loc.useGyro type bool length 1
# probe id 2479: loc.magicGps.x type double length 8
# probe id 2480: loc.magicGps.y type double length 8
# probe id 2481: loc.magicGps.h type double length 8
#self.odometeryX = Odometer("loc.rawOdometry.x",2456,self.mp)
#self.odometeryY = Odometer("loc.rawOdometry.y",2457,self.mp)
self.odometery = Odometer(("loc.rawOdometry.x","loc.rawOdometry.y","loc.rawOdometry.h", "loc.rawOdometry.v", "loc.rawOdometry.w"),(2456,2457,2458,2459,2460),self.mp)
self.odometeryGyro = Odometer(("loc.rawOdometryWGyro.x","loc.rawOdometryWGyro.y","loc.rawOdometryWGyro.h", "loc.rawOdometryWGyro.v", "loc.rawOdometryWGyro.w"),(2463,2464,2465,2466,2467),self.mp)
#probe id 1100: manip.botGotPot type bool length 1
self.gripper = Gripper("manip.botGotPot",1100,self.mp)
def __init__(self, context):
super(WaterPulse, self).__init__(context)
self.setObjectName('WaterPulse')
self._widget = QWidget()
self._widget.setFixedSize(600, 600)
self._sound_client = SoundClient()
rospy.Subscriber('robotsound', SoundRequest, self.sound_cb)
self.l_joint_names = [
'l_shoulder_pan_joint', 'l_shoulder_lift_joint',
'l_upper_arm_roll_joint', 'l_elbow_flex_joint',
'l_forearm_roll_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint'
]
l_traj_contoller_name = None
l_traj_controller_name = '/l_arm_controller/joint_trajectory_action'
self.l_traj_action_client = SimpleActionClient(l_traj_controller_name,
JointTrajectoryAction)
rospy.loginfo('Waiting for a response from the trajectory ' +
'action server for LEFT arm...')
self.l_traj_action_client.wait_for_server()
QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10))
self.sound_sig.connect(self.sound_sig_cb)
large_box = QtGui.QVBoxLayout()
# Textbox to enter words for PR2 to say
sound_textbox = QtGui.QLineEdit("Squirtle Squirtle") # Default Text
sound_textbox.setFixedWidth(450)
self.marker_publisher = rospy.Publisher('visualization_marker', Marker)
# Set a handler on the textbox to retrieve the text when button clicked
self.sound_textbox = sound_textbox
button_box = QtGui.QHBoxLayout()
button_box.addItem(QtGui.QSpacerItem(15, 20))
button_box.addWidget(self.create_button('Speak', self.command_cb))
button_box.addWidget(sound_textbox)
button_box.addStretch(1)
large_box.addLayout(button_box)
# 'Trick or Treat' & 'Thank You' Buttons
halloween_box = QtGui.QHBoxLayout()
halloween_box.addItem(QtGui.QSpacerItem(15, 20))
halloween_box.addWidget(
self.create_button('Trick or Treat', self.command_cb))
halloween_box.addWidget(
self.create_button('Thank You', self.command_cb))
halloween_box.addStretch(1)
large_box.addLayout(halloween_box)
# Buttons to move the PR2's head
up_head = Head(Head.UP, self)
head_box = QtGui.QVBoxLayout()
up_head_box = QtGui.QHBoxLayout()
up_head_button = self.create_button('^', up_head.create_closure())
down_head = Head(Head.DOWN, self)
down_head_box = QtGui.QHBoxLayout()
down_head_button = self.create_button('v', down_head.create_closure())
right_head = Head(Head.RIGHT, self)
right_head_button = self.create_button('>',
right_head.create_closure())
left_head = Head(Head.LEFT, self)
left_head_button = self.create_button('<', left_head.create_closure())
left_right_head_box = QtGui.QHBoxLayout()
head_speed_sld = QtGui.QSlider(QtCore.Qt.Horizontal)
head_speed_sld.setFocusPolicy(QtCore.Qt.NoFocus)
head_speed_sld.setMinimum(1)
head_speed_sld.setMaximum(5)
head_speed_sld.valueChanged[int].connect(Head.set_speed)
up_head_box.addItem(QtGui.QSpacerItem(235, 20))
up_head_box.addWidget(up_head_button)
up_head_box.addItem(QtGui.QSpacerItem(275, 20))
left_right_head_box.addItem(QtGui.QSpacerItem(160, 20))
left_right_head_box.addWidget(left_head_button)
left_right_head_box.addItem(QtGui.QSpacerItem(60, 20))
left_right_head_box.addWidget(right_head_button)
left_right_head_box.addItem(QtGui.QSpacerItem(225, 20))
down_head_box.addItem(QtGui.QSpacerItem(235, 20))
down_head_box.addWidget(down_head_button)
down_head_box.addItem(QtGui.QSpacerItem(275, 20))
head_sld_box = QtGui.QHBoxLayout()
head_sld_box.addItem(QtGui.QSpacerItem(225, 20))
head_sld_box.addWidget(head_speed_sld)
head_sld_box.addItem(QtGui.QSpacerItem(225, 20))
head_box.addLayout(up_head_box)
head_box.addLayout(left_right_head_box)
head_box.addLayout(down_head_box)
head_box.addLayout(head_sld_box)
large_box.addLayout(head_box)
# Buttons to move the grippers
gripper = Gripper(Gripper.RIGHT, Gripper.OPEN, self)
right_gripper = self.create_button('Right gripper',
gripper.create_closure())
gripper = Gripper(Gripper.LEFT, Gripper.OPEN, self)
left_gripper = self.create_button('Left gripper',
gripper.create_closure())
knock_button = self.create_button('Knock', self.knock)
large_box.addItem(QtGui.QSpacerItem(100, 250))
gripper_box = QtGui.QHBoxLayout()
gripper_box.addItem(QtGui.QSpacerItem(75, 20))
gripper_box.addWidget(left_gripper)
gripper_box.addWidget(knock_button)
gripper_box.addItem(QtGui.QSpacerItem(450, 20))
gripper_box.addWidget(right_gripper)
gripper_box.addItem(QtGui.QSpacerItem(75, 20))
large_box.addLayout(gripper_box)
# Buttons to move the base
base_box = QtGui.QVBoxLayout()
large_box.addItem(QtGui.QSpacerItem(100, 100))
forward_base_box = QtGui.QHBoxLayout()
forward_base = Base(Base.FORWARD, self)
forward_base_button = self.create_button('move forward',
forward_base.create_closure())
forward_base_box.addItem(QtGui.QSpacerItem(400, 20))
forward_base_box.addWidget(forward_base_button)
forward_base_box.addItem(QtGui.QSpacerItem(400, 20))
base_box.addLayout(forward_base_box)
left_right_base_box = QtGui.QHBoxLayout()
left_base = Base(Base.LEFT, self)
left_base_button = self.create_button('move left',
left_base.create_closure())
right_base = Base(Base.RIGHT, self)
right_base_button = self.create_button('move right',
right_base.create_closure())
left_right_base_box.addItem(QtGui.QSpacerItem(300, 20))
left_right_base_box.addWidget(left_base_button)
left_right_base_box.addItem(QtGui.QSpacerItem(50, 20))
left_right_base_box.addWidget(right_base_button)
left_right_base_box.addItem(QtGui.QSpacerItem(300, 20))
base_box.addLayout(left_right_base_box)
backward_base_box = QtGui.QHBoxLayout()
backward_base = Base(Base.BACKWARD, self)
backward_base_button = self.create_button(
'move backward', backward_base.create_closure())
backward_base_box.addItem(QtGui.QSpacerItem(400, 20))
backward_base_box.addWidget(backward_base_button)
backward_base_box.addItem(QtGui.QSpacerItem(400, 20))
base_box.addLayout(backward_base_box)
large_box.addLayout(base_box)
turn_base_box = QtGui.QHBoxLayout()
counter_base = Base(Base.COUNTER, self)
counter_base_button = self.create_button('\\\n -->',
counter_base.create_closure())
clockwise_base = Base(Base.CLOCKWISE, self)
clockwise_base_button = self.create_button(
' /\n<--', clockwise_base.create_closure())
turn_base_box.addItem(QtGui.QSpacerItem(75, 20))
turn_base_box.addWidget(counter_base_button)
turn_base_box.addItem(QtGui.QSpacerItem(225, 20))
turn_base_box.addWidget(clockwise_base_button)
turn_base_box.addItem(QtGui.QSpacerItem(100, 20))
large_box.addLayout(turn_base_box)
self._widget.setObjectName('WaterPulse')
self._widget.setLayout(large_box)
context.add_widget(self._widget)
self._widget.setStyleSheet(
"QWidget { image: url(%s) }" %
(str(os.path.dirname(os.path.realpath(__file__))) +
"/../../rosie_background.jpg"))
def load_gripper(self,
gripper_type,
gripper_size,
open_scales=None,
gripper_final_state=False,
remove=False,
**kwargs):
# reset gripper & get gripper_observation
# balance 2f and 3f grippers when they are added together
if gripper_type == "train":
# load training grippers; half 2f half 3f
if np.random.rand() > 0.5:
gripper_type = [
"wsg_32", "sawyer", "franka", "robotiq_2f_140", "ezgripper"
]
else:
gripper_type = ["robotiq_3f", "kinova_3f"]
elif gripper_type == "test":
# load testing grippers; half 2f half 3f
if np.random.rand() > 0.5:
gripper_type = ["wsg_50", "rg2", "robotiq_2f_85"]
else:
gripper_type = ["barrett_hand"]
self._gripper_type = gripper_type if isinstance(
gripper_type, str) else np.random.choice(gripper_type)
gripper_kwargs = dict()
if remove:
if self._gripper_type == 'barrett_hand':
self._gripper_record_dict[(
self._gripper_type, kwargs['palm_joint'])] = gripper_kwargs
else:
self._gripper_record_dict[self._gripper_type] = gripper_kwargs
self._open_scales = [] if open_scales is None else open_scales
self._gripper_size = gripper_size
self._gripper_orientation = [0, 0, 0]
gripper_kwargs['gripper_size'] = self._gripper_size
if self._gripper_type == 'barrett_hand':
gripper_kwargs['palm_joint'] = 0.25 * np.random.rand(
) if kwargs['palm_joint'] is None else kwargs['palm_joint']
self._gripper = Gripper(gripper_type=self._gripper_type,
bullet_client=self.bullet_client,
home_position=self._gripper_home_position,
num_side_images=8,
**gripper_kwargs)
gripper_tsdf = np.array([
self._gripper.get_tsdf(open_scale)
for open_scale in self._open_scales
])
gripper_close_tsdf = self._gripper.get_tsdf(
0) if gripper_final_state else None
vis_pts = [
self._gripper.get_vis_pts(open_scale)
for open_scale in self._open_scales
]
gripper_observation = {
'gripper_tsdf': gripper_tsdf,
'gripper_close_tsdf': gripper_close_tsdf,
'vis_pts': vis_pts,
'open_scales': self._open_scales,
'gripper_type': self._gripper_type,
'gripper_size': self._gripper_size,
}
if remove:
self._gripper.remove()
self._gripper = None
else:
self._gripper.move(self._gripper_home_position, 0)
return gripper_observation
def __init__(self, context):
super(SimpleGUI, self).__init__(context)
self.setObjectName('SimpleGUI')
self._widget = QWidget()
self._widget.setFixedSize(600, 600)
self._sound_client = SoundClient()
rospy.Subscriber('robotsound', SoundRequest, self.sound_cb)
QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10))
self.sound_sig.connect(self.sound_sig_cb)
large_box = QtGui.QVBoxLayout()
#Sound textbox
sound_textbox = QtGui.QLineEdit("Squirtle Squirtle") #Default Text
sound_textbox.setFixedWidth(450)
#Set a handle on the textbox to retrieve the text when button clicked
self.sound_textbox = sound_textbox
button_box = QtGui.QHBoxLayout()
button_box.addItem(QtGui.QSpacerItem(15, 20))
button_box.addWidget(self.create_button('Speak', self.command_cb))
button_box.addWidget(sound_textbox)
button_box.addStretch(1)
large_box.addLayout(button_box)
speech_box = QtGui.QHBoxLayout()
speech_box.addItem(QtGui.QSpacerItem(15, 20))
self.speech_label = QtGui.QLabel('Robot has not spoken yet.')
palette = QtGui.QPalette()
palette.setColor(QtGui.QPalette.Foreground, QtCore.Qt.blue)
self.speech_label.setPalette(palette)
speech_box.addWidget(self.speech_label)
large_box.addLayout(speech_box)
large_box.addStretch(1)
#large_box.addItem(QtGui.QSpacerItem(50,20))
up_head = Head(Head.UP)
head_box = QtGui.QVBoxLayout()
up_head_box = QtGui.QHBoxLayout()
up_head_button = self.create_button('^', up_head.create_closure())
#large_box.addWidget(up_head_button)
down_head = Head(Head.DOWN)
down_head_box = QtGui.QHBoxLayout()
down_head_button = self.create_button('v', down_head.create_closure())
#large_box.addWidget(down_head_button)
right_head = Head(Head.RIGHT)
right_head_button = self.create_button('>',
right_head.create_closure())
#large_box.addWidget(right_head_button)
left_head = Head(Head.LEFT)
left_head_button = self.create_button('<', left_head.create_closure())
left_right_head_box = QtGui.QHBoxLayout()
up_head_box.addItem(QtGui.QSpacerItem(235, 20))
up_head_box.addWidget(up_head_button)
up_head_box.addItem(QtGui.QSpacerItem(275, 20))
left_right_head_box.addItem(QtGui.QSpacerItem(160, 20))
left_right_head_box.addWidget(left_head_button)
left_right_head_box.addItem(QtGui.QSpacerItem(60, 20))
left_right_head_box.addWidget(right_head_button)
left_right_head_box.addItem(QtGui.QSpacerItem(225, 20))
down_head_box.addItem(QtGui.QSpacerItem(235, 20))
down_head_box.addWidget(down_head_button)
down_head_box.addItem(QtGui.QSpacerItem(275, 20))
head_box.addLayout(up_head_box)
head_box.addLayout(left_right_head_box)
head_box.addLayout(down_head_box)
large_box.addLayout(head_box)
#large_box.addItem(QtGui.QSpacerItem(500,20))
#large_box.addWidget(left_head_button)
gripper = Gripper(Gripper.RIGHT, Gripper.OPEN)
right_gripper = self.create_button('Right gripper!',
gripper.create_closure())
gripper = Gripper(Gripper.LEFT, Gripper.OPEN)
left_gripper = self.create_button('Left gripper!',
gripper.create_closure())
large_box.addItem(QtGui.QSpacerItem(100, 250))
gripper_box = QtGui.QHBoxLayout()
gripper_box.addItem(QtGui.QSpacerItem(75, 20))
gripper_box.addWidget(left_gripper)
gripper_box.addItem(QtGui.QSpacerItem(450, 20))
gripper_box.addWidget(right_gripper)
gripper_box.addItem(QtGui.QSpacerItem(75, 20))
large_box.addLayout(gripper_box)
base_box = QtGui.QVBoxLayout()
large_box.addItem(QtGui.QSpacerItem(100, 100))
#forward
forward_base_box = QtGui.QHBoxLayout()
forward_base = Base(Base.FORWARD)
forward_base_button = self.create_button('move forward',
forward_base.create_closure())
forward_base_box.addItem(QtGui.QSpacerItem(400, 20))
forward_base_box.addWidget(forward_base_button)
forward_base_box.addItem(QtGui.QSpacerItem(400, 20))
base_box.addLayout(forward_base_box)
#large_box.addWidget(forward_base_button)
#left
left_right_base_box = QtGui.QHBoxLayout()
left_base = Base(Base.LEFT)
left_base_button = self.create_button('move left',
left_base.create_closure())
#large_box.addWidget(left_base_button)
#right
right_base = Base(Base.RIGHT)
right_base_button = self.create_button('move right',
right_base.create_closure())
left_right_base_box.addItem(QtGui.QSpacerItem(300, 20))
left_right_base_box.addWidget(left_base_button)
left_right_base_box.addItem(QtGui.QSpacerItem(50, 20))
left_right_base_box.addWidget(right_base_button)
left_right_base_box.addItem(QtGui.QSpacerItem(300, 20))
base_box.addLayout(left_right_base_box)
#large_box.addWidget(right_base_button)
#backward
backward_base_box = QtGui.QHBoxLayout()
backward_base = Base(Base.BACKWARD)
backward_base_button = self.create_button(
'move backward', backward_base.create_closure())
backward_base_box.addItem(QtGui.QSpacerItem(400, 20))
backward_base_box.addWidget(backward_base_button)
backward_base_box.addItem(QtGui.QSpacerItem(400, 20))
base_box.addLayout(backward_base_box)
#large_box.addWidget(backward_base_button)
large_box.addLayout(base_box)
turn_base_box = QtGui.QHBoxLayout()
#turn left
turnleft_base = Base(Base.TURNLEFT)
turnleft_base_button = self.create_button(
' /\n<--', turnleft_base.create_closure())
#large_box.addWidget(turnleft_base_button)
#turn right
turnright_base = Base(Base.TURNRIGHT)
turnright_base_button = self.create_button(
'\\\n -->', turnright_base.create_closure())
turn_base_box.addItem(QtGui.QSpacerItem(75, 20))
turn_base_box.addWidget(turnright_base_button)
turn_base_box.addItem(QtGui.QSpacerItem(225, 20))
turn_base_box.addWidget(turnleft_base_button)
turn_base_box.addItem(QtGui.QSpacerItem(100, 20))
large_box.addLayout(turn_base_box)
#large_box.addWidget(turnright_base_button)
self._widget.setObjectName('SimpleGUI')
self._widget.setLayout(large_box)
context.add_widget(self._widget)
self._widget.setStyleSheet(
"QWidget { image: url(%s) }" %
"/home/vjampala/catkin_ws/src/cse481/hw1/rqt_simplegui/rosie_background.jpg"
)
def step(self, action, gripper_type=None, is_position=False, **kwargs):
"""Execute action and return reward and next observation.
Args:
action: tuple or list (y_pix, x_pix, angle, open_scale).
y_pix is the first channel of the heightmap.
x_pix is the second channel of the heightmap.
angle is the rotation angle (radians) around z axis.
Returns:
reward: float number
observation: dict, scene observation + gripper observation
"""
if self.gripper_selection:
if len(kwargs) == 0:
if gripper_type.startswith('barrett_hand-'):
palm_joint = float(gripper_type.split('-')[-1])
gripper_type = 'barrett_hand'
gripper_record = self._gripper_record_dict[(gripper_type,
palm_joint)]
else:
gripper_record = self._gripper_record_dict[gripper_type]
else:
gripper_record = kwargs
self._gripper = Gripper(gripper_type=gripper_type,
bullet_client=self.bullet_client,
home_position=self._gripper_home_position,
num_side_images=8,
**gripper_record)
self._gripper.move(self._gripper_home_position, 0)
a_y, a_x, angle, open_scale = action
if is_position:
y, x = a_y, a_x
y_pix = int(
(y - self._view_bounds[1, 0]) / self._heightmap_pix_size)
x_pix = int(
(x - self._view_bounds[1, 0]) / self._heightmap_pix_size)
else:
y_pix, x_pix = a_y, a_x
x = x_pix * self._heightmap_pix_size + self._view_bounds[0, 0]
y = y_pix * self._heightmap_pix_size + self._view_bounds[1, 0]
grasp_pix = np.array([y_pix, x_pix])
valid_pix = np.array(np.where(self._depth_heightmap > 0)).T # y,x
dist_to_valid = np.sqrt(np.sum((valid_pix - grasp_pix)**2, axis=1))
closest_valid_pix = grasp_pix
if np.min(dist_to_valid
) < 2: # get nearest non-zero pixel less than 2 pixels away
closest_valid_pix = valid_pix[np.argmin(dist_to_valid), :]
z = self._depth_heightmap[closest_valid_pix[0],
closest_valid_pix[1]] - 0.05
z = max(z, 0)
grasp_position = np.array([x, y, z])
grasp_position = np.clip(
grasp_position, self._workspace_bounds[:, 0],
self._workspace_bounds[:, 1]
) # clamp grasp position w.r.t. workspace bounds
self._gripper.primitive_grasping(grasp_position,
angle % (2 * np.pi),
open_scale=open_scale,
stop_at_contact=True)
try:
self.wait_till_stable()
except:
for _ in range(240 * 2):
self.bullet_client.stepSimulation()
objects_lifted = 0
target_ids_copy = list(self._target_ids)
for target_id in target_ids_copy:
pos = self.bullet_client.getBasePositionAndOrientation(
target_id)[0]
if pos[-1] > 0.2:
self.bullet_client.resetBasePositionAndOrientation(
target_id, [1, target_id, 0.3], [0, 0, 0, 1])
self.bullet_client.stepSimulation()
objects_lifted += 1
self._target_ids.remove(target_id)
elif np.any(pos < self._workspace_bounds[:, 0]) or np.any(
pos > self._workspace_bounds[:, 1]):
self.bullet_client.resetBasePositionAndOrientation(
target_id, [1, target_id, 0.3], [0, 0, 0, 1])
self.bullet_client.stepSimulation()
self._target_ids.remove(target_id)
# should not lift obstacles
for target_id in self._obstacle_ids:
pos = self.bullet_client.getBasePositionAndOrientation(
target_id)[0]
if pos[-1] > 0.2:
self.bullet_client.resetBasePositionAndOrientation(
target_id, [1, target_id, 0.3], [0, 0, 0, 1])
self.bullet_client.stepSimulation()
objects_lifted = -1
reward = objects_lifted == 1
scene_observation = self._get_scene_observation()
observation = {**self._gripper_observation, **scene_observation}
if self.gripper_selection:
self._gripper.remove()
self._gripper = None
return reward, observation
import rtde_control
import time
import json
from gripper import Gripper
import numpy as np
print("Connecting?")
rtde_c = rtde_control.RTDEControlInterface("192.168.1.6")
print("Connected?")
gripper = Gripper(2, 1)
# Parameters
velocity = 0.25
acceleration = 0.25
dt = 1.0 / 10 # 2ms
lookahead_time = 0.2
gain = 200
joint_q = [-1.54, -1.4, -2.28, -0.59, 1.60, 0.023]
sequence = json.load(open("records/20201217-171704/arm_trajectory.json"))
input("Play?")
#offset = [0.25493689, -0.56119273, -0.02703432, 0, 0, 0]
offset = [0, 0, 0, 0, 0, 0]
# Move to initial joint position with a regular moveJ
rtde_c.moveL(np.add(offset, sequence[0][0]).tolist())
print("moved?")
time.sleep(1)
print("Moved to inital position")
def __init__(self, context):
super(Milestone1GUI, self).__init__(context)
self._sound_client = SoundClient()
rospy.Subscriber('robotsound', SoundRequest, self.sound_cb)
switch_srv_name = 'pr2_controller_manager/switch_controller'
rospy.loginfo('Waiting for switch controller service...')
rospy.wait_for_service(switch_srv_name)
self.switch_service_client = rospy.ServiceProxy(
switch_srv_name, SwitchController)
self.r_joint_names = [
'r_shoulder_pan_joint', 'r_shoulder_lift_joint',
'r_upper_arm_roll_joint', 'r_elbow_flex_joint',
'r_forearm_roll_joint', 'r_wrist_flex_joint', 'r_wrist_roll_joint'
]
self.l_joint_names = [
'l_shoulder_pan_joint', 'l_shoulder_lift_joint',
'l_upper_arm_roll_joint', 'l_elbow_flex_joint',
'l_forearm_roll_joint', 'l_wrist_flex_joint', 'l_wrist_roll_joint'
]
# Create a trajectory action client
r_traj_controller_name = '/r_arm_controller/joint_trajectory_action'
self.r_traj_action_client = SimpleActionClient(r_traj_controller_name,
JointTrajectoryAction)
rospy.loginfo(
'Waiting for a response from the trajectory action server for RIGHT arm...'
)
self.r_traj_action_client.wait_for_server()
l_traj_controller_name = '/l_arm_controller/joint_trajectory_action'
self.l_traj_action_client = SimpleActionClient(l_traj_controller_name,
JointTrajectoryAction)
rospy.loginfo(
'Waiting for a response from the trajectory action server for LEFT arm...'
)
self.l_traj_action_client.wait_for_server()
#init torso
self.torso = Torso()
#init gripper
self.l_gripper = Gripper('l')
self.r_gripper = Gripper('r')
#init navigation
self.navigation = Navigation()
self._widget = QWidget()
self._widget.setFixedSize(600, 600)
QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10))
large_box = QtGui.QVBoxLayout()
#Button for outreaching to recieve book from Human
button_box = QtGui.QVBoxLayout()
box_1 = QtGui.QHBoxLayout()
box_2 = QtGui.QHBoxLayout()
box_3 = QtGui.QHBoxLayout()
box_4 = QtGui.QHBoxLayout()
box_5 = QtGui.QHBoxLayout()
box_6 = QtGui.QHBoxLayout()
box_7 = QtGui.QHBoxLayout()
box_8 = QtGui.QHBoxLayout()
box_9 = QtGui.QHBoxLayout()
box_10 = QtGui.QHBoxLayout()
box_1.addItem(QtGui.QSpacerItem(15, 2))
box_1.addWidget(
self.create_button('Prepare To Take', self.prepare_to_take))
box_1.addItem(QtGui.QSpacerItem(445, 2))
box_2.addItem(QtGui.QSpacerItem(15, 2))
box_2.addWidget(
self.create_button('Take From Human', self.take_from_human))
box_2.addItem(QtGui.QSpacerItem(445, 2))
box_3.addItem(QtGui.QSpacerItem(15, 2))
box_3.addWidget(
self.create_button('Prepare To Navigate',
self.prepare_to_navigate))
box_3.addItem(QtGui.QSpacerItem(445, 2))
# Button to move to shelf
box_5.addItem(QtGui.QSpacerItem(15, 2))
box_5.addWidget(
self.create_button('Navigate To Shelf', self.navigate_to_shelf))
box_5.addItem(QtGui.QSpacerItem(445, 2))
box_4.addItem(QtGui.QSpacerItem(15, 2))
box_4.addWidget(
self.create_button('Place On Shelf', self.place_on_shelf))
box_4.addItem(QtGui.QSpacerItem(445, 2))
box_6.addItem(QtGui.QSpacerItem(15, 2))
box_6.addWidget(
self.create_button('Give Information', self.give_information))
box_6.addItem(QtGui.QSpacerItem(445, 2))
box_7.addItem(QtGui.QSpacerItem(15, 2))
box_7.addWidget(
self.create_button('Navigate To Person', self.navigate_to_person))
box_7.addItem(QtGui.QSpacerItem(445, 2))
self.book_textbox = QtGui.QLineEdit()
self.book_textbox.setFixedWidth(100)
box_8.addItem(QtGui.QSpacerItem(15, 2))
box_8.addWidget(self.book_textbox)
box_8.addWidget(
self.create_button('Pick Up Book', self.pick_up_from_shelf_button))
box_8.addItem(QtGui.QSpacerItem(445, 2))
box_9.addItem(QtGui.QSpacerItem(15, 2))
box_9.addWidget(self.create_button('Localize', self.spin_base))
box_9.addItem(QtGui.QSpacerItem(445, 2))
box_10.addItem(QtGui.QSpacerItem(15, 2))
box_10.addWidget(
self.create_button('Non-nav Pick Up', self.pick_up_from_shelf))
box_10.addItem(QtGui.QSpacerItem(445, 2))
button_box.addItem(QtGui.QSpacerItem(20, 120))
button_box.addLayout(box_1)
button_box.addLayout(box_2)
button_box.addLayout(box_3)
button_box.addLayout(box_5)
button_box.addLayout(box_4)
button_box.addLayout(box_6)
button_box.addLayout(box_7)
button_box.addLayout(box_8)
button_box.addLayout(box_9)
button_box.addLayout(box_10)
button_box.addItem(QtGui.QSpacerItem(20, 240))
large_box.addLayout(button_box)
self.marker_perception = ReadMarkers()
self.speech_recognition = SpeechRecognition(self)
self.bookDB = BookDB()
self.book_map = self.bookDB.getAllBookCodes()
self._widget.setObjectName('Milestone1GUI')
self._widget.setLayout(large_box)
context.add_widget(self._widget)
self._widget.setStyleSheet(
"QWidget { image: url(%s) }" %
(str(os.path.dirname(os.path.realpath(__file__))) +
"/../../librarian_gui_background.jpg"))