def __init__(self, env = None):
     if env == None:
         self.env = openravepy.Environment()
         self.robot = self.env.ReadRobotXMLFile("robots/pr2-beta-sim.robot.xml")
         self.env.Add(self.robot)
     
     elif len(env.GetRobots())>0:
             self.env = env
             self.robot = env.GetRobots()[0]
     else:
         rospy.loginfo("Valid env not found")
         sys.exit()
          
     
     rospy.loginfo("Loading IK for rightarm")
     self.robot.SetActiveManipulator("rightarm")
     self.rightarm_ik = InverseKinematicsModel(self.robot, 
                                               iktype=openravepy.IkParameterization.Type.Transform6D)
     if not self.rightarm_ik.load():
         self.rightarm_ik.autogenerate()
         
     rospy.loginfo("Loading IK for leftarm")
     self.robot.SetActiveManipulator("leftarm")
     self.leftarm_ik = InverseKinematicsModel(self.robot, 
                                              iktype=openravepy.IkParameterization.Type.Transform6D)
     if not self.leftarm_ik.load():
         self.leftarm_ik.autogenerate()
         
     self.robot_state = pr2_control_utilities.RobotState()
     self.controller = pr2_control_utilities.PR2JointMover(robot_state = self.robot_state,
                                                           name = "PR2 Controller",
                                                           time_to_reach=5.0
                                                           )
     self.listener = tf.TransformListener()
     
     #fixing the joints
     joint_msg = self.robot_state.last_joint_msg
     ros_names = joint_msg.name
     inds_ros2rave = np.array([self.robot.GetJointIndex(name) for name in ros_names])
     self.good_ros_inds = np.flatnonzero(inds_ros2rave != -1) # ros joints inds with matching rave joint
     self.rave_inds = inds_ros2rave[self.good_ros_inds] # openrave indices corresponding to those joints
     
     # make the joint limits match the PR2 soft limits
     low_limits, high_limits = self.robot.GetDOFLimits()
     rarm_low_limits = [-2.1353981634, -0.3536, -3.75, -2.1213, None, -2.0, None]
     rarm_high_limits = [0.564601836603, 1.2963, 0.65, -0.15, None, -0.1, None]
     for rarm_index, low, high in zip(self.robot.GetManipulator("rightarm").GetArmIndices(),
                                      rarm_low_limits, rarm_high_limits):
         if low is not None and high is not None:
             low_limits[rarm_index] = low
             high_limits[rarm_index] = high
     larm_low_limits = [-0.564601836603, -0.3536, -0.65, -2.1213, None, -2.0, None]
     larm_high_limits = [2.1353981634, 1.2963, 3.75, -0.15, None, -0.1, None]
     for larm_index, low, high in zip(self.robot.GetManipulator("leftarm").GetArmIndices(),
                                      larm_low_limits, larm_high_limits):
         if low is not None and high is not None:
             low_limits[larm_index] = low
             high_limits[larm_index] = high
     self.robot.SetDOFLimits(low_limits, high_limits)
Beispiel #2
0
    def _SetupIK(self, iktype):
        from openravepy.databases.inversekinematics import InverseKinematicsModel

        robot = self.GetRobot()
        self.ikmodel = InverseKinematicsModel(robot=robot, manip=self,
                                              iktype=iktype)
        if not self.ikmodel.load():
            self.ikmodel.generate(iktype=iktype, precision=4,
                                  freeindices=[ self.GetIndices()[2] ])
            self.ikmodel.save()
Beispiel #3
0
 def load_ikfast(self, freeinc=np.pi / 6.):
     success = False
     from openravepy.databases.inversekinematics import InverseKinematicsModel
     manipulators = [self.left_manip, self.right_manip]
     if None not in manipulators:
         for manip in manipulators:
             ikmodel = InverseKinematicsModel(self.robot,
                                              iktype=self.iktype,
                                              manip=manip)
             success = ikmodel.load(freeinc=[freeinc])
             if not success:
                 break
     return success
Beispiel #4
0
    def _SetupIK(self, iktype):
        from openravepy.databases.inversekinematics import InverseKinematicsModel

        robot = self.GetRobot()
        self.ikmodel = InverseKinematicsModel(robot=robot, manip=self, iktype=iktype)
        if not self.ikmodel.load():
            self.ikmodel.generate(iktype=iktype, precision=4, freeindices=[self.GetIndices()[2]])
            self.ikmodel.save()
Beispiel #5
0
    def _SetupIK(self, iktype):
        """ creates Ikfast file for chosen manipulator
		"""
        from openravepy.databases.inversekinematics import InverseKinematicsModel

        robot = self.GetRobot()
        self.ikmodel = InverseKinematicsModel(robot=robot,
                                              manip=self,
                                              iktype=iktype)
        if not self.ikmodel.load():
            if (self.namespace == 'arm'):
                self.ikmodel.generate(iktype=iktype,
                                      precision=4,
                                      freeindices=[self.GetIndices()[2]])
                self.ikmodel.save()
            else:
                self.ikmodel.generate(
                    iktype=iktype,
                    precision=4,
                    freeindices=[self.GetIndices()[4],
                                 self.GetIndices()[3]])
                self.ikmodel.save()
