コード例 #1
0
    def default_action(self):
        """ Compute the relative position and rotation of the robot

        The measurements are taken with respect to the previous position
        and orientation of the robot
        """
        # Compute the position of the sensor within the original frame
        current_pos = self.original_pos.transformation3d_with(self.position_3d)

        # Compute the difference in positions with the previous loop
        self._dx = current_pos.x - self.previous_pos.x
        self._dy = current_pos.y - self.previous_pos.y
        self.local_data['dx'] = self._dx
        self.local_data['dy'] = self._dy
        self.local_data['dz'] = current_pos.z - self.previous_pos.z

        # Compute the difference in orientation with the previous loop
        dyaw = current_pos.yaw - self.previous_pos.yaw
        dpitch = current_pos.pitch - self.previous_pos.pitch
        droll = current_pos.roll - self.previous_pos.roll
        self.local_data['dyaw'] = normalise_angle(dyaw)
        self.local_data['dpitch'] = normalise_angle(dpitch)
        self.local_data['droll'] = normalise_angle(droll)

        # Store the 'new' previous data
        self.previous_pos = current_pos
コード例 #2
0
ファイル: odometry.py プロジェクト: DAInamite/morse
    def default_action(self):
        """ Compute the relative position and rotation of the robot

        The measurements are taken with respect to the previous position
        and orientation of the robot
        """
        # Compute the position of the sensor within the original frame
        current_pos = self.original_pos.transformation3d_with(self.position_3d)

        # Compute the difference in positions with the previous loop
        self._dx = current_pos.x - self.previous_pos.x
        self._dy = current_pos.y - self.previous_pos.y
        self.local_data['dx'] = self._dx
        self.local_data['dy'] = self._dy
        self.local_data['dz'] = current_pos.z - self.previous_pos.z

        # Compute the difference in orientation with the previous loop
        dyaw = current_pos.yaw - self.previous_pos.yaw
        dpitch = current_pos.pitch - self.previous_pos.pitch
        droll = current_pos.roll - self.previous_pos.roll
        self.local_data['dyaw'] = normalise_angle(dyaw)
        self.local_data['dpitch'] = normalise_angle(dpitch)
        self.local_data['droll'] = normalise_angle(droll)

        # Store the 'new' previous data
        self.previous_pos = current_pos
コード例 #3
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:
            # 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))

            # 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)
コード例 #4
0
ファイル: rotorcraft_attitude.py プロジェクト: caomw/morse
    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)
コード例 #5
0
    def default_action(self):
        """ Change the parent robot orientation. """

        if self._speed == float('inf'):
            # New parent orientation
            orientation = mathutils.Euler([self.local_data['roll'],
                                           self.local_data['pitch'],
                                           self.local_data['yaw']])

            self.robot_parent.force_pose(None, orientation.to_matrix())
        else:
            goal = [self.local_data['roll'], self.local_data['pitch'], self.local_data['yaw']]
            current_rot = [self.position_3d.roll, self.position_3d.pitch, self.position_3d.yaw]
            cmd = [0.0, 0.0, 0.0]
            for i in range(0, 3):
                diff = goal[i] - current_rot[i]
                diff = normalise_angle(diff)
                diff_abs = abs(diff)
                if diff_abs < self._tolerance:
                    cmd[i] = 0.0
                else:
                    sign = diff_abs / diff
                    if diff_abs > self._speed / self._frequency:
                        cmd[i] = sign * self._speed
                    else:
                        cmd[i] = diff_abs * self._frequency
                if self._type == 'Position':
                    cmd[i] /= self._frequency 

            self.robot_parent.apply_speed(self._type, [0.0, 0.0, 0.0], cmd)
コード例 #6
0
ファイル: odometry.py プロジェクト: warp1337/morse
    def default_action(self):
        # Compute the position of the sensor within the original frame
        current_pos = self.original_pos.transformation3d_with(self.position_3d)

        # Integrated version
        self._dx = current_pos.x - self.previous_pos.x
        self._dy = current_pos.y - self.previous_pos.y
        self._dyaw = normalise_angle(current_pos.yaw - self.previous_pos.yaw)

        self.local_data['x'] = current_pos.x
        self.local_data['y'] = current_pos.y
        self.local_data['z'] = current_pos.z
        self.local_data['yaw'] = current_pos.yaw
        self.local_data['pitch'] = current_pos.pitch
        self.local_data['roll'] = current_pos.roll

        # speed in the sensor frame, related to the robot pose
        lin_vel = linear_velocities(self.previous_pos, current_pos, self.frequency)
        ang_vel = angular_velocities(self.previous_pos, current_pos, 1/self.frequency)

        self.local_data['vx'] = lin_vel[0]
        self.local_data['vy'] = lin_vel[1]
        self.local_data['vz'] = lin_vel[2]
        self.local_data['wx'] = ang_vel[0]
        self.local_data['wy'] = ang_vel[1]
        self.local_data['wz'] = ang_vel[2]

        # Store the 'new' previous data
        self.previous_pos = current_pos
