Esempio n. 1
0
class MoveitPlanner:

    def __init__(self, manipulator_side = 'left'):

        # Enable the robot at start and disable at the end
        BaxterRobot().EnableBaxterRobot()
        rospy.on_shutdown(BaxterRobot().DisableBaxterRobot)

        # Test Open Both Grippers and Close Them 
        # baxter_manipulator = BaxterManipulatorAndGripper()
        
        # baxter_manipulator.SetupBaxterManipualtor()
        # baxter_manipulator.MoveBaxterManipulatorToNeutralPose()

        # baxter_manipulator.SetupBaxterGrippers()
        # baxter_manipulator.OpenGripper('left')
        # baxter_manipulator.OpenGripper('right')
        # rospy.sleep(3.0)
        # baxter_manipulator.CloseGripper('left')
        # baxter_manipulator.CloseGripper('right')

        # Referenceing baxter arm
        self.__manipulator_name = manipulator_side
        self.limb = MoveGroupCommander("{}_arm".format(manipulator_side))
        self.limb.set_end_effector_link("{}_gripper".format(manipulator_side))

        # Baxter Model configuration with MoveIt not configured to control its gripper,
        # thus using Baxter SDK API gripper class
        baxter_manipulator = BaxterManipulatorAndGripper()
        baxter_manipulator.SetupBaxterChosenGripperAndManipulator(manipulator_side)

        self.limb.set_planner_id("RRTConnectKConfigDefualt")
        self.limb.set_goal_position_tolerance(0.01)
        self.limb.set_goal_orientation_tolerance(0.01)
Esempio n. 2
0
class GenericDualArmClient():
    _safe_kinematicsolver = "RRTConnectkConfigDefault"

    def __init__(self, *args, **kwargs):
        larm_name = args[0]
        rarm_name = args[1]  # "arm_right" for Motoman SDA10F
        self.r_arm = MoveGroupCommander(rarm_name)
        self.r_arm.set_planner_id(self._safe_kinematicsolver)

    def move_rarm_shift_forward(self, axis, value):
        '''
        Ref. http://docs.ros.org/indigo/api/moveit_commander/html/classmoveit__commander_1_1move__group_1_1MoveGroupCommander.html#a22f9ec1477c3d61e728660850d50ce1f
        '''
        self.r_arm.shift_pose_target(axis, value)
        self.r_arm.plan()
        self.r_arm.go()

    def move_rarm_fixedpose_forward(self):
        tpose = Pose()
        #TODO Currently the following position may work with SDA10F but not with NXO
        tpose.position.x = 0.599
        tpose.position.y = -0.379
        tpose.position.z = 1.11
        self.r_arm.set_pose_target(tpose)
        self.r_arm.plan()
        self.r_arm.go()
Esempio n. 3
0
def callback(states):
    scene = PlanningSceneInterface()
    robot = RobotCommander()
    arm = MoveGroupCommander("manipulator")
    arm.set_planner_id("RRTConnectkConfigDefault")
    arm.set_num_planning_attempts(20)
    arm.allow_replanning(True)
    pose = copy.deepcopy(states.pose[-1])
    temp = tf.transformations.euler_from_quaternion(
        (pose.orientation.x, pose.orientation.y, pose.orientation.z,
         pose.orientation.w))
    quaternion = tf.transformations.quaternion_from_euler(
        math.pi, temp[1], temp[2])
    pose.position.z += 0.22
    pose.orientation.x = quaternion[0]
    pose.orientation.y = quaternion[1]
    pose.orientation.z = quaternion[2]
    pose.orientation.w = quaternion[3]
    print pose
    arm.set_pose_target(pose)
    move_plan = arm.plan()
    i = 0
    while (not move_plan.joint_trajectory.joint_names):
        move_plan = arm.plan()
        i += 1
        if (i == 5): break
    state = arm.execute(move_plan)
Esempio n. 4
0
class MoveItArm:
    """
    Represents Baxter's Arm in MoveIt! world.

    Represents a hand of  Baxter through MoveIt!  Group. Some reusable code has
    been  packed  together  into  this  class  to make  the code more readable.
    """
    def __init__(self, side_name):
        """Will configure and initialise Baxter's hand."""
        self._side_name = side_name

        # This is the reference to Baxter's arm.
        self.limb = MoveGroupCommander("{}_arm".format(side_name))
        self.limb.set_end_effector_link("{}_gripper".format(side_name))

        # Unfortunetly, MoveIt was not able to make the gripper work, neither
        # did was able to find a very good pose using Forward Kinematics,
        # so instead the Baxter SDK is used here to do these two jobs.
        self.gripper = Gripper(side_name, CHECK_VERSION)
        self.gripper.calibrate()
        self._limb = Limb(side_name)

        # This solver seems to be better for finding solution among obstacles
        self.limb.set_planner_id("RRTConnectkConfigDefault")

        # Error tollerance should be as low as possible for better accuracy.
        self.limb.set_goal_position_tolerance(0.01)
        self.limb.set_goal_orientation_tolerance(0.01)

    def __str__(self):
        """
        String representation of the arm.

        String representation of the arm,  either 'left' or 'right' string will
        be returned.  Useful  when the  string  representation  of the  arm  is
        needed, like when accessing the IK solver.
        """
        return self._side_name

    def is_left(self):
        """Will return True if this is the left arm, false otherwise."""
        return True if self._side_name == "left" else False

    def is_right(self):
        """Will return True if this is the right arm, false otherwise."""
        return True if self._side_name == "right" else False

    def open_gripper(self):
        """Will open Baxter's gripper on his active hand."""
        # Block ensures that the function does not return until the operation
        # is completed.
        self.gripper.open(block=True)

    def close_gripper(self):
        """Will close Baxter's gripper on his active hand."""
        # Block ensures that the function does not return until the operation
        # is completed.
        self.gripper.close(block=True)
def get_move_group_commander(group):
    '''
    Gets the move_group_commander for this process.

    '''
    global _mgc_dict
    with _mgc_dict_creation_lock:
        if not group in _mgc_dict:
            _mgc_group = MoveGroupCommander(group)
            _mgc_group.set_planner_id('RRTConnectkConfigDefault')
            _mgc_dict[group] = _mgc_group
            add_ground()
        return _mgc_dict[group]
def get_move_group_commander(group):
    '''
    Gets the move_group_commander for this process.

    '''
    global _mgc_dict
    with _mgc_dict_creation_lock:
        if not group in _mgc_dict:
            _mgc_group = MoveGroupCommander(group)
            _mgc_group.set_planner_id('RRTConnectkConfigDefault')
            _mgc_dict[group] = _mgc_group
	    add_ground()
        return _mgc_dict[group]
Esempio n. 7
0
    def setUpClass(self):

        rospy.init_node('test_moveit')
        rospy.sleep(5)  # intentinally wait for starting up hrpsys

        self.robot = RobotCommander()

        # TODO: Read groups from SRDF file ideally.

        for mg_attr in self._MOVEGROUP_ATTRS:
            mg = MoveGroupCommander(mg_attr[0])
            # Temporary workaround of planner's issue similar to https://github.com/tork-a/rtmros_nextage/issues/170
            mg.set_planner_id(self._KINEMATICSOLVER_SAFE)
            # Append MoveGroup instance to the MoveGroup attribute list.
            mg_attr.append(mg)
Esempio n. 8
0
class MoveitWorkshopExercise(object):
    def __init__(self):
        rospy.init_node('commander_example', anonymous=True)
        self.group_l = MoveGroupCommander("left_arm")
        self.group_r = MoveGroupCommander("right_arm")
        self.group_l.set_planner_id('RRTConnectkConfigDefault')
        self.group_r.set_planner_id('RRTConnectkConfigDefault')

    def exercise1(self):
        pass

    def exercise2(self):
        pass

    def exercise3(self):
        pass
Esempio n. 9
0
def main():
    rospy.init_node('arm_start_up', anonymous=True)
    scene = PlanningSceneInterface()
    robot = RobotCommander()
    right_arm = MoveGroupCommander("right_arm")
    left_arm = MoveGroupCommander("left_arm")
    right_arm.set_planner_id("RRTConnect:"); 
    left_arm.set_planner_id("RRTConnect:")
    
  
    # move to a random target
    #group.set_named_target("ahead")
    #group.go()
    #rospy.sleep(1)
    right_arm.set_named_target("right_start")
    right_arm.go()
    rospy.sleep(1)
    left_arm.set_named_target("left_start")
    left_arm.go()
    rospy.sleep(1)
class CalibrationMovements:
    def __init__(self,
                 move_group_name,
                 max_velocity_scaling=0.5,
                 max_acceleration_scaling=0.5):
        #self.client = HandeyeClient()  # TODO: move around marker when eye_on_hand, automatically take samples via trigger topic
        self.mgc = MoveGroupCommander(move_group_name)
        self.mgc.set_planner_id("RRTConnectkConfigDefault")
        self.mgc.set_max_velocity_scaling_factor(max_velocity_scaling)
        self.mgc.set_max_acceleration_scaling_factor(max_acceleration_scaling)
        self.start_pose = self.mgc.get_current_pose()
        self.poses = []
        self.current_pose_index = -1
        self.fallback_joint_limits = [math.radians(90)] * 4 + [
            math.radians(90)
        ] + [math.radians(180)] + [math.radians(350)]
        if len(self.mgc.get_active_joints()) == 6:
            self.fallback_joint_limits = self.fallback_joint_limits[1:]

    def compute_poses_around_current_state(self, angle_delta,
                                           translation_delta):
        self.start_pose = self.mgc.get_current_pose()
        basis = np.eye(3)

        pos_deltas = [
            quaternion_from_euler(*rot_axis * angle_delta)
            for rot_axis in basis
        ]
        neg_deltas = [
            quaternion_from_euler(*rot_axis * (-angle_delta))
            for rot_axis in basis
        ]

        quaternion_deltas = list(
            chain.from_iterable(izip(pos_deltas, neg_deltas)))  # interleave

        final_rots = []
        for qd in quaternion_deltas:
            final_rots.append(list(qd))

        # TODO: clean up

        pos_deltas = [
            quaternion_from_euler(*rot_axis * angle_delta / 2)
            for rot_axis in basis
        ]
        neg_deltas = [
            quaternion_from_euler(*rot_axis * (-angle_delta / 2))
            for rot_axis in basis
        ]

        quaternion_deltas = list(
            chain.from_iterable(izip(pos_deltas, neg_deltas)))  # interleave
        for qd in quaternion_deltas:
            final_rots.append(list(qd))

        final_poses = []
        for rot in final_rots:
            fp = deepcopy(self.start_pose)
            ori = fp.pose.orientation
            combined_rot = quaternion_multiply([ori.x, ori.y, ori.z, ori.w],
                                               rot)
            fp.pose.orientation = Quaternion(*combined_rot)
            final_poses.append(fp)

        fp = deepcopy(self.start_pose)
        fp.pose.position.x += translation_delta / 2
        final_poses.append(fp)

        fp = deepcopy(self.start_pose)
        fp.pose.position.x -= translation_delta / 2
        final_poses.append(fp)

        fp = deepcopy(self.start_pose)
        fp.pose.position.y += translation_delta
        final_poses.append(fp)

        fp = deepcopy(self.start_pose)
        fp.pose.position.y -= translation_delta
        final_poses.append(fp)

        fp = deepcopy(self.start_pose)
        fp.pose.position.z += translation_delta / 3
        final_poses.append(fp)

        self.poses = final_poses
        self.current_pose_index = -1

    def check_poses(self, joint_limits):
        if len(self.fallback_joint_limits) == 6:
            joint_limits = joint_limits[1:]
        for fp in self.poses:
            self.mgc.set_pose_target(fp)
            plan = self.mgc.plan()
            if len(plan.joint_trajectory.points
                   ) == 0 or CalibrationMovements.is_crazy_plan(
                       plan, joint_limits):
                return False
        return True

    def plan_to_start_pose(self):
        return self.plan_to_pose(self.start_pose)

    def plan_to_pose(self, pose):
        self.mgc.set_start_state_to_current_state()
        self.mgc.set_pose_target(pose)
        plan = self.mgc.plan()
        return plan

    def execute_plan(self, plan):
        if CalibrationMovements.is_crazy_plan(plan,
                                              self.fallback_joint_limits):
            raise RuntimeError("got crazy plan!")
        self.mgc.execute(plan)

    @staticmethod
    def rot_per_joint(plan, degrees=False):
        np_traj = np.array([p.positions for p in plan.joint_trajectory.points])
        if len(np_traj) == 0:
            raise ValueError
        np_traj_max_per_joint = np_traj.max(axis=0)
        np_traj_min_per_joint = np_traj.min(axis=0)
        ret = abs(np_traj_max_per_joint - np_traj_min_per_joint)
        if degrees:
            ret = [math.degrees(j) for j in ret]
        return ret

    @staticmethod
    def is_crazy_plan(plan, max_rotation_per_joint):
        abs_rot_per_joint = CalibrationMovements.rot_per_joint(plan)
        if (abs_rot_per_joint > max_rotation_per_joint).any():
            return True
        else:
            return False
Esempio n. 11
0
#!/usr/bin/env python

import rospy
import geometry_msgs.msg

from moveit_commander import MoveGroupCommander

if __name__ == '__main__':
    rospy.init_node('commander_example', anonymous=True)
    group = MoveGroupCommander("right_arm")
    group.set_planner_id('RRTConnectkConfigDefault')

    pose_rarm_curr = group.get_current_pose('RARM_JOINT5_Link')
    rospy.loginfo('Current pose: {}'.format(pose_rarm_curr))

    # right hand move to approx. 2cm ahead
    pose_target = geometry_msgs.msg.Pose()
    pose_target.position.x = 0.392
    pose_target.position.y = -0.182
    pose_target.position.z = 0.0748
    pose_target.orientation.y = -0.708
    pose_target.orientation.w = 0.706
    rospy.loginfo("set target to {}".format(pose_target))
    group.set_pose_target(pose_target)
    plan = group.plan()
    rospy.loginfo("plan is {}".format(plan))
    ret = group.go()
    rospy.loginfo("Command ran ... {}".format(ret))

    pose_rarm_curr = group.get_current_pose('RARM_JOINT5_Link')
    rospy.loginfo('Pose after: {}'.format(pose_rarm_curr))
Esempio n. 12
0
class TestMoveit(unittest.TestCase):
    _MOVEGROUP_MAIN = 'manipulator'
    _KINEMATICSOLVER_SAFE = 'RRTConnectkConfigDefault'

    @classmethod
    def setUpClass(self):
        rospy.init_node('test_moveit_vs060')
        self.robot = RobotCommander()
        self._mvgroup = MoveGroupCommander(self._MOVEGROUP_MAIN)
        # Temporary workaround of planner's issue similar to https://github.com/tork-a/rtmros_nextage/issues/170
        self._mvgroup.set_planner_id(self._KINEMATICSOLVER_SAFE)

        self._movegroups = sorted(['manipulator', 'manipulator_flange'])
        self._joints_movegroup_main = sorted(
            ['j1', 'j2', 'j3', 'j4', 'j5', 'flange'])

    @classmethod
    def tearDownClass(self):
        True  # TODO impl something meaningful

    def _set_sample_pose(self):
        '''
        @return: Pose() with some values populated in.
        '''
        pose_target = Pose()
        pose_target.orientation.x = 0.00
        pose_target.orientation.y = 0.00
        pose_target.orientation.z = -0.20
        pose_target.orientation.w = 0.98
        pose_target.position.x = 0.18
        pose_target.position.y = 0.18
        pose_target.position.z = 0.94
        return pose_target

    def _plan(self):
        '''
        Run `clear_pose_targets` at the beginning.
        @rtype: RobotTrajectory http://docs.ros.org/api/moveit_msgs/html/msg/RobotTrajectory.html
        '''
        self._mvgroup.clear_pose_targets()

        pose_target = self._set_sample_pose()
        self._mvgroup.set_pose_target(pose_target)
        plan = self._mvgroup.plan()
        rospy.loginfo('  plan: '.format(plan))
        return plan

    def test_list_movegroups(self):
        '''Check if the defined move groups are loaded.'''
        self.assertEqual(self._movegroups,
                         sorted(self.robot.get_group_names()))

    def test_list_activejoints(self):
        '''Check if the defined joints in a move group are loaded.'''
        self.assertEqual(self._joints_movegroup_main,
                         sorted(self._mvgroup.get_active_joints()))

    def test_plan(self):
        '''Evaluate plan (RobotTrajectory)'''
        plan = self._plan()
        # TODO Better way to check the plan is valid.
        # Currently the following checks if the number of waypoints is not zero,
        # which (hopefully) indicates that a path is computed.
        self.assertNotEqual(0, plan.joint_trajectory.points)

    def test_planandexecute(self):
        '''
        Evaluate Plan and Execute works.
        Currently no value checking is done (, which is better to be implemented)
        '''
        self._plan()
        # TODO Better way to check the plan is valid.
        # The following checks if plan execution was successful or not.
        self.assertTrue(self._mvgroup.go())

    def test_set_pose_target(self):
        '''
        Check for simple planning, originally testd in moved from denso_vs060_moveit_demo_test.py
        '''
        self._plan()
        p = [0.1, -0.35, 0.4]
        pose = PoseStamped(
            header=rospy.Header(stamp=rospy.Time.now(), frame_id='/BASE'),
            pose=Pose(position=Point(*p),
                      orientation=Quaternion(
                          *quaternion_from_euler(1.57, 0, 1.57, 'sxyz'))))
        self._mvgroup.set_pose_target(pose)
        self.assertTrue(self._mvgroup.go() or self._mvgroup.go())

    def test_planning_scene(self):
        '''
        Check if publish_simple_scene.py is working
        '''
        rospy.wait_for_service('/get_planning_scene', timeout=20)
        get_planning_scene = rospy.ServiceProxy("/get_planning_scene",
                                                GetPlanningScene)
        collision_objects = []
        # wait for 10 sec
        time_start = rospy.Time.now()
        while not collision_objects and (rospy.Time.now() -
                                         time_start).to_sec() < 10.0:
            world = get_planning_scene(
                PlanningSceneComponents(components=PlanningSceneComponents.
                                        WORLD_OBJECT_NAMES)).scene.world
            collision_objects = world.collision_objects
            rospy.loginfo("collision_objects = " +
                          str(world.collision_objects))
            rospy.sleep(1)
        self.assertTrue(world.collision_objects != [], world)
Esempio n. 13
0
class RobotArm:
    """
    A class used to represent the arm of a robot
    using both "Moveit" API and Baxter SDK API.
    """
    def __init__(self, side_name):
        """
        Constructor.

        @param side_name: which arm side we are refering to 'left' or 'right'.
        """

        self.__side_name = side_name

        # MoveIt interface to a group of arm joints.
        # Either left arm joint group or right arm joint group.
        self.moveit_limb = MoveGroupCommander('{}_arm'.format(side_name))

        # MoveIt limb setting.
        self.moveit_limb.set_end_effector_link('{}_gripper'.format(side_name))
        self.moveit_limb.set_planner_id('RRTConnectKConfigDefault')
        self.moveit_limb.set_goal_position_tolerance(0.01)
        self.moveit_limb.set_goal_orientation_tolerance(0.01)

        # MoveIt does not provide support for Baxter gripper.
        # Thus we use Baxter SDK gripper instead.
        self.gripper = Gripper(side_name, CHECK_VERSION)
        self.gripper.calibrate()

        self.baxter_sdk_limb = Limb(side_name)

    def __str__(self):
        """
        Built-in function to return a printable string
        representing the arm used in either print(object)
        or str(object).
        """
        return str(self.__side_name)

    def open_gripper(self):
        """
        Command the robot arm end-effector i.e. the gripper to open.
        """
        self.gripper.open(block=True)

    def close_gripper(self):
        """
        Command the robot arm end-effector i.e. the gripper to close.
        """
        self.gripper.close(block=True)

    def is_left_arm(self):
        """
        Return true if the arm is corresponding to the left arm.
        """
        return self.__side_name == 'left'

    def is_right_arm(self):
        """
        Return true if the arm is corresponding to the right arm.
        """
        return self.__side_name == 'right'