Beispiel #6
0
def load_ikfast(robot,
                iktype,
                freejoints=['J6'],
                freeinc=[0.01],
                autogenerate=True):
    """
  Load the IKFast solver

  Parameters
  ----------
  robot: orpy.Robot
    The OpenRAVE robot
  iktype: orpy.IkParameterizationType
    Inverse kinematics type to be used
  freeinc: list
    The increment (discretization) to be used for the free DOF when the target
    `iktype` is `TranslationDirection5D`
  autogenerate: bool, optional
    If true, auto-generate the IKFast solver

  Returns
  -------
  success: bool
    `True` if succeeded, `False` otherwise
  """
    # Improve code readability
    from openravepy.databases.inversekinematics import InverseKinematicsModel
    # Initialize the ikmodel
    if iktype == orpy.IkParameterizationType.TranslationDirection5D:
        ikmodel = InverseKinematicsModel(robot,
                                         iktype=iktype,
                                         freejoints=freejoints)
    elif iktype == orpy.IkParameterizationType.Transform6D:
        ikmodel = InverseKinematicsModel(robot, iktype=iktype)
    else:
        raise TypeError('Unsupported ikmodel: {}'.format(iktype.name))
    # Load or generate
    if not ikmodel.load() and autogenerate:
        print 'Generating IKFast {0}. Will take few minutes...'.format(
            iktype.name)
        if iktype == orpy.IkParameterizationType.Transform6D:
            ikmodel.autogenerate()
        elif iktype == orpy.IkParameterizationType.TranslationDirection5D:
            ikmodel.generate(iktype=iktype, freejoints=freejoints)
            ikmodel.save()
        else:
            ikmodel.autogenerate()
        print 'IKFast {0} has been successfully generated'.format(iktype.name)
    if iktype == orpy.IkParameterizationType.TranslationDirection5D:
        success = ikmodel.load(freeinc=freeinc)
    elif iktype == orpy.IkParameterizationType.Transform6D:
        success = ikmodel.load()
    return success
class PR2Robot(object):
    def __init__(self, env = None):
        if env == None:
            self.env = openravepy.Environment()
            self.robot = self.env.ReadRobotXMLFile("robots/pr2-beta-sim.robot.xml")
            self.env.Add(self.robot)
        
        elif len(env.GetRobots())>0:
                self.env = env
                self.robot = env.GetRobots()[0]
        else:
            rospy.loginfo("Valid env not found")
            sys.exit()
             
        
        rospy.loginfo("Loading IK for rightarm")
        self.robot.SetActiveManipulator("rightarm")
        self.rightarm_ik = InverseKinematicsModel(self.robot, 
                                                  iktype=openravepy.IkParameterization.Type.Transform6D)
        if not self.rightarm_ik.load():
            self.rightarm_ik.autogenerate()
            
        rospy.loginfo("Loading IK for leftarm")
        self.robot.SetActiveManipulator("leftarm")
        self.leftarm_ik = InverseKinematicsModel(self.robot, 
                                                 iktype=openravepy.IkParameterization.Type.Transform6D)
        if not self.leftarm_ik.load():
            self.leftarm_ik.autogenerate()
            
        self.robot_state = pr2_control_utilities.RobotState()
        self.controller = pr2_control_utilities.PR2JointMover(robot_state = self.robot_state,
                                                              name = "PR2 Controller",
                                                              time_to_reach=5.0
                                                              )
        self.listener = tf.TransformListener()
        
        #fixing the joints
        joint_msg = self.robot_state.last_joint_msg
        ros_names = joint_msg.name
        inds_ros2rave = np.array([self.robot.GetJointIndex(name) for name in ros_names])
        self.good_ros_inds = np.flatnonzero(inds_ros2rave != -1) # ros joints inds with matching rave joint
        self.rave_inds = inds_ros2rave[self.good_ros_inds] # openrave indices corresponding to those joints
        
        # make the joint limits match the PR2 soft limits
        low_limits, high_limits = self.robot.GetDOFLimits()
        rarm_low_limits = [-2.1353981634, -0.3536, -3.75, -2.1213, None, -2.0, None]
        rarm_high_limits = [0.564601836603, 1.2963, 0.65, -0.15, None, -0.1, None]
        for rarm_index, low, high in zip(self.robot.GetManipulator("rightarm").GetArmIndices(),
                                         rarm_low_limits, rarm_high_limits):
            if low is not None and high is not None:
                low_limits[rarm_index] = low
                high_limits[rarm_index] = high
        larm_low_limits = [-0.564601836603, -0.3536, -0.65, -2.1213, None, -2.0, None]
        larm_high_limits = [2.1353981634, 1.2963, 3.75, -0.15, None, -0.1, None]
        for larm_index, low, high in zip(self.robot.GetManipulator("leftarm").GetArmIndices(),
                                         larm_low_limits, larm_high_limits):
            if low is not None and high is not None:
                low_limits[larm_index] = low
                high_limits[larm_index] = high
        self.robot.SetDOFLimits(low_limits, high_limits)
        
    
    def convertPose(self, pose):
        assert isinstance(pose, PoseStamped)
        self.listener.waitForTransform("/base_footprint", pose.header.frame_id,
                                       rospy.Time.now(), rospy.Duration(1))
        newpose = self.listener.transformPose("/base_footprint", pose)        
        translation = tf.listener.xyz_to_mat44(newpose.pose.position), 
        orientation = tf.listener.xyzw_to_mat44(newpose.pose.orientation)
        
        matrix4 = np.dot(translation, orientation).squeeze()
        return matrix4
    
    def __ik_solution(self, pose, manip, end_effector_link, ignore_end_effector=True,
                      multiple_soluitions = False):
        T = self.convertPose(pose)
        self.update_rave()
        worldFromEE = utils.tf_for_link(T, manip, end_effector_link)
        filter_options = openravepy.IkFilterOptions.CheckEnvCollisions
        if ignore_end_effector:
            filter_options = filter_options | openravepy.IkFilterOptions.IgnoreEndEffectorCollisions
        
        if multiple_soluitions:
            sol = manip.FindIKSolutions(worldFromEE, filter_options)
        else:
            sol = manip.FindIKSolution(worldFromEE, filter_options)
        return sol
    
    def find_rightarm_ik(self, pose, ignore_end_effector=True,
                      multiple_soluitions = False):
        manip = self.robot.SetActiveManipulator("rightarm")
        return self.__ik_solution(pose, manip, "r_wrist_roll_link",
                                  ignore_end_effector, 
                                  multiple_soluitions)
    
    def find_leftarm_ik(self, pose, ignore_end_effector=True,
                      multiple_soluitions = False):
        manip = self.robot.SetActiveManipulator("leftarm")
        return self.__ik_solution(pose, manip, "l_wrist_roll_link",
                                  ignore_end_effector,
                                  multiple_soluitions)    
    
    def move_right_arm(self, pose, ignore_end_effector = True):
        sol = self.find_rightarm_ik(pose, ignore_end_effector)
        if sol is None:
            rospy.logerr("Could not find an IK solution!")
            return False
        self.controller.set_arm_state(sol.tolist(), "right", True)
        self.update_rave()
        return True        
    
    def move_left_arm(self, pose, ignore_end_effector = True):
        sol = self.find_leftarm_ik(pose, ignore_end_effector)
        if sol is None:
            rospy.logerr("Could not find an IK solution!")
            return False
        self.controller.set_arm_state(sol.tolist(), "left", True)
        self.update_rave()
        return True
        
    def update_rave(self):
        ros_values = self.robot_state.last_joint_msg.position
        rave_values = [ros_values[i_ros] for i_ros in self.good_ros_inds]
        self.robot.SetJointValues(rave_values[:20],self.rave_inds[:20])
        self.robot.SetJointValues(rave_values[20:],self.rave_inds[20:])
        
        
    def tuck_left_arm(self):
        self.controller.set_arm_state(pr2_l_arm_tucked, "left", True)
        self.update_rave()
        
    def tuck_right_arm(self):
        self.controller.set_arm_state(pr2_r_arm_tucked, "right", True)
        self.update_rave()
        
    def tuck_arms(self):
        self.tuck_left_arm()
        self.tuck_right_arm()