コード例 #7
0
    def default_action(self):
        # Compute the position of the sensor within the original frame
        current_pos = copy.copy(self.position_3d)

        # Integrated version
        self._dx = current_pos.x - self.previous_pos.x
        self._dy = current_pos.y - self.previous_pos.y
        self._dyaw = normalise_angle(current_pos.yaw - self.previous_pos.yaw)

        self.local_data['x'] = current_pos.x
        self.local_data['y'] = current_pos.y
        self.local_data['z'] = current_pos.z
        self.local_data['yaw'] = current_pos.yaw
        self.local_data['pitch'] = current_pos.pitch
        self.local_data['roll'] = current_pos.roll

        self.delta_pos = self.previous_pos.transformation3d_with(current_pos)
        self.local_data['vx'] = self.delta_pos.x * self.frequency
        self.local_data['vy'] = self.delta_pos.y * self.frequency
        self.local_data['vz'] = self.delta_pos.z * self.frequency
        self.local_data['wz'] = self.delta_pos.yaw * self.frequency
        self.local_data['wy'] = self.delta_pos.pitch * self.frequency
        self.local_data['wx'] = self.delta_pos.roll * self.frequency

        # Store the 'new' previous data
        self.previous_pos = current_pos
コード例 #8
0
ファイル: odometry.py プロジェクト: razinanu/AssistantRobotic
    def default_action(self):
        # Compute the position of the sensor within the original frame
        current_pos = self.original_pos.transformation3d_with(self.position_3d)

        # Integrated version
        self._dx = current_pos.x - self.previous_pos.x
        self._dy = current_pos.y - self.previous_pos.y
        self._dyaw = normalise_angle(current_pos.yaw - self.previous_pos.yaw)

        self.local_data['x'] = current_pos.x
        self.local_data['y'] = current_pos.y
        self.local_data['z'] = current_pos.z
        self.local_data['yaw'] = current_pos.yaw
        self.local_data['pitch'] = current_pos.pitch
        self.local_data['roll'] = current_pos.roll

        # speed in the sensor frame, related to the robot pose
        lin_vel = linear_velocities(self.previous_pos, current_pos, 1/self.frequency)
        ang_vel = angular_velocities(self.previous_pos, current_pos,1/self.frequency)

        self.local_data['vx'] = lin_vel[0]
        self.local_data['vy'] = lin_vel[1]
        self.local_data['vz'] = lin_vel[2]
        self.local_data['wx'] = ang_vel[0]
        self.local_data['wy'] = ang_vel[1]
        self.local_data['wz'] = ang_vel[2]

        # Store the 'new' previous data
        self.previous_pos = current_pos
コード例 #9
0
ファイル: odometry.py プロジェクト: DAInamite/morse
    def default_action(self):
        # Compute the position of the sensor within the original frame
        current_pos = self.original_pos.transformation3d_with(self.position_3d)

        # Integrated version
        self._dx = current_pos.x - self.previous_pos.x
        self._dy = current_pos.y - self.previous_pos.y
        self._dyaw = normalise_angle(current_pos.yaw - self.previous_pos.yaw)

        self.local_data['x'] = current_pos.x
        self.local_data['y'] = current_pos.y
        self.local_data['z'] = current_pos.z
        self.local_data['yaw'] = current_pos.yaw
        self.local_data['pitch'] = current_pos.pitch
        self.local_data['roll'] = current_pos.roll

        # speed in the sensor frame, related to the robot pose
        self.delta_pos = self.previous_pos.transformation3d_with(current_pos)
        self.local_data['vx'] = self.delta_pos.x * self.frequency
        self.local_data['vy'] = self.delta_pos.y * self.frequency
        self.local_data['vz'] = self.delta_pos.z * self.frequency
        self.local_data['wz'] = self.delta_pos.yaw * self.frequency
        self.local_data['wy'] = self.delta_pos.pitch * self.frequency
        self.local_data['wx'] = self.delta_pos.roll * self.frequency

        # Store the 'new' previous data
        self.previous_pos = current_pos
