def _get_bezier_point(self, b_matrix, idx, t, cmd_time, dimensions_dict):
pnt = JointTrajectoryPoint()
pnt.time_from_start = rospy.Duration(cmd_time)
num_joints = b_matrix.shape[0]
pnt.positions = [0.0] * num_joints
for jnt in range(num_joints):
b_point = bezier.bezier_point(b_matrix[jnt, :, :, :], idx, t)
# Positions at specified time
pnt.positions[jnt] = b_point[0]
return pnt
def _get_bezier_point(self, b_matrix, idx, t, cmd_time, dimensions_dict):
pnt = JointTrajectoryPoint()
pnt.time_from_start = rospy.Duration(cmd_time)
num_joints = b_matrix.shape[0]
pnt.positions = [0.0] * num_joints
if dimensions_dict['velocities']:
pnt.velocities = [0.0] * num_joints
if dimensions_dict['accelerations']:
pnt.accelerations = [0.0] * num_joints
for jnt in range(num_joints):
b_point = bezier.bezier_point(b_matrix[jnt, :, :, :], idx, t)
# Positions at specified time
pnt.positions[jnt] = b_point[0]
# Velocities at specified time
if dimensions_dict['velocities']:
pnt.velocities[jnt] = b_point[1]
# Accelerations at specified time
if dimensions_dict['accelerations']:
pnt.accelerations[jnt] = b_point[-1]
return pnt
def fitness(self):
self.rmse = 0
for i, point in enumerate(self.pointcloud):
approx = bz.bezier_point(self.strand, self.u[i], self.v[i])
self.rmse = np.linalg.norm((point - approx), 2)
return