Beispiel #8
0
class WAM(Manipulator):
    def __init__(self, sim, namespace='',
                 iktype=openravepy.IkParameterization.Type.Transform6D):
        Manipulator.__init__(self)

        self.simulated = sim
        self._iktype = iktype
        self.namespace = namespace

        if iktype is not None:
            self._SetupIK(iktype)

        if sim:
            from prpy.simulation import ServoSimulator
            self.servo_simulator = ServoSimulator(
                self, rate=20, watchdog_timeout=0.1)

    def IsSimulated(self):
        return self.simulated

    def GetJointNames(self):
        return [self.namespace + '/' + j for j in
                ['j1', 'j2', 'j3', 'j4', 'j5', 'j6', 'j7']]

    def CloneBindings(self, parent):
        Manipulator.CloneBindings(self, parent)

        self.simulated = True
        self._iktype = parent._iktype

        if self._iktype is not None:
            self._SetupIK(self._iktype)

    def _SetupIK(self, iktype):
        from openravepy.databases.inversekinematics import InverseKinematicsModel

        robot = self.GetRobot()
        self.ikmodel = InverseKinematicsModel(robot=robot, manip=self,
                                              iktype=iktype)
        if not self.ikmodel.load():
            self.ikmodel.generate(iktype=iktype, precision=4,
                                  freeindices=[ self.GetIndices()[2] ])
            self.ikmodel.save()

    def SetStiffness(self, stiffness):
        """Set the WAM's stiffness.
        Stiffness False/0 is gravity compensation and stiffness True/1 is position
        control. Values between 0 and 1 are experimental.
        @param stiffness boolean or decimal value between 0.0 and 1.0
        """
        self.GetRobot().SetStiffness(stiffness, manip=self)


    def SetTrajectoryExecutionOptions(self, traj, stop_on_stall=False,
            stop_on_ft=False, force_magnitude=None, force_direction=None,
            torque=None):
        """Set OWD's trajectory execution options on trajectory.
        @param stop_on_stall aborts the trajectory if the arm stalls
        @param stop_on_ft aborts the trajectory if the force/torque
                          sensor reports a force or torque that exceeds
                          threshold specified by the force_magnitude,
                          force_direction, and torque options
        @param force_magnitude force threshold value, in Newtons
        @param force_direction unit vector in the force/torque coordinate
                               frame, which is int he same orientation as the
                               hand frame.
        @param torque vector of the three torques
        """
        raise NotImplementedError('OWD execution options not supported under ros_control')

    def Servo(self, velocities):
        """Servo with a vector of instantaneous joint velocities.
        @param velocities joint velocities, in radians per second
        """
        num_dof = len(self.GetArmIndices())
        if len(velocities) != num_dof:
            raise ValueError('Incorrect number of joint velocities. '
                             'Expected {0:d}; got {0:d}.'.format(
                             num_dof, len(velocities)))

        if not self.simulated:
            raise NotImplementedError('Servoing and velocity control not yet'
                                      'supported under ros_control.')

        else:
            self.servo_simulator.SetVelocity(velocities)

    def ServoTo(self, target, duration, timeStep=0.05, collisionChecking=True):
        """Servo the arm towards a target configuration over some duration.
        Servos the arm towards a target configuration with a constant joint
        velocity. This function uses the \ref Servo command to control the arm
        and must be called repeatidly until the arm reaches the goal. If \tt
        collisionChecking is enabled, then the servo will terminate and return
        False if a collision is detected with the simulation environment.
        @param target desired configuration
        @param duration duration in seconds
        @param timestep period of the control loop, in seconds
        @param collisionchecking check collisions in the simulation environment
        @return whether the servo was successful
        """
        steps = int(math.ceil(duration/timeStep))
        original_dofs = self.GetRobot().GetDOFValues(self.GetArmIndices())
        velocity = numpy.array(target - self.GetRobot().GetDOFValues(self.GetArmIndices()))
        velocities = [v/steps for v in velocity]
        inCollision = False 
        if collisionChecking:
            inCollision = self.CollisionCheck(target)

        if not inCollision:
            for i in range(1,steps):
                import time
                self.Servo(velocities)
                time.sleep(timeStep)
            self.Servo([0] * len(self.GetArmIndices()))
            # new_dofs = self.GetRobot().GetDOFValues(self.GetArmIndices())
            return True
        else:
            return False

    def GetVelocityLimits(self, openrave=None, owd=None):
        """Get the OpenRAVE and OWD joint velocity limits.
        This function checks both the OpenRAVE and OWD joint velocity limits.
        If they do not match, a warning is printed and the minimum value is
        returned.
        @param openrave flag to set the OpenRAVE velocity limits
        @param owd flag to set the OWD velocity limits
        @return list of velocity limits, in radians per second
        """
        if openrave is not None or owd is not None:
            warnings.warn(
                'The "openrave" and "owd" flags are deprecated in'
                ' GetVelocityLimits and will be removed in a future version.',
                DeprecationWarning)

        return Manipulator.GetVelocityLimits(self)
        
    def SetVelocityLimits(self, velocity_limits, min_accel_time,
                          openrave=True, owd=None):
        """Change the OpenRAVE and OWD joint velocity limits.
        Joint velocities that exceed these limits will trigger a velocity fault.
        @param velocity_limits vector of joint velocity limits in radians per second
        @param min_accel_time minimum acceleration time used to compute acceleration limits
        @param openrave flag to set the OpenRAVE velocity limits
        @param owd flag to set the OWD velocity limits
        """
        # TODO implement in ros_control
        if owd is not None:
            warnings.warn(
                'The "owd" flag is deprecated in'
                ' SetVelocityLimits and will be removed in a future version.',
                DeprecationWarning)

        # Update the OpenRAVE limits.
        if openrave:
            Manipulator.SetVelocityLimits(self, velocity_limits, min_accel_time)

    def GetTrajectoryStatus(manipulator):
        """Gets the status of the current (or previous) trajectory executed by OWD.
        @return status of the current (or previous) trajectory executed
        """
        raise NotImplementedError('GetTrajectoryStatus not supported on manipulator.'
                                  ' Use returned TrajectoryFuture instead.') 

    def ClearTrajectoryStatus(manipulator):
        """Clears the current trajectory execution status.
        This resets the output of \ref GetTrajectoryStatus.
        """
        raise NotImplementedError('ClearTrajectoryStatus not supported on manipulator.')

    def MoveUntilTouch(manipulator, direction, distance, max_distance=None,
                       max_force=5.0, max_torque=None, ignore_collisions=None,
                       velocity_limit_scale=0.25, **kw_args):
        """Execute a straight move-until-touch action.
        This action stops when a sufficient force is is felt or the manipulator
        moves the maximum distance. The motion is considered successful if the
        end-effector moves at least distance. In simulation, a move-until-touch
        action proceeds until the end-effector collids with the environment.

        @param direction unit vector for the direction of motion in the world frame
        @param distance minimum distance in meters
        @param max_distance maximum distance in meters
        @param max_force maximum force in Newtons
        @param max_torque maximum torque in Newton-Meters
        @param ignore_collisions collisions with these objects are ignored when 
        planning the path, e.g. the object you think you will touch
        @param velocity_limit_scale A multiplier to use to scale velocity limits 
        when executing MoveUntilTouch ( < 1 in most cases).           
        @param **kw_args planner parameters
        @return felt_force flag indicating whether we felt a force.
        """
        from contextlib import nested
        from openravepy import CollisionReport, KinBody, Robot, RaveCreateTrajectory
        from prpy.planning.exceptions import CollisionPlanningError

        delta_t = 0.01

        robot = manipulator.GetRobot()
        env = robot.GetEnv()
        dof_indices = manipulator.GetArmIndices()

        direction = numpy.array(direction, dtype='float')

        # Default argument values.
        if max_distance is None:
            max_distance = 1.
            warnings.warn(
                'MoveUntilTouch now requires the "max_distance" argument.'
                ' This will be an error in the future.',
                DeprecationWarning)

        if max_torque is None:
            max_torque = numpy.array([100.0, 100.0, 100.0])

        if ignore_collisions is None:
            ignore_collisions = []

        with env:
            # Compute the expected force direction in the hand frame.
            hand_pose = manipulator.GetEndEffectorTransform()
            force_direction = numpy.dot(hand_pose[0:3, 0:3].T, -direction)

            # Disable the KinBodies listed in ignore_collisions. We backup the
            # "enabled" state of all KinBodies so we can restore them later.
            body_savers = [
                body.CreateKinBodyStateSaver() for body in ignore_collisions]
            robot_saver = robot.CreateRobotStateSaver(
                  Robot.SaveParameters.ActiveDOF
                | Robot.SaveParameters.ActiveManipulator
                | Robot.SaveParameters.LinkTransformation)

            with robot_saver, nested(*body_savers) as f:
                manipulator.SetActive()
                robot_cspec = robot.GetActiveConfigurationSpecification()

                for body in ignore_collisions:
                    body.Enable(False)

                path = robot.PlanToEndEffectorOffset(direction=direction,
                    distance=distance, max_distance=max_distance, **kw_args)

        # Execute on the real robot by tagging the trajectory with options that
        # tell the controller to stop on force/torque input.
        if not manipulator.simulated:
            raise NotImplementedError('MoveUntilTouch not yet implemented under ros_control.')
        # Forward-simulate the motion until it hits an object.
        else:
            traj = robot.PostProcessPath(path)
            is_collision = False

            traj_cspec = traj.GetConfigurationSpecification()
            new_traj = RaveCreateTrajectory(env, '')
            new_traj.Init(traj_cspec)

            robot_saver = robot.CreateRobotStateSaver(
                Robot.SaveParameters.LinkTransformation)
            
            with env, robot_saver:
                for t in numpy.arange(0, traj.GetDuration(), delta_t):
                    waypoint = traj.Sample(t)

                    dof_values = robot_cspec.ExtractJointValues(
                        waypoint, robot, dof_indices, 0)
                    manipulator.SetDOFValues(dof_values)

                    # Terminate if we detect collision with the environment.
                    report = CollisionReport()
                    if env.CheckCollision(robot, report=report):
                        logger.info('Terminated from collision: %s',
                            str(CollisionPlanningError.FromReport(report)))
                        is_collision = True
                        break
                    elif robot.CheckSelfCollision(report=report):
                        logger.info('Terminated from self-collision: %s',
                            str(CollisionPlanningError.FromReport(report)))
                        is_collision = True
                        break

                    # Build the output trajectory that stops in contact.
                    if new_traj.GetNumWaypoints() == 0:
                        traj_cspec.InsertDeltaTime(waypoint, 0.)
                    else:
                        traj_cspec.InsertDeltaTime(waypoint, delta_t)
                    
                    new_traj.Insert(new_traj.GetNumWaypoints(), waypoint)

            if new_traj.GetNumWaypoints() > 0:
                robot.ExecuteTrajectory(new_traj)

            return is_collision
