Пример #1
0
    def __init__(self, obj, parent=None):

        logger.info('%s initialization' % obj.name)
        # Call the constructor of the parent class
        morse.core.actuator.Actuator.__init__(self, obj, parent)

        logger.setLevel(logging.INFO)

        self._destination = self.robot_parent.bge_object.worldPosition
        self._wp_object = None

        # set desired position to current position
        self.local_data['x'] = self._destination[0]
        self.local_data['y'] = self._destination[1]
        self.local_data['z'] = self._destination[2]
        self.local_data['yaw'] = self.robot_parent.position_3d.yaw

        logger.info("inital wp: (%.3f %.3f %.3f)", self._destination[0],
                    self._destination[0], self._destination[0])
        self._pos_initalized = False

        # Make new reference to the robot velocities (mathutils.Vector)
        self.robot_w = self.robot_parent.bge_object.localAngularVelocity

        # get the robot inertia (list [ix, iy, iz])
        robot_inertia = self.robot_parent.bge_object.localInertia
        self.inertia = Vector(tuple(robot_inertia))
        logger.info("robot inertia: (%.3f %.3f %.3f)" % tuple(self.inertia))

        self.nominal_thrust = self.robot_parent.bge_object.mass * 9.81
        logger.info("nominal thrust: %.3f", self.nominal_thrust)
        self._attitude_compensation_limit = cos(self._max_bank_angle)**2

        # current attitude setpoints in radians
        self.roll_setpoint = 0.0
        self.pitch_setpoint = 0.0
        self.yaw_setpoint = 0.0

        self.thrust = 0.0

        #previous attitude error
        self.prev_err = Vector((0.0, 0.0, 0.0))

        # Initially (ie, before receiving a waypoint),
        # the robot is in 'Arrived' state
        self.robot_parent.move_status = "Arrived"

        # Identify an object as the target of the motion
        try:
            wp_name = self._target
            if wp_name != '':
                scene = blenderapi.scene()
                self._wp_object = scene.objects[wp_name]
                logger.info("Using object '%s' to indicate motion target",
                            wp_name)
        except KeyError as detail:
            self._wp_object = None
            logger.info("Not using a target object")

        logger.info("Component initialized, runs at %.2f Hz ", self.frequency)
Пример #2
0
    def default_action(self):
        """ Run attitude controller and apply resulting force and torque to the parent robot. """
        # Get the the parent robot
        robot = self.robot_parent

        if self.local_data['thrust'] > 0:
            if self._yaw_rate_control:
                # yaw_rate and yaw_setpoint in NED
                self.yaw_setpoint += self.local_data['yaw'] / self.frequency
                # wrap angle
                self.yaw_setpoint = normalise_angle(self.yaw_setpoint)

                logger.debug("yaw setpoint: %.3f", degrees(self.yaw_setpoint))
                logger.debug("yaw current: %.3f   setpoint: %.3f", -degrees(self.position_3d.yaw), degrees(self.yaw_setpoint))
            else:
                self.yaw_setpoint = normalise_angle(self.local_data['yaw'])

            # Compute errors
            #
            # e = att_sp - att = attitude error
            # current angles to horizontal plane in NED
            roll = self.position_3d.roll
            pitch = -self.position_3d.pitch
            yaw = -self.position_3d.yaw
            roll_err = self.local_data['roll'] - roll
            pitch_err = self.local_data['pitch'] - pitch
            # wrapped yaw error
            yaw_err = normalise_angle(self.yaw_setpoint - yaw)

            err = Vector((roll_err, pitch_err, yaw_err))
            logger.debug("attitude error: (% .3f % .3f % .3f)", degrees(err[0]), degrees(err[1]), degrees(err[2]))

            # derivative
            we = (err - self.prev_err) * self.frequency
            logger.debug("yaw rate error: %.3f", we[2])

            kp = Vector((self._rp_pgain, self._rp_pgain, self._yaw_pgain))
            kd = Vector((self._rp_dgain, self._rp_dgain, self._yaw_dgain))

            # torque = self.inertia * (kp * err + kd * we)
            t = []
            for i in range(3):
                t.append(self.inertia[i] * (kp[i] * err[i] + kd[i] * we[i]))
            # convert to blender frame and scale with thrust
            torque = Vector((t[0], -t[1], -t[2])) * self.local_data['thrust']
            logger.debug("applied torques: (% .3f % .3f % .3f)", torque[0], torque[1], torque[2])

            force = Vector((0.0, 0.0, self.local_data['thrust'] * self._thrust_factor))
            logger.debug("applied thrust force: %.3f", force[2])

            self.prev_err = err.copy()
        else:
            force = Vector((0.0, 0.0, 0.0))
            torque = Vector((0.0, 0.0, 0.0))

        # directly apply local forces and torques to the blender object of the parent robot
        robot.bge_object.applyForce(force, True)
        robot.bge_object.applyTorque(torque, True)
