def test_achieve_goal():
    env = OpenManipulatorReacherEnv(cfg)
    for iter in range(20):
        block_pose = Pose()
        block_pose.position.x = np.random.uniform(0.25, 0.6)
        block_pose.position.y = np.random.uniform(-0.4, 0.4)
        block_pose.position.z = 0.00
        block_pose.orientation = overhead_orientation
        env.ros_interface.set_target_block(block_pose)

        r_pose = Pose()
        r_pose.position = block_pose.position
        r_pose.position.z = 0.08
        forward_pose = KinematicsPose()
        forward_pose.pose = r_pose
        forward_pose.max_accelerations_scaling_factor = 0.0
        forward_pose.max_velocity_scaling_factor = 0.0
        forward_pose.tolerance = 0.0
        try:
            task_space_srv = rospy.ServiceProxy(
                "/open_manipulator/goal_task_space_path", SetKinematicsPose)
            _ = task_space_srv("arm", "gripper", forward_pose, 2.0)
        except rospy.ServiceException as e:
            rospy.loginfo("Path planning service call failed: {0}".format(e))
        rospy.sleep(5.0)
        env.ros_interface.delete_target_block()
def test_workspace_limit():
    env = OpenManipulatorReacherEnv(cfg)
    for iter in range(100):
        _polar_rad = np.random.uniform(0.134, 0.32)
        _polar_theta = np.random.uniform(-pi * 0.7 / 4, pi * 0.7 / 4)

        block_pose = Pose()
        block_pose.position.x = _polar_rad * cos(_polar_theta)
        block_pose.position.y = _polar_rad * sin(_polar_theta)
        block_pose.position.z = np.random.uniform(0.05, 0.28)
        block_pose.orientation = overhead_orientation
        env.ros_interface.set_target_block(block_pose)

        r_pose = Pose()
        r_pose.position = block_pose.position
        forward_pose = KinematicsPose()
        forward_pose.pose = r_pose
        forward_pose.max_accelerations_scaling_factor = 0.0
        forward_pose.max_velocity_scaling_factor = 0.0
        forward_pose.tolerance = 0.0
        try:
            task_space_srv = rospy.ServiceProxy(
                "/open_manipulator/goal_task_space_path", SetKinematicsPose)
            _ = task_space_srv("arm", "gripper", forward_pose, 3.0)
        except rospy.ServiceException as e:
            rospy.loginfo("Path planning service call failed: {0}".format(e))
        rospy.sleep(3.0)
        env.ros_interface.check_for_termination()
        env.ros_interface.delete_target_block()
Esempio n. 3
0
        def eef_pose_request_cb(userdata, request):
            eef = KinematicsPose()
            eef.pose = userdata.input_pose
            rospy.loginfo('eef.position.x : %f', eef.pose.position.x)
            rospy.loginfo('eef.position.y : %f', eef.pose.position.y)
            rospy.loginfo('eef.position.z : %f', eef.pose.position.z)
            eef.max_velocity_scaling_factor = 1.0
            eef.max_accelerations_scaling_factor = 1.0
            eef.tolerance = userdata.input_tolerance

            request.planning_group = userdata.input_planning_group
            request.kinematics_pose = eef
            return request
def test_forward():
    env = OpenManipulatorReacherEnv(cfg)
    _ = env.reset()
    _pose = Pose()
    _pose.position.x = 0.4
    _pose.position.y = 0.0
    _pose.position.z = 0.1
    _pose.orientation.x = 0.0
    _pose.orientation.y = 0.0
    _pose.orientation.z = 0.0
    _pose.orientation.w = 1.0
    forward_pose = KinematicsPose()
    forward_pose.pose = _pose
    forward_pose.max_accelerations_scaling_factor = 0.0
    forward_pose.max_velocity_scaling_factor = 0.0
    forward_pose.tolerance = 0.0
    try:
        task_space_srv = rospy.ServiceProxy(
            "/open_manipulator/goal_task_space_path", SetKinematicsPose)
        _ = task_space_srv("arm", "gripper", forward_pose, 2.0)
    except rospy.ServiceException as e:
        rospy.loginfo("Path planning service call failed: {0}".format(e))