Beispiel #9
0
class WAM(Manipulator):
    def __init__(self, sim, owd_namespace,
                 iktype=openravepy.IkParameterization.Type.Transform6D):
        Manipulator.__init__(self)

        self.simulated = sim
        self._iktype = iktype

        if iktype is not None:
            self._SetupIK(iktype)

        # Enable servo motions in simulation mode.
        self.controller = self.GetRobot().AttachController(name=self.GetName(),
            args='OWDController {0:s} {1:s}'.format('prpy', owd_namespace),
            dof_indices=self.GetArmIndices(), affine_dofs=0, simulated=sim
        )

        if sim:
            from prpy.simulation import ServoSimulator
            self.servo_simulator = ServoSimulator(
                self, rate=20, watchdog_timeout=0.1)

    def CloneBindings(self, parent):
        Manipulator.CloneBindings(self, parent)

        self.simulated = True
        self.controller = None
        self._iktype = parent._iktype

        if self._iktype is not None:
            self._SetupIK(self._iktype)

    def _SetupIK(self, iktype):
        from openravepy.databases.inversekinematics import InverseKinematicsModel

        robot = self.GetRobot()
        self.ikmodel = InverseKinematicsModel(robot=robot, manip=self,
                                              iktype=iktype)
        if not self.ikmodel.load():
            self.ikmodel.generate(iktype=iktype, precision=4,
                                  freeindices=[ self.GetIndices()[2] ])
            self.ikmodel.save()

    def SetStiffness(manipulator, stiffness):
        """Set the WAM's stiffness.
        Stiffness 0 is gravity compensation and stiffness 1 is position
        control. Values between 0 and 1 are experimental.
        @param stiffness value between 0.0 and 1.0
        """
        if not (0 <= stiffness <= 1):
            raise Exception('Stiffness must in the range [0, 1]; got %f.' % stiffness)

        if not manipulator.simulated:
            manipulator.controller.SendCommand('SetStiffness {0:f}'.format(stiffness))

    def SetTrajectoryExecutionOptions(self, traj, stop_on_stall=False,
            stop_on_ft=False, force_magnitude=None, force_direction=None,
            torque=None):
        """Set OWD's trajectory execution options on trajectory.
        @param stop_on_stall aborts the trajectory if the arm stalls
        @param stop_on_ft aborts the trajectory if the force/torque
                          sensor reports a force or torque that exceeds
                          threshold specified by the force_magnitude,
                          force_direction, and torque options
        @param force_magnitude force threshold value, in Newtons
        @param force_direction unit vector in the force/torque coordinate
                               frame, which is int he same orientation as the
                               hand frame.
        @param torque vector of the three torques
        """
        util.SetTrajectoryTags(traj, {
            'owd_options': {
                'stop_on_stall': bool(stop_on_stall),
                'stop_on_ft': bool(stop_on_ft),
                'force_magnitude': float(force_magnitude),
                'force_direction': list(force_direction),
                'torque': list(torque),
        }}, append=True)

    def Servo(manipulator, velocities):
        """Servo with a vector of instantaneous joint velocities.
        @param velocities joint velocities, in radians per second
        """
        num_dof = len(manipulator.GetArmIndices())
        if len(velocities) != num_dof:
            raise ValueError('Incorrect number of joint velocities. '
                             'Expected {0:d}; got {0:d}.'.format(
                             num_dof, len(velocities)))

        if not manipulator.simulated:
            manipulator.controller.SendCommand('Servo ' + ' '.join([ str(qdot) for qdot in velocities ]))
        else:
            manipulator.controller.Reset(0)
            manipulator.servo_simulator.SetVelocity(velocities)

    def ServoTo(manipulator, target, duration, timeStep=0.05, collisionChecking=True):
        """Servo the arm towards a target configuration over some duration.
        Servos the arm towards a target configuration with a constant joint
        velocity. This function uses the \ref Servo command to control the arm
        and must be called repeatidly until the arm reaches the goal. If \tt
        collisionChecking is enabled, then the servo will terminate and return
        False if a collision is detected with the simulation environment.
        @param target desired configuration
        @param duration duration in seconds
        @param timestep period of the control loop, in seconds
        @param collisionchecking check collisions in the simulation environment
        @return whether the servo was successful
        """
        steps = int(math.ceil(duration/timeStep))
        original_dofs = manipulator.GetRobot().GetDOFValues(manipulator.GetArmIndices())
        velocity = numpy.array(target-manipulator.GetRobot().GetDOFValues(manipulator.GetArmIndices()))
        velocities = v/steps#[v/steps for v in velocity]
        inCollision = False 
        if collisionChecking:
            inCollision = manipulator.CollisionCheck(target)

        if not inCollision:
            for i in range(1,steps):
                manipulator.Servo(velocities)
                time.sleep(timeStep)
            manipulator.Servo([0] * len(manipulator.GetArmIndices()))
            new_dofs = manipulator.GetRobot().GetDOFValues(manipulator.GetArmIndices())
            return True
        else:
            return False

    def GetVelocityLimits(self, openrave=True, owd=True):
        """Get the OpenRAVE and OWD joint velocity limits.
        This function checks both the OpenRAVE and OWD joint velocity limits.
        If they do not match, a warning is printed and the minimum value is
        returned.
        @param openrave flag to set the OpenRAVE velocity limits
        @param owd flag to set the OWD velocity limits
        @return list of velocity limits, in radians per second
        """
        # Update the OpenRAVE limits.
        if openrave:
            or_velocity_limits = Manipulator.GetVelocityLimits(self)
            if self.simulated or not owd:
                return or_velocity_limits

        # Update the OWD limits.
        if owd and not self.simulated:
            args  = [ 'GetSpeed' ]
            args_str = ' '.join(args)
            owd_speed_limits_all = self.controller.SendCommand(args_str)
            #if we get nothing back, e.g. if the arm isn't running, return openrave lims
            if owd_speed_limits_all is None:
                return or_velocity_limits

            owd_speed_limits = map(float, owd_speed_limits_all.split(','));
            #first 7 numbers are velocity limits
            owd_velocity_limits = numpy.array(owd_speed_limits[0:len(self.GetIndices())])

            diff_arr = numpy.subtract(or_velocity_limits, owd_velocity_limits)
            max_diff = max(abs(diff_arr))

            if max_diff > 0.01:
                # TODO: Change this to use the logging framework.
                print('GetVelocityLimits Error: openrave and owd limits very different')
                print('\tOpenrave limits:\t' + str(or_velocity_limits))
                print('\tOWD limits:\t\t' + str(owd_velocity_limits))

            return numpy.minimum(or_velocity_limits, owd_velocity_limits)

        return or_velocity_limits
        
    def SetVelocityLimits(self, velocity_limits, min_accel_time,
                          openrave=True, owd=True):
        """Change the OpenRAVE and OWD joint velocity limits.
        Joint velocities that exceed these limits will trigger a velocity fault.
        @param velocity_limits vector of joint velocity limits in radians per second
        @param min_accel_time minimum acceleration time used to compute acceleration limits
        @param openrave flag to set the OpenRAVE velocity limits
        @param owd flag to set the OWD velocity limits
        """
        # Update the OpenRAVE limits.
        if openrave:
            Manipulator.SetVelocityLimits(self, velocity_limits, min_accel_time)

        # Update the OWD limits.
        if owd and not self.simulated:
            args  = [ 'SetSpeed' ]
            args += [ str(min_accel_time) ]
            args += [ str(velocity) for velocity in velocity_limits ]
            args_str = ' '.join(args)
            self.controller.SendCommand(args_str)

    def GetTrajectoryStatus(manipulator):
        """Gets the status of the current (or previous) trajectory executed by OWD.
        @return status of the current (or previous) trajectory executed
        """
        if not manipulator.simulated:
            return manipulator.controller.SendCommand('GetStatus')
        else:
            if manipulator.controller.IsDone():
                return 'done'
            else:
                return 'active'

    def ClearTrajectoryStatus(manipulator):
        """Clears the current trajectory execution status.
        This resets the output of \ref GetTrajectoryStatus.
        """
        if not manipulator.simulated:
            manipulator.controller.SendCommand('ClearStatus')

    def MoveUntilTouch(manipulator, direction, distance, max_distance=float('+inf'),
                       max_force=5.0, max_torque=None, ignore_collisions=None, **kw_args):
        """Execute a straight move-until-touch action.
        This action stops when a sufficient force is is felt or the manipulator
        moves the maximum distance. The motion is considered successful if the
        end-effector moves at least distance. In simulation, a move-until-touch
        action proceeds until the end-effector collids with the environment.
        @param direction unit vector for the direction of motion in the world frame
        @param distance minimum distance in meters
        @param max_distance maximum distance in meters
        @param max_force maximum force in Newtons
        @param max_torque maximum torque in Newton-Meters
        @param execute optionally execute the trajectory
        @param ignore_collisions collisions with these objects are ignored in simulation
        @param **kw_args planner parameters
        @return felt_force flag indicating whether we felt a force.
        """
        # TODO: Is ignore_collisions a list of names or KinBody pointers?
        if max_torque is None:
            max_torque = numpy.array([100.0, 100.0, 100.0 ])
        
        ignore_col_obj_oldstate = []
        if ignore_collisions is None:
            ignore_collisions = []

        for ignore_col_with in ignore_collisions:
            ignore_col_obj_oldstate.append(ignore_col_with.IsEnabled())
            ignore_col_with.Enable(False)

        with manipulator.GetRobot().GetEnv():
            manipulator.GetRobot().GetController().SimulationStep(0)

            # Compute the expected force direction in the hand frame.
            direction = numpy.array(direction)
            hand_pose = manipulator.GetEndEffectorTransform()
            force_direction = numpy.dot(hand_pose[0:3, 0:3].T, -direction)

            with manipulator.GetRobot():
                old_active_manipulator = manipulator.GetRobot().GetActiveManipulator()
                manipulator.SetActive()
                traj = manipulator.PlanToEndEffectorOffset(direction, distance, max_distance=max_distance,
                                                           execute=False, **kw_args)

                collided_with_obj = False
        try:
            if not manipulator.simulated:
                manipulator.SetTrajectoryExecutionOptions(traj, stop_on_ft=True,
                    force_direction=force_direction, force_magnitude=max_force,
                    torque=max_torque)

                manipulator.hand.TareForceTorqueSensor()
                manipulator.GetRobot().ExecutePath(traj)

                for (ignore_col_with, oldstate) in zip(ignore_collisions, ignore_col_obj_oldstate):
                    ignore_col_with.Enable(oldstate)
            else:

                traj_duration = traj.GetDuration()
                delta_t = 0.01

                traj_config_spec = traj.GetConfigurationSpecification()
                traj_angle_group = traj_config_spec.GetGroupFromName('joint_values')
                path_config_spec = openravepy.ConfigurationSpecification()