Esempio n. 14
0
class MoveIt(object):
    def __init__(self):
        moveit_commander.roscpp_initialize(sys.argv)
        self.scene = PlanningSceneInterface()
        self.add_table()
        # self.clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty)

        self.arm = MoveGroupCommander("arm")
        # self.arm.set_goal_joint_tolerance(0.1)
        self.gripper = MoveGroupCommander("gripper")

        # already default
        self.arm.set_planner_id("RRTConnectkConfigDefault")

        self.end_effector_link = self.arm.get_end_effector_link()

        self.arm.allow_replanning(True)
        self.arm.set_planning_time(5)

        self.transformer = tf.TransformListener()

        rospy.sleep(2)  # allow some time for initialization of moveit

    def __del__(self):
        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)

    def _open_gripper(self):
        joint_trajectory = JointTrajectory()
        joint_trajectory.header.stamp = rospy.get_rostime()
        joint_trajectory.joint_names = [
            "m1n6s200_joint_finger_1", "m1n6s200_joint_finger_2"
        ]

        joint_trajectory_point = JointTrajectoryPoint()
        joint_trajectory_point.positions = [0, 0]
        joint_trajectory_point.time_from_start = rospy.Duration(5.0)

        joint_trajectory.points.append(joint_trajectory_point)
        return joint_trajectory

    def _set_gripper_width(self, width):
        joint_trajectory = JointTrajectory()
        joint_trajectory.header.stamp = rospy.get_rostime()
        joint_trajectory.joint_names = [
            "m1n6s200_joint_finger_1", "m1n6s200_joint_finger_2"
        ]

        joint_trajectory_point = JointTrajectoryPoint()
        joint_trajectory_point.positions = [0, 0]
        joint_trajectory_point.time_from_start = rospy.Duration(5.0)

        joint_trajectory.points.append(joint_trajectory_point)
        return joint_trajectory

    def _close_gripper(self):
        joint_trajectory = JointTrajectory()
        joint_trajectory.header.stamp = rospy.get_rostime()
        joint_trajectory.joint_names = [
            "m1n6s200_joint_finger_1", "m1n6s200_joint_finger_2"
        ]

        joint_trajectory_point = JointTrajectoryPoint()
        joint_trajectory_point.positions = [1.2, 1.2]
        joint_trajectory_point.time_from_start = rospy.Duration(5.0)

        joint_trajectory.points.append(joint_trajectory_point)
        return joint_trajectory

    # Template function for creating the Grasps
    def _create_grasp(self, x, y, z, roll, pitch, yaw):
        grasp = Grasp()

        # pre_grasp
        grasp.pre_grasp_posture = self._open_gripper()
        grasp.pre_grasp_approach.direction.header.frame_id = self.end_effector_link
        grasp.pre_grasp_approach.direction.vector.z = 1.0
        grasp.pre_grasp_approach.direction.vector.y = 0.0
        grasp.pre_grasp_approach.direction.vector.x = 0.0
        grasp.pre_grasp_approach.min_distance = 0.05
        grasp.pre_grasp_approach.desired_distance = 0.1

        # grasp
        grasp.grasp_posture = self._close_gripper()
        grasp.grasp_pose.pose.position.x = x
        grasp.grasp_pose.pose.position.y = y
        grasp.grasp_pose.pose.position.z = z
        q = quaternion_from_euler(roll, pitch, yaw)
        grasp.grasp_pose.pose.orientation.x = q[0]
        grasp.grasp_pose.pose.orientation.y = q[1]
        grasp.grasp_pose.pose.orientation.z = q[2]
        grasp.grasp_pose.pose.orientation.w = q[3]
        grasp.grasp_pose.header.frame_id = "m1n6s200_link_base"

        # post_grasp
        grasp.post_grasp_retreat.direction.header.frame_id = self.end_effector_link
        grasp.post_grasp_retreat.direction.vector.z = -1.0
        grasp.post_grasp_retreat.direction.vector.x = 0.0
        grasp.post_grasp_retreat.direction.vector.y = 0.0
        grasp.post_grasp_retreat.min_distance = 0.05
        grasp.post_grasp_retreat.desired_distance = 0.25

        return [grasp]

    # Template function, you can add parameters if needed!
    def grasp(self, x, y, z, roll, pitch, yaw, width):

        self.add_object('object', [0.37, -0.24, 0.1, math.pi, 0., 0.],
                        [0.1, 0.1, 0.1])

        grasps = self._create_grasp(x, y, z, roll, pitch, yaw)
        result = self.arm.pick('object', grasps)
        self.remove_object()

        if result == MoveItErrorCodes.SUCCESS:
            print 'Success grasp'
            return True
        else:
            print 'Failed grasp'
            return False

    def open_fingers(self):
        self.gripper.set_joint_value_target([0.0, 0.0])
        self.gripper.go(wait=True)
        rospy.sleep(2.0)

    def close_fingers(self):
        self.gripper.set_joint_value_target([1.3, 1.3])
        self.gripper.go(wait=True)
        rospy.sleep(2.0)

    def move_to(self,
                x,
                y,
                z,
                roll,
                pitch,
                yaw,
                frame_id="m1n6s200_link_base"):
        q = quaternion_from_euler(roll, pitch, yaw)
        pose = PoseStamped()
        pose.header.frame_id = frame_id
        pose.pose.position.x = x
        pose.pose.position.y = y
        pose.pose.position.z = z
        pose.pose.orientation.x = q[0]
        pose.pose.orientation.y = q[1]
        pose.pose.orientation.z = q[2]
        pose.pose.orientation.w = q[3]

        self.arm.set_pose_target(pose, self.end_effector_link)
        plan = self.arm.plan()
        success = True
        success = self.arm.go(wait=True)
        self.arm.stop()
        self.arm.clear_pose_targets()
        return success

    def remove_object(self, object='object'):
        self.scene.remove_attached_object(self.end_effector_link, object)
        self.scene.remove_world_object(object)
        rospy.loginfo("Object removed")

    def add_object(self, name, pose, size):
        object_pose = PoseStamped()
        object_pose.header.frame_id = "m1n6s200_link_base"
        object_pose.pose.position.x = pose[0]
        object_pose.pose.position.y = pose[1]
        object_pose.pose.position.z = pose[2]
        q = quaternion_from_euler(*pose[3:])
        object_pose.pose.orientation.x = q[0]
        object_pose.pose.orientation.y = q[1]
        object_pose.pose.orientation.z = q[2]
        object_pose.pose.orientation.w = q[3]
        self.scene.add_box(name, object_pose, size)

    def add_table(self):
        self.add_object('table', [0, 0, -0.005, 0, 0, 0], (2, 2, 0.01))
Esempio n. 15
0
class MoveIt(object):
    def __init__(self):
        moveit_commander.roscpp_initialize(sys.argv)
        self.scene = PlanningSceneInterface()
        self.clear_octomap = rospy.ServiceProxy("/clear_octomap", Empty)

        self.arm = MoveGroupCommander("arm")
        # self.arm.set_goal_joint_tolerance(0.1)
        self.gripper = MoveGroupCommander("gripper")

        # already default
        self.arm.set_planner_id("RRTConnectkConfigDefault")

        self.end_effector_link = self.arm.get_end_effector_link()

        self.arm.allow_replanning(True)
        self.arm.set_planning_time(5)

        self.transformer = tf.TransformListener()

        rospy.sleep(2)  # allow some time for initialization of moveit

    def __del__(self):
        moveit_commander.roscpp_shutdown()
        moveit_commander.os._exit(0)

    def _open_gripper(self):
        joint_trajectory = JointTrajectory()
        joint_trajectory.header.stamp = rospy.get_rostime()
        joint_trajectory.joint_names = [
            "m1n6s200_joint_finger_1", "m1n6s200_joint_finger_2"
        ]

        joint_trajectory_point = JointTrajectoryPoint()
        joint_trajectory_point.positions = [0, 0]
        joint_trajectory_point.time_from_start = rospy.Duration(5.0)

        joint_trajectory.points.append(joint_trajectory_point)
        return joint_trajectory

    def _close_gripper(self):
        joint_trajectory = JointTrajectory()
        joint_trajectory.header.stamp = rospy.get_rostime()
        joint_trajectory.joint_names = [
            "m1n6s200_joint_finger_1", "m1n6s200_joint_finger_2"
        ]

        joint_trajectory_point = JointTrajectoryPoint()
        joint_trajectory_point.positions = [1.2, 1.2]
        joint_trajectory_point.time_from_start = rospy.Duration(5.0)

        joint_trajectory.points.append(joint_trajectory_point)
        return joint_trajectory

    # Template function for creating the Grasps
    def _create_grasps(self, x, y, z, z_max, rotation):
        grasps = []

        # You can create multiple grasps and add them to the grasps list
        grasp = Grasp()  # create a new grasp

        # Set the pre grasp posture (the fingers)
        grasp.pre_grasp_posture = self._open_gripper()
        # Set the grasp posture (the fingers)
        grasp.grasp_posture = self._close_gripper()
        # Set the position of where to grasp
        grasp.grasp_pose.pose.position.x = x
        grasp.grasp_pose.pose.position.y = y
        grasp.grasp_pose.pose.position.z = z
        # Set the orientation of the end effector
        q = quaternion_from_euler(math.pi, 0.0, rotation)
        grasp.grasp_pose.pose.orientation.x = q[0]
        grasp.grasp_pose.pose.orientation.y = q[1]
        grasp.grasp_pose.pose.orientation.z = q[2]
        grasp.grasp_pose.pose.orientation.w = q[3]
        grasp.grasp_pose.header.frame_id = "m1n6s200_link_base"
        # Set the pre_grasp_approach
        grasp.pre_grasp_approach.direction.header.frame_id = self.end_effector_link
        grasp.pre_grasp_approach.direction.vector.z = 1.0
        grasp.pre_grasp_approach.direction.vector.y = 0.0
        grasp.pre_grasp_approach.direction.vector.x = 0.0
        grasp.pre_grasp_approach.min_distance = 0.05
        grasp.pre_grasp_approach.desired_distance = 0.1
        # # Set the post_grasp_approach
        grasp.post_grasp_retreat.direction.header.frame_id = self.end_effector_link
        grasp.post_grasp_retreat.direction.vector.z = -1.0
        grasp.post_grasp_retreat.direction.vector.x = 0.0
        grasp.post_grasp_retreat.direction.vector.y = 0.0
        grasp.post_grasp_retreat.min_distance = 0.05
        grasp.post_grasp_retreat.desired_distance = 0.25

        grasp.grasp_pose.header.frame_id = "m1n6s200_link_base"  # setting the planning frame (Positive x is to the left, negative Y is to the front of the arm)

        grasps.append(
            grasp
        )  # add all your grasps in the grasps list, MoveIT will pick the best one

        for z_offset in np.arange(z + 0.02, z_max, 0.01):
            new_grasp = copy.deepcopy(grasp)
            new_grasp.grasp_pose.pose.position.z = z_offset
            grasps.append(new_grasp)
        return grasps

    # Template function, you can add parameters if needed!
    def grasp(self, x, y, z, z_max, rotation, size):
        print '******************* grasp'
        # Object distance:
        obj_dist = np.linalg.norm(np.asarray((x, y, z)))
        if obj_dist > 0.5:
            rospy.loginfo(
                "Object too far appart ({} m), skipping pick".format(obj_dist))
            return False

        # Add collision object, easiest to name the object, "object"
        object_pose = PoseStamped()
        object_pose.header.frame_id = "m1n6s200_link_base"
        object_pose.pose.position.x = x
        object_pose.pose.position.y = y
        object_pose.pose.position.z = z
        q = quaternion_from_euler(math.pi, 0.0, rotation)
        object_pose.pose.orientation.x = q[0]
        object_pose.pose.orientation.y = q[1]
        object_pose.pose.orientation.z = q[2]
        object_pose.pose.orientation.w = q[3]

        self.scene.add_box("object", object_pose, size)

        rospy.sleep(0.5)
        self.clear_octomap()
        rospy.sleep(1.0)

        # Create and return grasps
        # z += size[2]/2  # Focus on the top of the object only
        # z += size[2]/2 + 0.02  # Focus on the top of the object only

        grasps = self._create_grasps(x, y, z, z_max, rotation)
        print '******************************************************************************'
        result = self.arm.pick(
            'object', grasps)  # Perform pick on "object", returns result
        print '******************************************************************************'
        # self.move_to(x, y, z + 0.15, rotation)

        if result == MoveItErrorCodes.SUCCESS:
            print 'Success grasp'
            return True
        else:
            print 'Failed grasp'
            return False

    def clear_object(self, x, y, z, z_max, rotation, size):
        print '******************* clear_object'

        self.move_to_waypoint()
        success = self.grasp(x, y, z, z_max, rotation, size)

        if success:
            self.move_to_waypoint()
            success = self.move_to_drop_zone()
            if success:
                print 'success move to drop zone'
            else:
                print 'failed move to drop zone'

        self.open_fingers()
        self.remove_object()
        rospy.sleep(1.0)
        self.close_fingers()

        return success

    def open_fingers(self):
        print '******************* open_fingers'
        self.gripper.set_joint_value_target([0.0, 0.0])
        self.gripper.go(wait=True)
        rospy.sleep(2.0)

    def close_fingers(self):
        print '******************* close_fingers'
        self.gripper.set_joint_value_target([1.3, 1.3])
        self.gripper.go(wait=True)
        rospy.sleep(2.0)

    def move_to(self, x, y, z, rotation, frame_id="m1n6s200_link_base"):
        print '******************* move_to'
        q = quaternion_from_euler(math.pi, 0.0, rotation)
        pose = PoseStamped()
        pose.header.frame_id = frame_id
        pose.pose.position.x = x
        pose.pose.position.y = y
        pose.pose.position.z = z
        pose.pose.orientation.x = q[0]
        pose.pose.orientation.y = q[1]
        pose.pose.orientation.z = q[2]
        pose.pose.orientation.w = q[3]

        self.arm.set_pose_target(pose, self.end_effector_link)
        plan = self.arm.plan()
        success = self.arm.go(wait=True)
        self.arm.stop()
        self.arm.clear_pose_targets()
        return success

    def move_to_waypoint(self):
        print '******************* move_to_waypoint'
        return self.move_to(0.35, 0, 0.25, 1.57)

    def rtb(self):
        print '******************* rtb'
        self.move_to_drop_zone()
        # pose = PoseStamped()
        # pose.header.frame_id = 'base_footprint'
        # pose.pose.position.x = -0.191258927921
        # pose.pose.position.y = 0.1849306168113
        # pose.pose.position.z = 0.813729734732
        # pose.pose.orientation.x = -0.934842026356
        # pose.pose.orientation.y = 0.350652799078
        # pose.pose.orientation.z = -0.00168532388516
        # pose.pose.orientation.w = 0.0557688079539
        #
        # self.arm.set_pose_target(pose, self.end_effector_link)
        # plan = self.arm.plan()
        # self.arm.go(wait=True)
        # self.arm.stop()
        # self.arm.clear_pose_targets()

    def move_to_drop_zone(self):
        print '******************* move_to_drop_zone'
        pose = PoseStamped()
        pose.header.frame_id = "m1n6s200_link_base"
        pose.pose.position.x = 0.2175546259709541
        pose.pose.position.y = 0.18347985269448372
        pose.pose.position.z = 0.16757751444136426

        pose.pose.orientation.x = 0.6934210704552356
        pose.pose.orientation.y = 0.6589390059796749
        pose.pose.orientation.z = -0.23223137602833943
        pose.pose.orientation.w = -0.17616808290725341

        self.arm.set_pose_target(pose, self.end_effector_link)
        plan = self.arm.plan()
        success = self.arm.go(wait=True)
        self.arm.stop()
        self.arm.clear_pose_targets()
        return success

    def print_position(self):
        pose = self.arm.get_current_pose()
        self.transformer.waitForTransform("m1n6s200_link_base",
                                          "base_footprint", rospy.Time.now(),
                                          rospy.Duration(10))
        eef_pose = self.transformer.transformPose("m1n6s200_link_base", pose)

        orientation = eef_pose.pose.orientation
        orientation = [
            orientation.x, orientation.y, orientation.z, orientation.w
        ]
        euler = euler_from_quaternion(orientation)

        print "z:", eef_pose.pose.position.x
        print "y:", eef_pose.pose.position.y
        print "z:", eef_pose.pose.position.z
        print "yaw (degrees):", math.degrees(euler[2])

    def remove_object(self):
        self.scene.remove_attached_object(self.end_effector_link, "object")
        self.scene.remove_world_object("object")
        rospy.loginfo("Object removed")
Esempio n. 16
0
from moveit_commander import RobotCommander, MoveGroupCommander
from moveit_commander import PlanningSceneInterface, roscpp_initialize, roscpp_shutdown
from geometry_msgs.msg import PoseStamped
from moveit_msgs.msg import Grasp, GripperTranslation, PlaceLocation
from trajectory_msgs.msg import JointTrajectoryPoint

if __name__=='__main__':

    roscpp_initialize(sys.argv)
    rospy.init_node('moveit_py_demo', anonymous=True)
    GRIPPER_FRAME = 'right_gripper_link'
    scene = PlanningSceneInterface()
    robot = RobotCommander()
    right_arm = MoveGroupCommander("right_arm")
    right_gripper = MoveGroupCommander("right_gripper")
    right_arm.set_planner_id("KPIECEkConfigDefault");
    rospy.sleep(1)

    # clean the scene
    scene.remove_world_object("table")
    scene.remove_world_object("part")
    scene.remove_attached_object(GRIPPER_FRAME, "part")
    #rospy.logwarn("cleaning world")
    #right_arm.set_named_target("r_start")
    #right_arm.go()
   
    #right_gripper.set_named_target("open")
    #right_gripper.go()
   
    rospy.sleep(3)
Esempio n. 17
0
    def __init__(self):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)

        rospy.init_node('moveit_demo')

        # Construct the initial scene object
        scene = PlanningSceneInterface()

        # Create a scene publisher to push changes to the scene
        self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)

        # Create a dictionary to hold object colors
        self.colors = dict()

        # Pause for the scene to get ready
        rospy.sleep(1)

        # Initialize the move group for the right arm
        right_arm = MoveGroupCommander('right_arm')
        # Initialize the move group for the left arm
        left_arm = MoveGroupCommander('left_arm')

        right_arm.set_planner_id("KPIECEkConfigDefault")
        left_arm.set_planner_id("KPIECEkConfigDefault")
        rospy.sleep(1)

        # Get the name of the end-effector link
        end_effector_link = right_arm.get_end_effector_link()

        # Allow some leeway in position (meters) and orientation (radians)
        right_arm.set_goal_position_tolerance(0.01)
        right_arm.set_goal_orientation_tolerance(0.05)

        # Allow replanning to increase the odds of a solution
        right_arm.allow_replanning(True)

        # Set the reference frame for pose targets
        reference_frame = 'base_footprint'

        # Set the right arm reference frame accordingly
        right_arm.set_pose_reference_frame(reference_frame)

        # Allow 5 seconds per planning attempt
        right_arm.set_planning_time(5)

        # Give each of the scene objects a unique name
        table_id = 'table'
        box1_id = 'box1'
        box2_id = 'box2'

        # Remove leftover objects from a previous run
        scene.remove_world_object(table_id)
        scene.remove_world_object(box1_id)
        scene.remove_world_object(box2_id)

        # Give the scene a chance to catch up
        rospy.sleep(1)

        # Start the arm in the "resting" pose stored in the SRDF file
        #left_arm.set_named_target('left_start')
        #left_arm.go()

        # Start the arm in the "resting" pose stored in the SRDF file
        right_arm.set_named_target('right_start')
        right_arm.go()

        rospy.sleep(2)

        # Set the height of the table off the ground
        table_ground = 0.75

        # Set the length, width and height of the table and boxes
        table_size = [0.2, 0.7, 0.01]
        box1_size = [0.1, 0.05, 0.05]
        box2_size = [0.05, 0.05, 0.15]

        # Add a table top and two boxes to the scene
        table_pose = PoseStamped()
        table_pose.header.frame_id = reference_frame
        table_pose.pose.position.x = 0.56
        table_pose.pose.position.y = 0.0
        table_pose.pose.position.z = table_ground + table_size[2] / 2.0
        table_pose.pose.orientation.w = 1.0
        scene.add_box(table_id, table_pose, table_size)

        box1_pose = PoseStamped()
        box1_pose.header.frame_id = reference_frame
        box1_pose.pose.position.x = 0.51
        box1_pose.pose.position.y = -0.1
        box1_pose.pose.position.z = table_ground + table_size[
            2] + box1_size[2] / 2.0
        box1_pose.pose.orientation.w = 1.0
        scene.add_box(box1_id, box1_pose, box1_size)

        box2_pose = PoseStamped()
        box2_pose.header.frame_id = reference_frame
        box2_pose.pose.position.x = 0.49
        box2_pose.pose.position.y = 0.15
        box2_pose.pose.position.z = table_ground + table_size[
            2] + box2_size[2] / 2.0
        box2_pose.pose.orientation.w = 1.0
        scene.add_box(box2_id, box2_pose, box2_size)

        # Make the table red and the boxes orange
        self.setColor(table_id, 0.8, 0, 0, 1.0)
        self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
        self.setColor(box2_id, 0.8, 0.4, 0, 1.0)

        # Send the colors to the planning scene
        self.sendColors()

        # Set the target pose in between the boxes and above the table
        target_pose = PoseStamped()
        target_pose.header.frame_id = reference_frame
        target_pose.pose.position.x = 0.4
        target_pose.pose.position.y = 0.0
        target_pose.pose.position.z = table_pose.pose.position.z + table_size[
            2] + 0.05
        target_pose.pose.orientation.w = 1.0

        # Set the target pose for the arm
        right_arm.set_pose_target(target_pose, end_effector_link)

        # Move the arm to the target pose (if possible)
        right_arm.go()

        # Pause for a moment...
        rospy.sleep(2)

        # Return the arm to the "resting" pose stored in the SRDF file
        right_arm.set_named_target('right_start')
        right_arm.go()

        # Exit MoveIt cleanly
        moveit_commander.roscpp_shutdown()

        # Exit the script
        moveit_commander.os._exit(0)
