def handle_TrajDes_service(self, req):
# if GUI request certain trajectory, update flag on desired trajectory
flagTrajDes = req.trajectory
TrajDes_OffSet = numpy.array(req.offset)
ee = numpy.array(req.rotation)
TrajDes_Rotation = GetRotFromEulerAnglesDeg(ee)
# some parameters user can change easily
# req.parameters is a tuple
if len(req.parameters) == 0:
# if tuple req.parameters is empty:
TrajDes_parameters = None
else:
# if tuple is not empty, cast parameters as numpy array
TrajDes_parameters = numpy.array(req.parameters)
# update class for TrajectoryGenerator
traj_class = trajectories_dictionary[flagTrajDes]
self.TrajGenerator = traj_class(TrajDes_OffSet, TrajDes_Rotation,
TrajDes_parameters)
# we need to update initial time for trajectory generation
self.time_TrajDes_t0 = rospy.get_time()
# return message to Gui, to let it know resquest has been fulfilled
return TrajDes_SrvResponse(True)
def output(self, t, states, states_d):
# convert euler angles from deg to rotation matrix
ee = states[6:9]
R = GetRotFromEulerAnglesDeg(ee)
R = numpy.reshape(R, 9)
# collecting states
states = numpy.concatenate([states[0:6], R])
return controller(states, states_d, self.parameters)
def output(self, t, states, states_d):
# convert euler angles from deg to rotation matrix
ee = states[6:9]
R = GetRotFromEulerAnglesDeg(ee)
R = numpy.reshape(R, 9)
# collecting states
states_body = numpy.concatenate([states[0:6], R])
if states.size == 18:
# load
eel = states[15:18]
Rl = GetRotFromEulerAnglesDeg(eel)
Rl = numpy.reshape(Rl, 9)
states_load = numpy.concatenate([states[9:15], Rl])
self._old_input = controller(states_body, states_load, states_d,
self.parameters)
return self._old_input
return self._old_input