#                path_config_spec.AddDeltaTimeGroup()
                path_config_spec.AddGroup(traj_angle_group.name, traj_angle_group.dof, '')
#                path_config_spec.AddDerivativeGroups(1, False);
#                path_config_spec.AddDerivativeGroups(2, False);
#                path_config_spec.AddGroup('owd_blend_radius', 1, 'next')
                path_config_spec.ResetGroupOffsets()

                new_traj = openravepy.RaveCreateTrajectory(manipulator.GetRobot().GetEnv(), '')
                new_traj.Init(path_config_spec)

                for (ignore_col_with, oldstate) in zip(ignore_collisions, ignore_col_obj_oldstate):
                    ignore_col_with.Enable(oldstate)
                
                with manipulator.GetRobot():
                    manipulator.SetActive()
                    waypoint_ind = 0
                    for t in numpy.arange(0, traj_duration, delta_t):
                        traj_sample = traj.Sample(t)

                        waypoint = traj_config_spec.ExtractJointValues(traj_sample, manipulator.GetRobot(), manipulator.GetArmIndices())
                        manipulator.SetDOFValues(waypoint)
                        #if manipulator.GetRobot().GetEnv().CheckCollision(manipulator.GetRobot()):
                        for body in manipulator.GetRobot().GetEnv().GetBodies():
                            if manipulator.GetRobot().GetEnv().CheckCollision(manipulator.GetRobot(), body):
                                collided_with_obj = True
                                break
                        if collided_with_obj:
                            break
                        else:
                            #waypoint = numpy.append(waypoint,t)
                            new_traj.Insert(int(waypoint_ind), waypoint, path_config_spec)
                            waypoint_ind += 1

                manipulator.GetRobot().ExecuteTrajectory(new_traj, execute = True, retime=True, blend=True)

            return collided_with_obj
        # Trajectory is aborted by OWD because we felt a force.
        except exceptions.TrajectoryAborted:
            return True