Пример #3
0
 def modify(self):
     data_vars = []
     if not self._2D:
         data_vars.append('roll')
         data_vars.append('pitch')
     data_vars.append('yaw')
     # generate a gaussian noise rotation vector
     rot_vec = Vector((0.0, 0.0, 0.0))
     for i in range(0, 3):
         if data_vars[i] in self._rot_std_dev:
             rot_vec[i] = random.gauss(rot_vec[i],
                                       self._rot_std_dev[data_vars[i]])
     # convert rotation vector to a quaternion representing the random rotation
     angle = rot_vec.length
     if angle > 0:
         axis = rot_vec / angle
         noise_quat = Quaternion(axis, angle)
     else:
         noise_quat = Quaternion()
         noise_quat.identity()
     try:
         self.data['orientation'] = (noise_quat *
                                     self.data['orientation']).normalized()
     except KeyError:
         # for eulers this is a bit crude, maybe should use the noise_quat here as well...
         for var in data_vars:
             if var in self.data and var in self._rot_std_dev:
                 self.data[var] = random.gauss(self.data[var],
                                               self._rot_std_dev[var])
Пример #4
0
    def default_action(self):
        """ Run attitude controller and apply resulting force and torque to the parent robot. """
        # Get the the parent robot
        robot = self.robot_parent

        engines_input = Vector(self.local_data['engines'])
        engines_input_sq = Vector.Fill(len(engines_input), 0.0)
        for i in range(0, len(engines_input)):
            engines_input_sq[i] = engines_input[i] * engines_input[i]
        moment_vector = self.transformation * engines_input_sq

        logger.debug("%s => %s" % (engines_input, moment_vector))

        force = (0.0, 0.0, moment_vector[0])
        torque = (moment_vector[1], moment_vector[2], moment_vector[3])

        # directly apply local forces and torques to the blender object of the parent robot
        robot.bge_object.applyForce(force, True)
        robot.bge_object.applyTorque(torque, True)
Пример #5
0
    def __init__(self, obj, parent=None):

        logger.info('%s initialization' % obj.name)
        # Call the constructor of the parent class
        super(self.__class__, self).__init__(obj, parent)

        logger.setLevel(logging.INFO)

        robot_obj = self.robot_parent.bge_object

        # set desired velocity to zero
        self.local_data['vx'] = 0.0
        self.local_data['vy'] = 0.0
        self.local_data['vz'] = 0.0
        self.local_data['vyaw'] = 0.0

        # get the robot inertia (list [ix, iy, iz])
        robot_inertia = robot_obj.localInertia
        self.inertia = Vector(tuple(robot_inertia))
        logger.info("robot inertia: (%.3f %.3f %.3f)" % tuple(self.inertia))

        self.nominal_thrust = robot_obj.mass * abs(blenderapi.gravity()[2])
        logger.info("nominal thrust: %.3f", self.nominal_thrust)
        self._attitude_compensation_limit = cos(self._max_bank_angle)**2

        # current attitude setpoints in radians
        self.roll_setpoint = 0.0
        self.pitch_setpoint = 0.0
        self.yaw_setpoint = 0.0

        self.thrust = 0.0

        self._prev_vel_blender = Vector((0.0, 0.0, 0.0))
        self._prev_yaw = 0.0

        # previous attitude error
        self._prev_err = Vector((0.0, 0.0, 0.0))

        logger.info("Component initialized, runs at %.2f Hz ", self.frequency)