Esempio n. 18
0
class UR5_Gripped_Manipulator:
    """
  Class Attributes
  Robot        : Robot Commander Object
  Scene        : Planning Scene Interface Object (current scene)
  Man          : MoveGroupCommander object to control manipulator
  EEF          : MoveGroupCommander object to control endeffector
  Target_poses : Pose list of created target objects
  Picked       : List of part indexes to ensure to picked non-picked ones
  """
    def __init__(self, manip_name="manipulator", eef_name="endeffector"):
        self.robot = RobotCommander()
        self.man = MoveGroupCommander(manip_name)
        self.eef = MoveGroupCommander(eef_name)
        self.target_poses = []
        self.picked = []

    """
  ----------------Function Name: clean_scene----------------
  Definition: Clears the target_poses and picked lists in order to establish a new execution. Necessary for test_simulation method
  """

    def clear_poses(self):
        for i in xrange(len(self.target_poses)):
            self.target_poses.pop()
        for i in xrange(len(self.picked)):
            self.picked.pop()

    """
  ----------------Function Name: clean_scene----------------
  Definition   : Clears all non-attached objects from planning scene
  Group        : MoveGroupCommander object to control given group
  """

    def default_state_gripper(self, grp):
        rospy.loginfo("Openning Gripper")
        joint_vals = grp.get_current_joint_values()
        joint_vals[0] = 0.0
        grp.set_joint_value_target(joint_vals)
        init_plan = grp.plan()
        return grp.execute(init_plan)

    """
  ----------------Function Name: closed_state_gripper----------------
  Definition   : Function that opens gripper and detachs the gripped object
  Obj          : Name of the Object that is needed to detach
  """

    def closed_state_gripper(self, curr_scene, obj):
        rospy.loginfo("Closing Gripper")

        def convert(width):
            return 0.77 - width / 0.15316 * 0.77

        width = curr_scene.get_objects([obj])[obj].primitives[0].dimensions[0]
        width = convert(width)
        now = self.robot.endeffector.get_current_joint_values()[0]
        cnt = 0
        while abs(now - width) > 0.05:
            now = self.robot.endeffector.get_current_joint_values()[0]
            cnt = cnt + 1
            tmp = width - abs(now - width) / 2.0
            self.robot.endeffector.set_joint_value_target('finger_joint', tmp)
            self.robot.endeffector.go()
            rospy.sleep(0.05)
            if cnt == 7:
                break
        rospy.sleep(1.0)
        ret = self.robot.manipulator.attach_object(obj)
        return ret

    """
  ----------------Function Name:  set_mid_state----------------
  Definition  : Motion planning function for defined mid state for manipulator, can be used for enhancing the performance if needed
  Group       : MoveGroupCommander object to control given group 
  """

    def set_mid_state(self, group):
        rospy.loginfo("Going to Mid-State")
        pos = PoseStamped()
        pos.header.frame_id = self.robot.get_planning_frame()
        pos.pose.position.x = 0.45358
        pos.pose.position.y = 0.188518
        pos.pose.position.z = 0.473063
        pos.pose.orientation.x = 0.0004738
        pos.pose.orientation.y = 1
        pos.pose.orientation.z = 0
        pos.pose.orientation.w = 0.000155894
        group.set_pose_target(pos)
        move_plan = group.plan()
        while (not move_plan.joint_trajectory.joint_names):
            move_plan = group.plan()
        return group.execute(move_plan)

    """     
  ----------------Function Name: add_object----------------
  Name          : Object Name
  Pose	        : Pose of the Object (x,y,z,xo,yo,zo,wo)
  Dimension     : Dimensions of the Obhect (Tuple) (d1,d2,d3),
  Type          : Box(0),Sphere(1)
              d1 is radius for sphere i.e typ==1,
  """

    def add_object(self,
                   name,
                   x,
                   y,
                   z,
                   xo=0.0,
                   yo=0.0,
                   zo=0.0,
                   wo=0.0,
                   d1=0.1,
                   d2=0.1,
                   d3=0.1,
                   curr_scene=None,
                   typ=0):
        pos = PoseStamped()
        pos.header.frame_id = self.robot.get_planning_frame()
        pos.pose.position.x = x
        pos.pose.position.y = y
        pos.pose.position.z = z
        pos.pose.orientation.x = xo
        pos.pose.orientation.y = yo
        pos.pose.orientation.z = zo
        pos.pose.orientation.w = wo
        rospy.loginfo("ADDING OBJECT")
        if (typ == 0):
            curr_scene.add_box(name, pos, (d1, d2, d3))
        elif (typ == 1):
            curr_scene.add_sphere(name, pos, d1)
        else:
            print("ERROR in Type")
        return pos

    """
  ----------------Function Name: clean_scene----------------
  Definition: Clears all non-attached objects from planning scene
  """

    def clean_scene(self, curr_scene):
        rospy.loginfo("Clearing the Scene")
        curr_scene.remove_world_object()

    """
  ----------------Function Name:  create_environment----------------
  Definition  : Creates the simulation environment with non-target collision objects, can be edited to create different default environment
  """

    def create_environment(self, curr_scene):
        self.add_object(name="wall",
                        x=0.0,
                        y=0.8,
                        z=0.5,
                        d1=0.1,
                        d2=0.35,
                        d3=1,
                        curr_scene=curr_scene,
                        typ=0)
        self.add_object(name="wall_2",
                        x=0.0,
                        y=-0.8,
                        z=0.5,
                        d1=0.1,
                        d2=0.35,
                        d3=1,
                        curr_scene=curr_scene,
                        typ=0)
        self.add_object(name="table",
                        x=0.0,
                        y=0.0,
                        z=-0.05,
                        d1=2,
                        d2=2,
                        d3=0.0001,
                        curr_scene=curr_scene,
                        typ=0)

    """
  ----------------Function Name: check_targets----------------
  Definition  : Creates given number of objects within the distance provided. It prevents collision object overlapping.
         	It returns the pose list of the objects
  Number      : Number of collision objects required to spawn
  Distance    : Required minimum distance between each collision objects
  """

    def check_targets(self, curr_scene, number, distance):
        rospy.loginfo("Creating Boxes!")

        def al(typ, x=0.0):
            if typ == 'x':
                return random.uniform(0.35, 0.65)
            elif typ == "y":
                rang = math.sqrt(0.7**2 - x**2)
                return random.uniform(-rang, rang)

        hemme = []
        dims = []
        cnt = 0
        while len(hemme) != number:
            if cnt == 200:
                hemme = []
                cnt = 0
            cnt = cnt + 1
            dim = (random.uniform(0.08, 0.12), random.uniform(0.08, 0.10),
                   random.uniform(0.05, 0.2))
            quaternion = tf.transformations.quaternion_from_euler(
                0.0, 0.0, random.uniform(-math.pi, math.pi))
            box = PoseStamped()
            box.header.frame_id = self.robot.get_planning_frame()
            box.pose.position.z = (-0.04 + (dim[2] / 2))
            box.pose.position.x = al('x')
            box.pose.position.y = al('y', box.pose.position.x)
            box.pose.orientation.x = quaternion[0]
            box.pose.orientation.y = quaternion[1]
            box.pose.orientation.z = quaternion[2]
            box.pose.orientation.w = quaternion[3]
            flag = 1
            for i in hemme:
                if abs(i.pose.position.x -
                       box.pose.position.x) < distance or abs(
                           i.pose.position.y - box.pose.position.y) < distance:
                    flag = 0
            if flag == 0:
                continue
            hemme.append(box)
            dims.append(dim)
        cnt = 0
        names = []
        for i in xrange(len(hemme)):
            now = "part" + str(cnt)
            cnt = cnt + 1
            names.append(now)
            self.add_object(name=now,
                            x=hemme[i].pose.position.x,
                            y=hemme[i].pose.position.y,
                            z=hemme[i].pose.position.z,
                            xo=hemme[i].pose.orientation.x,
                            yo=hemme[i].pose.orientation.y,
                            zo=hemme[i].pose.orientation.z,
                            wo=hemme[i].pose.orientation.w,
                            d1=dims[i][0],
                            d2=dims[i][1],
                            d3=dims[i][2],
                            curr_scene=curr_scene,
                            typ=0)
        print("End Check!")
        return hemme

    """
  ----------------Function Name:  pick_object----------------
  Definition  : It moves the given group i.e robot to the collision object whose index is given and picks that object.
  Group       : MoveGroupCommander object to control given group 
  Part_index  : Index of the target object to obtain pose
  """

    def pick_object(self, curr_scene, group, part_index):
        rospy.loginfo("Pick Operation starts!")
        gripped_object = curr_scene.get_objects(["part" + str(part_index)
                                                 ])["part" + str(part_index)]
        pos = copy.deepcopy(self.target_poses[part_index])
        temp = tf.transformations.euler_from_quaternion(
            (pos.pose.orientation.x, pos.pose.orientation.y,
             pos.pose.orientation.z, pos.pose.orientation.w))
        quaternion = tf.transformations.quaternion_from_euler(
            math.pi, temp[1], temp[2])
        pos.pose.position.z += 0.17 + (
            gripped_object.primitives[0].dimensions[2] / 2.0)
        pos.pose.orientation.y = 1
        group.set_pose_target(pos)
        move_plan = group.plan()
        i = 0
        while (not move_plan.joint_trajectory.joint_names):
            move_plan = group.plan()
            i += 1
            if (i == 500): break
        state = group.execute(move_plan)
        rospy.loginfo("Execute operation for Object is %s" % str(part_index))
        if (state):
            self.closed_state_gripper(curr_scene, "part" + str(part_index))
            rospy.sleep(1.0)
            self.place_object(curr_scene, group, part_index)
            return
        else:
            return

    """
  ----------------Function Name:  place_object----------------
  Definition  : It places the gripped object to the target location
  Group       : MoveGroupCommander object to control given group 
  Part_index  : Index of the target object to obtain pose

  """

    def place_object(self, curr_scene, group, part_index):
        pos = PoseStamped()

        def al(typ, x=0.0):
            if typ == 'x':
                return random.uniform(-0.35, -0.6)
            elif typ == 'y':
                rang = math.sqrt(0.75**2 - x**2)
                x = random.uniform(-rang, rang)
                while abs(x) < 0.15:
                    x = random.uniform(-rang, rang)
                return x

        pos = PoseStamped()
        pos.header.frame_id = robot.get_planning_frame()
        pos.pose.position.x = al("x")
        pos.pose.position.y = al("y", pos.pose.position.x)
        #This line makes placing possible by setting a valid z position for gripped object
        pos.pose.position.z = -0.04 + 0.17 + ((curr_scene.get_attached_objects(
            ["part" + str(part_index)
             ])["part" + str(part_index)]).object.primitives[0].dimensions[2])
        pos.pose.orientation.y = 1.0
        group.set_pose_target(pos)
        move_plan = group.plan()
        while (not move_plan.joint_trajectory.joint_names):
            move_plan = group.plan()
        state = group.execute(move_plan)
        if (state):
            detached = group.detach_object("part" + str(part_index))
            rospy.sleep(1.5)
            if (detached):
                curr_scene.remove_world_object("part" + str(part_index))
                rospy.sleep(1.5)
                self.default_state_gripper(self.eef)
                self.picked.append(part_index)
            else:
                self.default_state_gripper(robot.endeffector)
                group.detach_object("part" + str(part_index))
                rospy.sleep(2)

    """
  ----------------Function Name: execute_simulation----------------
  Definition  : Creates the specified environment , generates motion plans and  does the pick and place operation based on these plans. To visualize RViz is needed.
		Planner ID can be changed based on MoveIT's supported OMPL.
  """

    def execute_simulation(self, curr_scene=None, num_of_objects=3):
        is_success = False
        #Reset State of the Gripper
        self.default_state_gripper(self.eef)
        ####
        #Reset the position of the Arm
        # Will be implemented if needed
        ####
        #Clean the scene
        self.clean_scene(curr_scene=curr_scene)
        #Create environment
        self.create_environment(curr_scene=curr_scene)
        #Create targets
        self.target_poses = self.check_targets(curr_scene, num_of_objects,
                                               0.10)
        rospy.sleep(5)
        #Planner setup
        self.man.set_planner_id("RRTConnectkConfigDefault")
        self.man.set_num_planning_attempts(20)
        self.man.allow_replanning(True)
        #Pick and place every object
        for i in xrange(len(self.target_poses)):
            if i not in self.picked:
                self.pick_object(curr_scene=curr_scene,
                                 group=self.man,
                                 part_index=i)
        rospy.loginfo("END OF PICK AND PLACE")
        if (len(self.picked) == len(self.target_poses)):
            is_success = True
        self.clear_poses()
        return is_success

    """
  ----------------Function Name: execute_simulation----------------
  Definition     : Executes the simulation num_attempts times. Stores the success rate 
                   and writes into the specified document
  Num_Attempts   : Limiting number for executing simulation. (Default Value: 10)
  File_Name      : File name (with path if needed) to write test results            
  """

    def test_simulation(self,
                        curr_scene=None,
                        num_attempts=10,
                        num_of_objects=3,
                        file_name="ur5_pick_place_test.txt"):
        success_rate = 0
        file = open(file_name, "a+")
        file.write("Start Test \n")
        for case in xrange(1, num_attempts + 1):
            state = self.execute_simulation(curr_scene=curr_scene,
                                            num_of_objects=num_of_objects)
            if (state):
                success_rate += 1
                rospy.loginfo("Success Rate: " + str(success_rate) + "/" +
                              str(case))
                file.write("Test Case %s is successfull" % str(case))
            else:
                rospy.loginfo("Success Rate: " + str(success_rate) + "/" +
                              str(case))
                file.write("Test Case %s is failedl" % str(case))
        file.write("Success: %" + str(((success_rate / (case - 1) * 100.0))))
        file.write("----END OF TEST----")
        file.close()
        return
Esempio n. 19
0
def graspobject():

                


                 
        # Connect to the right_arm move group
        right_arm = MoveGroupCommander('manipulator')
	right_arm.set_planner_id('RRTConnectkConfigDefault')   

    
        # Allow replanning to increase the odds of a solution
        right_arm.allow_replanning(True)
        
        # Set the right arm reference frame
        right_arm.set_pose_reference_frame('base_link')
       
               
        # Allow some leeway in position(meters) and orientation (radians)
        right_arm.set_goal_position_tolerance(0.1)
      
        
        # Get the name of the end-effector link
        end_effector_link = right_arm.get_end_effector_link()
	#right_arm.set_end_effector_link(end_effector_link)
    

	#start_pose = Pose()
        #start_pose.position.x = 0.587  
        #start_pose.position.y = 0.034 
        #start_pose.position.z =  -0.010
        #start_pose.orientation.x =0.508 
        #start_pose.orientation.y =  0.508  
        #start_pose.orientation.z =  -0.491 
        #start_pose.orientation.w =  0.491
        #right_arm.set_pose_target(start_pose)    
        #traj = right_arm.plan()
	#rospy.sleep(2)
        #right_arm.execute(traj)
        #rospy.sleep(2)

 

 
 	global getkey
        print '回到初始位置结束'

	startclient = rospy.ServiceProxy('/detect_grasps/actionstart', npy)  
	startclient(0)
        print 'call start over'

  	sub = rospy.Subscriber('/detect_grasps/clustered_grasps', GraspConfigList, graspcallback)    
	rate = rospy.Rate(200)
	while not rospy.is_shutdown(): 

                if getkey==1:
                   break

 	        rate.sleep()
	getkey=0

	global grasps
	print '检测到抓取位置,开始执行'



        global ser
        ser.write(('0\n').encode())
        rospy.sleep(2)


	print "-start movingA------------------------------------"
	pose = []
	currentcamera=current_cameralinkpose()
        grasppose1,grasppose2= transformgrasp(currentcamera,grasps,0.24,0.15)
	start_pose = current_pose()
        print 'start_pose',start_pose
	pose.append(start_pose)
        pose.append(grasppose1)
        pose.append(grasppose2)
	#end_pose = copy.deepcopy(start_pose)
       # end_pose.position.x = grasps.approach.x 
      #  end_pose.position.y = grasps.approach.y 
      #  end_pose.position.z =  grasps.approach.z 

 	catisian_moving(pose,right_arm)#迪卡尔插值到目标姿态
	print "-start movingB------------------------------------"
	print '抓' 
        ser.write(('1\n').encode())
        rospy.sleep(2)

##############################################################
 


	group_variable_values = right_arm.get_current_joint_values()
        print "============ Joint values: ", group_variable_values 
	group_variable_values[0] =1.5740725994110107 #机器人的观测姿态定义在这里
	group_variable_values[1] =  -1.5415833632098597
	group_variable_values[2] =   -1.2562201658831995
	group_variable_values[3] = -1.8575199286090296
	group_variable_values[4] = 1.572489857673645
	group_variable_values[5] =   1.5713907480239868
	right_arm.set_joint_value_target(group_variable_values)

	plan2 = right_arm.plan()
        
	print "============ Waiting while RVIZ displays plan2..."
	rospy.sleep(1)
 
        right_arm.execute(plan2)#先移到观测姿态  













	group_variable_values = right_arm.get_current_joint_values()
        print "============ Joint values: ", group_variable_values 
	group_variable_values[0] =2.6649599075317383#机器人的观测姿态定义在这里
	group_variable_values[1] =  -1.493981663380758
	group_variable_values[2] =  -1.7679532209979456
	group_variable_values[3] =-1.3932693640338343
	group_variable_values[4] =  1.5719022750854492
	group_variable_values[5] =    1.5657243728637695
	right_arm.set_joint_value_target(group_variable_values)

	plan2 = right_arm.plan()
        
	print "============ Waiting while RVIZ displays plan2..."
	rospy.sleep(2)
 
        right_arm.execute(plan2)#先移到观测姿态  
	print "-start movingB------------------------------------"
	print '笛卡尔插值到放置位置完成' 
        ser.write(('0\n').encode())
        rospy.sleep(2)

       # pose = []
        #posecurrent=current_pose()#获取现有的机器人末端姿态
        
#        pose.append(posecurrent)

	#start_pose = Pose()#似乎这里不能接受相对的移动
        #start_pose= objectpose#获取目标末端姿态
 #       posecurrent.position.z =posecurrent.position.z -0.1



  #      pose.append(posecurrent)
       # print start_pose
        print "-------------------------------------"
       # print pose
        print "-------------------------------------"
   #     catisian_moving(pose,right_arm)#迪卡尔插值到目标姿态

	#回到初始位置
	group_variable_values = right_arm.get_current_joint_values()
        print "============ Joint values: ", group_variable_values 
	group_variable_values[0] =1.5740725994110107 #机器人的观测姿态定义在这里
	group_variable_values[1] =  -1.5415833632098597
	group_variable_values[2] =   -1.2562201658831995
	group_variable_values[3] = -1.8575199286090296
	group_variable_values[4] = 1.572489857673645
	group_variable_values[5] =   1.5713907480239868
	right_arm.set_joint_value_target(group_variable_values)

	plan2 = right_arm.plan()
        
	print "============ Waiting while RVIZ displays plan2..."
	rospy.sleep(1)
 
        right_arm.execute(plan2)#先移到观测姿态  
	rospy.sleep(3)
  
        print '回到初始位置结束'

	startclient2 = rospy.ServiceProxy('/detect_grasps/actionover', npy)  
	startclient2(0)
        print 'call start over'
 
 # modify the pose
 p.pose.position.x = 0.72
 p.pose.position.z = 0.05
 # add the pen
 scene.add_mesh("pen",p,resourcepath+'objects/pen.dae')
 rospy.sleep(1)
  
 # print the existing groups 
 robot = RobotCommander()
 print "Available groups: ",robot.get_group_names()
 
 # setup the arm group and its planner
 arm = MoveGroupCommander("arm")
 arm.set_start_state_to_current_state()
 arm.set_planner_id("RRTstarkConfigDefault") 
 arm.set_planning_time(5.0)
 arm.detach_object("pen")
 
 # set the arm to a safe target
 arm.set_named_target("gamma")
 # plan and execute the motion 
 arm.go()
 
 # setup the hand group and its planner
 hand = MoveGroupCommander("hand")
 hand.set_start_state_to_current_state()
 hand.set_planner_id("LBKPIECEkConfigDefault") 
 hand.set_planning_time(10.0)
 
 # set the hand to a safe target
