def rewardFn(self, state, action):
if state[-1] < 0.5:
return 0.0
# compute the final position and velocity
joints = state[7:-1]
pos = physx.forwardKinematics(self.lengths, state[:7])[-1, :]
p0 = physx.forwardKinematics(self.lengths, state[:7] - 0.005 * joints)
p1 = physx.forwardKinematics(self.lengths, state[:7] + 0.005 * joints)
vel = (p1[-1, :3] - p0[-1, :3]) / 0.01
# compute the time it would take to reach the goal (kinematics equations)
g = -4.9
vz = vel[2]
dz = pos[2] - self.goal[2]
# quadratic formula (+/- sqrt)
# the parabola doesn't even reach the line
b2_4ac = vz * vz - 4 * g * dz
if b2_4ac < 0:
return 0.0
dt1 = (-vz + math.sqrt(b2_4ac)) / (2 * g)
dt2 = (-vz - math.sqrt(b2_4ac)) / (2 * g)
dt = max(dt1, dt2)
# the ball would have to go backwards in time to reach the goal
if dt < 0:
return 0.0
# find the distance from the goal (in the xy-plane) that the ball has hit
dp = self.goal[:2] - (pos[:2] + vel[:2] * dt)
# use a kernel distance
return np.exp(-np.dot(dp, dp))
def render(self, mode="human", close=False):
pos = physx.forwardKinematics(self.lengths, self.angles[:7])
if self.armsim == None:
self.armsim = simulation.Arm()
self.armsim.default_length = self.lengths
self.armsim.setPositions(pos)
#if self.ballsim == None:
# self.ballsim = simulation.Ball()
#pos0 = pos[-1, :]
#self.ballsim.setPosition(pos0)
t = time.time()
if self.time_render == None or self.time_render <= t:
self.time_render = t + 1.0 / self.fps
else:
time.sleep(self.time_render - t)
self.time_render += 1.0 / self.fps
"""
def main():
parser = argparse.ArgumentParser()
parser.add_argument("trajectory_csv",
type=str,
help="Trajectory file as a csv of 7 angles per line")
args = parser.parse_args()
# grab the trajectory
with open(args.trajectory_csv, "r") as fp:
trajectory = [np.array([eval(n) for n in line.strip().split(",")]) \
for line in fp]
# connect to the visualizer (remove if unnecessary)
arm = simulation.Arm()
# continuously visualize the trajectory
while True:
for q in trajectory:
positions = physx.forwardKinematics(arm.default_length, q)
arm.setPositions(positions)
time.sleep(0.02)
def main():
parser = argparse.ArgumentParser()
parser.add_argument("--printpos",
action="store_true",
help="Print the global positions")
args = parser.parse_args()
# connect to the visualizer
arm = simulation.Arm()
# create a window for the controls
pygame.init()
screen = pygame.display.set_mode((640, 360))
clock = pygame.time.Clock()
# place widgets to respresent the controls that the user can input
labels = []
linkSlides = []
jointSlides = []
for i in range(arm.num_joints):
labels.append(
TextBox((0, 50 * i + 15), (80, 30),
initialValue="Link " + str(i + 1)))
linkSlides.append(
SliderCounter((0, 10), (80, 50 * i + 15), (240, 30),
radius=8,
counterWidth=60,
fmt="%0.1f",
initialValue=arm.default_length[i]))
labels.append(
TextBox((320, 50 * i + 15), (80, 30),
initialValue="Joint " + str(i + 1)))
jointSlides.append(
SliderCounter((arm.joint_limits[i][0], arm.joint_limits[i][1]),
(400, 50 * i + 15), (240, 30),
radius=8,
counterWidth=60,
fmt="%d"))
while True:
# update the controls events
for event in pygame.event.get():
if event.type == pygame.QUIT:
pygame.quit()
sys.exit()
elif event.type == pygame.MOUSEBUTTONDOWN:
for link in linkSlides:
link.setActive(pygame.mouse.get_pos())
for joint in jointSlides:
joint.setActive(pygame.mouse.get_pos())
elif event.type == pygame.MOUSEMOTION:
for link in linkSlides:
link.update(pygame.mouse.get_pos())
for joint in jointSlides:
joint.update(pygame.mouse.get_pos())
elif event.type == pygame.MOUSEBUTTONUP:
for link in linkSlides:
link.setInactive()
for joint in jointSlides:
joint.setInactive()
# using the inputs from the controls panel, calculate the forward kinematics
# to get the positions and set those on the arm
positions = physx.forwardKinematics(
np.array([link.getValue() for link in linkSlides]),
np.array([joint.getValue() for joint in jointSlides]))
arm.setPositions(positions)
if args.printpos:
print(positions)
# update the controls render
screen.fill((0xF5, 0xF5, 0xF5))
for label in labels:
label.render(screen)
for link in linkSlides:
link.render(screen)
for joint in jointSlides:
joint.render(screen)
pygame.display.flip()
clock.tick(50)