Пример #6
0
    def default_action(self):
        """ Move the object towards the destination. """
        robot = self.robot_parent

        if self._pos_initalized:
            self._destination = Vector((self.local_data["x"], self.local_data["y"], self.local_data["z"]))
        else:
            self._destination = self.robot_parent.bge_object.worldPosition
            self.local_data["x"] = self._destination[0]
            self.local_data["y"] = self._destination[1]
            self.local_data["z"] = self._destination[2]
            self.local_data["yaw"] = self.robot_parent.position_3d.yaw
            self._pos_initalized = True

        # logger.debug("Robot %s move status: '%s'", robot.bge_object.name, robot.move_status)
        # Place the target marker where the robot should go
        if self._wp_object:
            self._wp_object.worldPosition = self._destination
            self._wp_object.worldOrientation = Matrix.Rotation(self.local_data["yaw"], 3, "Z")

        # current angles to horizontal plane (not quite, but approx good enough)
        roll = self.position_3d.roll
        pitch = self.position_3d.pitch
        yaw = self.position_3d.yaw

        # current position and velocity of robot
        pos_blender = robot.bge_object.worldPosition
        vel_blender = robot.bge_object.worldLinearVelocity

        pos_error = self._destination - pos_blender
        # zero velocity setpoint for now
        vel_error = -vel_blender

        logger.debug(
            "pos current: (% .3f % .3f % .3f) setpoint: (% .3f % .3f % .3f)",
            pos_blender[0],
            pos_blender[1],
            pos_blender[2],
            self._destination[0],
            self._destination[1],
            self._destination[2],
        )
        logger.debug("velocity: (% .3f % .3f % .3f)", vel_blender[0], vel_blender[1], vel_blender[2])

        # simple PD controller on horizontal position
        command_world_x = self._h_pgain * pos_error[0] + self._h_dgain * vel_error[0]
        command_world_y = self._h_pgain * pos_error[1] + self._h_dgain * vel_error[1]

        # setpoints in body frame
        self.roll_setpoint = sin(yaw) * command_world_x - cos(yaw) * command_world_y
        self.pitch_setpoint = cos(yaw) * command_world_x + sin(yaw) * command_world_y
        self.yaw_setpoint = self.local_data["yaw"]

        # saturate max roll/pitch angles
        if fabs(self.roll_setpoint) > self._max_bank_angle:
            self.roll_setpoint = copysign(self._max_bank_angle, self.roll_setpoint)
        if fabs(self.pitch_setpoint) > self._max_bank_angle:
            self.pitch_setpoint = copysign(self._max_bank_angle, self.pitch_setpoint)

        # wrap yaw setpoint
        self.yaw_setpoint = normalise_angle(self.yaw_setpoint)

        logger.debug("roll  current: % 2.3f   setpoint: % 2.3f", degrees(roll), degrees(self.roll_setpoint))
        logger.debug("pitch current: % 2.3f   setpoint: % 2.3f", degrees(pitch), degrees(self.pitch_setpoint))
        logger.debug("yaw   current: % 2.3f   setpoint: % 2.3f", degrees(yaw), degrees(self.yaw_setpoint))

        # compute thrust
        # nominal command to keep altitude (feed forward)
        thrust_attitude_compensation = max(self._attitude_compensation_limit, cos(roll) * cos(pitch))
        thrust_ff = self.nominal_thrust / thrust_attitude_compensation
        # feedback to correct altitude
        thrust_fb = self._v_pgain * pos_error[2] + self._v_dgain * vel_error[2]
        self.thrust = max(0, thrust_ff + thrust_fb)

        # Compute attitude errors
        #
        # e = att_sp - att = attitude error
        roll_err = normalise_angle(self.roll_setpoint - roll)
        pitch_err = normalise_angle(self.pitch_setpoint - pitch)
        yaw_err = normalise_angle(self.yaw_setpoint - yaw)

        err = Vector((roll_err, pitch_err, yaw_err))

        # derivative
        we = (err - self.prev_err) * self.frequency
        # we = mathutils.Vector((0.0, 0.0, 0.0))
        # logger.debug("yaw rate error: %.3f", we[2])

        kp = Vector((self._rp_pgain, self._rp_pgain, self._yaw_pgain))
        kd = Vector((self._rp_dgain, self._rp_pgain, self._yaw_dgain))

        # torque = self.inertia * (kp * err + kd * we)
        t = []
        for i in range(3):
            t.append(self.inertia[i] * (kp[i] * err[i] + kd[i] * we[i]))
        # scale with thrust
        torque = Vector((t[0], t[1], t[2])) * self.thrust / self.nominal_thrust
        logger.debug("applied torques: (% .3f % .3f % .3f)", torque[0], torque[1], torque[2])

        force = Vector((0.0, 0.0, self.thrust))
        logger.debug("applied thrust force: %.3f", force[2])

        self.prev_err = err.copy()

        # directly apply local forces and torques to the blender object of the parent robot
        robot.bge_object.applyForce(force, True)
        robot.bge_object.applyTorque(torque, True)

        # Vectors returned are already normalized
        wp_distance, global_vector, local_vector = self.bge_object.getVectTo(self._destination)

        # logger.debug("GOT DISTANCE: xyz: %.4f", wp_distance)

        # If the target has been reached, change the status
        if wp_distance - self.local_data["tolerance"] <= 0:
            robot.move_status = "Arrived"

            # Do we have a running request? if yes, notify the completion
            self.completed(status.SUCCESS, robot.move_status)

            # logger.debug("TARGET REACHED")
            # logger.debug("Robot %s move status: '%s'", robot.bge_object.name, robot.move_status)

        else:
            robot.move_status = "Transit"