Esempio n. 21
0
class SingleArmROS(SingleArmReal):
    """
    A class for single arms with ROS-related functions.

    Args:
        cfgs (YACS CfgNode): configurations for the arm.
        moveit_planner (str): motion planning algorithm.
        eetool_cfg (dict): arguments to pass in the constructor
            of the end effector tool class.

    Attributes:
        max_vel (float): Maximum joint velocity, read in from
            MoveIt config file. Can be scaled with self.scale_motion().
        max_acc (float): Maximum joint acceleration, read in from
            MoveIt config file. Can be scaled with self.scale_motion().
        tf_listner (tf.TransformListener): ROS subscriber to the /tf topic.
        moveit_group (MoveGroupCommander): Internal interface to MoveIt!
            move_group, used to call motion planning functions.
        moveit_scene (MoveitScene): Internal interface to MoveIt! planning
            scene, used to add objects, remove objects, etc. from the
            motion planning environment.
        moveit_planner (str): The name of the motion planning algorithm
            being used for all motion planning requests.
    """
    def __init__(self,
                 cfgs,
                 moveit_planner='RRTstarkConfigDefault',
                 eetool_cfg=None):
        super(SingleArmROS, self).__init__(cfgs=cfgs, eetool_cfg=eetool_cfg)

        self.moveit_planner = moveit_planner
        self._gazebo_sim = rospy.get_param('sim')
        self._init_ros_consts()

    def set_jpos(self, position, joint_name=None, wait=True, *args, **kwargs):
        """
        Method to send a joint position command to the robot (units in rad)

        Args:
            position (float or list or flattened np.ndarray):
                desired joint position(s)
                (shape: :math:`[DOF,]` if list, otherwise a single value).
            joint_name (str): If not provided, position should be a list and
                all actuated joints will be moved to specified positions. If
                provided, only specified joint will move. Defaults to None.
            wait (bool): whether position command should be blocking or non
                blocking. Defaults to True.

        Returns:
            bool: True if command was completed successfully, returns
            False if wait flag is set to False.
        """
        position = copy.deepcopy(position)
        if joint_name is None:
            if len(position) != self.arm_dof:
                raise ValueError('position should contain %d elements if '
                                 'joint_name is not provided' % self.arm_dof)
            tgt_pos = position
        else:
            if not isinstance(position, numbers.Number):
                raise TypeError('position should be individual float value'
                                ' if joint_name is provided')
            if joint_name not in self.arm_jnt_names_set:
                raise TypeError('Joint name [%s] is not in the arm'
                                ' joint list!' % joint_name)
            else:
                tgt_pos = self.get_jpos()
                arm_jnt_idx = self.arm_jnt_names.index(joint_name)
                tgt_pos[arm_jnt_idx] = position
        self.moveit_group.set_joint_value_target(tgt_pos)
        success = self.moveit_group.go(tgt_pos, wait=wait)

        return success

    def set_jvel(self, velocity, joint_name=None, wait=False, *args, **kwargs):
        """
        Set joint velocity command to the robot (units in rad/s).

        Args:
            velocity (float or list or flattened np.ndarray): list of target
                joint velocity value(s)
                (shape: :math:`[6,]` if list, otherwise a single value).
            joint_name (str, optional): If not provided, velocity should be
                list and all joints will be turned on at specified velocity.
                Defaults to None.
            wait (bool, optional): If True, block until robot reaches
                desired joint velocity value(s). Defaults to False.

        Returns:
            bool: True if command was completed successfully, returns
            False if wait flag is set to False.
        """
        raise NotImplementedError

    def set_ee_pose(self, pos=None, ori=None, wait=True, *args, **kwargs):
        """
        Set cartesian space pose of end effector.

        Args:
            pos (list or np.ndarray): Desired x, y, z positions in the robot's
                base frame to move to (shape: :math:`[3,]`).
            ori (list or np.ndarray, optional): It can be euler angles
                ([roll, pitch, yaw], shape: :math:`[4,]`),
                or quaternion ([qx, qy, qz, qw], shape: :math:`[4,]`),
                or rotation matrix (shape: :math:`[3, 3]`). If it's None,
                the solver will use the current end effector
                orientation as the target orientation.
            wait (bool): wait until the motion completes.

        Returns:
            bool: Returns True is robot successfully moves to goal pose.
        """
        if ori is None and pos is None:
            return True
        if ori is None:
            pose = self.get_ee_pose()  # last index is euler angles
            quat = pose[1]
        else:
            quat = arutil.to_quat(ori)
        if pos is None:
            pose = self.get_ee_pose()
            pos = pose[0]

        pose = self.moveit_group.get_current_pose()
        pose.pose.position.x = pos[0]
        pose.pose.position.y = pos[1]
        pose.pose.position.z = pos[2]
        pose.pose.orientation.x = quat[0]
        pose.pose.orientation.y = quat[1]
        pose.pose.orientation.z = quat[2]
        pose.pose.orientation.w = quat[3]
        self.moveit_group.set_pose_target(pose)
        success = self.moveit_group.go(wait=wait)
        return success

    def move_ee_xyz(self,
                    delta_xyz,
                    eef_step=0.005,
                    wait=True,
                    *args,
                    **kwargs):
        """
        Move end effector in straight line while maintaining orientation.

        Args:
            delta_xyz (list or np.ndarray): Goal change in x, y, z position of
                end effector.
            eef_step (float, optional): Discretization step in cartesian space
                for computing waypoints along the path. Defaults to 0.005 (m).
            wait (bool, optional): True if robot should not do anything else
                until this goal is reached, or the robot times out.
                Defaults to True.

        Returns:
            bool: True if robot successfully reached the goal pose.
        """
        ee_pos, ee_quat, ee_rot_mat, ee_euler = self.get_ee_pose()

        plan = moveit_cartesian_path(ee_pos, ee_quat, delta_xyz,
                                     self.moveit_group, eef_step)
        success = self.moveit_group.execute(plan, wait=wait)
        return success

    def get_jpos(self, joint_name=None):
        """
        Gets the current joint position of the robot. Gets the value
        from the internally updated dictionary that subscribes to the ROS
        topic /joint_states.

        Args:
            joint_name (str, optional): If it's None,
                it will return joint positions
                of all the actuated joints. Otherwise, it will
                return the joint position of the specified joint.

        Returns:
            One of the following

            - float: joint position given joint_name.
            - list: joint positions if joint_name is None
              (shape: :math:`[DOF]`).

        """
        self._j_state_lock.acquire()
        if joint_name is not None:
            if joint_name not in self.arm_jnt_names:
                raise TypeError('Joint name [%s] '
                                'not recognized!' % joint_name)
            jpos = self._j_pos[joint_name]
        else:
            jpos = []
            for joint in self.arm_jnt_names:
                jpos.append(self._j_pos[joint])
        self._j_state_lock.release()
        return jpos

    def get_jvel(self, joint_name=None):
        """
        Gets the current joint angular velocities of the robot. Gets the value
        from the internally updated dictionary that subscribes to the ROS
        topic /joint_states.

        Args:
            joint_name (str, optional): If it's None,
                it will return joint velocities
                of all the actuated joints. Otherwise, it will
                return the joint position of the specified joint.

        Returns:
            One of the following

            - float: joint velocity given joint_name.
            - list: joint velocities if joint_name is None
              (shape: :math:`[DOF]`).
        """
        self._j_state_lock.acquire()
        if joint_name is not None:
            if joint_name not in self.arm_jnt_names:
                raise TypeError('Joint name [%s] not recognized!' % joint_name)
            jvel = self._j_vel[joint_name]
        else:
            jvel = []
            for joint in self.arm_jnt_names:
                jvel.append(self._j_vel[joint])
        self._j_state_lock.release()
        return jvel

    def get_ee_pose(self):
        """
        Get current cartesian pose of the EE, in the robot's base frame,
        using ROS subscriber to the tf tree topic.

        Returns:
            4-element tuple containing

            - np.ndarray: x, y, z position of the EE (shape: :math:`[3]`).
            - np.ndarray: quaternion representation ([x, y, z, w]) of the EE
              orientation (shape: :math:`[4]`).
            - np.ndarray: rotation matrix representation of the EE orientation
              (shape: :math:`[3, 3]`).
            - np.ndarray: euler angle representation of the EE orientation
              (roll, pitch, yaw with static reference frame)
              (shape: :math:`[3]`).
        """
        pos, quat = get_tf_transform(self.tf_listener,
                                     self.cfgs.ARM.ROBOT_BASE_FRAME,
                                     self.cfgs.ARM.ROBOT_EE_FRAME)
        rot_mat = arutil.quat2rot(quat)
        euler_ori = arutil.quat2euler(quat)
        return np.array(pos), np.array(quat), rot_mat, euler_ori

    def get_ee_vel(self):
        """
        Return the end effector's velocity.

        Returns:
            2-element tuple containing

            - np.ndarray: translational velocity (vx, vy, vz)
              (shape: :math:`[3,]`).
            - np.ndarray: rotational velocity
              (wx, wy, wz) (shape: :math:`[3,]`).
        """
        jpos = self.get_jpos()
        jvel = self.get_jvel()
        ee_vel = self.compute_fk_velocity(jpos, jvel,
                                          self.cfgs.ARM.ROBOT_EE_FRAME)
        return ee_vel[:3], ee_vel[3:]

    def scale_motion(self, vel_scale=1.0, acc_scale=1.0):
        """
        Sets the maximum velocity and acceleration for the robot
        motion. Specified as a fraction
        from 0.0 - 1.0 of the maximum velocity and acceleration
        specified in the MoveIt joint limits configuration file.

        Args:
            vel_scale (float): velocity scale, Defaults to 1.0.
            acc_scale (float): acceleration scale, Defaults to 1.0.
        """
        vel_scale = arutil.clamp(vel_scale, 0.0, 1.0)
        acc_scale = arutil.clamp(acc_scale, 0.0, 1.0)
        self.moveit_group.set_max_velocity_scaling_factor(vel_scale)
        self.moveit_group.set_max_acceleration_scaling_factor(acc_scale)
        self._motion_vel = self.max_vel * vel_scale
        self._motion_acc = self.max_acc * acc_scale

    def _init_ros_consts(self):
        """
        Initialize constants
        """
        moveit_commander.roscpp_initialize(sys.argv)
        self.moveit_group = MoveGroupCommander(self.cfgs.ARM.MOVEGROUP_NAME)
        self.moveit_group.set_planner_id(self.moveit_planner)
        self.moveit_group.set_planning_time(1.0)
        self.moveit_scene = MoveitScene()

        # read the joint limit (max velocity and acceleration) from the
        # moveit configuration file
        jnt_params = []
        max_vels = []
        max_accs = []
        for arm_jnt in self.arm_jnt_names:
            jnt_param = self.cfgs.ROBOT_DESCRIPTION + \
                        '_planning/joint_limits/' + arm_jnt
            jnt_params.append(copy.deepcopy(jnt_param))
            max_vels.append(rospy.get_param(jnt_param + '/max_velocity'))
            max_accs.append(rospy.get_param(jnt_param + '/max_acceleration'))
        self.max_vel = np.min(max_vels)
        self.max_acc = np.min(max_accs)

        self._j_pos = dict()
        self._j_vel = dict()
        self._j_torq = dict()
        self._j_state_lock = threading.RLock()
        self.tf_listener = tf.TransformListener()
        rospy.Subscriber(self.cfgs.ARM.ROSTOPIC_JOINT_STATES, JointState,
                         self._callback_joint_states)

    def _callback_joint_states(self, msg):
        """
        ROS subscriber callback for arm joint states

        Args:
            msg (sensor_msgs/JointState): Contains message published in topic.
        """
        self._j_state_lock.acquire()
        for idx, name in enumerate(msg.name):
            if name in self.arm_jnt_names:
                if idx < len(msg.position):
                    self._j_pos[name] = msg.position[idx]
                if idx < len(msg.velocity):
                    self._j_vel[name] = msg.velocity[idx]
                if idx < len(msg.effort):
                    self._j_torq[name] = msg.effort[idx]
        self._j_state_lock.release()
Esempio n. 22
0
class Arm():
    def __init__(self):
        self.p = Pose()

        self.gripper = MoveGroupCommander("onine_gripper")
        self.arm = MoveGroupCommander("onine_arm")

        self.arm.set_goal_tolerance(0.004)
        self.arm.allow_replanning(True)
        # self.arm.set_goal_position_tolerance(0.005)
        # self.arm.set_goal_orientation_tolerance(0.1)
        self.arm.set_num_planning_attempts(10)
        self.arm.set_planning_time(5)
        self.arm.set_planner_id("RRTkConfigDefault")

    def go(self, x, y, z, roll, pitch, yaw):
        self.p.position.x = x
        self.p.position.y = y
        self.p.position.z = z
        self.p.orientation = Quaternion(
            *quaternion_from_euler(roll, pitch, yaw))
        self.arm.set_pose_target(self.p)

        os.system("rosservice call clear_octomap")
        rospy.loginfo("Moving to arm target")
        self.arm.go(wait=True)

        rospy.sleep(1)

    def get_valid_pose(self, x, y, z, distance):
        origin_translation = [0.095, 0.00, 0.00]

        delta_x = x - origin_translation[0]
        delta_y = y - origin_translation[1]

        theta = math.atan(delta_y / delta_x)
        grasp_yaw = theta
        grasp_x = x + (distance * math.cos(theta))
        grasp_y = y + (distance * math.sin(theta))

        return (grasp_x, grasp_y, z, grasp_yaw)

    def ready(self):
        self.arm.set_named_target("onine_ready")
        self.arm.go(wait=True)

    def home(self):
        self.arm.set_named_target("onine_home")
        self.arm.go(wait=True)

    def feed_pos(self):
        self.arm.set_named_target("feed_pos")
        self.arm.go(wait=True)

    def tilt_food(self):
        self.arm.set_named_target("tilt_food")
        self.arm.go(wait=True)

    def open_gripper(self):
        os.system("rostopic pub /onine_gripper std_msgs/Float64 0.085 -1")
        os.system("rosservice call clear_octomap")

    def close_gripper(self):
        os.system("rostopic pub /onine_gripper std_msgs/Float64 0.035 -1")
        os.system("rosservice call clear_octomap")

    def pickup_sim(self, x, y, z):
        # self.ready()
        self.open_gripper()
        (aim_x, aim_y, aim_z,
         aim_yaw) = self.get_valid_pose(x, y, z + 0.15, 0.00)
        self.go(aim_x, aim_y, aim_z, 0.0, 0.0, aim_yaw)

        (aim_x, aim_y, aim_z, aim_yaw) = self.get_valid_pose(x, y, z, 0.00)
        self.go(aim_x, aim_y, aim_z, 0.0, 0.0, aim_yaw)

        self.close_gripper()
Esempio n. 23
0
    def __init__(self):

        roscpp_initialize(sys.argv)
        rospy.init_node('moveit_py_demo', anonymous=True)

        scene = PlanningSceneInterface()
        robot = RobotCommander()

        right_arm = MoveGroupCommander(GROUP_NAME_ARM)
        right_arm.set_planner_id("KPIECEkConfigDefault")
        right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)

        eef = right_arm.get_end_effector_link()

        rospy.sleep(2)

        scene.remove_attached_object(GRIPPER_FRAME, "part")

        # clean the scene
        scene.remove_world_object("table")
        scene.remove_world_object("part")

        #right_arm.set_named_target("r_start")
        #right_arm.go()

        #right_gripper.set_named_target("right_gripper_open")
        #right_gripper.go()

        rospy.sleep(1)

        # publish a demo scene
        p = PoseStamped()
        p.header.frame_id = robot.get_planning_frame()

        # add a table
        #p.pose.position.x = 0.42
        #p.pose.position.y = -0.2
        #p.pose.position.z = 0.3
        #scene.add_box("table", p, (0.5, 1.5, 0.6))

        # add an object to be grasped
        p.pose.position.x = 0.40
        p.pose.position.y = -0.0
        p.pose.position.z = 0.85
        scene.add_box("part", p, (0.07, 0.01, 0.2))

        rospy.sleep(1)

        g = Grasp()
        g.id = "test"
        start_pose = PoseStamped()
        start_pose.header.frame_id = FIXED_FRAME

        # start the gripper in a neutral pose part way to the target
        start_pose.pose.position.x = 0.37636
        start_pose.pose.position.y = 0.21886
        start_pose.pose.position.z = 0.9
        start_pose.pose.orientation.x = 0.00080331
        start_pose.pose.orientation.y = 0.001589
        start_pose.pose.orientation.z = -2.4165e-06
        start_pose.pose.orientation.w = 1

        right_arm.set_pose_target(start_pose)
        right_arm.go()

        rospy.sleep(2)

        # generate a list of grasps
        grasps = self.make_grasps(start_pose)

        result = False
        n_attempts = 0

        # repeat until will succeed
        while result == False:
            result = robot.right_arm.pick("part", grasps)
            n_attempts += 1
            print "Attempts: ", n_attempts
            rospy.sleep(0.3)

        rospy.spin()
        roscpp_shutdown()
Esempio n. 24
0
class MoveItObstaclesDemo:
    def __init__(self):
        # 初始化move_group的API
        moveit_commander.roscpp_initialize(sys.argv)

        # 初始化ROS节点
        rospy.init_node('moveit_obstacles_demo')

        # 等待场景准备就绪
        rospy.sleep(1)

        # 初始化需要使用move group控制的机械臂中的arm group
        self.arm = MoveGroupCommander('manipulator')

        # 设置机械臂工作空间
        self.arm.set_workspace([-100, -100, 0, 100, 0.3, 100])

        # 设置机械臂最大速度
        self.arm.set_max_velocity_scaling_factor(value=0.1)

        # 获取终端link的名称
        self.end_effector_link = self.arm.get_end_effector_link()
        rospy.loginfo('end effector link: {}'.format(self.end_effector_link))

        # 设置位置(单位:米)和姿态(单位:弧度)的允许误差
        self.arm.set_goal_position_tolerance(0.01)
        self.arm.set_goal_orientation_tolerance(0.05)

        # 当运动规划失败后,允许重新规划
        # self.arm.allow_replanning(True)
        self.arm.set_num_planning_attempts(10)
        # self.arm.allow_looking(True)

        # 设置目标位置所使用的参考坐标系
        self.reference_frame = 'base_link'
        self.arm.set_pose_reference_frame(self.reference_frame)

        # 设置每次运动规划的时间限制:5s
        self.arm.set_planning_time(3)

        # 控制机械臂先回到初始化位置
        self.arm.set_named_target('home')
        self.arm.go()
        rospy.sleep(2)

    def planning(self, start_point, end_point):
        """
        功能:动态避障
        :param start_point: 起始点, type: dict
        :param end_point: 终点, type: dict
        :return: None
        """
        # 移动机械臂到指定位置,并获取当前位姿数据为机械臂运动的起始位姿
        if start_point:
            self.move_arm(p=start_point)
        self.start_pose = self.arm.get_current_pose(self.end_effector_link).pose

        # 使用moveit自带的求解器规划出A到B的离散路径点, 并存到列表way_points中
        # way_points = self.get_way_points(start_point, end_point)

        while True:
            print('set planner id: RRT')
            self.arm.set_planner_id('TRRTkConfigDefault')
            self.arm.set_named_target('up')
            self.arm.go()
            rospy.sleep(5)


            print('set planner id: PRM')
            self.arm.set_planner_id('TRRTkConfigDefault')
            self.arm.set_named_target('home')
            self.arm.go()
            rospy.sleep(5)


    # -------------------------------------------------------------------

    def get_way_points(self, a, b):
        way_points = []

        # plan 1
        self.arm.set_named_target('up')
        traj = self.arm.plan()
        if traj.joint_trajectory.points:  # True if trajectory contains points
            rospy.loginfo("get trajectory success")
        else:
            rospy.logerr("Trajectory is empty. Planning false!")
        self.arm.clear_pose_targets()

        # plan 2
        # traj = self.moveit_planning(p=b)

        for i, p in enumerate(traj.joint_trajectory.points):
            # rospy.loginfo('waypoint: {}'.format(i))
            if i%2 == 0:
                point = {
                    'x': p.positions[0],
                    'y': p.positions[1],
                    'z': p.positions[2],
                    'ox': p.positions[3],
                    'oy': p.positions[4],
                    'oz': p.positions[5]
                }
                way_points.append(point)
        rospy.loginfo('waypoint: \n {}'.format(way_points))
        rospy.loginfo(len(way_points))
        return way_points

    def moveit_planning(self, p):
        """
        使用moveit求解器规划路径
        :param p: dict, e.g., {'x': 0, 'y': 0, 'z': 0, 'ox': 0, 'oy': 0, 'oz': 0, 'ow': 0}
        :return:
        """
        rospy.loginfo('start moveit planning...')
        target_pose = PoseStamped()
        target_pose.header.frame_id = self.reference_frame
        target_pose.pose.position.x = p['x']
        target_pose.pose.position.y = p['y']
        target_pose.pose.position.z = p['z']
        if 'ox' in p.keys() and p['ox']:
            target_pose.pose.orientation.x = p['ox']
        if 'oy' in p.keys() and p['oy']:
            target_pose.pose.orientation.y = p['oy']
        if 'oz' in p.keys() and p['oz']:
            target_pose.pose.orientation.z = p['oz']
        if 'ow' in p.keys() and p['ow']:
            target_pose.pose.orientation.w = p['ow']
        # 传入一个PoseStamped
        # self.arm.set_pose_target(target_pose, self.end_effector_link)

        # 尝试直接传入一个列表
        self.arm.set_pose_target([p['x'], p['y'], p['z'], p['ox'], p['oy'], p['oz']], self.end_effector_link)
        traj = self.arm.plan()
        if traj.joint_trajectory.points:  # True if trajectory contains points
            rospy.loginfo("get trajectory success")
            return traj
        else:
            rospy.logerr("Trajectory is empty. Planning false!")

    def move_arm(self, p):
        """
        移动机械臂到p点
        :param p: dict, e.g., {'x': 0, 'y': 0, 'z': 0, 'ox': 0, 'oy': 0, 'oz': 0, 'ow': 0}
        :return:
        """
        rospy.loginfo('start arm moving...')

        traj = self.moveit_planning(p)
        self.arm.execute(traj)

        # 设置当前位置为起始位置
        self.arm.set_start_state_to_current_state()
        rospy.sleep(1)

    def stop_arm(self):
        """
        急停
        :return:
        """
        pass

    def exist_danger_obstacle(self):
        """
        环境中是否存在危险的障碍物
        :return: True or False
        """
        return False

    def get_obstacle_octomap(self):
        """
        获取环境的octomap信息
        :return:
        """
        pass
Esempio n. 25
0
    def __init__(self):
        # Initialize the move_group API
        print 'starting'
        moveit_commander.roscpp_initialize(sys.argv)
 



                 
        # Connect to the right_arm move group
        right_arm = MoveGroupCommander('manipulator')
	right_arm.set_planner_id('RRTConnectkConfigDefault')   

    
        # Allow replanning to increase the odds of a solution
        right_arm.allow_replanning(True)
        
        # Set the right arm reference frame
        right_arm.set_pose_reference_frame('base_link')
       
               
        # Allow some leeway in position(meters) and orientation (radians)
        right_arm.set_goal_position_tolerance(0.1)
      
        
        # Get the name of the end-effector link
        end_effector_link = right_arm.get_end_effector_link()
	#right_arm.set_end_effector_link(end_effector_link)





        # Get the current pose so we can add it as a waypoint
	group_variable_values = right_arm.get_current_joint_values()
        print "============ Joint values: ", group_variable_values 
	group_variable_values[0] =1.5740725994110107 #机器人的观测姿态定义在这里
	group_variable_values[1] =  -1.5415833632098597
	group_variable_values[2] =   -1.2562201658831995
	group_variable_values[3] = -1.8575199286090296
	group_variable_values[4] = 1.572489857673645
	group_variable_values[5] =   1.5713907480239868
	right_arm.set_joint_value_target(group_variable_values)

	plan2 = right_arm.plan()
        
	print "============ Waiting while RVIZ displays plan2..."
	rospy.sleep(2)
 
        right_arm.execute(plan2)#先移到观测姿态  






	try:
		 
		rate = rospy.Rate(10)

		while not rospy.is_shutdown():
		    print '.'
		    graspobject( )
		    rate.sleep()

	 

	except rospy.ROSInterruptException:
		pass