Beispiel #10
0
def initialize(attach_viewer=False, sim=True, user_id='human', env=None):
    """Initialize the Human Robot"""
    
    prpy.logger.initialize_logging()
    
    if not env:
        env = Environment()

    #Setup Manipulators
    with env:
        robot = env.ReadKinBodyXMLFile('robots/man1.dae')
        robot.SetName(user_id)              #needed in order to have different humans in the same env
        env.AddKinBody(robot) 

        robot.left_arm = robot.GetManipulator('leftarm')
        robot.left_arm.hand = robot.left_arm.GetEndEffector()
        robot.left_hand = robot.left_arm.hand

        robot.right_arm = robot.GetManipulator('rightarm')
        robot.right_arm.hand = robot.right_arm.GetEndEffector()
        robot.right_hand = robot.right_arm.hand

        bind_subclass(robot, Robot, robot_name=user_id)
        bind_subclass(robot.left_arm, Manipulator)
        bind_subclass(robot.right_arm, Manipulator)
    
        bind_subclass(robot.left_arm.hand, HumanHand, manipulator=robot.left_arm, sim=True)
        bind_subclass(robot.right_arm.hand, HumanHand, manipulator=robot.right_arm, sim=True)
        
        #Setup Controller
        args = 'IdealController'         
        affine_dofs=0
        
        delegate_controllerr = RaveCreateController(env, args)
        if delegate_controllerr is None:
            message = 'Creating controller {0:s} of type {1:s} failed.'.format(robot.right_arm.GetName(), args)
            raise openrave_exception(message)
        robot.multicontroller.AttachController(delegate_controllerr, robot.right_arm.GetArmIndices(), affine_dofs)
        
        delegate_controllerl = RaveCreateController(env, args)
        if delegate_controllerl is None:
            message = 'Creating controller {0:s} of type {1:s} failed.'.format(robot.left_arm.GetName(), args)
            raise openrave_exception(message)
        robot.multicontroller.AttachController(delegate_controllerl, robot.left_arm.GetArmIndices(), affine_dofs)

        #Setup IK
        ikmodel_left = InverseKinematicsModel(
                robot,
                iktype=IkParameterizationType.Transform6D,
                manip=robot.left_arm)
        if not ikmodel_left.load():
            ikmodel_left.autogenerate()

        ikmodel_right = InverseKinematicsModel(
                robot,
                iktype=IkParameterizationType.Transform6D,
                manip=robot.right_arm)
        
        if not ikmodel_right.load():            
            ikmodel_right.autogenerate()

        #Setup planning pipeline
        robot.planner = Sequence(
                SnapPlanner(),
                VectorFieldPlanner(),
                CBiRRTPlanner())
        robot.simplifier = None
        robot.retimer = HauserParabolicSmoother() # hack
        robot.smoother = HauserParabolicSmoother()
        
        robot.actions = prpy.action.ActionLibrary() 

    #Setup viewer. Default to load rviz
    if env.GetViewer() is None:
        if attach_viewer == True:
            attach_viewer = 'rviz'
            env.SetViewer(attach_viewer)

            #Fallback on qtcoin if rviz couldnt load
            if env.GetViewer is None:
                logger.warning('Loading the Rviz viewer failed. Falling back on qtcoin')
                attach_viewer = 'qtcoin'

        if attach_viewer and env.GetViewer() is None:
            env.SetViewer(attach_viewer)
            if env.GetViewer() is None:
                raise Exception('Failed creating viewer of type "{0:s}"'.format(
                attach_viewer))

    #Remove ROS Logging since loading Rviz might have added it
    prpy.logger.remove_ros_logger()
        
    #changing orientation
    with env:
        robotLocation = numpy.array([[ -1. ,  0. ,  0. ,   0.],
                                    [ 0. ,  0. ,  1. ,  0.],
                                    [ 0. ,  1. ,  0. ,   0.],
                                    [ 0. ,  0. ,  0. ,   1. ]])
        robot.SetTransform(robotLocation)

    return env, robot