Esempio n. 1
0
    def setUp(self):
        env=Environment()
        #env.SetViewer('qtcoin')
        env.Load('test_collisionmap.env.xml')

        robot=env.GetRobots()[0]

        self.pose=Pose(robot)
        self.env=env
        self.env.SetDebugLevel(1)
        self.robot=robot
Esempio n. 2
0
class Turbine:
    """ This class provides a configuration file loading for the whole turbine environment,
    plus manipulation of the active elements of the turbine (robot/rail)
    and their interacting (collision)

    Args:
        config: (TurbineConfig) configuration file object.
    """

    env = None
    config = None

    # std names for printing propouses
    std_name = {
        _PRIMARY_RAIL: "primary rail",
        _SECONDARY_RAIL: "secondary rail",
        _BLADE: "blade",
        _RUNNER_AREA: "runner area",
        _IRIS: "iris",
        _ROTOR: "rotor"
    }

    def __init__(self, config):
        self.config = config

        # Create OpenRave environment
        RaveInitialize(True, 0)
        self.env = Environment()
        self.env.Load(config.environment.load)
        self.robot = self.env.GetRobots()[0]
        self.manipulator = self.robot.GetActiveManipulator()

        ikmodel = databases.inversekinematics.InverseKinematicsModel(
            robot=self.robot, iktype=IkParameterization.Type.Transform6D)
        if not ikmodel.load():
            ikmodel.autogenerate()

        # Principal turbine elements linked to attributes
        self.bodies = self.env.GetBodies()

        try:
            self.rotor = next(body for body in self.bodies
                              if body.GetName() == _ROTOR)
        except StopIteration:
            raise XMLStructError(_ROTOR)

        try:
            self.iris = next(body for body in self.bodies
                             if body.GetName() == _IRIS)
        except StopIteration:
            raise XMLStructError(_IRIS)

        try:
            self.runner_area = next(body for body in self.bodies
                                    if body.GetName() == _RUNNER_AREA)
        except StopIteration:
            raise XMLStructError(_RUNNER_AREA)

        try:
            self.primary = next(body for body in self.bodies
                                if body.GetName() == _PRIMARY_RAIL)
        except StopIteration:
            raise XMLStructError(_PRIMARY_RAIL)

        try:
            self.secondary = next(body for body in self.bodies
                                  if body.GetName() == _SECONDARY_RAIL)
        except StopIteration:
            raise XMLStructError(_SECONDARY_RAIL)

        blade_number = 1
        try:
            self.blades = [
                next(body for body in self.bodies
                     if body.GetName() == _BLADE + str(blade_number))
            ]
        except StopIteration:
            raise XMLStructError(_BLADE + str(blade_number))

        blade_found = True
        while blade_found:
            blade_number += 1
            try:
                self.blades.append(
                    next(body for body in self.bodies
                         if body.GetName() == _BLADE + str(blade_number)))
            except StopIteration:
                blade_found = False

    def place_rail(self, rail_place):
        """ Place both rails in the environment
        Args:
            rail_place: (RailPlace) rail object.
        """

        # P - primary rail,
        # S - secondary rail,
        # alpha - angle from the perpendicular to the primary rail
        # Using camera standard axis and transformLookat for rails

        # Primary Rail

        primary_extent = self.primary.GetLinks()[0].GetGeometries(
        )[0].GetBoxExtents()
        primary_offset = array([
            0, -primary_extent[1], -primary_extent[2]
        ]) + array([0, 0, self.config.environment.primary_safe_margin])
        primary_offset_transform = eye(4)
        primary_offset_transform[0:3, 3] = primary_offset

        # Secondary Rail
        secondary_extent = self.secondary.GetLinks()[0].GetGeometries(
        )[0].GetBoxExtents()
        # Resizing
        secondary_extent[2] = abs(
            rail_place.s) / 2.0 + self.config.environment.secondary_safe_margin
        self.env.Remove(self.secondary)
        self.secondary.InitFromBoxes(
            array([concatenate([zeros(3), secondary_extent])]), True)
        self.env.AddKinBody(self.secondary)
        #
        secondary_offset = array(
            [0, -secondary_extent[1], secondary_extent[2]]) + array([
                0, self.config.environment.robot_level_difference,
                -self.config.environment.secondary_safe_margin
            ])
        secondary_offset_transform = eye(4)
        secondary_offset_transform[0:3, 3] = secondary_offset

        # Rails Traonsform and Placement
        primary_transform = transformLookat(
            array([0, 0, self.config.environment.z_floor_level]),
            array([rail_place.p, 0, self.config.environment.z_floor_level]),
            [0, 0, 1])
        self.primary.SetTransform(
            dot(primary_transform, primary_offset_transform))

        secondary_transform = transformLookat(
            rail_place.getXYZ(self.config),
            array([rail_place.p, 0, self.config.environment.z_floor_level]),
            [0, 0, 1])
        self.secondary.SetTransform(
            dot(secondary_transform, secondary_offset_transform))

    def check_rail_collision(self):
        """ Check rail <-> blades collision """
        with self.env:
            collisions = [
                self.env.CheckCollision(self.primary, blade)
                or self.env.CheckCollision(self.secondary, blade)
                for blade in self.blades
            ]
        return any(collisions)

    def place_robot(self, rail_place):
        """ Place robot on the end of the secondary rail """
        T = rail_place.getTransform(self.config)
        self.robot.SetTransform(T)

    def check_robotbase_collision(self):
        """ Check robot's base <-> environment collision """
        with self.robot:
            for link in self.robot.GetLinks():
                link.Enable(False)
            self.robot.GetLink('Base').Enable(True)
            self.robot.GetLink('Arm0').Enable(True)
            with self.env:
                collision_base = [(self.env.CheckCollision(self.robot, body)
                                   and not (body == self.robot))
                                  for body in self.bodies]

            collisions = collision_base

        return any(collisions)

    def check_robot_collision(self):
        """ Check robot <-> environment collision """
        collisions = [(self.env.CheckCollision(self.robot, body)
                       and not (body == self.robot)) for body in self.bodies]
        return any(collisions)
# from tamp.learning_tamp import *
from random import random
from manipulation.primitives.utils import get_env, mirror_arm_config
from manipulation.primitives.transforms import *
from openravepy import *
from manipulation.bodies.robot import open_gripper, get_manip_trans
from manipulation.bodies.robot import manip_from_pose_grasp
from manipulation.bodies.robot import get_active_arm_indices, get_manipulator

import copy

env = Environment()
ENV_FILENAME = './just_robot.xml'
env.SetViewer('qtcoin')
env.Load(ENV_FILENAME)
robot = env.GetRobots()[0]

# define object and place it (0,0,0)

# define robot
robot.SetActiveManipulator('leftarm')
manip = robot.GetActiveManipulator()
ee = manip.GetEndEffector()
ikmodel = databases.inversekinematics.InverseKinematicsModel(robot=robot, \
                                                             iktype=IkParameterization.Type.Transform6D, \
                                                             forceikfast=True, freeindices=None, \
                                                             freejoints=None, manip=None)
if not ikmodel.load():
    ikmodel.autogenerate()

robot.SetActiveManipulator('rightarm_torso')