Esempio n. 26
0
    def __init__(self):

        roscpp_initialize(sys.argv)
        rospy.init_node("moveit_py_demo", anonymous=True)

        scene = PlanningSceneInterface()
        robot = RobotCommander()

        right_arm = MoveGroupCommander(GROUP_NAME_ARM)
        right_arm.set_planner_id("KPIECEkConfigDefault")
        right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)

        eef = right_arm.get_end_effector_link()

        rospy.sleep(2)

        scene.remove_attached_object(GRIPPER_FRAME, "part")

        # clean the scene
        scene.remove_world_object("table")
        scene.remove_world_object("part")

        # right_arm.set_named_target("r_start")
        # right_arm.go()

        # right_gripper.set_named_target("right_gripper_open")
        # right_gripper.go()

        rospy.sleep(1)

        # publish a demo scene
        p = PoseStamped()
        p.header.frame_id = robot.get_planning_frame()

        # add a table
        # p.pose.position.x = 0.42
        # p.pose.position.y = -0.2
        # p.pose.position.z = 0.3
        # scene.add_box("table", p, (0.5, 1.5, 0.6))

        # add an object to be grasped
        p.pose.position.x = 0.40
        p.pose.position.y = -0.0
        p.pose.position.z = 0.85
        scene.add_box("part", p, (0.07, 0.01, 0.2))

        rospy.sleep(1)

        g = Grasp()
        g.id = "test"
        start_pose = PoseStamped()
        start_pose.header.frame_id = FIXED_FRAME

        # start the gripper in a neutral pose part way to the target
        start_pose.pose.position.x = 0.37636
        start_pose.pose.position.y = 0.21886
        start_pose.pose.position.z = 0.9
        start_pose.pose.orientation.x = 0.00080331
        start_pose.pose.orientation.y = 0.001589
        start_pose.pose.orientation.z = -2.4165e-06
        start_pose.pose.orientation.w = 1

        right_arm.set_pose_target(start_pose)
        right_arm.go()

        rospy.sleep(2)

        # generate a list of grasps
        grasps = self.make_grasps(start_pose)

        result = False
        n_attempts = 0

        # repeat until will succeed
        while result == False:
            result = robot.right_arm.pick("part", grasps)
            n_attempts += 1
            print "Attempts: ", n_attempts
            rospy.sleep(0.3)

        rospy.spin()
        roscpp_shutdown()
Esempio n. 27
0
    group.set_path_constraints(constraints)
    rospy.loginfo("Get Path Constraints:\n{}".format(
        group.get_path_constraints()))

    # Pose Target 2
    rospy.loginfo("Start Pose Target 2")
    pose_target_2 = Pose()
    pose_target_2.position.x = 0.3
    pose_target_2.position.y = -0.5
    pose_target_2.position.z = 0.15
    pose_target_2.orientation.x = 0.0
    pose_target_2.orientation.y = -0.707
    pose_target_2.orientation.z = 0.0
    pose_target_2.orientation.w = 0.707

    group.set_planner_id("RRTConnectkConfigDefault")
    group.allow_replanning(True)

    rospy.loginfo("Set Target to Pose:\n{}".format(pose_target_2))

    xyz = [
        pose_target_2.position.x, pose_target_2.position.y,
        pose_target_2.position.z
    ]
    group.set_position_target(xyz)
    result_p = group.go()

    rospy.loginfo("Moving to the Position Executed... {}".format(result_p))

    group.clear_path_constraints()
Esempio n. 28
0
class TestPlanners(object):
    def __init__(self, group_id, planner):

        rospy.init_node('moveit_test_planners', anonymous=True)
        self.pass_list = []
        self.fail_list = []
        self.planner = planner
        self.scene = PlanningSceneInterface()
        self.robot = RobotCommander()
        self.group = MoveGroupCommander(group_id)
        rospy.sleep(1)

        self.group.set_planner_id(self.planner)

        self.test_trajectories_empty_environment()
        self.test_waypoints()
        self.test_trajectories_with_walls_and_ground()

        for pass_test in self.pass_list:
            print(pass_test)

        for fail_test in self.fail_list:
            print(fail_test)

    def _add_walls_and_ground(self):
        # publish a scene
        p = geometry_msgs.msg.PoseStamped()
        p.header.frame_id = self.robot.get_planning_frame()

        p.pose.position.x = 0
        p.pose.position.y = 0
        # offset such that the box is below ground (to prevent collision with
        # the robot itself)
        p.pose.position.z = -0.11
        p.pose.orientation.x = 0
        p.pose.orientation.y = 0
        p.pose.orientation.z = 0
        p.pose.orientation.w = 1
        self.scene.add_box("ground", p, (3, 3, 0.1))

        p.pose.position.x = 0.4
        p.pose.position.y = 0.85
        p.pose.position.z = 0.4
        p.pose.orientation.x = 0.5
        p.pose.orientation.y = -0.5
        p.pose.orientation.z = 0.5
        p.pose.orientation.w = 0.5
        self.scene.add_box("wall_front", p, (0.8, 2, 0.01))

        p.pose.position.x = 1.33
        p.pose.position.y = 0.4
        p.pose.position.z = 0.4
        p.pose.orientation.x = 0.0
        p.pose.orientation.y = -0.707388
        p.pose.orientation.z = 0.0
        p.pose.orientation.w = 0.706825
        self.scene.add_box("wall_right", p, (0.8, 2, 0.01))

        p.pose.position.x = -0.5
        p.pose.position.y = 0.4
        p.pose.position.z = 0.4
        p.pose.orientation.x = 0.0
        p.pose.orientation.y = -0.707107
        p.pose.orientation.z = 0.0
        p.pose.orientation.w = 0.707107
        self.scene.add_box("wall_left", p, (0.8, 2, 0.01))
        # rospy.sleep(1)

    def _check_plan(self, plan):
        if len(plan.joint_trajectory.points) > 0:
            return True
        else:
            return False

    def _plan_joints(self, joints):
        self.group.clear_pose_targets()
        group_variable_values = self.group.get_current_joint_values()
        group_variable_values[0:6] = joints[0:6]
        self.group.set_joint_value_target(group_variable_values)

        plan = self.group.plan()
        return self._check_plan(plan)

    def test_trajectories_rotating_each_joint(self):
        # test_joint_values = [numpy.pi/2.0, numpy.pi-0.33, -numpy.pi/2]
        test_joint_values = [numpy.pi / 2.0]
        joints = [0.0, 0.0, 0.0, -numpy.pi / 2.0, 0.0, 0.0]
        # Joint 4th is colliding with the hand
        # for joint in range(6):
        for joint in [0, 1, 2, 3, 5]:
            for value in test_joint_values:
                joints[joint] = value
            if not self._plan_joints(joints):
                self.fail_list.append(
                    "Failed: test_trajectories_rotating_each_joint, " +
                    self.planner + ", joints:" + str(joints))
            else:
                self.pass_list.append(
                    "Passed: test_trajectories_rotating_each_joint, " +
                    self.planner + ", joints:" + str(joints))

    def test_trajectories_empty_environment(self):
        # Up - Does not work with sbpl but it does with ompl
        joints = [0.0, -1.99, 2.19, 0.58, 0.0, -0.79, 0.0, 0.0]
        if not self._plan_joints(joints):
            self.fail_list.append(
                "Failed: test_trajectories_empty_environment, " +
                self.planner + ", joints:" + str(joints))
        else:
            self.pass_list.append(
                "Passed: test_trajectories_empty_environment, " +
                self.planner + ", joints:" + str(joints))

        # All joints up
        joints = [
            -1.67232, -2.39104, 0.264862, 0.43346, 2.44148, 2.48026, 0.0, 0.0
        ]
        if not self._plan_joints(joints):
            self.fail_list.append(
                "Failed: test_trajectories_empty_environment, " +
                self.planner + ", joints:" + str(joints))
        else:
            self.pass_list.append(
                "Passed: test_trajectories_empty_environment, " +
                self.planner + ", joints:" + str(joints))

        # Down
        joints = [
            -0.000348431194526, 0.397651011661, 0.0766181197394,
            -0.600353691727, -0.000441966540076, 0.12612019707, 0.0, 0.0
        ]
        if not self._plan_joints(joints):
            self.fail_list.append(
                "Failed: test_trajectories_empty_environment, " +
                self.planner + ", joints:" + str(joints))
        else:
            self.pass_list.append(
                "Passed: test_trajectories_empty_environment, " +
                self.planner + ", joints:" + str(joints))

        # left
        joints = [
            0.146182953165, -2.6791929848, -0.602721109682, -3.00575848765,
            0.146075718452, 0.00420656698366, 0.0, 0.0
        ]
        if not self._plan_joints(joints):
            self.fail_list.append(
                "Failed: test_trajectories_empty_environment, " +
                self.planner + ", joints:" + str(joints))
        else:
            self.pass_list.append(
                "Passed: test_trajectories_empty_environment, " +
                self.planner + ", joints:" + str(joints))

        # Front
        joints = [
            1.425279839, -0.110370375874, -1.52548746261, -1.50659865247,
            -1.42700242769, 3.1415450794, 0.0, 0.0
        ]
        if not self._plan_joints(joints):
            self.fail_list.append(
                "Failed: test_trajectories_empty_environment, " +
                self.planner + ", joints:" + str(joints))
        else:
            self.pass_list.append(
                "Passed: test_trajectories_empty_environment, " +
                self.planner + ", joints:" + str(joints))

        # Behind
        joints = [
            1.57542451065, 3.01734161219, 2.01043257686, -1.14647092839,
            0.694689321451, -0.390769365032, 0.0, 0.0
        ]
        if not self._plan_joints(joints):
            self.fail_list.append(
                "Failed: test_trajectories_empty_environment, " +
                self.planner + ", joints:" + str(joints))
        else:
            self.pass_list.append(
                "Passed: test_trajectories_empty_environment, " +
                self.planner + ", joints:" + str(joints))

        # Should fail because it is in self-collision
        joints = [
            -0.289797803762, 2.37263860495, 2.69118483159, 1.65486712181,
            1.04235601797, -1.69730925867, 0.0, 0.0
        ]
        if not self._plan_joints(joints):
            self.fail_list.append(
                "Failed: test_trajectories_empty_environment, " +
                self.planner + ", joints:" + str(joints))
        else:
            self.pass_list.append(
                "Passed: test_trajectories_empty_environment, " +
                self.planner + ", joints:" + str(joints))

    def test_waypoints(self):
        # Start planning in a given joint position
        joints = [
            -0.324590029242, -1.42602359749, -1.02523472017, -0.754761892979,
            0.344227622185, -3.03250264451, 0.0, 0.0
        ]
        current = RobotState()
        current.joint_state.name = self.robot.get_current_state(
        ).joint_state.name
        current_joints = list(
            self.robot.get_current_state().joint_state.position)
        current_joints[0:8] = joints
        current.joint_state.position = current_joints

        self.group.set_start_state(current)

        waypoints = []

        initial_pose = self.group.get_current_pose().pose

        initial_pose.position.x = -0.301185959729
        initial_pose.position.y = 0.517069787724
        initial_pose.position.z = 1.20681710541
        initial_pose.orientation.x = 0.0207499700474
        initial_pose.orientation.y = -0.723943002716
        initial_pose.orientation.z = -0.689528413407
        initial_pose.orientation.w = 0.00515118111221

        # start with a specific position
        waypoints.append(initial_pose)

        # first move it down
        wpose = geometry_msgs.msg.Pose()
        wpose.orientation = waypoints[0].orientation
        wpose.position.x = waypoints[0].position.x
        wpose.position.y = waypoints[0].position.y
        wpose.position.z = waypoints[0].position.z - 0.20
        waypoints.append(copy.deepcopy(wpose))

        # second front
        wpose.position.y += 0.20
        waypoints.append(copy.deepcopy(wpose))

        # third side
        wpose.position.x -= 0.20
        waypoints.append(copy.deepcopy(wpose))

        # fourth return to initial pose
        wpose = waypoints[0]
        waypoints.append(copy.deepcopy(wpose))

        (plan3,
         fraction) = self.group.compute_cartesian_path(waypoints, 0.01, 0.0)
        if not self._check_plan(plan3) and fraction > 0.8:
            self.fail_list.append("Failed: test_waypoints, " + self.planner)
        else:
            self.pass_list.append("Passed: test_waypoints, " + self.planner)

    def test_trajectories_with_walls_and_ground(self):
        self._add_walls_and_ground()

        # Should fail to plan: Goal is in collision with the wall_front
        joints = [
            0.302173213174, 0.192487443763, -1.94298265002, 1.74920382275,
            0.302143499777, 0.00130280337897, 0.0, 0.0
        ]
        if not self._plan_joints(joints):
            self.fail_list.append(
                "Failed: test_trajectories_with_walls_and_ground, " +
                self.planner + ", joints:" + str(joints))
        else:
            self.pass_list.append(
                "Passed: test_trajectories_with_walls_and_ground, " +
                self.planner + ", joints:" + str(joints))

        # Should fail to plan: Goal is in collision with the ground
        joints = [
            3.84825722288e-05, 0.643694953509, -1.14391175311, 1.09463824437,
            0.000133883149666, -0.594498939239, 0.0, 0.0
        ]
        if not self._plan_joints(joints):
            self.fail_list.append(
                "Failed: test_trajectories_with_walls_and_ground, " +
                self.planner + ", joints:" + str(joints))
        else:
            self.pass_list.append(
                "Passed: test_trajectories_with_walls_and_ground, " +
                self.planner + ", joints:" + str(joints))

        # Goal close to right corner
        joints = [
            0.354696232081, -0.982224980654, 0.908055961723, -1.92328051116,
            -1.3516255551, 2.8225061435, 0.0, 0.0
        ]
        if not self._plan_joints(joints):
            self.fail_list.append(
                "Failed: test_trajectories_with_walls_and_ground, " +
                self.planner + ", joints:" + str(joints))
        else:
            self.pass_list.append(
                "Passed, test_trajectories_with_walls_and_ground, " +
                self.planner + ", joints:" + str(joints))

        self.scene.remove_world_object("ground")
        self.scene.remove_world_object("wall_left")
        self.scene.remove_world_object("wall_right")
        self.scene.remove_world_object("wall_front")
Esempio n. 29
0
    def __init__(self, pickPos, placePos):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)

        #        rospy.init_node('moveit_demo')

        # Use the planning scene object to add or remove objects
        scene = PlanningSceneInterface()

        # Create a scene publisher to push changes to the scene
        self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)

        # Create a publisher for displaying gripper poses
        self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped)

        # Create a dictionary to hold object colors
        self.colors = dict()

        # Initialize the move group for the right arm
        pick_arm = MoveGroupCommander(GROUP_NAME_ARM)

        # Initialize the momitive group for the right gripper
        pick_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)

        # Get the name of the end-effector link
        end_effector_link = pick_arm.get_end_effector_link()

        # Allow some leeway in position (meters) and orientation (radians)
        pick_arm.set_goal_position_tolerance(0.05)
        pick_arm.set_goal_orientation_tolerance(0.1)

        # Allow replanning to increase the odds of a solution
        pick_arm.allow_replanning(True)

        # Set the right arm reference frame
        pick_arm.set_pose_reference_frame(REFERENCE_FRAME)

        #pick_arm.set_planner_id("RRTConnectkConfigDefault")
        #pick_arm.set_planner_id("RRTstarkConfigDefault")
        pick_arm.set_planner_id("RRTstarkConfigDefault")
        # Allow 5 seconds per planning attempt
        pick_arm.set_planning_time(3)

        # Set a limit on the number of pick attempts before bailing
        max_pick_attempts = 5

        # Set a limit on the number of place attempts
        max_place_attempts = 5

        # Give the scene a chance to catch up
        rospy.sleep(2)

        # Give each of the scene objects a unique name
        base_table_id = 'base_table'
        target_id = 'target'
        limit_table_id = 'limit_table'
        #tool_id = 'tool'

        # Remove leftover objects from a previous run
        scene.remove_world_object(base_table_id)
        #scene.remove_world_object(table_id)
        #scene.remove_world_object(box1_id)
        #scene.remove_world_object(box2_id)
        scene.remove_world_object(target_id)
        #scene.remove_world_object(tool_id)
        scene.remove_world_object(limit_table_id)

        # Remove any attached objects from a previous session
        scene.remove_attached_object(GRIPPER_FRAME, target_id)

        # Open the gripper to the neutral position
        # pick_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
        # pick_gripper.go()

        rospy.sleep(1)

        # Set the height of the table off the ground
        # table_ground = 0.6
        table_ground = 0.0

        # Set the dimensions of the scene objects [l, w, h]
        base_table_size = [2, 2, 0.04]
        #table_size = [0.2, 0.7, 0.01]
        #box1_size = [0.1, 0.05, 0.05]
        #box2_size = [0.05, 0.05, 0.15]

        # Set the target size [l, w, h]
        target_size = [0.055, 0.055, 0.12]
        target_radius = 0.03305
        target_height = 0.12310
        limit_table_size = [0.6, 1.8, 0.04]

        # Add a base table to the scene
        base_table_pose = PoseStamped()
        base_table_pose.header.frame_id = REFERENCE_FRAME
        base_table_pose.pose.position.x = 0.0
        base_table_pose.pose.position.y = 0.0
        base_table_pose.pose.position.z = -0.3
        base_table_pose.pose.orientation.w = 1.0
        scene.add_box(base_table_id, base_table_pose, base_table_size)

        # Give the scene a chance to catch up
        rospy.sleep(1)

        # Start the arm in the "resting" pose stored in the SRDF file
        pick_arm.set_named_target(ARM_HOME_POSE)
        pick_arm.go()

        rospy.sleep(1)

        # Add a table top and two boxes to the scene
        #table_pose = PoseStamped()
        #table_pose.header.frame_id = REFERENCE_FRAME
        #table_pose.pose.position.x = 0.5
        #table_pose.pose.position.y = -0.4
        #table_pose.pose.position.z = table_ground + table_size[2] / 2.0
        #table_pose.pose.orientation.w = 1.0
        #scene.add_box(table_id, table_pose, table_size)

        # Set the target pose in between the boxes and on the table
        target_pose = PoseStamped()
        target_pose.header.frame_id = REFERENCE_FRAME

        target_pose.pose.position.x = pickPos.pose.position.x
        target_pose.pose.position.y = pickPos.pose.position.y
        target_pose.pose.position.z = pickPos.pose.position.z
        target_pose.pose.orientation.w = pickPos.pose.orientation.w

        # Add the target object to the scene
        scene.add_box(target_id, target_pose, target_size)
        #scene.add_cylinder(target_id,target_pose,target_height,target_radius)

        #Add the limit_table object to the scene
        limit_table_pose = PoseStamped()
        limit_table_pose.header.frame_id = REFERENCE_FRAME
        limit_table_pose.pose.position.x = 0.58
        limit_table_pose.pose.position.y = -0.4
        limit_table_pose.pose.position.z = 0.18
        limit_table_pose.pose.orientation.w = 1.0
        #scene.add_box(limit_table_id, limit_table_pose, limit_table_size)
        self.setColor(limit_table_id, 0.6, 0.2, 0.2, 1.0)

        # Make the table red and the boxes orange
        #self.setColor(table_id, 0.8, 0, 0, 1.0)
        #        self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
        #        self.setColor(box2_id, 0.8, 0.4, 0, 1.0)

        # Make the target yellow
        self.setColor(target_id, 0.9, 0.9, 0, 1.0)

        # Send the colors to the planning scene
        self.sendColors()

        # Set the support surface name to the table object
        #pick_arm.set_support_surface_name(table_id)

        # Specify a pose to place the target after being picked up
        #指定拾取后的放置目标姿态
        place_pose = PoseStamped()
        place_pose.header.frame_id = REFERENCE_FRAME
        place_pose.pose.position.x = placePos.pose.position.x
        place_pose.pose.position.y = placePos.pose.position.y
        place_pose.pose.position.z = placePos.pose.position.z
        place_pose.pose.orientation.w = placePos.pose.orientation.w
        place_pose.pose.orientation.x = placePos.pose.orientation.x
        place_pose.pose.orientation.y = placePos.pose.orientation.y
        place_pose.pose.orientation.z = placePos.pose.orientation.z

        #设置机器人运行最大速度位百分之~
        pick_arm.set_max_velocity_scaling_factor(0.5)

        # Initialize the grasp pose to the target pose
        #初始化抓取目标点位
        grasp_pose = target_pose

        # Shift the grasp pose by half the width of the target to center it
        # this way is just used by PR2 robot
        #grasp_pose.pose.position.y -= target_size[1] / 2.0

        # Generate a list of grasps
        grasps = self.make_grasps(grasp_pose, [target_id])

        # Publish the grasp poses so they can be viewed in RV
        for grasp in grasps:
            self.gripper_pose_pub.publish(grasp.grasp_pose)
            rospy.sleep(0.2)

        # Track success/failure and number of attempts for pick operation
        result = None
        n_attempts = 0

        # Repeat until we succeed or run out of attempts
        #重复直到成功或者超出尝试的次数
        #while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
        while result != 1 and n_attempts < max_pick_attempts:
            n_attempts += 1
            rospy.loginfo("Pick attempt: " + str(n_attempts))
            #moveit中的pick接口,target_id为moveit添加场景的id,此处为目标物体,grasps为可尝试抓取的点位序列
            result = pick_arm.pick(target_id, grasps)
            #打印信息
            rospy.logerr("pick_arm.pick: " + str(result))
            rospy.sleep(0.2)
        # If the pick was successful, attempt the place operation