Пример #7
0
    def default_action(self):
        """ Move the object towards the destination. """
        robot = self.robot_parent

        if self._pos_initalized:
            self._destination = Vector(
                (self.local_data['x'], self.local_data['y'],
                 self.local_data['z']))
        else:
            self._destination = self.robot_parent.bge_object.worldPosition
            self.local_data['x'] = self._destination[0]
            self.local_data['y'] = self._destination[1]
            self.local_data['z'] = self._destination[2]
            self.local_data['yaw'] = self.robot_parent.position_3d.yaw
            self._pos_initalized = True

        #logger.debug("Robot %s move status: '%s'", robot.bge_object.name, robot.move_status)
        # Place the target marker where the robot should go
        if self._wp_object:
            self._wp_object.worldPosition = self._destination
            self._wp_object.worldOrientation = Matrix.Rotation(
                self.local_data['yaw'], 3, 'Z')

        # current angles to horizontal plane (not quite, but approx good enough)
        roll = self.position_3d.roll
        pitch = self.position_3d.pitch
        yaw = self.position_3d.yaw

        # current position and velocity of robot
        pos_blender = robot.bge_object.worldPosition
        vel_blender = robot.bge_object.worldLinearVelocity

        pos_error = self._destination - pos_blender
        # zero velocity setpoint for now
        vel_error = -vel_blender

        logger.debug("pos current: (% .3f % .3f % .3f) setpoint: (% .3f % .3f % .3f)", \
                     pos_blender[0], pos_blender[1], pos_blender[2], \
                     self._destination[0], self._destination[1], self._destination[2])
        logger.debug("velocity: (% .3f % .3f % .3f)", vel_blender[0],
                     vel_blender[1], vel_blender[2])

        # simple PD controller on horizontal position
        command_world_x = self._h_pgain * pos_error[
            0] + self._h_dgain * vel_error[0]
        command_world_y = self._h_pgain * pos_error[
            1] + self._h_dgain * vel_error[1]

        # setpoints in body frame
        self.roll_setpoint = sin(yaw) * command_world_x - cos(
            yaw) * command_world_y
        self.pitch_setpoint = cos(yaw) * command_world_x + sin(
            yaw) * command_world_y
        self.yaw_setpoint = self.local_data['yaw']

        # saturate max roll/pitch angles
        if fabs(self.roll_setpoint) > self._max_bank_angle:
            self.roll_setpoint = copysign(self._max_bank_angle,
                                          self.roll_setpoint)
        if fabs(self.pitch_setpoint) > self._max_bank_angle:
            self.pitch_setpoint = copysign(self._max_bank_angle,
                                           self.pitch_setpoint)

        # wrap yaw setpoint
        self.yaw_setpoint = normalise_angle(self.yaw_setpoint)

        logger.debug("roll  current: % 2.3f   setpoint: % 2.3f", \
                     degrees(roll), degrees(self.roll_setpoint))
        logger.debug("pitch current: % 2.3f   setpoint: % 2.3f", \
                     degrees(pitch), degrees(self.pitch_setpoint))
        logger.debug("yaw   current: % 2.3f   setpoint: % 2.3f", \
                     degrees(yaw), degrees(self.yaw_setpoint))

        # compute thrust
        # nominal command to keep altitude (feed forward)
        thrust_attitude_compensation = max(self._attitude_compensation_limit,
                                           cos(roll) * cos(pitch))
        thrust_ff = self.nominal_thrust / thrust_attitude_compensation
        # feedback to correct altitude
        thrust_fb = self._v_pgain * pos_error[2] + self._v_dgain * vel_error[2]
        self.thrust = max(0, thrust_ff + thrust_fb)

        # Compute attitude errors
        #
        # e = att_sp - att = attitude error
        roll_err = normalise_angle(self.roll_setpoint - roll)
        pitch_err = normalise_angle(self.pitch_setpoint - pitch)
        yaw_err = normalise_angle(self.yaw_setpoint - yaw)

        err = Vector((roll_err, pitch_err, yaw_err))

        # derivative
        we = (err - self.prev_err) * self.frequency
        #we = mathutils.Vector((0.0, 0.0, 0.0))
        #logger.debug("yaw rate error: %.3f", we[2])

        kp = Vector((self._rp_pgain, self._rp_pgain, self._yaw_pgain))
        kd = Vector((self._rp_dgain, self._rp_dgain, self._yaw_dgain))

        #torque = self.inertia * (kp * err + kd * we)
        t = []
        for i in range(3):
            t.append(self.inertia[i] * (kp[i] * err[i] + kd[i] * we[i]))
        # scale with thrust
        torque = Vector((t[0], t[1], t[2])) * self.thrust / self.nominal_thrust
        logger.debug("applied torques: (% .3f % .3f % .3f)", torque[0],
                     torque[1], torque[2])

        force = Vector((0.0, 0.0, self.thrust))
        logger.debug("applied thrust force: %.3f", force[2])

        self.prev_err = err.copy()

        # directly apply local forces and torques to the blender object of the parent robot
        robot.bge_object.applyForce(force, True)
        robot.bge_object.applyTorque(torque, True)

        # Vectors returned are already normalized
        wp_distance, global_vector, local_vector = self.bge_object.getVectTo(
            self._destination)

        #logger.debug("GOT DISTANCE: xyz: %.4f", wp_distance)

        # If the target has been reached, change the status
        if wp_distance - self.local_data['tolerance'] <= 0:
            robot.move_status = "Arrived"

            #Do we have a running request? if yes, notify the completion
            self.completed(status.SUCCESS, robot.move_status)

            #logger.debug("TARGET REACHED")
            #logger.debug("Robot %s move status: '%s'", robot.bge_object.name, robot.move_status)

        else:
            robot.move_status = "Transit"