コード例 #10
0
ファイル: orientation.py プロジェクト: chgrand/morse
    def default_action(self):
        """ Change the parent robot orientation. """

        if self._speed == float('inf'):
            # New parent orientation
            orientation = mathutils.Euler([self.local_data['roll'],
                                           self.local_data['pitch'],
                                           self.local_data['yaw']])

            self.robot_parent.force_pose(None, orientation.to_matrix())
        else:
            goal = [self.local_data['roll'], self.local_data['pitch'], self.local_data['yaw']]
            current_rot = [self.position_3d.roll, self.position_3d.pitch, self.position_3d.yaw]
            cmd = [0.0, 0.0, 0.0]
            for i in range(0, 3):
                diff = goal[i] - current_rot[i]
                diff = normalise_angle(diff)
                diff_abs = abs(diff)
                if diff_abs < self._tolerance:
                    cmd[i] = 0.0
                else:
                    sign = diff_abs / diff
                    if diff_abs > self._speed / self._frequency:
                        cmd[i] = sign * self._speed
                    else:
                        cmd[i] = diff_abs * self._frequency
                if self._type == 'Position':
                    cmd[i] /= self._frequency 

            self.robot_parent.apply_speed(self._type, [0.0, 0.0, 0.0], cmd)
コード例 #11
0
ファイル: odometry.py プロジェクト: Arkapravo/morse-0.6
    def default_action(self):
        """ Compute the relative position and rotation of the robot

        The measurements are taken with respect to the previous position
        and orientation of the robot
        """
        # Compute the position of the sensor within the original frame
        current_pos = self.original_pos.transformation3d_with(self.position_3d)

        # Compute the difference in positions with the previous loop
        self.local_data['dS'] = current_pos.distance(self.previous_pos)

        self.local_data['dx'] = current_pos.x - self.previous_pos.x
        self.local_data['dy'] = current_pos.y - self.previous_pos.y
        self.local_data['dz'] = current_pos.z - self.previous_pos.z

        # Compute the difference in orientation with the previous loop
        dyaw = current_pos.yaw - self.previous_pos.yaw
        dpitch = current_pos.pitch - self.previous_pos.pitch
        droll = current_pos.roll - self.previous_pos.roll
        self.local_data['dyaw'] = normalise_angle(dyaw)
        self.local_data['dpitch'] = normalise_angle(dpitch)
        self.local_data['droll'] = normalise_angle(droll)

        # Integrated version
        self.local_data['x'] = current_pos.x
        self.local_data['y'] = current_pos.y
        self.local_data['z'] = current_pos.z
        self.local_data['yaw'] = current_pos.yaw
        self.local_data['pitch'] = current_pos.pitch
        self.local_data['roll'] = current_pos.roll

        # speed in the sensor frame, related to the robot pose
        self.delta_pos = self.previous_pos.transformation3d_with(current_pos)
        self.local_data['vx'] = self.delta_pos.x * self.frequency
        self.local_data['vy'] = self.delta_pos.y * self.frequency
        self.local_data['vz'] = self.delta_pos.z * self.frequency
        self.local_data['wz'] = self.delta_pos.yaw * self.frequency
        self.local_data['wy'] = self.delta_pos.pitch * self.frequency
        self.local_data['wx'] = self.delta_pos.roll * self.frequency

        # Store the 'new' previous data
        self.previous_pos = current_pos
コード例 #12
0
ファイル: ptu.py プロジェクト: tiberiusferreira/VilmaProject
    def default_action(self):
        """ Apply rotation to the platine unit """
        # Reset movement variables
        ry, rz = 0.0, 0.0

        if self._is_manual_mode:
            return

        try:
            normal_speed = self._speed / self.frequency
        # For the moment ignoring the division by zero
        # It happens apparently when the simulation starts
        except ZeroDivisionError:
            pass

        self._current_pan = self._pan_orientation.to_euler().z
        self._current_tilt = self._tilt_orientation.to_euler().y

        logger.debug("PTU: pan=%.4f, tilt=%.4f" %
                     (self._current_pan, self._current_tilt))

        # Get the angles in a range of -PI, PI
        target_pan = normalise_angle(self.local_data['pan'])
        target_tilt = normalise_angle(self.local_data['tilt'])
        logger.debug("Targets: pan=%.4f, tilt=%.4f" %
                     (target_pan, target_tilt))

        if (abs(target_pan - self._current_pan) < self._tolerance
                and abs(target_tilt - self._current_tilt) < self._tolerance):
            self.completed(status.SUCCESS)

        # Determine the direction of the rotation, if any
        ry = rotation_direction(self._current_tilt, target_tilt,
                                self._tolerance, normal_speed)
        rz = rotation_direction(self._current_pan, target_pan, self._tolerance,
                                normal_speed)

        # Give the rotation instructions directly to the parent
        # The second parameter specifies a "local" movement
        self._pan_base.applyRotation([0.0, 0.0, rz], True)
        self._tilt_base.applyRotation([0.0, ry, 0.0], True)