#如果抓取成功,尝试放置操作
        result_g = None
        n_attempts = 2
        #if result == MoveItErrorCodes.SUCCESS:
        while result_g != True and n_attempts < max_pick_attempts:  #and result == 1:
            # Generate valid place poses
            #places = self.make_places(place_pose)
            n_attempts += 1
            print("-------------------")
            print(place_pose)
            #更新当前的机械臂状态
            pick_arm.set_start_state_to_current_state()
            #设置moveit运动的目标点位
            pick_arm.set_pose_target(place_pose)
            # Repeat until we succeed or run out of attempts
            #while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts:
            #    n_attempts += 1
            #    rospy.loginfo("Place attempt: " +  str(n_attempts))
            #    for place in places:
            #        result = pick_arm.place(target_id, place)
            #        if result == MoveItErrorCodes.SUCCESS:
            #            break
            #    rospy.sleep(0.2)
            #moveit的运动接口,wait=True表示等到执行完成才返回
            result_g = pick_arm.go(wait=True)
            rospy.logerr("pick_arm.go: " + str(result_g))
            #打开夹爪
            open_client(500)
            rospy.sleep(0.2)
            #if result == "False"
            #    rospy.logerr("Place operation failed after " + str(n_attempts) + " attempts.")
        #else:
        #    rospy.loginfo("Pick operation failed after " + str(n_attempts) + " attempts.")

        # Return the arm to the "home" pose stored in the SRDF file
#将机械臂返回到SRDF中的“home”姿态
#scene.remove_world_object(target_id)
#scene.remove_attached_object(GRIPPER_FRAME, target_id)
#scene.remove_world_object(target_id)
        rospy.sleep(1)
        open_client(500)
        pick_arm.set_named_target(ARM_HOME_POSE)
        pick_arm.go()
        # Open the gripper to the neutral position
        # pick_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
        # pick_gripper.go()

        rospy.sleep(1)
        rospy.logerr("pick_server over ")
Esempio n. 30
0
    msg = Float64()
    msg.data = -1.0
    for i in range(30):
        pub.publish(msg)
        rospy.sleep(0.1)


if __name__ == '__main__':
    rospy.init_node('task_2_cheat', anonymous=True)

    # for for 5 sec
    rospy.sleep(5)

    rospy.loginfo("start program %f" % rospy.get_time())
    arm = MoveGroupCommander("ur5_arm")
    arm.set_planner_id('RRTConnectkConfigDefault')
    pub = rospy.Publisher("/r_gripper_controller/command",
                          Float64,
                          queue_size=1)
    client = actionlib.SimpleActionClient(
        'arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
    client.wait_for_server()

    rospy.loginfo("init pose")
    msg = FollowJointTrajectoryGoal()
    msg.trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.2)
    msg.trajectory.joint_names = [
        'ur5_arm_shoulder_pan_joint', 'ur5_arm_shoulder_lift_joint',
        'ur5_arm_elbow_joint', 'ur5_arm_wrist_1_joint',
        'ur5_arm_wrist_2_joint', 'ur5_arm_wrist_3_joint'
    ]
Esempio n. 31
0
def close_hand():
    msg = Float64()
    msg.data = -1.0
    for i in range(30):
        pub.publish(msg)
        rospy.sleep(0.1)

if __name__ == '__main__':
    rospy.init_node('task_2_cheat', anonymous=True)

    # for for 5 sec
    rospy.sleep(5)

    rospy.loginfo("start program %f" % rospy.get_time())
    arm = MoveGroupCommander("ur5_arm")
    arm.set_planner_id('RRTConnectkConfigDefault')
    pub = rospy.Publisher("/r_gripper_controller/command", Float64, queue_size=1)
    client = actionlib.SimpleActionClient('arm_controller/follow_joint_trajectory', FollowJointTrajectoryAction)
    client.wait_for_server()

    rospy.loginfo("init pose")
    msg = FollowJointTrajectoryGoal()
    msg.trajectory.header.stamp = rospy.Time.now() + rospy.Duration(0.2)
    msg.trajectory.joint_names = ['ur5_arm_shoulder_pan_joint',
                                  'ur5_arm_shoulder_lift_joint',
                                  'ur5_arm_elbow_joint',
                                  'ur5_arm_wrist_1_joint',
                                  'ur5_arm_wrist_2_joint',
                                  'ur5_arm_wrist_3_joint']
    msg.trajectory.points.append(JointTrajectoryPoint(positions=[-1.57, -0.1745, -2.79, -1.57, 0, 0],
                                                      time_from_start=rospy.Duration(2)))
Esempio n. 32
0
from moveit_commander import RobotCommander, MoveGroupCommander
from moveit_commander import PlanningSceneInterface, roscpp_initialize, roscpp_shutdown
from geometry_msgs.msg import PoseStamped
from moveit_msgs.msg import Grasp, GripperTranslation, PlaceLocation
from trajectory_msgs.msg import JointTrajectoryPoint

if __name__ == '__main__':

    roscpp_initialize(sys.argv)
    rospy.init_node('moveit_py_demo', anonymous=True)
    GRIPPER_FRAME = 'right_gripper_link'
    scene = PlanningSceneInterface()
    robot = RobotCommander()
    right_arm = MoveGroupCommander("right_arm")
    right_gripper = MoveGroupCommander("right_gripper")
    right_arm.set_planner_id("KPIECEkConfigDefault")
    rospy.sleep(1)

    # clean the scene
    scene.remove_world_object("table")
    scene.remove_world_object("part")
    scene.remove_attached_object(GRIPPER_FRAME, "part")
    #rospy.logwarn("cleaning world")
    #right_arm.set_named_target("r_start")
    #right_arm.go()

    #right_gripper.set_named_target("open")
    #right_gripper.go()

    rospy.sleep(3)
Esempio n. 33
0
from geometry_msgs.msg import PoseStamped
import moveit_msgs.msg
import geometry_msgs.msg
import moveit_commander

if __name__=='__main__':

    roscpp_initialize(sys.argv)
    rospy.init_node('move_demo', anonymous=True)
    
    scene = PlanningSceneInterface()
    robot = RobotCommander()
    group = MoveGroupCommander("manipulator")

    # specify the planner
    group.set_planner_id("RRTkConfigDefault")

    rospy.sleep(3)

    ## We create this DisplayTrajectory publisher which is used below to publish
    ## trajectories for RVIZ to visualize.

    # display_trajectory_publisher = rospy.Publisher(
    #                                   '/move_group/display_planned_path',
    #                                   moveit_msgs.msg.DisplayTrajectory)

    print ">>>>> remove scenes"

    # clean the scene
    scene.remove_world_object("pole")
    scene.remove_world_object("ball")
Esempio n. 34
0
    def __init__(self):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)

        rospy.init_node('moveit_demo')

        # Use the planning scene object to add or remove objects
        scene = PlanningSceneInterface()

        # Create a scene publisher to push changes to the scene
        self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)

        # Create a publisher for displaying gripper poses
        self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped)

        # Initialize the move group for the right arm
        Arm = MoveGroupCommander(GROUP_NAME_ARM)

        Arm.set_planner_id("RRTstarkConfigDefault")
        # Initialize the move group for the right gripper
        Hand = MoveGroupCommander(GROUP_NAME_GRIPPER)

        # Get the name of the end-effector link
        end_effector_link = Arm.get_end_effector_link()

        # Allow some leeway in position (meters) and orientation (radians)
        Arm.set_goal_position_tolerance(0.05)
        Arm.set_goal_orientation_tolerance(0.1)

        # Allow replanning to increase the odds of a solution
        Arm.allow_replanning(True)

        # Set the right arm reference frame
        Arm.set_pose_reference_frame(REFERENCE_FRAME)

        # Allow 5 seconds per planning attempt
        Arm.set_planning_time(10)

        # Set a limit on the number of pick attempts before bailing
        max_pick_attempts = 3

        # Set a limit on the number of place attempts
        max_place_attempts = 3

        # Give the scene a chance to catch up
        rospy.sleep(2)

        # Give each of the scene objects a unique name

        target_id = 'target'

        # Remove leftover objects from a previous run

        scene.remove_world_object(target_id)

        # Remove any attached objects from a previous session
        scene.remove_attached_object(GRIPPER_FRAME, target_id)

        # Give the scene a chance to catch up
        rospy.sleep(1)

        # Open the gripper to the neutral position
        #Hand.set_joint_value_target(GRIPPER_NEUTRAL)
        #Hand.go()

        #rospy.sleep(1)

        target_size = [0.01, 0.01, 0.01]

        # Set the target pose in between the boxes and on the table
        target_pose = PoseStamped()
        target_pose.header.frame_id = REFERENCE_FRAME
        target_pose.pose.position.x = 0.3
        target_pose.pose.position.y = 0.03
        target_pose.pose.position.z = -0.3
        target_pose.pose.orientation.w = 1.0

        # Add the target object to the scene
        scene.add_box(target_id, target_pose, target_size)

        # Initialize the grasp pose to the target pose
        grasp_pose = PoseStamped()
        grasp_pose.header.frame_id = REFERENCE_FRAME
        grasp_pose.pose = target_pose.pose
        grasp_pose.pose.position.y = 0.2

        # Generate a list of grasps
        grasps = self.make_grasps(grasp_pose, [target_id])

        # Publish the grasp poses so they can be viewed in RViz
        for grasp in grasps:
            self.gripper_pose_pub.publish(grasp.grasp_pose)
            rospy.sleep(0.2)

        # Track success/failure and number of attempts for pick operation
        result = None
        n_attempts = 0

        # Repeat until we succeed or run out of attempts
        while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
            n_attempts += 1
            rospy.loginfo("Pick attempt: " + str(n_attempts))
            result = Arm.pick(target_id, grasps)
            rospy.sleep(0.2)

        rospy.sleep(1)

        # Shut down MoveIt cleanly
        moveit_commander.roscpp_shutdown()

        # Exit the script
        moveit_commander.os._exit(0)
Esempio n. 35
0
class CalibrationMovements:
    def __init__(self,
                 move_group_name,
                 max_velocity_scaling,
                 max_acceleration_scaling,
                 angle_delta,
                 translation_delta,
                 move_group_namespace='/'):
        # self.client = HandeyeClient()  # TODO: move around marker when eye_on_hand, automatically take samples via trigger topic
        if not move_group_namespace.endswith('/'):
            move_group_namespace = move_group_namespace + '/'
        if move_group_namespace != '/':
            self.mgc = MoveGroupCommander(
                move_group_name,
                robot_description=move_group_namespace + 'robot_description',
                ns=move_group_namespace)
        else:
            self.mgc = MoveGroupCommander(move_group_name)
        self.mgc.set_planner_id("RRTConnectkConfigDefault"
                                )  # TODO: this is only needed for the UR5
        self.mgc.set_max_velocity_scaling_factor(max_velocity_scaling)
        self.mgc.set_max_acceleration_scaling_factor(max_acceleration_scaling)
        self.start_pose = self.mgc.get_current_pose()
        self.joint_limits = [math.radians(90)] * 5 + [math.radians(180)] + [
            math.radians(350)
        ]  # TODO: make param
        self.target_poses = []
        self.current_pose_index = None
        self.current_plan = None
        self.fallback_joint_limits = [math.radians(90)] * 4 + [
            math.radians(90)
        ] + [math.radians(180)] + [math.radians(350)]
        if len(self.mgc.get_active_joints()) == 6:
            self.fallback_joint_limits = self.fallback_joint_limits[1:]
        self.angle_delta = angle_delta
        self.translation_delta = translation_delta

    def set_and_check_starting_position(self):
        # sets the starting position to the current robot cartesian EE position and checks movement in all directions
        # TODO: make repeatable
        #  - save the home position and each target position as joint position
        #  - plan to each target position and back, save plan if not crazy
        #  - return true if can plan to each target position without going crazy
        self.start_pose = self.mgc.get_current_pose()
        self.target_poses = self._compute_poses_around_state(
            self.start_pose, self.angle_delta, self.translation_delta)
        self.current_pose_index = -1
        ret = self._check_target_poses(self.joint_limits)
        if ret:
            rospy.loginfo("Set current pose as home")
            return True
        else:
            rospy.logerr("Can't calibrate from this position!")
            self.start_pose = None
            self.target_poses = None
            return False

    def select_target_pose(self, i):
        number_of_target_poses = len(self.target_poses)
        if 0 <= i < number_of_target_poses:
            rospy.loginfo("Selected pose {} for next movement".format(i))
            self.current_pose_index = i
            return True
        else:
            rospy.logerr(
                "Index {} is out of bounds: there are {} target poses".format(
                    i, number_of_target_poses))
            return False

    def plan_to_start_pose(self):
        # TODO: use joint position http://docs.ros.org/melodic/api/moveit_tutorials/html/doc/move_group_python_interface/move_group_python_interface_tutorial.html#planning-to-a-joint-goal
        rospy.loginfo("Planning to home pose")
        return self._plan_to_pose(self.start_pose)

    def plan_to_current_target_pose(self):
        # TODO: use joint position http://docs.ros.org/melodic/api/moveit_tutorials/html/doc/move_group_python_interface/move_group_python_interface_tutorial.html#planning-to-a-joint-goal
        i = self.current_pose_index
        rospy.loginfo("Planning to target pose {}".format(i))
        return self._plan_to_pose(self.target_poses[i])

    def execute_plan(self):
        if self.plan is None:
            rospy.logerr("No plan found!")
            return False
        if CalibrationMovements._is_crazy_plan(self.plan,
                                               self.fallback_joint_limits):
            rospy.logerr("Crazy plan found, not executing!")
            return False
        self.mgc.execute(self.plan)
        return True

    def _plan_to_pose(self, pose):
        self.mgc.set_start_state_to_current_state()
        self.mgc.set_pose_target(pose)
        ret = self.mgc.plan()
        if type(ret) is tuple:
            # noetic
            success, plan, planning_time, error_code = ret
        else:
            # melodic
            plan = ret
        if CalibrationMovements._is_crazy_plan(plan,
                                               self.fallback_joint_limits):
            rospy.logwarn("Planning failed")
            self.plan = None
            return False
        else:
            rospy.loginfo("Planning successful")
            self.plan = plan
            return True

    def _check_target_poses(self, joint_limits):
        if len(self.fallback_joint_limits) == 6:
            joint_limits = joint_limits[1:]
        for fp in self.target_poses:
            self.mgc.set_pose_target(fp)
            ret = self.mgc.plan()
            if type(ret) is tuple:
                # noetic
                success, plan, planning_time, error_code = ret
            else:
                # melodic
                plan = ret
            if len(plan.joint_trajectory.points
                   ) == 0 or CalibrationMovements._is_crazy_plan(
                       plan, joint_limits):
                return False
        return True

    @staticmethod
    def _compute_poses_around_state(start_pose, angle_delta,
                                    translation_delta):
        basis = np.eye(3)

        pos_deltas = [
            quaternion_from_euler(*rot_axis * angle_delta)
            for rot_axis in basis
        ]
        neg_deltas = [
            quaternion_from_euler(*rot_axis * (-angle_delta))
            for rot_axis in basis
        ]

        quaternion_deltas = list(
            chain.from_iterable(zip(pos_deltas, neg_deltas)))  # interleave

        final_rots = []
        for qd in quaternion_deltas:
            final_rots.append(list(qd))

        # TODO: accept a list of delta values

        pos_deltas = [
            quaternion_from_euler(*rot_axis * angle_delta / 2)
            for rot_axis in basis
        ]
        neg_deltas = [
            quaternion_from_euler(*rot_axis * (-angle_delta / 2))
            for rot_axis in basis
        ]

        quaternion_deltas = list(
            chain.from_iterable(zip(pos_deltas, neg_deltas)))  # interleave
        for qd in quaternion_deltas:
            final_rots.append(list(qd))

        final_poses = []
        for rot in final_rots:
            fp = deepcopy(start_pose)
            ori = fp.pose.orientation
            combined_rot = quaternion_multiply([ori.x, ori.y, ori.z, ori.w],
                                               rot)
            fp.pose.orientation = Quaternion(*combined_rot)
            final_poses.append(fp)

        fp = deepcopy(start_pose)
        fp.pose.position.x += translation_delta / 2
        final_poses.append(fp)

        fp = deepcopy(start_pose)
        fp.pose.position.x -= translation_delta / 2
        final_poses.append(fp)

        fp = deepcopy(start_pose)
        fp.pose.position.y += translation_delta
        final_poses.append(fp)

        fp = deepcopy(start_pose)
        fp.pose.position.y -= translation_delta
        final_poses.append(fp)

        fp = deepcopy(start_pose)
        fp.pose.position.z += translation_delta / 3
        final_poses.append(fp)

        return final_poses

    @staticmethod
    def _rot_per_joint(plan, degrees=False):
        np_traj = np.array([p.positions for p in plan.joint_trajectory.points])
        if len(np_traj) == 0:
            raise ValueError
        np_traj_max_per_joint = np_traj.max(axis=0)
        np_traj_min_per_joint = np_traj.min(axis=0)
        ret = abs(np_traj_max_per_joint - np_traj_min_per_joint)
        if degrees:
            ret = [math.degrees(j) for j in ret]
        return ret

    @staticmethod
    def _is_crazy_plan(plan, max_rotation_per_joint):
        abs_rot_per_joint = CalibrationMovements._rot_per_joint(plan)
        if (abs_rot_per_joint > max_rotation_per_joint).any():
            return True
        else:
            return False
                                        rospy.Duration(1.0))

    # Instantiate a robotcommander object
    robot = moveit_commander.RobotCommander()
    # Connect to the manipulator move group and get end effector link
    manipulator = MoveGroupCommander('manipulator')
    end_effector_link = manipulator.get_end_effector_link()
    # Set the robot reference frame
    manipulator.set_pose_reference_frame('rb07_stand_link')
    # Allow replanning to increase the odds of a solution
    manipulator.allow_replanning(True)
    # Set accuracy in position(meters) and orientation (radians)
    manipulator.set_goal_position_tolerance(0.00001)
    manipulator.set_goal_orientation_tolerance(0.0001)

    manipulator.set_planner_id("RRTkConfigDefault")

    tr = tool_tf.transform.translation
    print([tr.x, tr.y, tr.z])

    # Get a pose to modify
    start_pose = manipulator.get_current_pose(end_effector_link).pose

    # go home
    manipulator.set_named_target('all-zeros')
    manipulator.go()
    rospy.sleep(1)
    # go to vise
    go_to_pose('vise', manipulator, start_pose)
    # go to g54
    go_to_pose('G54', manipulator, start_pose)
