def reset(event):
trajectories[:] = []
Trajectory.resetGlobID()
# clear canvas
w.delete('all')
#redraw background
w.create_image(1920 / 2, 1080 / 2, image=bg)
def buttonUp(event):
global newT, xold, yold
newT = True
xold = None
yold = None
# Check if last trajectory has 0 length
if trajectories[len(trajectories) - 1].length() == 0.0:
trajectories.pop()
Trajectory.decGlobID()
print(trajectories[-1])
def change_to_traj(trajectories):
out = []
for i in range(len(trajectories)):
temp = Trajectory()
for j in trajectories[i]:
temp.addPoint(j)
out.append(temp)
return out
def addNYGCTraj(event):
global newT, xold, yold, NYGC_index
## save point
# a first point for a new trajectory
if (newT):
trajectories.append(Trajectory(ci))
newT = False
for i in range(len(NYGC_data[NYGC_index])):
x_coord = np.floor(NYGC_data[NYGC_index][i][0])
y_coord = np.floor(NYGC_data[NYGC_index][i][1])
trajectories[len(trajectories) - 1].addPoint((x_coord, y_coord))
## paint one point
# c = COLORS[ci]
c = COLOR_BLACK
x1, y1 = (x_coord - 2), (y_coord - 2)
x2, y2 = (x_coord + 2), (y_coord + 2)
w.create_oval(x1, y1, x2, y2, fill=c)
## paint a line
if xold is not None and yold is not None:
w.create_line(xold, yold, x_coord, y_coord, smooth=True)
xold = x_coord
yold = y_coord
NYGC_index += 1
print('Number ' + str(NYGC_index) + ' trajectory')
print(trajectories[-1])
def replan(self, start, goal, goal_pose, T, timestep, seed=None):
"""
Replan the trajectory from start to goal given weights.
---
Parameters:
start -- Start position
goal -- Goal position.
goal_pose -- Goal pose (optional: can be None).
T [float] -- Time horizon for the desired trajectory.
timestep [float] -- Frequency of waypoints in desired trajectory.
Returns:
traj [Trajectory] -- The optimal trajectory satisfying the arguments.
"""
waypts = self.trajOpt(start, goal, goal_pose, traj_seed=seed)
waypts_time = np.linspace(0.0, T, self.num_waypts)
traj = Trajectory(waypts, waypts_time)
return traj.upsample(int(T / timestep) + 1)
def extract_solution(self, robot, qcom, qlimb, contact, contact_force,
contact_lambda, contact_sequence_array):
qcom = self.get_piecewise_solution(qcom)
vcom = qcom.map(Polynomial.derivative)
qlimb = [self.get_piecewise_solution(q) for q in qlimb]
vlimb = [q.map(Polynomial.derivative) for q in qlimb]
flimb = [self.get_piecewise_solution(f) for f in contact_force]
contact = self.extract_contact_solution(contact, contact_lambda,
contact_sequence_array)
# note that flimb, vlimb are lists so we have to be extra careful when constructing
# the Trajectory object for BoxAtlasInput
return (Trajectory([qcom, vcom] + qlimb,
lambda qcom, vcom, *qlimb: BoxAtlasState(
robot, qcom=qcom, vcom=vcom, qlimb=qlimb)),
Trajectory([flimb, vlimb], lambda flimb, vlimb: BoxAtlasInput(
robot, flimb=flimb, vlimb=vlimb)), contact)
def replan(self, start, goal, goal_pose, weights, T, timestep, seed=None):
"""
Replan the trajectory from start to goal given weights.
---
Parameters:
start -- Start position
goal -- Goal position.
goal_pose -- Goal pose (optional: can be None).
weights -- Weights used for the planning objective.
T [float] -- Time horizon for the desired trajectory.
timestep [float] -- Frequency of waypoints in desired trajectory.
Returns:
traj [Trajectory] -- The optimal trajectory satisfying the arguments.
"""
assert weights is not None, "The weights vector is empty. Cannot plan without a cost preference."
self.weights = weights
waypts = self.trajOpt(start, goal, goal_pose, traj_seed=seed)
waypts_time = np.linspace(0.0, T, self.num_waypts)
traj = Trajectory(waypts, waypts_time)
return traj.upsample(int(T / timestep) + 1)
def read_data(self, tr, dr):
for i in range(0, 182):
path = './data/%03d/Trajectory/' % i
files = os.listdir(path)
user = User(i) # 创建User对象
file_num = 0
for file in files:
file_num += 1
trajectory = Trajectory() # 创建Trajectory对象
file_name = os.path.join(path, file) # 定位路径并打开文件
with open(file_name) as f:
# plt数据文件的前六行无用
line_num = 0
for line in f:
line_num += 1
if line_num <= 6:
continue
content = line.split(',')
lat, lng, date, time = float(content[0]), float(content[1]), content[5], content[6][0:-1] # 所有的初始数据都是string格式
timestamp = date + ' ' + time
point = Point(lng, lat, timestamp)
trajectory.add_point(point)
trajectory.staypoint_detection(tr, dr)
if trajectory.staypoints: # 如果这条轨迹中检测到停留点的话
user.add_trajectory(trajectory)
print(i, 'th user,', file_num, 'th trajectory,', len(trajectory.staypoints), 'staypoints')
user.trajectory2staypoint_hist()
def load_parameters(self):
"""
Loading parameters and setting up variables from the ROS environment.
"""
# ----- General Setup ----- #
self.prefix = rospy.get_param("setup/prefix")
self.start = np.array(rospy.get_param("setup/start"))*(math.pi/180.0)
self.T = rospy.get_param("setup/T")
self.timestep = rospy.get_param("setup/timestep")
self.save_dir = rospy.get_param("setup/save_dir")
# Openrave parameters for the environment.
model_filename = rospy.get_param("setup/model_filename")
object_centers = rospy.get_param("setup/object_centers")
self.environment = Environment(model_filename, object_centers)
# ----- Controller Setup ----- #
# Retrieve controller specific parameters.
controller_type = rospy.get_param("controller/type")
if controller_type == "pid":
# P, I, D gains.
P = rospy.get_param("controller/p_gain") * np.eye(7)
I = rospy.get_param("controller/i_gain") * np.eye(7)
D = rospy.get_param("controller/d_gain") * np.eye(7)
# Stores proximity threshold.
epsilon = rospy.get_param("controller/epsilon")
# Stores maximum COMMANDED joint torques.
MAX_CMD = rospy.get_param("controller/max_cmd") * np.eye(7)
self.controller = PIDController(P, I, D, epsilon, MAX_CMD)
else:
raise Exception('Controller {} not implemented.'.format(controller_type))
# Tell controller to move to start.
self.controller.set_trajectory(Trajectory([self.start], [0.0]))
# Stores current COMMANDED joint torques.
self.cmd = np.eye(7)
# Utilities for recording data.
self.expUtil = experiment_utils.ExperimentUtils(self.save_dir)
def buttonMotion(event):
global newT, xold, yold
## save point
# a first point for a new trajectory
if (newT):
trajectories.append(Trajectory(ci))
newT = False
trajectories[len(trajectories) - 1].addPoint((event.x, event.y))
## paint one point
# c = COLORS[ci]
c = COLOR_BLACK
x1, y1 = (event.x - 2), (event.y - 2)
x2, y2 = (event.x + 2), (event.y + 2)
w.create_oval(x1, y1, x2, y2, fill=c)
## paint a line
if xold is not None and yold is not None:
w.create_line(xold, yold, event.x, event.y, smooth=True)
xold = event.x
yold = event.y
def __init__(self):
# Create ROS node.
rospy.init_node("demo_recorder")
# Load parameters and set up subscribers/publishers.
self.load_parameters()
self.register_callbacks()
ros_utils.start_admittance_mode(self.prefix)
# Publish to ROS at 100hz
r = rospy.Rate(100)
print "----------------------------------"
print "Moving robot, press ENTER to quit:"
while not rospy.is_shutdown():
if sys.stdin in select.select([sys.stdin], [], [], 0)[0]:
line = raw_input()
break
self.vel_pub.publish(ros_utils.cmd_to_JointVelocityMsg(self.cmd))
r.sleep()
print "----------------------------------"
# Process and save the recording.
raw_demo = self.expUtil.tracked_traj[:,1:8]
# Trim ends of waypoints and create Trajectory.
lo = 0
hi = raw_demo.shape[0] - 1
while np.linalg.norm(raw_demo[lo] - raw_demo[lo + 1]) < 0.01 and lo < hi:
lo += 1
while np.linalg.norm(raw_demo[hi] - raw_demo[hi - 1]) < 0.01 and hi > 0:
hi -= 1
waypts = raw_demo[lo:hi+1, :]
waypts_time = np.linspace(0.0, self.T, waypts.shape[0])
traj = Trajectory(waypts, waypts_time)
# Downsample/Upsample trajectory to fit desired timestep and T.
num_waypts = int(self.T / self.timestep) + 1
if num_waypts < len(traj.waypts):
demo = traj.downsample(int(self.T / self.timestep) + 1)
else:
demo = traj.upsample(int(self.T / self.timestep) + 1)
# Decide whether to save trajectory
openrave_utils.plotTraj(self.environment.env, self.environment.robot,
self.environment.bodies, demo.waypts, size=0.015, color=[0, 0, 1])
print "Type [yes/y/Y] if you're happy with the demonstration."
line = raw_input()
if (line is not "yes") and (line is not "Y") and (line is not "y"):
print "Not happy with demonstration. Terminating experiment."
else:
print "Please type in the ID number (e.g. [0/1/2/...])."
ID = raw_input()
print "Please type in the task number (e.g. [0/1/2/...])."
task = raw_input()
filename = "demo" + "_ID" + ID + "_task" + task
savefile = self.expUtil.get_unique_filepath("demos",filename)
pickle.dump(demo, open(savefile, "wb" ))
print "Saved demonstration in {}.".format(savefile)
ros_utils.stop_admittance_mode(self.prefix)