コード例 #13
0
ファイル: ptu.py プロジェクト: DAInamite/morse
    def default_action(self):
        """ Apply rotation to the platine unit """
        # Reset movement variables
        ry, rz = 0.0, 0.0

        if self._is_manual_mode:
            return

        try:
            normal_speed = self._speed / self.frequency
        # For the moment ignoring the division by zero
        # It happens apparently when the simulation starts
        except ZeroDivisionError:
            pass

        self._current_pan = self._pan_orientation.to_euler().z
        self._current_tilt = self._tilt_orientation.to_euler().y

        logger.debug("PTU: pan=%.4f, tilt=%.4f" % (self._current_pan,
                                                   self._current_tilt))

        # Get the angles in a range of -PI, PI
        target_pan = normalise_angle(self.local_data['pan'])
        target_tilt = normalise_angle(self.local_data['tilt'])
        logger.debug("Targets: pan=%.4f, tilt=%.4f" % (target_pan, target_tilt))

        if (abs(target_pan - self._current_pan) < self._tolerance and
            abs(target_tilt - self._current_tilt) < self._tolerance):
            self.completed(status.SUCCESS)

        # Determine the direction of the rotation, if any
        ry = rotation_direction(self._current_tilt, target_tilt,
                                self._tolerance, normal_speed)
        rz = rotation_direction(self._current_pan, target_pan,
                                self._tolerance, normal_speed)

        # Give the rotation instructions directly to the parent
        # The second parameter specifies a "local" movement
        self._pan_base.applyRotation([0.0, 0.0, rz], True)
        self._tilt_base.applyRotation([0.0, ry, 0.0], True)
コード例 #14
0
ファイル: attitude.py プロジェクト: adegroote/morse
    def default_action(self):
        if self._type == 'Velocity':
            rates = self.sim_attitude_with_physics()
        else:
            rates = self.sim_attitude_simple()

        # Store the important data
        self.local_data['rotation'] = self.position_3d.euler
        if self._use_angle_against_north:
            self.local_data['rotation'][2] = \
            normalise_angle(
                - self._coord_converter.angle_against_geographic_north(self.position_3d.euler))
        self.local_data['angular_velocity'] = rates
コード例 #15
0
ファイル: kuka_lwr.py プロジェクト: Arkapravo/morse-0.6
    def default_action(self):
        """ Apply rotation angles to the segments of the arm """

        armature = self.blender_obj
        logger.debug("The armature is: '%s' (%s)" % (armature, type(armature)))

        for channel in armature.channels:
            segment_angle = channel.joint_rotation
            logger.debug("Channel '%s' st: [%.4f, %.4f, %.4f]" % (channel, segment_angle[0], segment_angle[1], segment_angle[2]))

            # Get the normalised angle for this segment
            target_angle = normalise_angle(self.local_data[channel.name])

            # Use the corresponding direction for each rotation
            if self._dofs[channel.name][1] == 1:
                segment_angle[1] = target_angle
            elif self._dofs[channel.name][2] == 1:
                segment_angle[2] = target_angle

            logger.debug("Channel '%s' fn: [%.4f, %.4f, %.4f]" % (channel, segment_angle[0], segment_angle[1], segment_angle[2]))
            channel.joint_rotation = segment_angle

            armature.update()
コード例 #16
0
ファイル: rotorcraft_velocity.py プロジェクト: zyh1994/morse
    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)