Esempio n. 37
0
class PlannerAnnotationParser(AnnotationParserBase):
    """
    Parses the annotations files that contains the benchmarking
    information.
    """
    def __init__(self, path_to_annotation, path_to_data):
        super(PlannerAnnotationParser, self).__init__(path_to_annotation,
                                                      path_to_data)
        self.parse()

        self._load_scene()
        self._init_planning()
        self.benchmark()

    def check_results(self, results):
        """
        Returns the results from the planner, checking them against any eventual validation data
        (no validation data in our case).
        """
        return self.planner_data

    def _load_scene(self):
        """
        Loads the proper scene for the planner.
        It can be either a python static scene or bag containing an occupancy map.
        """
        scene = self._annotations["scene"]

        for element in scene:
            if element["type"] == "launch":
                self.play_launch(element["name"])
            elif element["type"] == "python":
                self.load_python(element["name"])
            elif element["type"] == "bag":
                self.play_bag(element["name"])
                for _ in range(150):
                    rospy.sleep(0.3)

        # wait for the scene to be spawned properly
        rospy.sleep(0.5)

    def _init_planning(self):
        """
        Initialises the needed connections for the planning.
        """

        self.group_id = self._annotations["group_id"]
        self.planners = self._annotations["planners"]
        self.scene = PlanningSceneInterface()
        self.robot = RobotCommander()
        self.group = MoveGroupCommander(self.group_id)
        self._marker_pub = rospy.Publisher('/visualization_marker_array',
                                           MarkerArray,
                                           queue_size=10,
                                           latch=True)
        self._planning_time_sub = rospy.Subscriber(
            '/move_group/result', MoveGroupActionResult,
            self._check_computation_time)
        rospy.sleep(1)

        self.group.set_num_planning_attempts(
            self._annotations["planning_attempts"])

        self.group.set_goal_tolerance(self._annotations["goal_tolerance"])
        self.group.set_planning_time(self._annotations["planning_time"])
        self.group.allow_replanning(self._annotations["allow_replanning"])

        self._comp_time = []

        self.planner_data = []

    def benchmark(self):
        for test_id, test in enumerate(self._annotations["tests"]):
            marker_position_1 = test["start_xyz"]
            marker_position_2 = test["goal_xyz"]

            self._add_markers(marker_position_1, "Start test \n sequence",
                              marker_position_2, "Goal")

            # Start planning in a given joint position
            joints = test["start_joints"]
            current = RobotState()
            current.joint_state.name = self.robot.get_current_state(
            ).joint_state.name
            current_joints = list(
                self.robot.get_current_state().joint_state.position)
            current_joints[0:6] = joints
            current.joint_state.position = current_joints

            self.group.set_start_state(current)
            joints = test["goal_joints"]

            for planner in self.planners:
                if planner == "stomp":
                    planner = "STOMP"
                elif planner == "sbpl":
                    planner = "AnytimeD*"
                self.planner_id = planner
                self.group.set_planner_id(planner)
                self._plan_joints(
                    joints,
                    self._annotations["name"] + "-test_" + str(test_id))

        return self.planner_data

    def _add_markers(self, point, text1, point_2, text2):
        # add marker for start and goal pose of EE
        marker_array = MarkerArray()
        marker_1 = Marker()
        marker_1.header.frame_id = "world"
        marker_1.header.stamp = rospy.Time.now()
        marker_1.type = Marker.SPHERE
        marker_1.scale.x = 0.04
        marker_1.scale.y = 0.04
        marker_1.scale.z = 0.04
        marker_1.lifetime = rospy.Duration()

        marker_2 = deepcopy(marker_1)

        marker_1.color.g = 0.5
        marker_1.color.a = 1.0
        marker_1.id = 0
        marker_1.pose.position.x = point[0]
        marker_1.pose.position.y = point[1]
        marker_1.pose.position.z = point[2]
        marker_2.color.r = 0.5
        marker_2.color.a = 1.0
        marker_2.id = 1
        marker_2.pose.position.x = point_2[0]
        marker_2.pose.position.y = point_2[1]
        marker_2.pose.position.z = point_2[2]

        marker_3 = Marker()
        marker_3.header.frame_id = "world"
        marker_3.header.stamp = rospy.Time.now()
        marker_3.type = Marker.TEXT_VIEW_FACING
        marker_3.scale.z = 0.10
        marker_3.lifetime = rospy.Duration()

        marker_4 = deepcopy(marker_3)

        marker_3.text = text1
        marker_3.id = 2
        marker_3.color.g = 0.5
        marker_3.color.a = 1.0
        marker_3.pose.position.x = point[0]
        marker_3.pose.position.y = point[1]
        marker_3.pose.position.z = point[2] + 0.15
        marker_4.text = text2
        marker_4.id = 3
        marker_4.color.r = 0.5
        marker_4.color.a = 1.0
        marker_4.pose.position.x = point_2[0]
        marker_4.pose.position.y = point_2[1]
        marker_4.pose.position.z = point_2[2] + 0.15

        marker_array.markers = [marker_1, marker_2, marker_3, marker_4]

        self._marker_pub.publish(marker_array)
        rospy.sleep(1)

    def _plan_joints(self, joints, test_name):
        # plan to joint target and determine success
        self.group.clear_pose_targets()
        group_variable_values = self.group.get_current_joint_values()
        group_variable_values[0:6] = joints[0:6]
        self.group.set_joint_value_target(group_variable_values)

        plan = self.group.plan()
        plan_time = "N/A"
        total_joint_rotation = "N/A"
        comp_time = "N/A"

        plan_success = self._check_plan_success(plan)
        if plan_success:
            plan_time = self._check_plan_time(plan)
            total_joint_rotation = self._check_plan_total_rotation(plan)
            while not self._comp_time:
                rospy.sleep(0.5)
            comp_time = self._comp_time.pop(0)
        self.planner_data.append([
            self.planner_id, test_name,
            str(plan_success), plan_time, total_joint_rotation, comp_time
        ])

    @staticmethod
    def _check_plan_success(plan):
        if len(plan.joint_trajectory.points) > 0:
            return True
        else:
            return False

    @staticmethod
    def _check_plan_time(plan):
        # find duration of successful plan
        number_of_points = len(plan.joint_trajectory.points)
        time = plan.joint_trajectory.points[number_of_points -
                                            1].time_from_start.to_sec()
        return time

    @staticmethod
    def _check_plan_total_rotation(plan):
        # find total joint rotation in successful trajectory
        angles = [0, 0, 0, 0, 0, 0]
        number_of_points = len(plan.joint_trajectory.points)
        for i in range(number_of_points - 1):
            angles_temp = [
                abs(x - y)
                for x, y in zip(plan.joint_trajectory.points[i + 1].positions,
                                plan.joint_trajectory.points[i].positions)
            ]
            angles = [x + y for x, y in zip(angles, angles_temp)]

        total_angle_change = sum(angles)
        return total_angle_change

    def _check_computation_time(self, msg):
        # get computation time for successful plan to be found
        if msg.status.status == 3:
            self._comp_time.append(msg.result.planning_time)

    def _check_path_length(self, plan):
        # find distance travelled by end effector
        # not yet implemented
        return
Esempio n. 38
0
    def __init__(self):
        # Initialize the move_group API
        moveit_commander.roscpp_initialize(sys.argv)
        
        rospy.init_node('moveit_demo')
        
        # Construct the initial scene object
        scene = PlanningSceneInterface()
        
        # Create a scene publisher to push changes to the scene
        self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
        
        # Create a dictionary to hold object colors
        self.colors = dict()
        
        # Pause for the scene to get ready
        rospy.sleep(1)
                        
        # Initialize the move group for the right arm
        right_arm = MoveGroupCommander('right_arm')
        # Initialize the move group for the left arm
        left_arm = MoveGroupCommander('left_arm')

        right_arm.set_planner_id("KPIECEkConfigDefault");
        left_arm.set_planner_id("KPIECEkConfigDefault");
        rospy.sleep(1)
        
        # Get the name of the end-effector link
        end_effector_link = right_arm.get_end_effector_link()
        
        # Allow some leeway in position (meters) and orientation (radians)
        right_arm.set_goal_position_tolerance(0.01)
        right_arm.set_goal_orientation_tolerance(0.05)
       
        # Allow replanning to increase the odds of a solution
        right_arm.allow_replanning(True)
        
        # Set the reference frame for pose targets
        reference_frame = 'base_footprint'
        
        # Set the right arm reference frame accordingly
        right_arm.set_pose_reference_frame(reference_frame)
        
        # Allow 5 seconds per planning attempt
        right_arm.set_planning_time(5)
        
        # Give each of the scene objects a unique name
        table_id = 'table'
        box1_id = 'box1'
        box2_id = 'box2'
        
        # Remove leftover objects from a previous run
        scene.remove_world_object(table_id)
        scene.remove_world_object(box1_id)
        scene.remove_world_object(box2_id)
        
        # Give the scene a chance to catch up
        rospy.sleep(1)
        
        # Start the arm in the "resting" pose stored in the SRDF file
        #left_arm.set_named_target('left_start')
        #left_arm.go()

        # Start the arm in the "resting" pose stored in the SRDF file
        right_arm.set_named_target('right_start')
        right_arm.go()
        
        rospy.sleep(2)
        
        # Set the height of the table off the ground
        table_ground = 0.75
        
        # Set the length, width and height of the table and boxes
        table_size = [0.2, 0.7, 0.01]
        box1_size = [0.1, 0.05, 0.05]
        box2_size = [0.05, 0.05, 0.15]
        
        # Add a table top and two boxes to the scene
        table_pose = PoseStamped()
        table_pose.header.frame_id = reference_frame
        table_pose.pose.position.x = 0.56
        table_pose.pose.position.y = 0.0
        table_pose.pose.position.z = table_ground + table_size[2] / 2.0
        table_pose.pose.orientation.w = 1.0
        scene.add_box(table_id, table_pose, table_size)
        
        box1_pose = PoseStamped()
        box1_pose.header.frame_id = reference_frame
        box1_pose.pose.position.x = 0.51
        box1_pose.pose.position.y = -0.1
        box1_pose.pose.position.z = table_ground + table_size[2] + box1_size[2] / 2.0
        box1_pose.pose.orientation.w = 1.0   
        scene.add_box(box1_id, box1_pose, box1_size)
        
        box2_pose = PoseStamped()
        box2_pose.header.frame_id = reference_frame
        box2_pose.pose.position.x = 0.49
        box2_pose.pose.position.y = 0.15
        box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0
        box2_pose.pose.orientation.w = 1.0   
        scene.add_box(box2_id, box2_pose, box2_size)
        
        # Make the table red and the boxes orange
        self.setColor(table_id, 0.8, 0, 0, 1.0)
        self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
        self.setColor(box2_id, 0.8, 0.4, 0, 1.0)
        
        # Send the colors to the planning scene
        self.sendColors()    
        
        # Set the target pose in between the boxes and above the table
        target_pose = PoseStamped()
        target_pose.header.frame_id = reference_frame
        target_pose.pose.position.x = 0.4
        target_pose.pose.position.y = 0.0
        target_pose.pose.position.z = table_pose.pose.position.z + table_size[2] + 0.05
        target_pose.pose.orientation.w = 1.0
        
        # Set the target pose for the arm
        right_arm.set_pose_target(target_pose, end_effector_link)
        
        # Move the arm to the target pose (if possible)
        right_arm.go()
        
        # Pause for a moment...
        rospy.sleep(2)
        

        # Return the arm to the "resting" pose stored in the SRDF file
        right_arm.set_named_target('right_start')
        right_arm.go()
        
        # Exit MoveIt cleanly
        moveit_commander.roscpp_shutdown()
        
        # Exit the script        
        moveit_commander.os._exit(0)
