Example #1
0
    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)
Example #2
0
 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)
Example #3
0
    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