def _compute_bezier_coeff(self, trajectory_points, dimensions_dict):
# Compute Full Bezier Curve
num_joints = self.get_joint_number()
num_traj_pts = len(trajectory_points)
#print 'traj_pts', num_traj_pts
num_traj_dim = sum(dimensions_dict.values())
#print 'num_traj_dim', num_traj_dim
num_b_values = len(['b0', 'b1', 'b2', 'b3'])
b_matrix = np.zeros(shape=(num_joints, num_traj_dim, num_traj_pts-1, num_b_values))
#print 'b_init', b_matrix
for jnt in xrange(num_joints):
traj_array = np.zeros(shape=(len(trajectory_points), num_traj_dim))
for idx, point in enumerate(trajectory_points):
current_point = list()
current_point.append(point.positions[jnt])
traj_array[idx, :] = current_point
#print 'point.positions[%d]' % jnt, point.positions[jnt]
#print 'current_point', current_point
#print 'traj_array_hop', traj_array[idx, :]
#print 'traj_array_1', traj_array
#print 'traj_array_2', traj_array
d_pts = bezier.de_boor_control_pts(traj_array)
b_matrix[jnt, :, :, :] = bezier.bezier_coefficients(traj_array, d_pts)
#print b_matrix
return b_matrix
def _compute_bezier_coeff(self, joint_names, trajectory_points, dimensions_dict):
# Compute Full Bezier Curve
num_joints = len(joint_names)
num_traj_pts = len(trajectory_points)
num_traj_dim = sum(dimensions_dict.values())
num_b_values = len(["b0", "b1", "b2", "b3"])
b_matrix = np.zeros(shape=(num_joints, num_traj_dim, num_traj_pts - 1, num_b_values))
for jnt in xrange(num_joints):
traj_array = np.zeros(shape=(len(trajectory_points), num_traj_dim))
for idx, point in enumerate(trajectory_points):
current_point = list()
current_point.append(point.positions[jnt])
if dimensions_dict["velocities"]:
current_point.append(point.velocities[jnt])
if dimensions_dict["accelerations"]:
current_point.append(point.accelerations[jnt])
traj_array[idx, :] = current_point
d_pts = bezier.de_boor_control_pts(traj_array)
b_matrix[jnt, :, :, :] = bezier.bezier_coefficients(traj_array, d_pts)
return b_matrix
def _compute_bezier_coeff(self, joint_names, trajectory_points, dimensions_dict):
# Compute Full Bezier Curve
num_joints = len(joint_names)
num_traj_pts = len(trajectory_points)
num_traj_dim = sum(dimensions_dict.values())
num_b_values = len(['b0', 'b1', 'b2', 'b3'])
b_matrix = np.zeros(shape=(num_joints, num_traj_dim, num_traj_pts-1, num_b_values))
for jnt in xrange(num_joints):
traj_array = np.zeros(shape=(len(trajectory_points), num_traj_dim))
for idx, point in enumerate(trajectory_points):
current_point = list()
current_point.append(point.positions[jnt])
if dimensions_dict['velocities']:
current_point.append(point.velocities[jnt])
if dimensions_dict['accelerations']:
current_point.append(point.accelerations[jnt])
traj_array[idx, :] = current_point
d_pts = bezier.de_boor_control_pts(traj_array)
b_matrix[jnt, :, :, :] = bezier.bezier_coefficients(traj_array, d_pts)
return b_matrix