Пример #8
0
    def default_action(self):
        """ Move the robot. """
        robot_obj = self.robot_parent.bge_object

        vel_body_sp = Vector((self.local_data['vx'], self.local_data['vy'],
                              self.local_data['vz']))

        # TODO: limit max_velocity setpoint

        # current angles to horizontal plane in NED
        # (not quite, but approx good enough)
        roll = self.position_3d.roll
        pitch = self.position_3d.pitch
        yaw = self.position_3d.yaw

        # convert the commands in body frame to blender frame
        # in which frame do we want to command??

        #body2blender = Matrix.Rotation(math.pi, 3, 'X') * Matrix.Rotation(yaw, 3, 'Z')
        body2blender = Matrix.Rotation(yaw, 3, 'Z')
        vel_blender_sp = body2blender * vel_body_sp

        # current velocity of robot in Blender world frame
        vel_blender = robot_obj.worldLinearVelocity

        # errors in blender frame
        vel_error = vel_blender_sp - vel_blender
        # zero desired acceleration for now... no reference...
        accel_error = -(vel_blender - self._prev_vel_blender) * self.frequency

        logger.debug("velocity in blender frame: (% .3f % .3f % .3f)",
                     vel_blender[0], vel_blender[1], vel_blender[2])
        logger.debug("velocity setpoint:: (% .3f % .3f % .3f)",
                     vel_blender_sp[0], vel_blender_sp[1], vel_blender_sp[2])
        logger.debug("velocity error: (% .3f % .3f % .3f)", vel_error[0],
                     vel_error[1], vel_error[2])
        logger.debug("acceleration error: (% .3f % .3f % .3f)", accel_error[0],
                     accel_error[1], accel_error[2])

        cmd_blender_x = self._h_pgain * vel_error[
            0] + self._h_dgain * accel_error[0]
        cmd_blender_y = self._h_pgain * vel_error[
            1] + self._h_dgain * accel_error[1]

        self.roll_setpoint = sin(yaw) * cmd_blender_x - cos(
            yaw) * cmd_blender_y
        self.pitch_setpoint = cos(yaw) * cmd_blender_x + sin(
            yaw) * cmd_blender_y
        self.yaw_setpoint = self._prev_yaw - (self.local_data['vyaw'] /
                                              self.frequency)

        # saturate max roll/pitch angles
        if fabs(self.roll_setpoint) > self._max_bank_angle:
            self.roll_setpoint = copysign(self._max_bank_angle,
                                          self.roll_setpoint)
        if fabs(self.pitch_setpoint) > self._max_bank_angle:
            self.pitch_setpoint = copysign(self._max_bank_angle,
                                           self.pitch_setpoint)

        # wrap yaw setpoint
        self.yaw_setpoint = normalise_angle(self.yaw_setpoint)

        logger.debug("roll  current: % 2.3f   setpoint: % 2.3f", degrees(roll),
                     degrees(self.roll_setpoint))
        logger.debug("pitch current: % 2.3f   setpoint: % 2.3f",
                     degrees(pitch), degrees(self.pitch_setpoint))
        logger.debug("yaw   current: % 2.3f   setpoint: % 2.3f", degrees(yaw),
                     degrees(self.yaw_setpoint))

        # compute thrust
        # nominal command to keep altitude (feed forward)
        thrust_attitude_compensation = max(self._attitude_compensation_limit,
                                           cos(roll) * cos(pitch))
        thrust_ff = self.nominal_thrust / thrust_attitude_compensation
        # feedback to correct vertical speed
        thrust_fb = self._v_pgain * vel_error[
            2]  # + self._v_dgain * accel_error[2]
        self.thrust = max(0, thrust_ff + thrust_fb)

        # Compute attitude errors
        #
        # e = att_sp - att = attitude error
        roll_err = normalise_angle(self.roll_setpoint - roll)
        pitch_err = normalise_angle(self.pitch_setpoint - pitch)
        yaw_err = normalise_angle(self.yaw_setpoint - yaw)

        err = Vector((roll_err, pitch_err, yaw_err))

        # derivative
        we = (err - self._prev_err) * self.frequency
        # we = Vector((0.0, 0.0, 0.0))
        # logger.debug("yaw rate error: %.3f", we[2])

        kp = Vector((self._rp_pgain, self._rp_pgain, self._yaw_pgain))
        kd = Vector((self._rp_dgain, self._rp_dgain, self._yaw_dgain))

        # torque = self.inertia * (kp * err + kd * we)
        t = []
        for i in range(3):
            t.append(self.inertia[i] * (kp[i] * err[i] + kd[i] * we[i]))
        # convert to blender frame and scale with thrust
        torque = Vector((t[0], t[1], t[2])) * self.thrust / self.nominal_thrust
        logger.debug("applied torques: (% .3f % .3f % .3f)", torque[0],
                     torque[1], torque[2])

        force = Vector((0.0, 0.0, self.thrust))
        logger.debug("applied thrust force: %.3f", force[2])

        self._prev_err = err.copy()
        self._prev_vel_blender = vel_blender.copy()
        self._prev_yaw = yaw

        # directly apply local forces and torques to the blender object of the parent robot
        robot_obj.applyForce(force, True)
        robot_obj.applyTorque(torque, True)