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