def __init__(self):
super(CostFunction, self).__init__()
moveit_commander.roscpp_initialize(sys.argv)
self.collision_threshold = 0.07
self.robot_state = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface()
self.franka_k = FrankaKinematics()
def show_objects(self):
for k, v in self.scene.get_objects().items():
print "Pos:", v.primitive_poses[0].position.x, v.primitive_poses[0].position.y,\
v.primitive_poses[0].position.z
print "Ori:", v.primitive_poses[0].orientation.x, v.primitive_poses[0].orientation.y,\
v.primitive_poses[0].orientation.z, v.primitive_poses[0].orientation.w
print "Size:", v.primitives[0].dimensions
def __init__(
self,
nDoF,
obstacle_list=None,
ini_jv=None,
des_jv=None,
):
super(TrajectoryOptimize, self).__init__()
self.initial_joint_values = ini_jv
self.desired_joint_values = des_jv
self.scene = moveit_commander.PlanningSceneInterface()
# if obstacle_list is None:
# self.obstacle_list = np.array([[0.25, 0.21, 0.71, 0.1], [-0.15, 0.21, 0.51, 0.05]]) # for two spheres
# else:
# self.obstacle_list = obstacle_list
self.obstacle_list = obstacle_list
self.nDoF = nDoF
self.franka_kin = FrankaKinematics()
self.workspace_dim = (-0.5, -0.5, 0.2, 1.5, 0.5, 1.9)
# self.obstacle_list = np.array([[0.25, 0.21, 0.71, 0.1]]) # for one sphere (x,y,z, radius)
# self.obstacle_list = np.array([[0.25, 0.21, 0.71, 0.1],
# [-0.15, 0.21, 0.51, 0.05],
# [0.15, 0.61, 0.51, 0.05],
# [-0.15, -0.61, 0.51, 0.05],
# [-0.65, 0.31, 0.21, 0.05],
# [-0.55, 0.71, 0.71, 0.06],
# [-0.95, -0.51, 0.41, 0.07],
# [-0.85, 0.27, 0.61, 0.08],
# [-0.25, -0.31, 0.11, 0.04],
# [-0.75, 0.71, 0.91, 0.09],
# [-0.35, 0.51, 0.81, 0.02],
# [-0.95, -0.81, 0.61, 0.03],
# [-0.75, 0.11, 0.41, 0.02],
# [-0.55, -0.61, 0.21, 0.06],
# [0.25, 0.31, 0.71, 0.07],
# [-0.35, -0.41, 0.41, 0.02],
# [-0.25, -0.51, 0.61, 0.08],
# ]) # for 15 spheres
# self.obstacle_list1 = self.populate_obstacles()
self.optCurve, self.costs = [], []
self.step_size = 0.06
self.debug = False
self.lamda_smooth, self.lamda_obs = 10.0, 20.0
def __init__(self, nDoF):
self.nDoF = nDoF
self.scene = moveit_commander.PlanningSceneInterface()
self.group = moveit_commander.MoveGroupCommander("panda_arm")
self.robot_state = moveit_commander.RobotCommander()
self.workspace_dim = (-2, -0.2, 0.2, 1, 1, 1.5)
self.group.set_workspace(
self.workspace_dim
) # specify the workspace bounding box (static scene)
self.franka_kin = FrankaKinematics()
self.collision_threshold = 0.07
self.object_list = np.array([
[0.25, 0.21, 0.71, 0.1],
[-0.15, 0.21, 0.51, 0.05],
[0.15, 0.61, 0.51, 0.05],
[-0.15, -0.61, 0.51, 0.05],
[-0.65, 0.31, 0.21, 0.05],
[-0.55, 0.71, 0.71, 0.06],
[-0.95, -0.51, 0.41, 0.07],
[-0.85, 0.27, 0.61, 0.08],
[-0.25, -0.31, 0.11, 0.04],
[-0.75, 0.71, 0.91, 0.09],
[-0.35, 0.51, 0.81, 0.02],
[-0.95, -0.81, 0.61, 0.03],
[-0.75, 0.11, 0.41, 0.02],
[-0.55, -0.61, 0.21, 0.06],
[0.25, 0.31, 0.71, 0.07],
[-0.35, -0.41, 0.41, 0.02],
[-0.25, -0.51, 0.61, 0.08],
]) # for two spheres
# self.object_list = np.array([[0.25, 0.21, 0.71, 0.1], [-0.15, 0.21, 0.51, 0.05]]) # for two spheres
# self.object_list = np.array([[0.25, 0.21, 0.71, 0.1]]) # for one sphere (x,y,z, radius)
# self.object_list = []
# self.object_list = self.populate_obstacles()
self.initial_joint_values = np.zeros(7) # + 0.01 * np.random.random(7)
self.ang_deg = 60
self.desired_joint_values = np.array([
np.pi * self.ang_deg / 180, np.pi / 3, 0.0, np.pi / 6, np.pi / 6,
np.pi / 6, np.pi / 6
])
self.optCurve = []
self.step_size = 0.09
def promp_planner(self, mu_x, sig_x, mu_quat, sig_quat, obstacle_list, Qlists, timeLists, PromP_Planner=True):
time_normalised = np.linspace(0, 1, 35)
nDoF, nBf = 7, 5
cur_jv = self.group.get_current_joint_values()
weight_space_optim = optim_weight_space(time_normalised, nDoF, nBf, mu_x, sig_x, mu_quat, sig_quat,
obstacle_list, curr_jvs=cur_jv, QLists=Qlists, timeLists=timeLists)
weights = np.random.multivariate_normal(weight_space_optim.mean_cond_weight,
weight_space_optim.covMat_cond_weight)
if not PromP_Planner:
################ Trajectory space optimization: CHOMP
franka_kin = FrankaKinematics()
des_jv = weight_space_optim.post_mean_q
n = np.linalg.norm((des_jv - cur_jv)) / (len(time_normalised) - 1)
traj2optimise = self.discretize(cur_jv, des_jv, step_size=n)
traj2optimise = np.insert(traj2optimise, len(traj2optimise), des_jv,
axis=0) # inserting goal state at the end of discretized
traj2optimise = np.insert(traj2optimise, 0, cur_jv, axis=0)
print 'chomp n', n, traj2optimise.shape[0]
save_path = '/home/automato/Ash/MPlib/src/ProMPlib/with_obstacles/CHOMP/'
uniq_name = str(time.time())
traj_opt = TrajectoryOptimize(7, obstacle_list)
start = time.time()
optimised_trajectory = traj_opt.optimise(traj2optimise, withGrad=True)
np.savetxt(save_path + 'traj_CHOMP_%s.out' % uniq_name, optimised_trajectory, delimiter=',')
total_time = time.time() - start
print total_time
T, _ = franka_kin.fwd_kin(optimised_trajectory[-1])
quat = np.array(tf_tran.quaternion_from_matrix(T))
print 'quat_diff = ', quat - mu_quat
print '#########'
print 'pos_diff = ', T[:3, 3] - mu_x
vel, accel, tme = self.create_q_v_a(optimised_trajectory)
rt = self.create_trajectory(optimised_trajectory, vel, accel, tme)
rt_back = self.create_trajectory(np.flipud(optimised_trajectory), np.flipud(vel), np.flipud(accel), tme)
smooth_cost_promp = 0.0
return optimised_trajectory, total_time, rt, rt_back, smooth_cost_promp
##################################
else:
# Weight space optimization:
start = time.time()
optimised_trajectory, opt_weights = weight_space_optim.optimise(weights, withGrad=True)
smooth_cost_promp = weight_space_optim.smoothness_cost(opt_weights)
total_time = time.time() - start
print total_time
save_path = '/home/automato/Ash/MPlib/src/ProMPlib/with_obstacles/ProMP/'
uniq_name = str(time.time())
np.savetxt(save_path+'traj_promp_%s.out' % uniq_name, optimised_trajectory, delimiter=',')
np.savetxt(save_path+'weights_promp_%s.out' % uniq_name, opt_weights, delimiter=',')
franka_kin = FrankaKinematics()
T, _ = franka_kin.fwd_kin(optimised_trajectory[-1])
quat = np.array(tf_tran.quaternion_from_matrix(T))
print 'quat_diff = ', quat - mu_quat
print '#########'
print 'pos_diff = ', T[:3, 3] - mu_x
vel, accel, tme = self.create_q_v_a(optimised_trajectory)
rt = self.create_trajectory(optimised_trajectory, vel, accel, tme)
rt_back = self.create_trajectory(np.flipud(optimised_trajectory), np.flipud(vel), np.flipud(accel), tme)
return optimised_trajectory, total_time, rt, rt_back, smooth_cost_promp
class TrajectoryOptimize(object):
def __init__(
self,
nDoF,
obstacle_list=None,
ini_jv=None,
des_jv=None,
):
super(TrajectoryOptimize, self).__init__()
self.initial_joint_values = ini_jv
self.desired_joint_values = des_jv
self.scene = moveit_commander.PlanningSceneInterface()
# if obstacle_list is None:
# self.obstacle_list = np.array([[0.25, 0.21, 0.71, 0.1], [-0.15, 0.21, 0.51, 0.05]]) # for two spheres
# else:
# self.obstacle_list = obstacle_list
self.obstacle_list = obstacle_list
self.nDoF = nDoF
self.franka_kin = FrankaKinematics()
self.workspace_dim = (-0.5, -0.5, 0.2, 1.5, 0.5, 1.9)
# self.obstacle_list = np.array([[0.25, 0.21, 0.71, 0.1]]) # for one sphere (x,y,z, radius)
# self.obstacle_list = np.array([[0.25, 0.21, 0.71, 0.1],
# [-0.15, 0.21, 0.51, 0.05],
# [0.15, 0.61, 0.51, 0.05],
# [-0.15, -0.61, 0.51, 0.05],
# [-0.65, 0.31, 0.21, 0.05],
# [-0.55, 0.71, 0.71, 0.06],
# [-0.95, -0.51, 0.41, 0.07],
# [-0.85, 0.27, 0.61, 0.08],
# [-0.25, -0.31, 0.11, 0.04],
# [-0.75, 0.71, 0.91, 0.09],
# [-0.35, 0.51, 0.81, 0.02],
# [-0.95, -0.81, 0.61, 0.03],
# [-0.75, 0.11, 0.41, 0.02],
# [-0.55, -0.61, 0.21, 0.06],
# [0.25, 0.31, 0.71, 0.07],
# [-0.35, -0.41, 0.41, 0.02],
# [-0.25, -0.51, 0.61, 0.08],
# ]) # for 15 spheres
# self.obstacle_list1 = self.populate_obstacles()
self.optCurve, self.costs = [], []
self.step_size = 0.06
self.debug = False
self.lamda_smooth, self.lamda_obs = 10.0, 20.0
def populate_obstacles(self, ):
# if the obstacle is within robot workspace, then consider those as obstacles
obs = []
x_min, x_max = self.workspace_dim[0], self.workspace_dim[3]
y_min, y_max = self.workspace_dim[1], self.workspace_dim[4]
z_min, z_max = self.workspace_dim[2], self.workspace_dim[5]
for k, v in self.scene.get_objects().items():
px, py, pz = v.primitive_poses[0].position.x, v.primitive_poses[
0].position.y, v.primitive_poses[0].position.z
if px >= x_min and px <= x_max:
if py >= y_min and py <= y_max:
if pz >= z_min and pz <= z_max:
obs.append([px, py, pz, 0.05])
return np.array(obs)
def sphere(self, ax, radius, centre):
u = np.linspace(0, 2 * np.pi, 13)
v = np.linspace(0, np.pi, 7)
x = centre[0] + radius * np.outer(np.cos(u), np.sin(v))
y = centre[1] + radius * np.outer(np.sin(u), np.sin(v))
z = centre[2] + radius * np.outer(np.ones(np.size(u)), np.cos(v))
xdata = scipy.ndimage.zoom(x, 3)
ydata = scipy.ndimage.zoom(y, 3)
zdata = scipy.ndimage.zoom(z, 3)
ax.plot_surface(xdata,
ydata,
zdata,
rstride=3,
cstride=3,
color='w',
shade=0)
def optimise(self, trajectory, withGrad=True):
self.initial_joint_values = trajectory[0]
self.desired_joint_values = trajectory[-1]
self.trajectory_to_optimise = trajectory[1:-1]
trajectory_flat = self.trajectory_to_optimise.T.reshape((-1))
if withGrad:
################### With Gradient #########################
now = time.time()
opti_traj = opt.minimize(self.calculate_total_cost,
trajectory_flat,
method='BFGS',
jac=self.cost_gradient_analytic,
options={
'maxiter': 150,
'disp': True
},
callback=self.optim_callback)
cur = time.time()
print 'CHOMP time with gradient:', cur - now
opti_trajec = np.transpose(opti_traj.x.reshape((self.nDoF, -1)))
opti_trajec = np.insert(opti_trajec,
0,
self.initial_joint_values,
axis=0)
opti_trajec = np.insert(opti_trajec,
len(opti_trajec),
self.desired_joint_values,
axis=0)
# #
else:
# ###################### Without Gradient ############################
now = time.time()
opti_traj = opt.minimize(self.calculate_total_cost,
trajectory_flat,
method='BFGS',
options={
'maxiter': 150,
'disp': True
},
callback=self.optim_callback)
cur = time.time()
print 'CHOMP time without gradient:', cur - now
opti_trajec = np.transpose(opti_traj.x.reshape((self.nDoF, -1)))
opti_trajec = np.insert(opti_trajec,
0,
self.initial_joint_values,
axis=0)
opti_trajec = np.insert(opti_trajec,
len(opti_trajec),
self.desired_joint_values,
axis=0)
return opti_trajec, self.costs, opti_traj.success
def calculate_total_cost(self, trajectoryFlat):
F_smooth = self.smoothness_objective(
trajectoryFlat) # smoothness of trajectory is captured here
if self.obstacle_list is not None:
obstacle_cost = self.obstacle_cost(trajectoryFlat,
self.obstacle_list)
else:
obstacle_cost = 0.0
return self.lamda_smooth * F_smooth + self.lamda_obs * obstacle_cost # for with gradient
# return 10 * F_smooth + 1.5 * obstacle_cost # for without gradient
def smoothness_objective(self, trajectoryFlat):
trajectoryFlat = np.squeeze(trajectoryFlat)
trajectory = trajectoryFlat.reshape((self.nDoF, -1)).T
dim = np.multiply(*trajectory.shape)
fd_matrix, b = self.finite_diff_matrix(trajectory)
trajectory = trajectory.T.reshape((dim, 1))
F_smooth = 0.5 * (np.dot(trajectory.T, np.dot(fd_matrix, trajectory)) +
np.dot(trajectory.T, b) + 0.25 * np.dot(b.T, b))
return F_smooth
def finite_diff_matrix(self, trajectory):
rows, columns = trajectory.shape # columns = nDoF
A = 2 * np.eye(rows)
A[0, 1] = -1
A[rows - 1, rows - 2] = -1
for ik in range(0, rows - 2):
A[ik + 1, ik] = -1
A[ik + 1, ik + 2] = -1
dim = rows * columns
fd_matrix = np.zeros((dim, dim))
b = np.zeros((dim, 1))
i, j = 0, 0
while i < dim:
fd_matrix[i:i + len(A), i:i + len(A)] = A
b[i] = -2 * self.initial_joint_values[j]
b[i + len(A) - 1] = -2 * self.desired_joint_values[j]
i = i + len(A)
j = j + 1
return fd_matrix, b
def obstacle_cost(
self,
trajectoryFlat,
obstacle_list,
):
trajectoryFlat = np.squeeze(trajectoryFlat)
trajectory = (trajectoryFlat.reshape(
(self.nDoF, trajectoryFlat.shape[0] / self.nDoF))).T
robot_body_points = self.calculate_robot_body_points(trajectory)
vel_normalised, vel_mag, vel = self.calculate_normalised_workspace_velocity(
robot_body_points)
dist = self.compute_minimum_distance_to_objects(
robot_body_points[:-1], obstacle_list)
obstacle_cost_potential = np.array(self.cost_potential(dist))
return np.sum(np.multiply(obstacle_cost_potential, vel_mag))
def cost_potential(self, D, collision_threshold=0.07):
c = np.zeros(D.shape)
return np.where(
D < 0, -D + 0.5 * collision_threshold,
np.where(D <= collision_threshold,
(0.5 *
(D - collision_threshold)**2) / collision_threshold, c))
def compute_minimum_distance_to_objects(self,
robot_body_points,
obstacle_list,
body_sphere_size=0.12):
rbp_shape = robot_body_points.shape
return np.min(
distance.cdist(robot_body_points.reshape(-1, 3, order='A'),
obstacle_list[:, :3]) - obstacle_list[:, 3] -
body_sphere_size,
axis=1).reshape(rbp_shape[:2])
def calculate_smoothness_gradient(self, trajectoryFlat):
trajectory = np.squeeze(trajectoryFlat).reshape((self.nDoF, -1)).T
dim = np.multiply(*trajectory.shape)
fd_matrix, b = self.finite_diff_matrix(trajectory)
trajectory = trajectory.T.reshape((dim, 1))
return 0.5 * b + fd_matrix.dot(trajectory)
def get_robot_discretised_points(self, fwd_k_j_positions, step_size=0.2):
discretised = list()
j_index = list()
# self.get_robot_discretised_joint_index(fwd_k_j_positions, step_size=0.2)
for j in range(len(fwd_k_j_positions) - 1):
w = fwd_k_j_positions[j + 1] - fwd_k_j_positions[j]
if len(w[w != 0.]):
step = step_size * w / np.linalg.norm(w)
n = int(np.linalg.norm(w) / np.linalg.norm(step)) + 1
discretised.extend(
np.outer(np.arange(1, n), step) + fwd_k_j_positions[j])
j_index.append(len(discretised))
self.j_index = j_index
return np.array(discretised)
def get_robot_joint_positions(self, joint_values):
_, t_joints = self.franka_kin.fwd_kin(joint_values)
return np.vstack(([0., 0., 0.], t_joints[:, :3, 3]))
def calculate_robot_body_points(self, trajectory):
trajectory = np.vstack((trajectory, self.desired_joint_values))
return np.array([
self.get_robot_discretised_points(
self.get_robot_joint_positions(joint_values),
self.step_size,
) for joint_values in trajectory
])
def calculate_normalised_workspace_velocity(self, robot_body_points):
velocity = np.diff(robot_body_points, axis=0)
vel_magnitude = np.linalg.norm(velocity, axis=2)
vel_normalised = np.divide(velocity,
vel_magnitude[:, :, None],
out=np.zeros_like(velocity),
where=vel_magnitude[:, :, None] != 0)
return vel_normalised, vel_magnitude, velocity
def extract_block_diag(self, A, M=3, k=0):
ny, nx = A.shape
ndiags = min(map(lambda x: x // M, A.shape))
offsets = (nx * M + M, nx, 1)
strides = map(lambda x: x * A.itemsize, offsets)
if k > 0:
B = A[:, k * M]
ndiags = min(nx // M - k, ny // M)
else:
k = -k
B = A[k * M]
ndiags = min(nx // M, ny // M - k)
return as_strided(B,
shape=(ndiags, M, M),
strides=((nx * M + M) * A.itemsize, nx * A.itemsize,
A.itemsize))
def calculate_curvature(self, vel_normalised, vel_magnitude, velocity):
s = vel_normalised.shape
acceleration = np.gradient(velocity, axis=0).reshape(-1, 3)
ttm = vel_normalised.reshape(-1, 1).dot(vel_normalised.reshape(1, -1))
orthogonal_projector = self.extract_block_diag(
(np.eye(ttm.shape[0]) - ttm), 3)
temp = np.array([
orthogonal_projector[i, :, :].dot(acceleration[i, :])
for i in range(orthogonal_projector.shape[0])
])
curvature = np.divide(temp,
vel_magnitude.reshape(-1, 1)**2,
out=np.zeros_like(temp),
where=vel_magnitude.reshape(-1, 1) != 0)
return curvature.reshape(s), orthogonal_projector.reshape(
(s[0], s[1], s[2], s[2]))
def fun(self, points):
dist = self.compute_minimum_distance_to_objects(
points.reshape((1, 1, 3)), self.obstacle_list)
return np.array(self.cost_potential(dist))
def gradient_cost_potential(self, robot_discretised_points):
gradient_cost_potential = np.zeros(
robot_discretised_points.reshape(-1, 3).shape)
for i, points in enumerate(robot_discretised_points.reshape((-1, 3))):
gradient_cost_potential[i, :] = opt.approx_fprime(
points, self.fun, [1e-06, 1e-06, 1e-06])
return np.array(gradient_cost_potential).reshape(
robot_discretised_points.shape[0], -1, 3)
def calculate_jacobian2(self,
robot_body_points,
joint_index,
trajectories=None):
# Body Points: time X body points X 3
# joint index: number of body points between joints as array
# trajectory: time X number of joints
joint_idxs = np.arange(self.nDoF)
p_map = np.tril(np.ones((self.nDoF, self.nDoF), dtype=np.float32))
t_joints = np.array(
zip(*[
self.franka_kin.fwd_kin(trajectory)
for trajectory in trajectories
])[1])
joint_axis = t_joints[:, :, :3, 2]
joint_positions = t_joints[:, :7, :3,
-1] # Excluding the fixed joint at the end
jacobian = np.zeros((robot_body_points.shape[0],
robot_body_points.shape[1], 3, self.nDoF))
# s = robot_body_points.shape
# robot_body_points = robot_body_points.reshape(-1, 3)
for ti in range(trajectories.shape[0]):
for i, points in enumerate(
np.split(robot_body_points, joint_index)[:-1]):
for point in points:
for joint in joint_idxs:
if i > 0 and p_map[i - 1, joint]:
jacobian[ti, i, :, joint] = np.cross(
joint_axis[ti, joint, :],
point[ti] - joint_positions[ti, joint])
return jacobian
def calculate_jacobian(self,
robot_body_points,
joint_index,
joint_values=None):
class ParentMap(object):
def __init__(self, num_joints):
self.joint_idxs = [i for i in range(num_joints)]
self.p_map = np.zeros((num_joints, num_joints))
for i in range(num_joints):
for j in range(num_joints):
if j <= i:
self.p_map[i][j] = True
def is_parent(self, parent, child):
if child not in self.joint_idxs or parent not in self.joint_idxs:
return False
return self.p_map[child][parent]
parent_map = ParentMap(7)
if joint_values is None:
joint_values = list(
self.robot_state.get_current_state().joint_state.position)[:7]
_, t_joints = self.franka_kin.fwd_kin(joint_values)
joint_axis = t_joints[:, :3, 2]
joint_positions = t_joints[:7, :3,
3] # Excluding the fixed joint at the end
jacobian = list()
for i, points in enumerate(
np.split(robot_body_points, joint_index)[:-1]):
# print i, len(points)
if i == 0:
for point in points:
jacobian.append(np.zeros((3, 7)))
else:
for point in points:
jacobian.append(np.zeros((3, 7)))
for joint in parent_map.joint_idxs:
if parent_map.is_parent(joint, i - 1):
# find cross product
col = np.cross(joint_axis[joint, :],
point - joint_positions[joint])
jacobian[-1][0][joint] = col[0]
jacobian[-1][1][joint] = col[1]
jacobian[-1][2][joint] = col[2]
else:
jacobian[-1][0][joint] = 0.0
jacobian[-1][1][joint] = 0.0
jacobian[-1][2][joint] = 0.0
return np.array(jacobian)
def calculate_obstacle_cost_gradient(self, trajectoryFlat):
s = trajectoryFlat.shape
trajectory = np.squeeze(trajectoryFlat).reshape((self.nDoF, -1)).T
robot_discretised_points = self.calculate_robot_body_points(trajectory)
vel_normalised, vel_magnitude, velocity = self.calculate_normalised_workspace_velocity(
robot_discretised_points)
curvature, orthogonal_projector = self.calculate_curvature(
vel_normalised, vel_magnitude, velocity)
dist = self.compute_minimum_distance_to_objects(
robot_discretised_points[:-1],
self.obstacle_list,
)
obstacle_cost_potential = np.array(self.cost_potential(dist))
gradient_cost_potential = self.gradient_cost_potential(
robot_discretised_points[:-1])
obstacle_gradient = np.zeros((trajectory.shape[0], self.nDoF))
# jacobian1 = self.calculate_jacobian2(robot_discretised_points[:-1], self.j_index, trajectory)
a = robot_discretised_points[:-1]
for jvs in range(trajectory.shape[0]):
obst_grad = np.zeros(trajectory.shape[1])
jacobian = self.calculate_jacobian(a[jvs], self.j_index,
trajectory[jvs])
for num_points in range(robot_discretised_points.shape[1]):
temp1 = orthogonal_projector[jvs, num_points].dot(
gradient_cost_potential[jvs, num_points, :])
temp2 = obstacle_cost_potential[jvs, num_points] * curvature[
jvs, num_points, :]
temp3 = vel_magnitude[jvs, num_points] * (temp1 - temp2)
obst_grad += jacobian[num_points, :].T.dot(temp3)
obstacle_gradient[jvs, :] = obst_grad
return obstacle_gradient.T
def cost_gradient_analytic(
self, trajectoryFlat
): # calculate grad(cost) = grad(smoothness_cost) + grad(obstacle_cost)
smoothness_gradient = self.calculate_smoothness_gradient(
trajectoryFlat)
if self.obstacle_list is not None:
obstacle_gradient = self.calculate_obstacle_cost_gradient(
trajectoryFlat)
else:
obstacle_gradient = np.zeros(len(trajectoryFlat))
trajectory = np.squeeze(trajectoryFlat)
cost_gradient = self.lamda_obs * obstacle_gradient.reshape(
(len(trajectory), 1)) + self.lamda_smooth * smoothness_gradient
return np.squeeze(cost_gradient)
def cost_gradient_numeric(self, trajectory):
trajectory = np.squeeze(trajectory)
obst_cost_grad_numeric = opt.approx_fprime(
trajectory, traj_opt.obstacle_cost,
1e-08 * np.ones(len(trajectory)))
smoothness_gradient = np.squeeze(
self.calculate_smoothness_gradient(trajectory))
return np.squeeze(obst_cost_grad_numeric + smoothness_gradient)
def optim_callback(self, xk):
self.costs.append(self.calculate_total_cost(xk)[0, 0])
self.optCurve.append(xk)
print 'Iteration {}: {:2.4}\n'.format(
len(self.optCurve), self.costs[len(self.optCurve) - 1])
def animation(self, optimized_trajectory, initial_trajectory):
fig = plt.figure()
ax = fig.add_subplot(111, projection="3d")
# ax.axis('off')
plt.show(block=False)
while True:
for i in range(len(optimized_trajectory)):
_, T_joint_optim = self.franka_kin.fwd_kin(
optimized_trajectory[i])
_, T_joint_init = self.franka_kin.fwd_kin(
initial_trajectory[i])
ax.clear()
if self.obstacle_list is not None:
for object in self.obstacle_list:
self.sphere(ax, object[-1], object[0:-1])
self.franka_kin.plotter(ax,
T_joint_optim,
'optim',
color='blue')
self.franka_kin.plotter(ax, T_joint_init, 'init', color='red')
fig.canvas.draw()
fig.canvas.flush_events()
plt.pause(0.01)
plt.pause(1)
def promp_planner(self,
mu_x,
sig_x,
mu_quat,
sig_quat,
obstacle_list,
Qlists,
timeLists,
PromP_Planner=True):
time_normalised = np.linspace(0, 1, 35)
nDoF, nBf = 7, 5
cur_jv = self.group.get_current_joint_values()
weight_space_optim = optim_weight_space(time_normalised,
nDoF,
nBf,
mu_x,
sig_x,
mu_quat,
sig_quat,
obstacle_list,
curr_jvs=cur_jv,
QLists=Qlists,
timeLists=timeLists)
weights = np.random.multivariate_normal(
weight_space_optim.mean_cond_weight,
weight_space_optim.covMat_cond_weight)
des_jv = weight_space_optim.post_mean_q
if not PromP_Planner:
################ Trajectory space optimization: CHOMP
franka_kin = FrankaKinematics()
n = np.linalg.norm((des_jv - cur_jv)) / (len(time_normalised) - 1)
traj2optimise = self.discretize(cur_jv, des_jv, step_size=n)
traj2optimise = np.insert(
traj2optimise, len(traj2optimise), des_jv,
axis=0) # inserting goal state at the end of discretized
traj2optimise = np.insert(traj2optimise, 0, cur_jv, axis=0)
print 'chomp n', n, traj2optimise.shape[0]
traj_opt = TrajectoryOptimize(7,
obstacle_list=obstacle_list,
ini_jv=cur_jv,
des_jv=des_jv)
fdm, _ = traj_opt.finite_diff_matrix(traj2optimise)
fdm_inv = np.linalg.inv(fdm)
traj_flat = traj2optimise.T.reshape((-1))
epsilon_k = np.random.multivariate_normal(
mean=np.zeros(len(traj_flat)),
cov=fdm_inv * (1. / len(traj_flat)))
traj_flat += epsilon_k
traj2optimise = (traj_flat.reshape(
(nDoF, -1))).T # trajectory initialization as in STOMP
start = time.time()
opti_traj, costs, success = traj_opt.optimise(traj2optimise,
withGrad=True)
total_time = time.time() - start
print total_time
save_path = '/home/automato/Ash/MPlib/src/ProMPlib/going_under_table/Simulations/CHOMP/'
uniq_name = str(time.time())
np.savetxt(save_path + 'traj_CHOMP_%s.out' % uniq_name,
opti_traj,
delimiter=',')
np.savetxt(save_path + 'costs_%s.out' % uniq_name,
costs,
delimiter=',')
T, _ = franka_kin.fwd_kin(opti_traj[-1])
quat = np.array(tf_tran.quaternion_from_matrix(T))
print 'quat_diff = ', quat - mu_quat
print '#########'
print 'pos_diff = ', T[:3, 3] - mu_x
vel, accel, tme = self.create_q_v_a(opti_traj)
rt = self.create_trajectory(opti_traj, vel, accel, tme)
rt_back = self.create_trajectory(np.flipud(opti_traj),
np.flipud(vel), np.flipud(accel),
tme)
smooth_cost_promp = 0.0
##################################
else:
# Weight space optimization:
trajectoryFlat = weight_space_optim.basisMultiDoF.dot(weights)
init_traj = (trajectoryFlat.reshape((nDoF, -1))).T
init_traj = init_traj[1:-1]
init_traj = np.insert(init_traj, 0, cur_jv, axis=0)
init_traj = np.insert(init_traj,
init_traj.shape[0],
des_jv,
axis=0)
start = time.time()
opti_traj, opt_weights, costs, success = weight_space_optim.optimise(
weights, withGrad=True)
smooth_cost_promp = weight_space_optim.smoothness_cost(opt_weights)
total_time = time.time() - start
print total_time
save_path = '/home/automato/Ash/MPlib/src/ProMPlib/going_under_table/Simulations/ProMP/'
uniq_name = str(time.time())
np.savetxt(save_path + 'traj_promp_%s.out' % uniq_name,
opti_traj,
delimiter=',')
np.savetxt(save_path + 'weights_promp_%s.out' % uniq_name,
opt_weights,
delimiter=',')
np.savetxt(save_path + 'costs_%s.out' % uniq_name,
costs,
delimiter=',')
# To plot demonstrated trajectories Vs time
# scale = 1
# for plotDoF in range(7):
# plt.figure()
# for i in range(init_traj.shape[0]):
# plt.plot(time_normalised, init_traj[:, 5], color='red', linestyle='--', label='initial trajectory')
# plt.plot(time_normalised, opti_traj[:, 5], color='green', label='optimized trajectory')
# # plt.xlim(1 * scale, 1 * scale)
# plt.ylim(1 * scale, 3 * scale)
# plt.title('DoF {}'.format(plotDoF))
# plt.xlabel('time')
franka_kin = FrankaKinematics()
T, _ = franka_kin.fwd_kin(opti_traj[-1])
quat = np.array(tf_tran.quaternion_from_matrix(T))
print 'quat_diff = ', quat - mu_quat
print '#########'
print 'pos_diff = ', T[:3, 3] - mu_x
vel, accel, tme = self.create_q_v_a(opti_traj)
rt = self.create_trajectory(opti_traj, vel, accel, tme)
rt_back = self.create_trajectory(np.flipud(opti_traj),
np.flipud(vel), np.flipud(accel),
tme)
return opti_traj, total_time, rt, rt_back, smooth_cost_promp, (quat - mu_quat)/mu_quat, \
(T[:3, 3] - mu_x)/mu_x, success
# "enter 0: RRTConnectkConfigDefault, 1: PRMkConfigDefault, 2: CHOMP, "
# "3: ProMP\n").strip())
planner_index = 3
# idxx = 0 # int(raw_input('Enter sample mu_x and Quart: 0 - 35 \n').strip())
for idxx in range(102):
print idxx
if planner_index == 0 or planner_index == 1:
planner_id = planner_ids[planner_index]
move_robot.group.set_planner_id(planner_id)
plan, plan_time = move_robot.plan(
mu_x[idxx][0], mu_x[idxx][1], mu_x[idxx][2],
Quaternion[idxx][0], Quaternion[idxx][1],
Quaternion[idxx][2], Quaternion[idxx][3])
franka_kin = FrankaKinematics()
# if not isinstance(plan, (list, tuple, np.ndarray)):
if plan.joint_trajectory.points:
# n = len(plan.joint_trajectory.points)
# joint_positions = np.zeros((n, 7))
# for i in range(n):
# joint_positions[i, :] = plan.joint_trajectory.points[i].positions
# if plan is not None:
final_jv = plan.joint_trajectory.points[
-1].positions
T, _ = franka_kin.fwd_kin(final_jv)
quat = np.array(tf_tran.quaternion_from_matrix(T))
quat_err = (quat -
Quaternion[idxx]) / Quaternion[idxx]
pos_err = (T[:3, 3] - mu_x[idxx]) / mu_x[idxx]
save_path = '/home/automato/Ash/MPlib/src/ProMPlib/going_under_table/RRT/'
class CostFunction(object):
def __init__(self):
super(CostFunction, self).__init__()
moveit_commander.roscpp_initialize(sys.argv)
self.collision_threshold = 0.07
self.robot_state = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface()
self.franka_k = FrankaKinematics()
def show_objects(self):
for k, v in self.scene.get_objects().items():
print "Pos:", v.primitive_poses[0].position.x, v.primitive_poses[0].position.y,\
v.primitive_poses[0].position.z
print "Ori:", v.primitive_poses[0].orientation.x, v.primitive_poses[0].orientation.y,\
v.primitive_poses[0].orientation.z, v.primitive_poses[0].orientation.w
print "Size:", v.primitives[0].dimensions
def setup_planner(self):
self.group = moveit_commander.MoveGroupCommander("panda_arm")
self.group.set_end_effector_link(
"panda_hand") # planning wrt to panda_hand or link8
self.group.set_max_velocity_scaling_factor(
0.05) # scaling down velocity
self.group.set_max_acceleration_scaling_factor(
0.05) # scaling down velocity
self.group.allow_replanning(True)
self.group.set_num_planning_attempts(5)
self.group.set_goal_position_tolerance(0.0002)
self.group.set_goal_orientation_tolerance(0.01)
self.group.set_planning_time(5)
self.group.set_planner_id("FMTkConfigDefault")
def get_robot_discretised_points(self,
joint_values=None,
step_size=0.2,
with_joint_index=False):
if joint_values is None:
joint_values = list(
self.robot_state.get_current_state().joint_state.position)[:7]
_, t_joints = self.franka_k.fwd_kin(joint_values)
fwd_k_j_positions = np.vstack(([0., 0., 0.], t_joints[:, :3, 3]))
discretised = list()
j_index = list()
for j in range(len(fwd_k_j_positions) - 1):
w = fwd_k_j_positions[j + 1] - fwd_k_j_positions[j]
if len(w[w != 0.]):
step = step_size * w / np.linalg.norm(w)
n = int(np.linalg.norm(w) / np.linalg.norm(step)) + 1
discretised.extend(
np.outer(np.arange(1, n), step) + fwd_k_j_positions[j])
j_index.append(len(discretised))
return (np.array(discretised),
j_index) if with_joint_index else np.array(discretised)
def get_object_collision_spheres(self):
return [(v.primitive_poses[0].position.x, v.primitive_poses[0].position.y, v.primitive_poses[0].position.z,\
v.primitives[0].dimensions[0]) for v in self.scene.get_objects().values()]
def compute_minimum_distance_to_objects(
self,
robot_body_points,
object_list=None,
):
if object_list is None:
object_list = self.get_object_collision_spheres()
D = list()
if len(robot_body_points.shape) == 1:
robot_body_points = robot_body_points.reshape(1, 3)
for r in robot_body_points: # expects robot_body_points as an array of dimension 1 x n
dist = []
for o in object_list:
ro = r - o[:3]
norm_ro = np.linalg.norm(ro)
dist.append(norm_ro - 0.15 - o[3])
D.append(np.min(np.array(dist)))
return D
def cost_potential(self, D):
c = list()
for d in D:
if d < 0.:
c.append(-d + 0.5 * self.collision_threshold)
elif d <= self.collision_threshold:
c.append((0.5 * (d - self.collision_threshold)**2) /
self.collision_threshold)
else:
c.append(0)
return c
def calculate_normalised_workspace_velocity(self, trajectory):
# We have not divided by time as this has been indexed and is thus not available
position_vectors = np.array([
self.get_robot_discretised_points(joint_values)
for joint_values in trajectory
])
velocity = np.gradient(position_vectors, axis=0)
vel_magnitude = np.linalg.norm(velocity, axis=2)
vel_normalised = np.divide(velocity,
vel_magnitude[:, :, None],
out=np.zeros_like(velocity),
where=vel_magnitude[:, :, None] != 0)
return vel_normalised, vel_magnitude, velocity
def calculate_jacobian(self,
robot_body_points,
joint_index,
joint_values=None):
class ParentMap(object):
def __init__(self, num_joints):
self.joint_idxs = [i for i in range(num_joints)]
self.p_map = np.zeros((num_joints, num_joints))
for i in range(num_joints):
for j in range(num_joints):
if j <= i:
self.p_map[i][j] = True
def is_parent(self, parent, child):
if child not in self.joint_idxs or parent not in self.joint_idxs:
return False
return self.p_map[child][parent]
parent_map = ParentMap(7)
if joint_values is None:
joint_values = list(
self.robot_state.get_current_state().joint_state.position)[:7]
_, t_joints = self.franka_k.fwd_kin(joint_values)
joint_axis = t_joints[:, :3, 2]
joint_positions = t_joints[:7, :3,
3] # Excluding the fixed joint at the end
jacobian = list()
for i, points in enumerate(
np.split(robot_body_points, joint_index)[:-1]):
# print i, len(points)
if i == 0:
for point in points:
jacobian.append(np.zeros((3, 7)))
else:
for point in points:
jacobian.append(np.zeros((3, 7)))
for joint in parent_map.joint_idxs:
if parent_map.is_parent(joint, i - 1):
# find cross product
col = np.cross(joint_axis[joint, :],
point - joint_positions[joint])
jacobian[-1][0][joint] = col[0]
jacobian[-1][1][joint] = col[1]
jacobian[-1][2][joint] = col[2]
else:
jacobian[-1][0][joint] = 0.0
jacobian[-1][1][joint] = 0.0
jacobian[-1][2][joint] = 0.0
return np.array(jacobian)
def calculate_curvature(self, trajectory):
vel_normalised, vel_magnitude, velocity = self.calculate_normalised_workspace_velocity(
trajectory)
temp = (np.eye(len(vel_normalised)) -
vel_normalised.dot(vel_normalised.T))
curvature = np.dot(temp, np.gradient(velocity)) / vel_magnitude**2
return curvature
def __init__(self, time, nDoF, nBf, mu_x=[0.543602, -0.239265, 0.470648], sig_x=np.eye(3) * 0.0000002,
mu_quat=[0.724822, -0.282179, 0.544974, -0.313067], sig_quat=np.eye(4) * 0.0002,
obstacle_list=None, curr_jvs=None, QLists=None, timeLists=None):
self.time_normalised = time
self.nDoF = nDoF
if curr_jvs is None:
self.curr_jvs = Q[2][0, :-2] # np.array([-0.25452656, -0.3225854, -0.24426211, -0.3497535 , 0.06971882, 0.12819999, 0.65348821])
else:
self.curr_jvs = curr_jvs
self.obstacle_list = obstacle_list
self.franka_kin = FrankaKinematics()
self.mu_x = mu_x
self.sig_x = sig_x
self.mu_quat_des = mu_quat
self.sig_quat = sig_quat
self.collision_threshold = 0.07
# self.obstacle_list = np.array([[0.25, 0.0, 0.71, 0.1], [-0.15, 0.21, 0.51, 0.05]]) # for two spheres
self.phaseGenerator = phase.LinearPhaseGenerator()
#self.phaseGenerator = phase.SmoothPhaseGenerator(duration = 1)
self.basisGenerator = basis.NormalizedRBFBasisGenerator(self.phaseGenerator, numBasis=nBf, duration=1,
basisBandWidthFactor=3,
numBasisOutside=1)
self.basisMultiDoF = self.basisGenerator.basisMultiDoF(self.time_normalised, self.nDoF)
self.learnedProMP = promps.ProMP(self.basisGenerator, self.phaseGenerator, self.nDoF)
self.learner = promps.MAPWeightLearner(self.learnedProMP)
self.Q_parsed, self.timeP = self.Q_parser(self.mu_x)
# self.learnedData = self.learner.learnFromData(self.Q_parsed, self.timeP)
if QLists is None:
QLists = A
else:
QLists = QLists
if timeLists is None:
timeList = timeData
else:
timeList = timeLists
self.learnedData = self.learner.learnFromData(QLists, timeList)
self.mu_theta, self.sig_theta = self.learnedProMP.getMeanAndCovarianceTrajectory(np.array([1.0]))
self.sig_theta = np.squeeze(self.sig_theta)
self.post_mean_q, self.post_cov_q = self.franka_kin.inv_kin_ash_pose_quaternion(np.squeeze(self.mu_theta),
self.sig_theta, self.mu_x, self.sig_x, self.mu_quat_des, self.sig_quat)
self.taskProMP0 = self.learnedProMP.jointSpaceConditioning(0, desiredTheta=self.curr_jvs, desiredVar=np.eye(len(self.curr_jvs)) * 0.00001)
self.taskProMP = self.taskProMP0.jointSpaceConditioning(1.0, desiredTheta=self.post_mean_q, desiredVar=np.eye(len(self.curr_jvs)) * 0.00001) #desiredVar=self.post_cov_q)
self.trajectories_learned = self.learnedProMP.getTrajectorySamples(self.time_normalised, n_samples=20)
self.trajectories_task_conditioned = self.taskProMP.getTrajectorySamples(self.time_normalised, n_samples=20)
self.mean_cond_weight = self.taskProMP.mu
self.covMat_cond_weight = self.taskProMP.covMat
self.inv_covMat_cond = np.linalg.inv(self.covMat_cond_weight)
self.trajectoryFlat = self.basisMultiDoF.dot(self.mean_cond_weight)
self.trajectory = (self.trajectoryFlat.reshape((self.nDoF, self.trajectoryFlat.shape[0] / self.nDoF))).T
# self.initial_joint_values = self.trajectory[0, :]
self.initial_joint_values = self.curr_jvs #Q[2][0, :-2] # np.array([-0.25452656, -0.3225854, -0.24426211, -0.3497535 , 0.06971882, 0.12819999, 0.65348821])
self.desired_joint_values = self.trajectory[-1, :] # same as self.post_mean_q
self.reguCoeff = 0.003
self.lamda_smooth, self.lamda_obs = 10.0, 20.0
self.optCurve, self.costs = [], []
self.step_size = 0.06
class optim_weight_space(object):
def __init__(self, time, nDoF, nBf, mu_x=[0.543602, -0.239265, 0.470648], sig_x=np.eye(3) * 0.0000002,
mu_quat=[0.724822, -0.282179, 0.544974, -0.313067], sig_quat=np.eye(4) * 0.0002,
obstacle_list=None, curr_jvs=None, QLists=None, timeLists=None):
self.time_normalised = time
self.nDoF = nDoF
if curr_jvs is None:
self.curr_jvs = Q[2][0, :-2] # np.array([-0.25452656, -0.3225854, -0.24426211, -0.3497535 , 0.06971882, 0.12819999, 0.65348821])
else:
self.curr_jvs = curr_jvs
self.obstacle_list = obstacle_list
self.franka_kin = FrankaKinematics()
self.mu_x = mu_x
self.sig_x = sig_x
self.mu_quat_des = mu_quat
self.sig_quat = sig_quat
self.collision_threshold = 0.07
# self.obstacle_list = np.array([[0.25, 0.0, 0.71, 0.1], [-0.15, 0.21, 0.51, 0.05]]) # for two spheres
self.phaseGenerator = phase.LinearPhaseGenerator()
#self.phaseGenerator = phase.SmoothPhaseGenerator(duration = 1)
self.basisGenerator = basis.NormalizedRBFBasisGenerator(self.phaseGenerator, numBasis=nBf, duration=1,
basisBandWidthFactor=3,
numBasisOutside=1)
self.basisMultiDoF = self.basisGenerator.basisMultiDoF(self.time_normalised, self.nDoF)
self.learnedProMP = promps.ProMP(self.basisGenerator, self.phaseGenerator, self.nDoF)
self.learner = promps.MAPWeightLearner(self.learnedProMP)
self.Q_parsed, self.timeP = self.Q_parser(self.mu_x)
# self.learnedData = self.learner.learnFromData(self.Q_parsed, self.timeP)
if QLists is None:
QLists = A
else:
QLists = QLists
if timeLists is None:
timeList = timeData
else:
timeList = timeLists
self.learnedData = self.learner.learnFromData(QLists, timeList)
self.mu_theta, self.sig_theta = self.learnedProMP.getMeanAndCovarianceTrajectory(np.array([1.0]))
self.sig_theta = np.squeeze(self.sig_theta)
self.post_mean_q, self.post_cov_q = self.franka_kin.inv_kin_ash_pose_quaternion(np.squeeze(self.mu_theta),
self.sig_theta, self.mu_x, self.sig_x, self.mu_quat_des, self.sig_quat)
self.taskProMP0 = self.learnedProMP.jointSpaceConditioning(0, desiredTheta=self.curr_jvs, desiredVar=np.eye(len(self.curr_jvs)) * 0.00001)
self.taskProMP = self.taskProMP0.jointSpaceConditioning(1.0, desiredTheta=self.post_mean_q, desiredVar=np.eye(len(self.curr_jvs)) * 0.00001) #desiredVar=self.post_cov_q)
self.trajectories_learned = self.learnedProMP.getTrajectorySamples(self.time_normalised, n_samples=20)
self.trajectories_task_conditioned = self.taskProMP.getTrajectorySamples(self.time_normalised, n_samples=20)
self.mean_cond_weight = self.taskProMP.mu
self.covMat_cond_weight = self.taskProMP.covMat
self.inv_covMat_cond = np.linalg.inv(self.covMat_cond_weight)
self.trajectoryFlat = self.basisMultiDoF.dot(self.mean_cond_weight)
self.trajectory = (self.trajectoryFlat.reshape((self.nDoF, self.trajectoryFlat.shape[0] / self.nDoF))).T
# self.initial_joint_values = self.trajectory[0, :]
self.initial_joint_values = self.curr_jvs #Q[2][0, :-2] # np.array([-0.25452656, -0.3225854, -0.24426211, -0.3497535 , 0.06971882, 0.12819999, 0.65348821])
self.desired_joint_values = self.trajectory[-1, :] # same as self.post_mean_q
self.reguCoeff = 0.003
self.lamda_smooth, self.lamda_obs = 10.0, 20.0
self.optCurve, self.costs = [], []
self.step_size = 0.06
def Q_parser(self, mu_x):
QQ, tme = [], []
for i in range(len(Q)):
jvs = Q[i][-1, :]
T, _ = self.franka_kin.fwd_kin(jvs)
endEffPos = T[:3, 3]
dist = np.linalg.norm((mu_x - endEffPos))
if dist <= 0.5:
QQ.append(Q[i])
tme.append(timeData[i])
return QQ, tme
def sphere(self, ax, radius, centre):
u = np.linspace(0, 2 * np.pi, 13)
v = np.linspace(0, np.pi, 7)
x = centre[0] + radius * np.outer(np.cos(u), np.sin(v))
y = centre[1] + radius * np.outer(np.sin(u), np.sin(v))
z = centre[2] + radius * np.outer(np.ones(np.size(u)), np.cos(v))
xdata = scipy.ndimage.zoom(x, 3)
ydata = scipy.ndimage.zoom(y, 3)
zdata = scipy.ndimage.zoom(z, 3)
ax.plot_surface(xdata, ydata, zdata, rstride=3, cstride=3, color='w', shade=0)
def smoothness_objective(self, trajectoryFlat): # used only to find the smoothness cost of traj sampled from ProMP
trajectoryFlat = np.squeeze(trajectoryFlat)
trajectory = trajectoryFlat.reshape((self.nDoF, -1)).T
dim = np.multiply(*trajectory.shape)
fd_matrix, b = self.finite_diff_matrix(trajectory)
trajectory = trajectory.T.reshape((dim, 1))
F_smooth = 0.5*(np.dot(trajectory.T, np.dot(fd_matrix, trajectory)) + np.dot(trajectory.T, b) + 0.25*np.dot(b.T, b))
return F_smooth
def promp_sampled_trajectory_cost(self, trajectories): # trajectories is 3D array of (time x nDoF x nSamples)
s = trajectories.shape
smoothness_cost_chomp = np.zeros(s[2]) # smoothness cost as described in chomp paper
obstacle_cost = np.zeros(s[2])
for i in range(s[2]):
trajectory = trajectories[:, :, i]
trajectoryFlat = trajectory.T.reshape((-1))
smoothness_cost_chomp[i] = self.smoothness_objective(trajectoryFlat)
if self.obstacle_list:
obstacle_cost[i] = self.obstacle_cost(trajectoryFlat, self.obstacle_list)
return smoothness_cost_chomp, obstacle_cost
def optimise(self, weights, withGrad=True):
if not withGrad:
################## Without Gradient #########################
start = tme.time()
optimized_weights = opt.minimize(self.calculate_total_cost, weights, method='BFGS',
options={'maxiter': 150, 'disp': True},
callback=self.optim_callback)
total_time = tme.time() - start
print 'ProMP time:without gradient:', total_time
trajectoryFlat = self.basisMultiDoF.dot(optimized_weights.x)
optimized_trajectory = trajectoryFlat.reshape((self.nDoF, -1)).T
optimized_trajectory = optimized_trajectory[1:-1]
optimized_trajectory = np.insert(optimized_trajectory, 0, self.initial_joint_values, axis=0)
optimized_trajectory = np.insert(optimized_trajectory, optimized_trajectory.shape[0],
self.desired_joint_values, axis=0)
else:
# ################### With Gradient ############################
start = tme.time()
optimized_weights = opt.minimize(self.calculate_total_cost, weights, method='BFGS', jac=self.cost_gradient_analytic,
options={'maxiter': 150, 'disp': True, 'gtol': 1e-6}, callback=self.optim_callback)
total_time = tme.time() - start
print 'ProMP time:with gradient:', total_time
trajectoryFlat = self.basisMultiDoF.dot(optimized_weights.x)
optimized_trajectory = (trajectoryFlat.reshape((self.nDoF, -1))).T
optimized_trajectory = optimized_trajectory[1:-1]
optimized_trajectory = np.insert(optimized_trajectory, 0, self.initial_joint_values, axis=0)
optimized_trajectory = np.insert(optimized_trajectory, optimized_trajectory.shape[0], self.desired_joint_values, axis=0)
return optimized_trajectory, optimized_weights.x, self.costs, optimized_weights.success
def calculate_jacobian(self, robot_body_points, joint_index, joint_values=None):
class ParentMap(object):
def __init__(self, num_joints):
self.joint_idxs = [i for i in range(num_joints)]
self.p_map = np.zeros((num_joints, num_joints))
for i in range(num_joints):
for j in range(num_joints):
if j <= i:
self.p_map[i][j] = True
def is_parent(self, parent, child):
if child not in self.joint_idxs or parent not in self.joint_idxs:
return False
return self.p_map[child][parent]
parent_map = ParentMap(7)
if joint_values is None:
joint_values = list(self.robot_state.get_current_state().joint_state.position)[:7]
_, t_joints = self.franka_kin.fwd_kin(joint_values)
joint_axis = t_joints[:, :3, 2]
joint_positions = t_joints[:7, :3, 3] # Excluding the fixed joint at the end
jacobian = list()
for i, points in enumerate(np.split(robot_body_points, joint_index)[:-1]):
# print i, len(points)
if i == 0:
for point in points:
jacobian.append(np.zeros((3, 7)))
else:
for point in points:
jacobian.append(np.zeros((3, 7)))
for joint in parent_map.joint_idxs:
if parent_map.is_parent(joint, i-1):
# find cross product
col = np.cross(joint_axis[joint, :], point-joint_positions[joint])
jacobian[-1][0][joint] = col[0]
jacobian[-1][1][joint] = col[1]
jacobian[-1][2][joint] = col[2]
else:
jacobian[-1][0][joint] = 0.0
jacobian[-1][1][joint] = 0.0
jacobian[-1][2][joint] = 0.0
return np.array(jacobian)
def finite_diff_matrix(self, trajectory):
rows, columns = trajectory.shape # columns = nDoF
A = 2 * np.eye(rows)
A[0, 1] = -1
A[rows-1, rows-2] = -1
for ik in range(0, rows-2):
A[ik + 1, ik] = -1
A[ik + 1, ik + 2] = -1
dim = rows*columns
fd_matrix = np.zeros((dim, dim))
b = np.zeros((dim, 1))
i, j = 0, 0
while i < dim:
fd_matrix[i:i+len(A), i:i+len(A)] = A
b[i] = -2 * self.initial_joint_values[j]
b[i+len(A)-1] = -2 * self.desired_joint_values[j]
i = i + len(A)
j = j + 1
return fd_matrix, b
def get_robot_discretised_points(self, fwd_k_j_positions, step_size=0.2):
discretised = list()
j_index = list()
# self.get_robot_discretised_joint_index(fwd_k_j_positions, step_size=0.2)
for j in range(len(fwd_k_j_positions) - 1):
w = fwd_k_j_positions[j+1] - fwd_k_j_positions[j]
if len(w[w != 0.]):
step = step_size * w / np.linalg.norm(w)
n = int(np.linalg.norm(w) / np.linalg.norm(step)) + 1
discretised.extend(np.outer(np.arange(1, n), step) + fwd_k_j_positions[j])
j_index.append(len(discretised))
self.j_index = j_index
return np.array(discretised)
def get_robot_joint_positions(self, joint_values):
_, t_joints = self.franka_kin.fwd_kin(joint_values)
return np.vstack(([0., 0., 0.], t_joints[:, :3, 3]))
def calculate_robot_body_points(self, trajectory):
# trajectory = np.vstack((trajectory, self.desired_joint_values))
return np.array([self.get_robot_discretised_points(self.get_robot_joint_positions(joint_values), self.step_size,
) for joint_values in trajectory])
def calculate_normalised_workspace_velocity(self, robot_body_points):
velocity = np.gradient(robot_body_points, axis=0)
vel_magnitude = np.linalg.norm(velocity, axis=2)
vel_normalised = np.divide(velocity, vel_magnitude[:, :, None], out=np.zeros_like(velocity), where=vel_magnitude[:, :, None] != 0)
return vel_normalised, vel_magnitude, velocity
def smoothness_cost(self, weights):
temp1 = (weights - self.mean_cond_weight).reshape(-1, 1)
temp2 = temp1.T.dot(self.inv_covMat_cond)
return 0.5 * temp2.dot(temp1) + 0.5 * self.reguCoeff * weights.dot(weights)
def obstacle_cost(self, trajectoryFlat, obstacle_list,):
trajectory = trajectoryFlat.reshape((self.nDoF, -1)).T
# trajectory = trajectory[1:-1]
robot_body_points = self.calculate_robot_body_points(trajectory)
vel_normalised, vel_mag, vel = self.calculate_normalised_workspace_velocity(robot_body_points)
dist = self.compute_minimum_distance_to_objects(robot_body_points, obstacle_list)
obstacle_cost_potential = np.array(self.cost_potential(dist))
return np.sum(np.multiply(obstacle_cost_potential, vel_mag))
def calculate_total_cost(self, weights):
weights = np.squeeze(weights)
trajectoryFlat = self.basisMultiDoF.dot(weights)
smoothness_cost = self.smoothness_cost(weights) # smoothness of trajectory is captured here
if self.obstacle_list is not None:
obstacle_cost = self.obstacle_cost(trajectoryFlat, self.obstacle_list)
else:
obstacle_cost = 0.0
# return 0.001 * smoothness_cost + 1.5 * obstacle_cost # for without gradient
return self.lamda_smooth * smoothness_cost + self.lamda_obs * obstacle_cost # for with gradient
def smoothness_gradient(self, weights):
return self.inv_covMat_cond.dot(weights - self.mean_cond_weight) + self.reguCoeff * weights
def compute_minimum_distance_to_objects(self, robot_body_points, obstacle_list, body_sphere_size=0.12):
rbp_shape = robot_body_points.shape
return np.min(
distance.cdist(
robot_body_points.reshape(-1, 3, order='A'),
obstacle_list[:, :3]
) - obstacle_list[:, 3] - body_sphere_size,
axis=1
).reshape(rbp_shape[:2])
def cost_potential(self, D, collision_threshold=0.07):
c = np.zeros(D.shape)
return np.where(
D < 0,
-D + 0.5 * collision_threshold,
np.where(
D <= collision_threshold,
(0.5 * (D - collision_threshold) ** 2) / collision_threshold,
c
)
)
def fun(self, points):
dist = self.compute_minimum_distance_to_objects(points.reshape((1, 1, 3)), self.obstacle_list)
return np.array(self.cost_potential(dist))
def gradient_cost_potential(self, robot_discretised_points):
gradient_cost_potential = np.zeros(robot_discretised_points.reshape(-1, 3).shape)
for i, points in enumerate(robot_discretised_points.reshape((-1, 3))):
gradient_cost_potential[i, :] = opt.approx_fprime(points, self.fun, [1e-06, 1e-06, 1e-06])
return np.array(gradient_cost_potential).reshape(robot_discretised_points.shape[0], -1, 3)
def extract_block_diag(self, A, M=3, k=0):
ny, nx = A.shape
ndiags = min(map(lambda x: x // M, A.shape))
offsets = (nx * M + M, nx, 1)
strides = map(lambda x: x * A.itemsize, offsets)
if k > 0:
B = A[:, k * M]
ndiags = min(nx // M - k, ny // M)
else:
k = -k
B = A[k * M]
ndiags = min(nx // M, ny // M - k)
return as_strided(B, shape=(ndiags, M, M),
strides=((nx * M + M) * A.itemsize, nx * A.itemsize, A.itemsize))
def calculate_curvature(self, vel_normalised, vel_magnitude, velocity):
s = vel_normalised.shape
acceleration = np.gradient(velocity, axis=0).reshape(-1, 3)
ttm = vel_normalised.reshape(-1, 1).dot(vel_normalised.reshape(1, -1))
orthogonal_projector = self.extract_block_diag((np.eye(ttm.shape[0]) - ttm), 3)
temp = np.array([orthogonal_projector[i, :, :].dot(acceleration[i, :]) for i in range(orthogonal_projector.shape[0])])
curvature = np.divide(temp, vel_magnitude.reshape(-1, 1)**2, out=np.zeros_like(temp), where=vel_magnitude.reshape(-1, 1) !=0)
return curvature.reshape(s), orthogonal_projector.reshape((s[0], s[1], s[2], s[2]))
def calculate_obstacle_cost_gradient(self, trajectoryFlat):
trajectory = np.squeeze(trajectoryFlat).reshape((self.nDoF, -1)).T
# trajectory = trajectory[1:-1]
robot_discretised_points = self.calculate_robot_body_points(trajectory)
vel_normalised, vel_magnitude, velocity = self.calculate_normalised_workspace_velocity(robot_discretised_points)
curvature, orthogonal_projector = self.calculate_curvature(vel_normalised, vel_magnitude, velocity)
dist = self.compute_minimum_distance_to_objects(robot_discretised_points, self.obstacle_list,)
obstacle_cost_potential = np.array(self.cost_potential(dist))
gradient_cost_potential = self.gradient_cost_potential(robot_discretised_points)
obstacle_gradient = np.zeros((trajectory.shape[0], self.nDoF))
a = robot_discretised_points # [:-1]
for jvs in range(trajectory.shape[0]):
obst_grad = np.zeros(trajectory.shape[1])
jacobian = self.calculate_jacobian(a[jvs], self.j_index, trajectory[jvs])
for num_points in range(robot_discretised_points.shape[1]):
temp1 = orthogonal_projector[jvs, num_points].dot(gradient_cost_potential[jvs, num_points, :])
temp2 = obstacle_cost_potential[jvs, num_points] * curvature[jvs, num_points, :]
temp3 = vel_magnitude[jvs, num_points] * (temp1 - temp2)
obst_grad += jacobian[num_points, :].T.dot(temp3)
# obst_grad1 += self.basisMultiDoFParsed[jvs].T.dot(jacobian[num_points, :].T.dot(temp3))
obstacle_gradient[jvs, :] = obst_grad
return obstacle_gradient.T
def cost_gradient_analytic(self, weights): # calculate grad(cost) = grad(smoothness_cost) + grad(obstacle_cost)
weights = np.squeeze(weights)
trajectoryFlat = self.basisMultiDoF.dot(weights)
smoothness_gradient = self.smoothness_gradient(weights)
if self.obstacle_list is not None:
obstacle_gradient_traj = self.calculate_obstacle_cost_gradient(trajectoryFlat)
obstacle_gradient_weight = self.basisMultiDoF.T.dot(obstacle_gradient_traj.reshape(-1))
else:
obstacle_gradient_weight = np.zeros(len(weights))
# trajectory = np.squeeze(trajectory)
cost_gradient = self.lamda_obs * obstacle_gradient_weight + self.lamda_smooth * smoothness_gradient
return np.squeeze(cost_gradient)
def optim_callback(self, xk):
self.costs.append(self.calculate_total_cost(xk)[0, 0])
self.optCurve.append(xk)
print 'Iteration {}: {:2.4}\n'.format(len(self.optCurve), self.costs[len(self.optCurve) - 1])
def animation(self, optimized_trajectory, initial_trajectory):
fig = plt.figure()
ax = fig.add_subplot(111, projection="3d")
# ax.axis('off')
plt.show(block=False)
while True:
for i in range(len(optimized_trajectory)):
_, T_joint_optim = self.franka_kin.fwd_kin(optimized_trajectory[i])
_, T_joint_init = self.franka_kin.fwd_kin(initial_trajectory[i])
ax.clear()
if self.obstacle_list is not None:
for object in self.obstacle_list:
self.sphere(ax, object[-1], object[0:-1])
self.franka_kin.plotter(ax, T_joint_optim, 'optim', color='blue')
self.franka_kin.plotter(ax, T_joint_init, 'init', color='red')
# for x, y, z in self.get_robot_discretised_points(trajectory[i],step_size=0.2):
# plt.grid()
# ax.scatter(x, y, z, 'gray')
fig.canvas.draw()
fig.canvas.flush_events()
plt.pause(0.01)
plt.pause(1)
# plt.show(block=True)
def discretize(self, start, goal, step_size=0.02):
w = goal - start
step = step_size * w / np.linalg.norm(w)
n = int(np.linalg.norm(w) / np.linalg.norm(step)) + 1
discretized = np.outer(np.arange(1, n), step) + start
return discretized
class trajectory_optimization():
def __init__(self, nDoF):
self.nDoF = nDoF
self.scene = moveit_commander.PlanningSceneInterface()
self.group = moveit_commander.MoveGroupCommander("panda_arm")
self.robot_state = moveit_commander.RobotCommander()
self.workspace_dim = (-2, -0.2, 0.2, 1, 1, 1.5)
self.group.set_workspace(
self.workspace_dim
) # specify the workspace bounding box (static scene)
self.franka_kin = FrankaKinematics()
self.collision_threshold = 0.07
self.object_list = np.array([
[0.25, 0.21, 0.71, 0.1],
[-0.15, 0.21, 0.51, 0.05],
[0.15, 0.61, 0.51, 0.05],
[-0.15, -0.61, 0.51, 0.05],
[-0.65, 0.31, 0.21, 0.05],
[-0.55, 0.71, 0.71, 0.06],
[-0.95, -0.51, 0.41, 0.07],
[-0.85, 0.27, 0.61, 0.08],
[-0.25, -0.31, 0.11, 0.04],
[-0.75, 0.71, 0.91, 0.09],
[-0.35, 0.51, 0.81, 0.02],
[-0.95, -0.81, 0.61, 0.03],
[-0.75, 0.11, 0.41, 0.02],
[-0.55, -0.61, 0.21, 0.06],
[0.25, 0.31, 0.71, 0.07],
[-0.35, -0.41, 0.41, 0.02],
[-0.25, -0.51, 0.61, 0.08],
]) # for two spheres
# self.object_list = np.array([[0.25, 0.21, 0.71, 0.1], [-0.15, 0.21, 0.51, 0.05]]) # for two spheres
# self.object_list = np.array([[0.25, 0.21, 0.71, 0.1]]) # for one sphere (x,y,z, radius)
# self.object_list = []
# self.object_list = self.populate_obstacles()
self.initial_joint_values = np.zeros(7) # + 0.01 * np.random.random(7)
self.ang_deg = 60
self.desired_joint_values = np.array([
np.pi * self.ang_deg / 180, np.pi / 3, 0.0, np.pi / 6, np.pi / 6,
np.pi / 6, np.pi / 6
])
self.optCurve = []
self.step_size = 0.09
def populate_obstacles(self, ):
# if the obstacle is within robot workspace, then consider those as obstacles
obs = []
x_min, x_max = self.workspace_dim[0], self.workspace_dim[3]
y_min, y_max = self.workspace_dim[1], self.workspace_dim[4]
z_min, z_max = self.workspace_dim[2], self.workspace_dim[5]
for k, v in self.scene.get_objects().items():
px, py, pz = v.primitive_poses[0].position.x, v.primitive_poses[
0].position.y, v.primitive_poses[0].position.z
if px >= x_min and px <= x_max:
if py >= y_min and py <= y_max:
if pz >= z_min and pz <= z_max:
obs.append([px, py, pz, 0.05])
# a.append([px, py, pz, 0.05])
return obs
# print "Ori:", v.primitive_poses[0].orientation.x, v.primitive_poses[0].orientation.y, \
# v.primitive_poses[0].orientation.z, v.primitive_poses[0].orientation.w
# print "Size:", v.primitives[0].dimensions
# self.object_list.append([v.primitive_poses[0].position.x, v.primitive_poses[0].position.y, \
# v.primitive_poses[0].position.z, 0.05])
def discretize(self, start, goal, step_size=0.02):
w = goal - start
step = step_size * w / np.linalg.norm(w)
n = int(np.linalg.norm(w) / np.linalg.norm(step)) + 1
discretized = np.outer(np.arange(1, n), step) + start
return discretized
def get_robot_discretised_points(self,
joint_values=None,
step_size=0.2,
with_joint_index=False):
if joint_values is None:
joint_values = list(
self.robot_state.get_current_state().joint_state.position)[:7]
_, t_joints = self.franka_kin.fwd_kin(joint_values)
fwd_k_j_positions = np.vstack(([0., 0., 0.], t_joints[:, :3, 3]))
discretised = list()
j_index = list()
for j in range(len(fwd_k_j_positions) - 1):
w = fwd_k_j_positions[j + 1] - fwd_k_j_positions[j]
if len(w[w != 0.]):
step = step_size * w / np.linalg.norm(w)
n = int(np.linalg.norm(w) / np.linalg.norm(step)) + 1
discretised.extend(
np.outer(np.arange(1, n), step) + fwd_k_j_positions[j])
j_index.append(len(discretised))
return (np.array(discretised),
j_index) if with_joint_index else np.array(discretised)
def compute_minimum_distance_to_objects(
self,
robot_body_points,
object_list,
):
# if object_list is None:
# object_list = self.get_object_collision_spheres()
D = list()
if len(robot_body_points.shape) == 1:
robot_body_points = robot_body_points.reshape(1, 3)
for r in robot_body_points: # expects robot_body_points as an array of dimension 1 x n
dist = []
for o in object_list:
ro = r - o[:3]
norm_ro = np.linalg.norm(ro)
dist.append(norm_ro - 0.15 - o[3])
D.append(np.min(np.array(dist)))
return D
def cost_potential(self, D):
c = list()
for d in D:
if d < 0.:
c.append(-d + 0.5 * self.collision_threshold)
elif d <= self.collision_threshold:
c.append((0.5 * (d - self.collision_threshold)**2) /
self.collision_threshold)
else:
c.append(0)
return c
def sphere(self, ax, radius, centre):
u = np.linspace(0, 2 * np.pi, 13)
v = np.linspace(0, np.pi, 7)
x = centre[0] + radius * np.outer(np.cos(u), np.sin(v))
y = centre[1] + radius * np.outer(np.sin(u), np.sin(v))
z = centre[2] + radius * np.outer(np.ones(np.size(u)), np.cos(v))
xdata = scipy.ndimage.zoom(x, 3)
ydata = scipy.ndimage.zoom(y, 3)
zdata = scipy.ndimage.zoom(z, 3)
ax.plot_surface(xdata,
ydata,
zdata,
rstride=3,
cstride=3,
color='w',
shade=0)
def finite_diff_matrix(self, trajectory):
rows, columns = trajectory.shape # columns = nDoF
A = 2 * np.eye(rows)
A[0, 1] = -1
A[rows - 1, rows - 2] = -1
for ik in range(0, rows - 2):
A[ik + 1, ik] = -1
A[ik + 1, ik + 2] = -1
dim = rows * columns
fd_matrix = np.zeros((dim, dim))
b = np.zeros((dim, 1))
i, j = 0, 0
while i < dim:
fd_matrix[i:i + len(A), i:i + len(A)] = A
b[i] = -2 * self.initial_joint_values[j]
b[i + len(A) - 1] = -2 * self.desired_joint_values[j]
i = i + len(A)
j = j + 1
return fd_matrix, b
def smoothness_objective(self, trajectory):
trajectoryFlat = np.squeeze(trajectory)
trajectory = (trajectoryFlat.reshape(
(self.nDoF, trajectoryFlat.shape[0] / self.nDoF))).T
rows, columns = trajectory.shape
dim = rows * columns
fd_matrix, b = self.finite_diff_matrix(trajectory)
trajectory = trajectory.T.reshape((dim, 1))
F_smooth = 0.5 * (np.dot(trajectory.T, np.dot(fd_matrix, trajectory)) +
np.dot(trajectory.T, b) + 0.25 * np.dot(b.T, b))
return F_smooth
def calculate_obstacle_cost(self, trajectory):
obstacle_cost = 0
trajectoryFlat = np.squeeze(trajectory)
trajectory = (trajectoryFlat.reshape(
(self.nDoF, trajectoryFlat.shape[0] / self.nDoF))).T
vel_normalised, vel_mag, vel = self.calculate_normalised_workspace_velocity(
trajectory) # vel_normalised = vel/vel_mag
for jvs in range(len(trajectory)):
# print trajectory.shape
robot_discretised_points = np.array(
self.get_robot_discretised_points(trajectory[jvs],
self.step_size))
dist = self.compute_minimum_distance_to_objects(
robot_discretised_points, self.object_list)
obsacle_cost_potential = np.array(self.cost_potential(dist))
obstacle_cost += np.sum(
np.multiply(obsacle_cost_potential, vel_mag[jvs, :]))
# obstacle_cost += np.sum(obsacle_cost_potential)
return obstacle_cost
def calculate_total_cost(self, trajectory):
trajectory = np.squeeze(trajectory)
F_smooth = self.smoothness_objective(
trajectory) # smoothness of trajectory is captured here
obstacle_cost = self.calculate_obstacle_cost(trajectory)
return 10 * F_smooth + 25.5 * obstacle_cost # for with gradient
# return 10 * F_smooth + 1.5 * obstacle_cost # for without gradient
def calculate_normalised_workspace_velocity(self,
trajectory,
desired_joint_values=None):
if desired_joint_values is None:
desired_joint_values = self.desired_joint_values
# We have not divided by time as this has been indexed and is thus not available
trajectory = np.insert(trajectory,
len(trajectory),
desired_joint_values,
axis=0)
robot_body_points = np.array([
self.get_robot_discretised_points(joint_values, self.step_size)
for joint_values in trajectory
])
velocity = np.diff(robot_body_points, axis=0)
vel_magnitude = np.linalg.norm(velocity, axis=2)
vel_normalised = np.divide(velocity,
vel_magnitude[:, :, None],
out=np.zeros_like(velocity),
where=vel_magnitude[:, :, None] != 0)
return vel_normalised, vel_magnitude, velocity
def calculate_jacobian(self,
robot_body_points,
joint_index,
joint_values=None):
class ParentMap(object):
def __init__(self, num_joints):
self.joint_idxs = [i for i in range(num_joints)]
self.p_map = np.zeros((num_joints, num_joints))
for i in range(num_joints):
for j in range(num_joints):
if j <= i:
self.p_map[i][j] = True
def is_parent(self, parent, child):
if child not in self.joint_idxs or parent not in self.joint_idxs:
return False
return self.p_map[child][parent]
parent_map = ParentMap(7)
if joint_values is None:
joint_values = list(
self.robot_state.get_current_state().joint_state.position)[:7]
_, t_joints = self.franka_kin.fwd_kin(joint_values)
joint_axis = t_joints[:, :3, 2]
joint_positions = t_joints[:7, :3,
3] # Excluding the fixed joint at the end
jacobian = list()
for i, points in enumerate(
np.split(robot_body_points, joint_index)[:-1]):
# print i, len(points)
if i == 0:
for point in points:
jacobian.append(np.zeros((3, 7)))
else:
for point in points:
jacobian.append(np.zeros((3, 7)))
for joint in parent_map.joint_idxs:
if parent_map.is_parent(joint, i - 1):
# find cross product
col = np.cross(joint_axis[joint, :],
point - joint_positions[joint])
jacobian[-1][0][joint] = col[0]
jacobian[-1][1][joint] = col[1]
jacobian[-1][2][joint] = col[2]
else:
jacobian[-1][0][joint] = 0.0
jacobian[-1][1][joint] = 0.0
jacobian[-1][2][joint] = 0.0
return np.array(jacobian)
def calculate_smoothness_gradient(self, trajectory):
trajectoryFlat = np.squeeze(trajectory)
trajectory = (trajectoryFlat.reshape(
(self.nDoF, trajectoryFlat.shape[0] / self.nDoF))).T
rows, columns = trajectory.shape
dim = rows * columns
fd_matrix, b = self.finite_diff_matrix(trajectory)
trajectory = trajectory.T.reshape((dim, 1))
smoothness_gradient = 0.5 * b + fd_matrix.dot(trajectory)
return smoothness_gradient
def calculate_curvature(self, vel_normalised, vel_magnitude, velocity):
time_instants, body_points, n = velocity.shape[0], velocity.shape[
1], velocity.shape[2]
acceleration = np.gradient(velocity, axis=0)
curvature, orthogonal_projector = np.zeros(
(time_instants, body_points, n)), np.zeros(
(time_instants, body_points, n, n))
for tm in range(time_instants):
for pts in range(body_points):
ttm = np.dot(vel_normalised[tm, pts, :].reshape(3, 1),
vel_normalised[tm, pts, :].reshape(1, 3))
temp = np.eye(3) - ttm
orthogonal_projector[tm, pts] = temp
if vel_magnitude[tm, pts]:
curvature[tm, pts] = np.dot(
temp, acceleration[tm, pts, :]) / vel_magnitude[tm,
pts]**2
else:
# curv.append(np.array([0, 0, 0]))
curvature[tm, pts] = np.array([0, 0, 0])
return curvature, orthogonal_projector
def fun(self, points):
dist = self.compute_minimum_distance_to_objects(
points, self.object_list)
return np.array(self.cost_potential(dist))
def gradient_cost_potential(self, robot_discretised_points):
gradient_cost_potential = list()
for points in robot_discretised_points:
grad = opt.approx_fprime(points, self.fun, [1e-06, 1e-06, 1e-06])
gradient_cost_potential.append(grad)
return np.array(gradient_cost_potential)
def calculate_mt(self, trajectory):
n_time, ndof = trajectory.shape #
M_t = np.zeros((n_time, ndof, n_time * ndof))
for t in range(n_time):
k = 0
for d in range(ndof):
M_t[t, d, k + t] = 1
k = k + n_time
return M_t
def calculate_obstacle_cost_gradient(self, trajectory):
trajectoryFlat = np.squeeze(trajectory)
trajectory = (trajectoryFlat.reshape(
(self.nDoF, trajectoryFlat.shape[0] / self.nDoF))).T
vel_normalised, vel_magnitude, velocity = self.calculate_normalised_workspace_velocity(
trajectory)
curvature, orthogonal_projector = self.calculate_curvature(
vel_normalised, vel_magnitude, velocity)
obstacle_gradient = list()
# a, b, c, d = [], [], [], []
for jvs in range(len(trajectory)):
obst_grad = np.zeros(trajectory.shape[1])
robot_discretised_points, joint_index = np.array(
self.get_robot_discretised_points(trajectory[jvs],
self.step_size,
with_joint_index=True))
dist = np.array(
self.compute_minimum_distance_to_objects(
robot_discretised_points,
self.object_list,
))
obstacle_cost_potential = np.array(self.cost_potential(dist))
gradient_cost_potential = self.gradient_cost_potential(
robot_discretised_points)
jacobian = self.calculate_jacobian(robot_discretised_points,
joint_index, trajectory[jvs])
# a.append(robot_discretised_points)
# b.append(obstacle_cost_potential)
# c.append(gradient_cost_potential)
# d.append(jacobian)
for num_points in range(robot_discretised_points.shape[0]):
temp1 = orthogonal_projector[jvs, num_points].dot(
gradient_cost_potential[num_points, :])
temp2 = obstacle_cost_potential[num_points] * curvature[
jvs, num_points, :]
temp3 = vel_magnitude[jvs, num_points] * (temp1 - temp2)
obst_grad += jacobian[num_points, :].T.dot(temp3)
# obst_grad += jacobian[num_points, :].T.dot(gradient_cost_potential[num_points, :])
obstacle_gradient.append(obst_grad)
return np.transpose(np.array(obstacle_gradient))
def cost_gradient_analytic(
self, trajectory
): # calculate grad(cost) = grad(smoothness_cost) + grad(obstacle_cost)
smoothness_gradient = self.calculate_smoothness_gradient(trajectory)
obstacle_gradient = self.calculate_obstacle_cost_gradient(trajectory)
trajectory = np.squeeze(trajectory)
cost_gradient = obstacle_gradient.reshape(
(len(trajectory), 1)) + smoothness_gradient
return np.squeeze(cost_gradient)
def cost_gradient_numeric(self, trajectory):
trajectory = np.squeeze(trajectory)
obst_cost_grad_numeric = opt.approx_fprime(
trajectory, traj_opt.calculate_obstacle_cost,
1e-08 * np.ones(len(trajectory)))
smoothness_gradient = np.squeeze(
self.calculate_smoothness_gradient(trajectory))
return np.squeeze(obst_cost_grad_numeric + smoothness_gradient)
def animation(self, optimized_trajectory, initial_trajectory):
fig = plt.figure()
ax = fig.add_subplot(111, projection="3d")
# ax.axis('off')
plt.show(block=False)
while True:
for i in range(len(optimized_trajectory)):
_, T_joint_optim = self.franka_kin.fwd_kin(
optimized_trajectory[i])
_, T_joint_init = self.franka_kin.fwd_kin(
initial_trajectory[i])
ax.clear()
for object in self.object_list:
self.sphere(ax, object[-1], object[0:-1])
self.franka_kin.plotter(ax,
T_joint_optim,
'optim',
color='blue')
self.franka_kin.plotter(ax, T_joint_init, 'init', color='red')
# for x, y, z in self.get_robot_discretised_points(trajectory[i],step_size=0.2):
# plt.grid()
# ax.scatter(x, y, z, 'gray')
fig.canvas.draw()
fig.canvas.flush_events()
plt.pause(0.01)
plt.pause(1)
# plt.show(block=True)
def optim_callback(self, xk):
# xk = xk.reshape(len(xk) / 7, 7)
costs = self.calculate_total_cost(xk)
self.optCurve.append(xk)
print 'Iteration {}: {}\n'.format(len(self.optCurve), costs)
import numpy as np
from franka_kinematics import FrankaKinematics
import tf.transformations as tf_tran
with open(
'/home/ash/Ash/scripts/TrajectoryRecorder/Trajectories_bag/tableandchar/format/bottle2cup.npz',
'r') as f:
data = np.load(f)
Q = data['Q']
timeData = data['time']
nDoF = 9
franka_kin = FrankaKinematics()
# end_robot_config = np.zeros((len(Q), nDoF))
euler, position, quaternion = [], [], []
for i in range(len(Q)):
end_robot_config = Q[i][-1]
T, _ = franka_kin.fwd_kin(end_robot_config[:-2])
euler.append(tf_tran.euler_from_matrix(T))
position.append(T[:3, -1])
quaternion.append(tf_tran.quaternion_from_matrix(T))
euler = np.array(euler)
position = np.array(position)
quaternion = np.array(quaternion)
# euler_mean, euler_std = np.mean(euler, axis=0), np.std(euler, axis=0)
# position_mean, position_std = np.mean(position, axis=0), np.std(position, axis=0)
# quaternion_mean, quaternion_std = np.mean(quaternion, axis=0), np.std(quaternion, axis=0)
#
# pos_cov = np.cov(position, rowvar=0)