コード例 #17
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"
コード例 #18
0
ファイル: pa_10.py プロジェクト: Greg8978/morse_MaRDi
    def default_action(self):
        """ Apply rotation to the arm segments """
        # Get the reference to the Sound actuator
        if self._sound == None:
            logger.debug ("ACTIVATING THE SOUND ACTUATOR")
            contr = blenderapi.controller()
            self._sound = contr.actuators['Sound']
            contr.activate(self._sound)
            self._sound.stopSound()

        # Reset movement variables
        rx, ry, rz = 0.0, 0.0, 0.0

        # Scale the speeds to the time used by Blender
        try:
            rotation = self._speed / self.frequency
        # For the moment ignoring the division by zero
        # It happens apparently when the simulation starts
        except ZeroDivisionError:
            pass

        self._moving = False

        for i in range(6):
            key = ('seg%d' % i)
            target_angle = normalise_angle(self.local_data[key])

            # Get the next segment
            segment = self._segments[i]

            # Extract the angles
            rot_matrix = segment.localOrientation
            segment_matrix = mathutils.Matrix((rot_matrix[0], rot_matrix[1], rot_matrix[2]))
            segment_euler = segment_matrix.to_euler()

            # Use the corresponding direction for each rotation
            if self._dofs[i] == 'y':
                ry = rotation_direction(segment_euler[1], target_angle, self._tolerance, rotation)
                #logger.debug("PARAMETERS Y: %.4f, %.4f, %.4f, %.4f = %.4f" % (segment_euler[1], target_angle, _tolerance, rotation, ry))

            elif self._dofs[i] == 'z':
                rz = rotation_direction(segment_euler[2], target_angle, self._tolerance, rotation)
                #logger.debug("PARAMETERS Z: %.4f, %.4f, %.4f, %.4f = %.4f" % (segment_euler[2], target_angle, _tolerance, rotation, rz))

            logger.debug("ry = %.4f, rz = %.4f" % (ry, rz))

            # Give the movement instructions directly to the parent
            # The second parameter specifies a "local" movement
            segment.applyRotation([rx, ry, rz], True)

            if ry != 0.0 or rz != 0.0:
                self._moving = True

            # Reset the rotations for the next segment
            ry = rz = 0

        if self._moving:
            self._sound.startSound()
            logger.debug("STARTING SOUND")
        else:
            self._sound.stopSound()
            logger.debug("STOPPING SOUND")
コード例 #19
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"
コード例 #20
0
ファイル: pa_10.py プロジェクト: imclab/morse
    def default_action(self):
        """ Apply rotation to the arm segments """
        # Get the reference to the Sound actuator
        if self._sound is None:
            logger.debug ("ACTIVATING THE SOUND ACTUATOR")
            contr = blenderapi.controller()
            self._sound = contr.actuators['Sound']
            contr.activate(self._sound)
            self._sound.stopSound()

        # Reset movement variables
        rx, ry, rz = 0.0, 0.0, 0.0

        # Scale the speeds to the time used by Blender
        try:
            rotation = self._speed / self.frequency
        # For the moment ignoring the division by zero
        # It happens apparently when the simulation starts
        except ZeroDivisionError:
            pass

        self._moving = False

        for i in range(6):
            key = ('seg%d' % i)
            target_angle = normalise_angle(self.local_data[key])

            # Get the next segment
            segment = self._segments[i]

            # Extract the angles
            rot_matrix = segment.localOrientation
            segment_matrix = mathutils.Matrix((rot_matrix[0], rot_matrix[1], rot_matrix[2]))
            segment_euler = segment_matrix.to_euler()

            # Use the corresponding direction for each rotation
            if self._dofs[i] == 'y':
                ry = rotation_direction(segment_euler[1], target_angle, self._tolerance, rotation)
                #logger.debug("PARAMETERS Y: %.4f, %.4f, %.4f, %.4f = %.4f" % (segment_euler[1], target_angle, _tolerance, rotation, ry))

            elif self._dofs[i] == 'z':
                rz = rotation_direction(segment_euler[2], target_angle, self._tolerance, rotation)
                #logger.debug("PARAMETERS Z: %.4f, %.4f, %.4f, %.4f = %.4f" % (segment_euler[2], target_angle, _tolerance, rotation, rz))

            logger.debug("ry = %.4f, rz = %.4f" % (ry, rz))

            # Give the movement instructions directly to the parent
            # The second parameter specifies a "local" movement
            segment.applyRotation([rx, ry, rz], True)

            if ry != 0.0 or rz != 0.0:
                self._moving = True

            # Reset the rotations for the next segment
            ry = rz = 0

        if self._moving:
            self._sound.startSound()
            logger.debug("STARTING SOUND")
        else:
            self._sound.stopSound()
            logger.debug("STOPPING SOUND")