Esempio n. 39
0
class SrRobotCommander(object):
    """
    Base class for hand and arm commanders
    """
    def __init__(self, name):
        """
        Initialize MoveGroupCommander object
        @param name - name of the MoveIt group
        """
        self._name = name
        self._move_group_commander = MoveGroupCommander(name)

        self._robot_commander = RobotCommander()

        self._robot_name = self._robot_commander._r.get_robot_name()

        self.refresh_named_targets()

        self._warehouse_name_get_srv = rospy.ServiceProxy(
            "get_robot_state", GetState)
        self._planning_scene = PlanningSceneInterface()

        self._joint_states_lock = threading.Lock()
        self._joint_states_listener = \
            rospy.Subscriber("joint_states", JointState,
                             self._joint_states_callback, queue_size=1)
        self._joints_position = {}
        self._joints_velocity = {}
        self._joints_effort = {}
        self._joints_state = None
        self._clients = {}
        self.__plan = None

        self._controllers = {}

        rospy.wait_for_service('compute_ik')
        self._compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
        self._forward_k = rospy.ServiceProxy('compute_fk', GetPositionFK)

        controller_list_param = rospy.get_param("/move_group/controller_list")

        # create dictionary with name of controllers and corresponding joints
        self._controllers = {
            item["name"]: item["joints"]
            for item in controller_list_param
        }

        self._set_up_action_client(self._controllers)

        self.tf_buffer = tf2_ros.Buffer()
        self.listener = tf2_ros.TransformListener(self.tf_buffer)

        threading.Thread(None, rospy.spin)

    def set_planner_id(self, planner_id):
        self._move_group_commander.set_planner_id(planner_id)

    def set_num_planning_attempts(self, num_planning_attempts):
        self._move_group_commander.set_num_planning_attempts(
            num_planning_attempts)

    def set_planning_time(self, seconds):
        self._move_group_commander.set_planning_time(seconds)

    def get_end_effector_pose_from_named_state(self, name):
        state = self._warehouse_name_get_srv(name, self._robot_name).state
        return self.get_end_effector_pose_from_state(state)

    def get_end_effector_pose_from_state(self, state):
        header = Header()
        fk_link_names = [self._move_group_commander.get_end_effector_link()]
        header.frame_id = self._move_group_commander.get_pose_reference_frame()
        response = self._forward_k(header, fk_link_names, state)
        return response.pose_stamped[0]

    def get_planning_frame(self):
        return self._move_group_commander.get_planning_frame()

    def set_pose_reference_frame(self, reference_frame):
        self._move_group_commander.set_pose_reference_frame(reference_frame)

    def get_group_name(self):
        return self._name

    def refresh_named_targets(self):
        self._srdf_names = self.__get_srdf_names()
        self._warehouse_names = self.__get_warehouse_names()

    def set_max_velocity_scaling_factor(self, value):
        self._move_group_commander.set_max_velocity_scaling_factor(value)

    def set_max_acceleration_scaling_factor(self, value):
        self._move_group_commander.set_max_acceleration_scaling_factor(value)

    def allow_looking(self, value):
        self._move_group_commander.allow_looking(value)

    def allow_replanning(self, value):
        self._move_group_commander.allow_replanning(value)

    def execute(self):
        """
        Executes the last plan made.
        """
        if self.check_plan_is_valid():
            self._move_group_commander.execute(self.__plan)
            self.__plan = None
        else:
            rospy.logwarn("No plans were made, not executing anything.")

    def execute_plan(self, plan):
        if self.check_given_plan_is_valid(plan):
            self._move_group_commander.execute(plan)
            self.__plan = None
        else:
            rospy.logwarn("Plan is not valid, not executing anything.")

    def move_to_joint_value_target(self,
                                   joint_states,
                                   wait=True,
                                   angle_degrees=False):
        """
        Set target of the robot's links and moves to it.
        @param joint_states - dictionary with joint name and value. It can
        contain only joints values of which need to be changed.
        @param wait - should method wait for movement end or not
        @param angle_degrees - are joint_states in degrees or not
        """
        joint_states_cpy = copy.deepcopy(joint_states)

        if angle_degrees:
            joint_states_cpy.update(
                (joint, radians(i)) for joint, i in joint_states_cpy.items())
        self._move_group_commander.set_start_state_to_current_state()
        self._move_group_commander.set_joint_value_target(joint_states_cpy)
        self._move_group_commander.go(wait=wait)

    def plan_to_joint_value_target(self, joint_states, angle_degrees=False):
        """
        Set target of the robot's links and plans.
        @param joint_states - dictionary with joint name and value. It can
        contain only joints values of which need to be changed.
        @param angle_degrees - are joint_states in degrees or not
        This is a blocking method.
        """
        joint_states_cpy = copy.deepcopy(joint_states)

        if angle_degrees:
            joint_states_cpy.update(
                (joint, radians(i)) for joint, i in joint_states_cpy.items())
        self._move_group_commander.set_start_state_to_current_state()
        self._move_group_commander.set_joint_value_target(joint_states_cpy)
        self.__plan = self._move_group_commander.plan()
        return self.__plan

    def check_plan_is_valid(self):
        """
        Checks if current plan contains a valid trajectory
        """
        return (self.__plan is not None
                and len(self.__plan.joint_trajectory.points) > 0)

    def check_given_plan_is_valid(self, plan):
        """
        Checks if given plan contains a valid trajectory
        """
        return (plan is not None and len(plan.joint_trajectory.points) > 0)

    def get_robot_name(self):
        return self._robot_name

    def named_target_in_srdf(self, name):
        return name in self._srdf_names

    def set_named_target(self, name):
        if name in self._srdf_names:
            self._move_group_commander.set_named_target(name)
        elif (name in self._warehouse_names):
            response = self._warehouse_name_get_srv(name, self._robot_name)

            active_names = self._move_group_commander._g.get_active_joints()
            joints = response.state.joint_state.name
            positions = response.state.joint_state.position
            js = {}

            for n, this_name in enumerate(joints):
                if this_name in active_names:
                    js[this_name] = positions[n]
            self._move_group_commander.set_joint_value_target(js)
        else:
            rospy.logerr("Unknown named state '%s'..." % name)
            return False
        return True

    def get_named_target_joint_values(self, name):
        output = dict()

        if (name in self._srdf_names):
            output = self._move_group_commander.\
                           _g.get_named_target_values(str(name))

        elif (name in self._warehouse_names):
            js = self._warehouse_name_get_srv(
                name, self._robot_name).state.joint_state

            for x, n in enumerate(js.name):
                if n in self._move_group_commander._g.get_joints():
                    output[n] = js.position[x]

        else:
            rospy.logerr("No target named %s" % name)

            return None

        return output

    def get_end_effector_link(self):
        return self._move_group_commander.get_end_effector_link()

    def get_current_pose(self, reference_frame=None):
        """
        Get the current pose of the end effector.
        @param reference_frame - The desired reference frame in which end effector pose should be returned.
        If none is passed, it will use the planning frame as reference.
        @return geometry_msgs.msg.Pose() - current pose of the end effector
        """
        if reference_frame is not None:
            try:
                trans = self.tf_buffer.lookup_transform(
                    reference_frame,
                    self._move_group_commander.get_end_effector_link(),
                    rospy.Time(0), rospy.Duration(5.0))
                current_pose = geometry_msgs.msg.Pose()
                current_pose.position.x = trans.transform.translation.x
                current_pose.position.y = trans.transform.translation.y
                current_pose.position.z = trans.transform.translation.z
                current_pose.orientation.x = trans.transform.rotation.x
                current_pose.orientation.y = trans.transform.rotation.y
                current_pose.orientation.z = trans.transform.rotation.z
                current_pose.orientation.w = trans.transform.rotation.w
                return current_pose
            except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
                    tf2_ros.ExtrapolationException):
                rospy.logwarn(
                    "Couldn't get the pose from " +
                    self._move_group_commander.get_end_effector_link() +
                    " in " + reference_frame + " reference frame")
            return None
        else:
            return self._move_group_commander.get_current_pose().pose

    def get_current_state(self):
        """
        Get the current joint state of the group being used.
        @return a dictionary with the joint names as keys and current joint values
        """
        joint_names = self._move_group_commander._g.get_active_joints()
        joint_values = self._move_group_commander._g.get_current_joint_values()

        return dict(zip(joint_names, joint_values))

    def get_current_state_bounded(self):
        """
        Get the current joint state of the group being used, enforcing that they are within each joint limits.
        @return a dictionary with the joint names as keys and current joint values
        """
        current = self._move_group_commander._g.get_current_state_bounded()
        names = self._move_group_commander._g.get_active_joints()
        output = {n: current[n] for n in names if n in current}

        return output

    def get_robot_state_bounded(self):
        return self._move_group_commander._g.get_current_state_bounded()

    def move_to_named_target(self, name, wait=True):
        """
        Set target of the robot's links and moves to it
        @param name - name of the target pose defined in SRDF
        @param wait - should method wait for movement end or not
        """
        self._move_group_commander.set_start_state_to_current_state()
        if self.set_named_target(name):
            self._move_group_commander.go(wait=wait)

    def plan_to_named_target(self, name):
        """
        Set target of the robot's links and plans
        This is a blocking method.
        @param name - name of the target pose defined in SRDF
        """
        self._move_group_commander.set_start_state_to_current_state()
        if self.set_named_target(name):
            self.__plan = self._move_group_commander.plan()

    def __get_warehouse_names(self):
        try:
            list_srv = rospy.ServiceProxy("list_robot_states", ListStates)
            return list_srv("", self._robot_name).states

        except rospy.ServiceException as exc:
            rospy.logwarn("Couldn't access warehouse: " + str(exc))
            return list()

    def _reset_plan(self):
        self.__plan = None

    def _set_plan(self, plan):
        self.__plan = plan

    def __get_srdf_names(self):
        return self._move_group_commander._g.get_named_targets()

    def get_named_targets(self):
        """
        Get the complete list of named targets, from SRDF
        as well as warehouse poses if available.
        @return list of strings containing names of targets.
        """
        return self._srdf_names + self._warehouse_names

    def get_joints_position(self):
        """
        Returns joints position
        @return - dictionary with joints positions
        """
        with self._joint_states_lock:
            return self._joints_position

    def get_joints_velocity(self):
        """
        Returns joints velocities
        @return - dictionary with joints velocities
        """
        with self._joint_states_lock:
            return self._joints_velocity

    def _get_joints_effort(self):
        """
        Returns joints effort
        @return - dictionary with joints efforts
        """
        with self._joint_states_lock:
            return self._joints_effort

    def get_joints_state(self):
        """
        Returns joints state
        @return - JointState message
        """
        with self._joint_states_lock:
            return self._joints_state

    def run_joint_trajectory(self, joint_trajectory):
        """
        Moves robot through all joint states with specified timeouts
        @param joint_trajectory - JointTrajectory class object. Represents
        trajectory of the joints which would be executed.
        """
        plan = RobotTrajectory()
        plan.joint_trajectory = joint_trajectory
        self._move_group_commander.execute(plan)

    def make_named_trajectory(self, trajectory):
        """
        Makes joint value trajectory from specified by named poses (either from
        SRDF or from warehouse)
        @param trajectory - list of waypoints, each waypoint is a dict with
                            the following elements (n.b either name or joint_angles is required)
                            - name -> the name of the way point
                            - joint_angles -> a dict of joint names and angles
                            - interpolate_time -> time to move from last wp
                            - pause_time -> time to wait at this wp
                            - degrees -> set to true if joint_angles is specified in degrees. Assumed false if absent.
        """
        current = self.get_current_state_bounded()

        joint_trajectory = JointTrajectory()
        joint_names = current.keys()
        joint_trajectory.joint_names = joint_names

        start = JointTrajectoryPoint()
        start.positions = current.values()
        start.time_from_start = rospy.Duration.from_sec(0.001)
        joint_trajectory.points.append(start)

        time_from_start = 0.0

        for wp in trajectory:

            joint_positions = None
            if 'name' in wp.keys():
                joint_positions = self.get_named_target_joint_values(
                    wp['name'])
            elif 'joint_angles' in wp.keys():
                joint_positions = copy.deepcopy(wp['joint_angles'])
                if 'degrees' in wp.keys() and wp['degrees']:
                    for joint, angle in joint_positions.iteritems():
                        joint_positions[joint] = radians(angle)

            if joint_positions is None:
                rospy.logerr(
                    "Invalid waypoint. Must contain valid name for named target or dict of joint angles."
                )
                return None

            new_positions = {}

            for n in joint_names:
                new_positions[n] = joint_positions[
                    n] if n in joint_positions else current[n]

            trajectory_point = JointTrajectoryPoint()
            trajectory_point.positions = [
                new_positions[n] for n in joint_names
            ]

            current = new_positions

            time_from_start += wp['interpolate_time']
            trajectory_point.time_from_start = rospy.Duration.from_sec(
                time_from_start)
            joint_trajectory.points.append(trajectory_point)

            if 'pause_time' in wp and wp['pause_time'] > 0:
                extra = JointTrajectoryPoint()
                extra.positions = trajectory_point.positions
                time_from_start += wp['pause_time']
                extra.time_from_start = rospy.Duration.from_sec(
                    time_from_start)
                joint_trajectory.points.append(extra)

        return joint_trajectory

    def send_stop_trajectory_unsafe(self):
        """
        Sends a trajectory of all active joints at their current position.
        This stops the robot.
        """

        current = self.get_current_state_bounded()

        trajectory_point = JointTrajectoryPoint()
        trajectory_point.positions = current.values()
        trajectory_point.time_from_start = rospy.Duration.from_sec(0.1)

        trajectory = JointTrajectory()
        trajectory.points.append(trajectory_point)
        trajectory.joint_names = current.keys()

        self.run_joint_trajectory_unsafe(trajectory)

    def run_named_trajectory_unsafe(self, trajectory, wait=False):
        """
        Moves robot through trajectory specified by named poses, either from
        SRDF or from warehouse. Runs trajectory directly via contoller.
        @param trajectory - list of waypoints, each waypoint is a dict with
                            the following elements:
                            - name -> the name of the way point
                            - interpolate_time -> time to move from last wp
                            - pause_time -> time to wait at this wp
        """
        joint_trajectory = self.make_named_trajectory(trajectory)
        if joint_trajectory is not None:
            self.run_joint_trajectory_unsafe(joint_trajectory, wait)

    def run_named_trajectory(self, trajectory):
        """
        Moves robot through trajectory specified by named poses, either from
        SRDF or from warehouse. Runs trajectory via moveit.
        @param trajectory - list of waypoints, each waypoint is a dict with
                            the following elements:
                            - name -> the name of the way point
                            - interpolate_time -> time to move from last wp
                            - pause_time -> time to wait at this wp
        """
        joint_trajectory = self.make_named_trajectory(trajectory)
        if joint_trajectory is not None:
            self.run_joint_trajectory(joint_trajectory)

    def move_to_position_target(self, xyz, end_effector_link="", wait=True):
        """
        Specify a target position for the end-effector and moves to it
        @param xyz - new position of end-effector
        @param end_effector_link - name of the end effector link
        @param wait - should method wait for movement end or not
        """
        self._move_group_commander.set_start_state_to_current_state()
        self._move_group_commander.set_position_target(xyz, end_effector_link)
        self._move_group_commander.go(wait=wait)

    def plan_to_position_target(self, xyz, end_effector_link=""):
        """
        Specify a target position for the end-effector and plans.
        This is a blocking method.
        @param xyz - new position of end-effector
        @param end_effector_link - name of the end effector link
        """
        self._move_group_commander.set_start_state_to_current_state()
        self._move_group_commander.set_position_target(xyz, end_effector_link)
        self.__plan = self._move_group_commander.plan()

    def move_to_pose_target(self, pose, end_effector_link="", wait=True):
        """
        Specify a target pose for the end-effector and moves to it
        @param pose - new pose of end-effector: a Pose message, a PoseStamped
        message or a list of 6 floats: [x, y, z, rot_x, rot_y, rot_z] or a list
        of 7 floats [x, y, z, qx, qy, qz, qw]
        @param end_effector_link - name of the end effector link
        @param wait - should method wait for movement end or not
        """
        self._move_group_commander.set_start_state_to_current_state()
        self._move_group_commander.set_pose_target(pose, end_effector_link)
        self._move_group_commander.go(wait=wait)

    def plan_to_pose_target(self,
                            pose,
                            end_effector_link="",
                            alternative_method=False):
        """
        Specify a target pose for the end-effector and plans.
        This is a blocking method.
        @param pose - new pose of end-effector: a Pose message, a PoseStamped
        message or a list of 6 floats: [x, y, z, rot_x, rot_y, rot_z] or a list
        of 7 floats [x, y, z, qx, qy, qz, qw]
        @param end_effector_link - name of the end effector link
        @param alternative_method - use set_joint_value_target instead of set_pose_target
        """
        self._move_group_commander.set_start_state_to_current_state()
        if alternative_method:
            self._move_group_commander.set_joint_value_target(
                pose, end_effector_link)
        else:
            self._move_group_commander.set_pose_target(pose, end_effector_link)
        self.__plan = self._move_group_commander.plan()
        return self.__plan

    def _joint_states_callback(self, joint_state):
        """
        The callback function for the topic joint_states.
        It will store the received joint position, velocity and efforts
        information into dictionaries
        @param joint_state - the message containing the joints data.
        """
        with self._joint_states_lock:
            self._joints_state = joint_state
            self._joints_position = {
                n: p
                for n, p in zip(joint_state.name, joint_state.position)
            }
            self._joints_velocity = {
                n: v
                for n, v in zip(joint_state.name, joint_state.velocity)
            }
            self._joints_effort = {
                n: v
                for n, v in zip(joint_state.name, joint_state.effort)
            }

    def _set_up_action_client(self, controller_list):
        """
        Sets up an action client to communicate with the trajectory controller
        """
        self._action_running = {}

        for controller_name in controller_list.keys():
            self._action_running[controller_name] = False
            service_name = controller_name + "/follow_joint_trajectory"
            self._clients[controller_name] = SimpleActionClient(
                service_name, FollowJointTrajectoryAction)
            if self._clients[controller_name].wait_for_server(
                    timeout=rospy.Duration(4)) is False:
                err_msg = 'Failed to connect to action server ({}) in 4 sec'.format(
                    service_name)
                rospy.logwarn(err_msg)

    def move_to_joint_value_target_unsafe(self,
                                          joint_states,
                                          time=0.002,
                                          wait=True,
                                          angle_degrees=False):
        """
        Set target of the robot's links and moves to it.
        @param joint_states - dictionary with joint name and value. It can
        contain only joints values of which need to be changed.
        @param time - time in s (counting from now) for the robot to reach the
        target (it needs to be greater than 0.0 for it not to be rejected by
        the trajectory controller)
        @param wait - should method wait for movement end or not
        @param angle_degrees - are joint_states in degrees or not
        """
        # self._update_default_trajectory()
        # self._set_targets_to_default_trajectory(joint_states)
        goals = {}
        joint_states_cpy = copy.deepcopy(joint_states)

        if angle_degrees:
            joint_states_cpy.update(
                (joint, radians(i)) for joint, i in joint_states_cpy.items())

        for controller in self._controllers:
            controller_joints = self._controllers[controller]
            goal = FollowJointTrajectoryGoal()
            goal.trajectory.joint_names = []
            point = JointTrajectoryPoint()
            point.positions = []

            for x in joint_states_cpy.keys():
                if x in controller_joints:
                    goal.trajectory.joint_names.append(x)
                    point.positions.append(joint_states_cpy[x])

            point.time_from_start = rospy.Duration.from_sec(time)

            goal.trajectory.points = [point]

            goals[controller] = goal

        self._call_action(goals)

        if not wait:
            return

        for i in self._clients.keys():
            if not self._clients[i].wait_for_result():
                rospy.loginfo("Trajectory not completed")

    def action_is_running(self, controller=None):
        if controller is not None:
            return self._action_running[controller]

        for controller_running in self._action_running.values():
            if controller_running:
                return True
        return False

    def _action_done_cb(self, controller, terminal_state, result):
        self._action_running[controller] = False

    def _call_action(self, goals):
        for client in self._clients:
            self._action_running[client] = True
            self._clients[client].send_goal(
                goals[client],
                lambda terminal_state, result: self._action_done_cb(
                    client, terminal_state, result))

    def run_joint_trajectory_unsafe(self, joint_trajectory, wait=True):
        """
        Moves robot through all joint states with specified timeouts
        @param joint_trajectory - JointTrajectory class object. Represents
        trajectory of the joints which would be executed.
        @param wait - should method wait for movement end or not
        """
        goals = {}
        for controller in self._controllers:
            controller_joints = self._controllers[controller]
            goal = FollowJointTrajectoryGoal()
            goal.trajectory = copy.deepcopy(joint_trajectory)

            indices_of_joints_in_this_controller = []

            for i, joint in enumerate(joint_trajectory.joint_names):
                if joint in controller_joints:
                    indices_of_joints_in_this_controller.append(i)

            goal.trajectory.joint_names = [
                joint_trajectory.joint_names[i]
                for i in indices_of_joints_in_this_controller
            ]

            for point in goal.trajectory.points:
                if point.positions:
                    point.positions = [
                        point.positions[i]
                        for i in indices_of_joints_in_this_controller
                    ]
                if point.velocities:
                    point.velocities = [
                        point.velocities[i]
                        for i in indices_of_joints_in_this_controller
                    ]
                if point.effort:
                    point.effort = [
                        point.effort[i]
                        for i in indices_of_joints_in_this_controller
                    ]

            goals[controller] = goal

        self._call_action(goals)

        if not wait:
            return

        for i in self._clients.keys():
            if not self._clients[i].wait_for_result():
                rospy.loginfo("Trajectory not completed")

    def plan_to_waypoints_target(self,
                                 waypoints,
                                 reference_frame=None,
                                 eef_step=0.005,
                                 jump_threshold=0.0):
        """
        Specify a set of waypoints for the end-effector and plans.
        This is a blocking method.
        @param reference_frame - the reference frame in which the waypoints are given
        @param waypoints - an array of poses of end-effector
        @param eef_step - configurations are computed for every eef_step meters
        @param jump_threshold - maximum distance in configuration space between consecutive points in the resulting path
        """
        old_frame = self._move_group_commander.get_pose_reference_frame()
        if reference_frame is not None:
            self.set_pose_reference_frame(reference_frame)
        (self.__plan,
         fraction) = self._move_group_commander.compute_cartesian_path(
             waypoints, eef_step, jump_threshold)
        self.set_pose_reference_frame(old_frame)

    def set_teach_mode(self, teach):
        """
        Activates/deactivates the teach mode for the robot.
        Activation: stops the the trajectory controllers for the robot, and
        sets it to teach mode.
        Deactivation: stops the teach mode and starts trajectory controllers
        for the robot.
        Currently this method blocks for a few seconds when called on a hand,
        while the hand parameters are reloaded.
        @param teach - bool to activate or deactivate teach mode
        """

        if teach:
            mode = RobotTeachModeRequest.TEACH_MODE
        else:
            mode = RobotTeachModeRequest.TRAJECTORY_MODE
        self.change_teach_mode(mode, self._name)

    def move_to_trajectory_start(self, trajectory, wait=True):
        """
        Make and execute a plan from the current state to the first state in an pre-existing trajectory
        @param trajectory - moveit_msgs/JointTrajectory
        @param wait - Bool to specify if movement should block untill finished.
        """

        if len(trajectory.points) <= 0:
            rospy.logerr("Trajectory has no points in it, can't reverse...")
            return None

        first_point = trajectory.points[0]
        end_state = dict(zip(trajectory.joint_names, first_point.positions))
        self.move_to_joint_value_target(end_state, wait=wait)

    @staticmethod
    def change_teach_mode(mode, robot):
        teach_mode_client = rospy.ServiceProxy('/teach_mode', RobotTeachMode)

        req = RobotTeachModeRequest()
        req.teach_mode = mode
        req.robot = robot
        try:
            resp = teach_mode_client(req)
            if resp.result == RobotTeachModeResponse.ERROR:
                rospy.logerr("Failed to change robot %s to mode %d", robot,
                             mode)
            else:
                rospy.loginfo("Changed robot %s to mode %d Result = %d", robot,
                              mode, resp.result)
        except rospy.ServiceException:
            rospy.logerr("Failed to call service teach_mode")

    def get_ik(self, target_pose, avoid_collisions=False, joint_states=None):
        """
        Computes the inverse kinematics for a given pose. It returns a JointState
        @param target_pose - A given pose of type PoseStamped
        @param avoid_collisions - Find an IK solution that avoids collisions. By default, this is false
        """
        service_request = PositionIKRequest()
        service_request.group_name = self._name
        service_request.ik_link_name = self._move_group_commander.get_end_effector_link(
        )
        service_request.pose_stamped = target_pose
        service_request.timeout.secs = 0.5
        service_request.avoid_collisions = avoid_collisions
        if joint_states is None:
            service_request.robot_state.joint_state = self.get_joints_state()
        else:
            service_request.robot_state.joint_state = joint_states

        try:
            resp = self._compute_ik(ik_request=service_request)
            # Check if error_code.val is SUCCESS=1
            if resp.error_code.val != 1:
                if resp.error_code.val == -10:
                    rospy.logerr("Unreachable point: Start state in collision")
                elif resp.error_code.val == -12:
                    rospy.logerr("Unreachable point: Goal state in collision")
                elif resp.error_code.val == -31:
                    rospy.logerr("Unreachable point: No IK solution")
                else:
                    rospy.logerr("Unreachable point (error: %s)" %
                                 resp.error_code)
                return
            else:
                return resp.solution.joint_state

        except rospy.ServiceException, e:
            rospy.logerr("Service call failed: %s" % e)
Esempio n. 40
0
class Arm_Controller:
	def __init__(self):
		# Give the launch a chance to catch up
		# rospy.sleep(5)

		# Initialize the move_group API
		moveit_commander.roscpp_initialize(sys.argv)

		rospy.init_node('Arm_Controller')
		rospy.loginfo("Launched Arm Controller")

		# constants
		self.GROUP_NAME_ARM = 'arm'
		self.GRIPPER_FRAME = 'gripper_link'
		self.REFERENCE_FRAME = 'base_link'
		self.ARM_BASE_FRAME = 'arm_base_link'

		self.done = True

		self.test_pose_publisher = rospy.Publisher('/test_arm_pose', PoseStamped)

		rospy.Subscriber("/arm_target_pose", PoseStamped, self.move_arm_to_pose, queue_size=1)
		self.robot_name = "gatlin"
		move_arm_service = createService('gatlin/move/arm', MoveRobot, self.handle_move_arm)

		# We need a tf listener to convert poses into arm reference base
		self.tfl = tf.TransformListener()

		# Initialize the move group for the right arm
		self.arm = MoveGroupCommander(self.GROUP_NAME_ARM)
		self.gripper = Gripper()

		self.robot = moveit_commander.RobotCommander()

		# Allow replanning to increase the odds of a solution
		self.arm.allow_replanning(True)

		# Set the planner
		self.arm.set_planner_id("RRTConnectkConfigDefault")

		# Set the right arm reference frame
		self.arm.set_pose_reference_frame(self.REFERENCE_FRAME)

		# Give the scene a chance to catch up
		rospy.sleep(1)

		# Allow some leeway in position (meters) and orientation (radians)
		# USELESS; do not work on pick and place! Explained on this issue:
		# https://github.com/ros-planning/moveit_ros/issues/577
		self.arm.set_goal_position_tolerance(0.005)
		self.arm.set_goal_orientation_tolerance(0.05)

		# Allow 2 seconds per planning attempt
		self.arm.set_planning_time(2.0)

		# Create a quaternion from the Euler angles
		#q = quaternion_from_euler(0, pitch, yaw)
		# horiz = Quaternion(-0.023604, 0.99942, 0.00049317, 0.024555)
		# deg45 = Quaternion(-0.022174, 0.9476, 0.0074215, 0.31861)
		down = Quaternion(-0.00035087, 0.73273, 0.00030411, 0.68052)
		# back_pos = Point(-0.03, 0.0313, 0.476)

		# init rest pose
		self.rest_pose = PoseStamped()
		self.rest_pose.header.frame_id = self.REFERENCE_FRAME
		self.rest_pose.pose.position = Point(-0.06, 0.00, 0.35)
		self.rest_pose.pose.orientation = Quaternion(0.0251355325061, 0.982948881414, -0.0046583987932, 0.182093384981)

		# init place_upper pose
		self.place_upper_pose = PoseStamped()
		self.place_upper_pose.header.frame_id = self.REFERENCE_FRAME
		self.place_upper_pose.pose.position = Point(0.24713, -0.0051618, 0.37998)
		self.place_upper_pose.pose.orientation = Quaternion(0.0030109, 0.1472, -0.020231, 0.9889)

		# init current pose
		self.current_pose = PoseStamped()
		self.current_pose.header.frame_id = self.REFERENCE_FRAME
		self.current_pose.pose.position = Point(0,0,0)
		self.current_pose.pose.orientation = down

		# Open the gripper
		rospy.loginfo("Set Gripper: open")
		self.gripper.set(1.0)

		# self.arm.set_pose_target(self.rest_pose)
		# self.arm.go()
		# rospy.sleep(1)

		# self.ar = ArbotixROS()

		# rate = rospy.Rate(30)
		# while not rospy.is_shutdown():
			
		# 	# rospy.logerr(self.ar.getVoltage(4))
		# 	# rospy.logerr(self.ar.getSpeed(5))
		# 	rospy.logerr(self.ar.getPosition(1))

		# 	rate.sleep()

		rospy.spin()

	def MoveToPoseWithIntermediate(self, ps, offsets) :
		success = False
		for offset in offsets:
			# interpose = getOffsetPose(hand_pose, offset)
			interpose = getOffsetPose(ps, offset)
			success = self.MoveToPose(interpose, "MoveToIntermediatePose")
			rospy.sleep(1)

		success = self.MoveToPose(ps, "MoveToPose")

		return success

	def MoveToPose(self, ps, name) :
		newpose = self.transform_pose(self.REFERENCE_FRAME, ps)
		down = Quaternion(-0.00035087, 0.73273, 0.00030411, 0.68052)
		newpose.pose.orientation = down

		if self.move_arm_to_pose(newpose) :
			rospy.loginfo("SUCCEEDED: %s" % name)
			return True
		else :
			rospy.logerr("FAILED %s" % name)
			return False

	def move_arm_to_pose(self, ps):
		arm_target_pose = deepcopy(ps)
		arm_target_pose.header.stamp = rospy.Time.now()
		
		self.test_pose_publisher.publish(arm_target_pose)
		
		self.arm.set_pose_target(arm_target_pose)
		success = self.arm.go()

		return success

	def handle_move_arm(self, req):
		success = True
		gripper = self.gripper

		if req.action == "OPEN_GRIPPER":
			rospy.loginfo("Beginning to open gripper")
			gripper.open(block=True)
			rospy.loginfo("Opened Gripper")

		elif req.action == "CLOSE_GRIPPER" :
			rospy.loginfo("Beginning to close Gripper")
			gripper.close(block=True)
			rospy.loginfo("Closed Gripper")

		elif req.action == "MOVE_TO_POSE_INTERMEDIATE" :
			rospy.loginfo("Trying to Move To Pose With Intermediate")
			offsets = [Vector3(0,0,.07)]
			success = self.MoveToPoseWithIntermediate(req.ps, offsets)

		elif req.action == "MOVE_TO_POSE" :
			rospy.loginfo("Trying to Move To Pose")
			success = self.MoveToPose(req.ps, "FAILED MoveToPose")

		elif req.action == "RESET_ARM" :
			rospy.loginfo("Trying to Move To Rest Pose")
			success = self.move_arm_to_pose(self.rest_pose)

		elif req.action == "PLACE_UPPER" :
			rospy.loginfo("Trying to Move To Place Upper Pose")
			success = self.move_arm_to_pose(self.place_upper_pose)
			# success = self.MoveToPose(self.rest_pose, "FAILED MoveToRestPose")

		# elif req.action == "MOVE_TO_POS" :
		# 	rospy.loginfo("Trying to Move To Pos")

		# 	new_pose = Pose()
		# 	if req.limb == 'left':
		# 		try:
		# 			self.initial_left
		# 			new_pose = deepcopy(self.initial_left)
		# 		except AttributeError:
		# 			new_pose = deepcopy(self.hand_pose_left)
		# 			self.initial_left = deepcopy(self.hand_pose_left)
		# 	elif req.limb == 'right':
		# 		try:
		# 			self.initial_right
		# 			new_pose = deepcopy(self.initial_right)
		# 		except AttributeError:
		# 			new_pose = deepcopy(self.hand_pose_right)
		# 			self.initial_right = deepcopy(self.hand_pose_right)

		# 	new_pose.position = deepcopy(req.pose.position)
		# 	# success = self.MoveToPose(req.limb, new_pose, "FAILED MoveToPose")
		# 	success = self.MoveToPoseWithIntermediate(req.limb, new_pose)
		# 	rospy.loginfo("Moved to pos: %r" % success)

		else :
			success = False
			rospy.logerr("invalid action")

		return MoveRobotResponse(success)


	def orientation_cb(self, data):
		if(data.x == -1.0 and data.y == -2.0):
			print "################################################"
			print "###################  Orientation!  #############"
			print "################################################"
			deg = data.z*15
			rad = -deg * pi/180 + pi/2
			print rad
			q = quaternion_from_euler(0,rad,0)
			self.current_pose.pose.orientation.x = q[0]
			self.current_pose.pose.orientation.y = q[1]
			self.current_pose.pose.orientation.z = q[2]
			self.current_pose.pose.orientation.w = q[3]
		else:
			return

	# transform the pose stamped to the new frame
	def transform_pose(self, new_frame, pose):
		if pose.header.frame_id == new_frame:
			return pose
		try:
			ps = deepcopy(pose)
			ps.header.stamp = rospy.Time(0)
			self.tfl.waitForTransform(ps.header.frame_id, new_frame, rospy.Time(0), rospy.Duration(4.0))
			new_pose = self.tfl.transformPose(new_frame, ps)
			new_pose.header.stamp = deepcopy(pose.header.stamp)
			return new_pose
		except Exception as e:
			rospy.logerr(e)
			rospy.logerr("no transform")
			return None