class setRobot:
    def __init__(self):
        self.rob = Robot()
        self.whole_body = self.rob.try_get('whole_body')
        self.omni_base = self.rob.try_get('omni_base')
        self.gripper = self.rob.try_get('gripper')

        collision_world = self.rob.try_get('global_collision_world')
        self.whole_body.collision_world = collision_world

        self.whole_body.end_effector_frame = u'hand_palm_link'
Exemplo n.º 2
0
class setRobot:
    def __init__(self):
        self.rob = Robot()
        self.tts = self.rob.try_get('default_tts')
        self.tts.language = tts.ENGLISH  ########## May need modification
        self.whole_body = self.rob.try_get('whole_body')
        self.omni_base = self.rob.try_get('omni_base')
        self.gripper = self.rob.try_get('gripper')

        collision_world = self.rob.try_get('global_collision_world')
        self.whole_body.collision_world = collision_world

        self.whole_body.end_effector_frame = u'hand_palm_link'
 def __init__(self, before='map', after='base_footprint'):
     super(hsr_Move2Center, self).__init__(outcomes=['continue', 'failed'],
                                           input_keys=['target_pose'])
     self._before = before
     self._after = after
     self.pose_transformed = None
     robot = Robot()
     self.omni_base = robot.try_get('omni_base')
Exemplo n.º 4
0
class hsr_look_around_server:
    def __init__(self):
        self.robot = Robot()
        self.whole_body = self.robot.try_get('whole_body')
        self.lookaroundSrv = rospy.Service("lookaround", lookaround,
                                           self.srvCb)
        rospy.loginfo("hsr_look_around_server: ready to serve")

    def srvCb(self, req):
        rospy.loginfo("hsr_look_around_server: got request")
        tilt_angle = req.TiltAngle
        pan_angle = req.PanAngle
        self.whole_body.move_to_joint_positions(
            {'head_tilt_joint': tilt_angle})
        self.whole_body.move_to_joint_positions({'head_pan_joint': pan_angle})
        rospy.sleep(2)
        return lookaroundResponse(True)
Exemplo n.º 5
0
from hsrb_interface import Robot
from hsrb_interface import geometry
import rospy
robot = Robot()
whole_body = robot.try_get('whole_body')
gripper = robot.try_get('gripper')
whole_body.linear_weight = 30.0
whole_body.looking_hand_constraint = True

whole_body.move_end_effector_pose(geometry.pose(z=-0.08), 'grasp_pose')
rospy.sleep(0.8)
whole_body.move_end_effector_pose(geometry.pose(), 'grasp_pose')
rospy.sleep(0.8)
gripper.grasp(-0.3)
rospy.sleep(0.8)
whole_body.move_end_effector_pose(geometry.pose(), 'execute1')
#rospy.sleep(0.8)
whole_body.move_end_effector_pose(geometry.pose(), 'execute2')
rospy.sleep(0.8)
gripper.command(1.0)
#!/usr/bin/python
# -*- coding: utf-8 -*-

from hsrb_interface import Robot
import rospy
import math

robot = Robot()
base = robot.try_get('omni_base')
tts = robot.try_get('default_tts')
whole_body = robot.try_get('whole_body')
tts.language = tts.ENGLISH

if __name__ == '__main__':
    tts.say(u'We will do 22 pushups!')
    rospy.sleep(1)
    tts.say(u'Are you ready?')
    rospy.sleep(2)
    tts.say(u'Here we go!')
    rospy.sleep(1)
    for i in range(0, 22):
        current_count = i + 1
        tts.say(str(i + 1))
        if (current_count == 11):
            rospy.sleep(0.2)
            tts.say("You're half way there!")
        rospy.sleep(1.5)
    tts.say(u'GOOD JOB!')
Exemplo n.º 7
0
        if action_state == GoalStatus.SUCCEEDED:
            userdata.S4_counter_out = userdata.S4_counter_in + 1
            print "userdata.S4_counter out", userdata.S4_counter_out

            return get_action(userdata.S4_counter_out)
        else:
            "goal was not achieved"
            return 'end_demo'


tts = whole_body = None

while not rospy.is_shutdown():
    try:
        robot = Robot()
        tts = robot.try_get('default_tts')
        whole_body = robot.try_get('whole_body')
        tts.language = tts.ENGLISH
        break
    except (exceptions.ResourceNotFoundError,
            exceptions.RobotConnectionError) as e:
        rospy.logerr("Failed to obtain resource: {}\nRetrying...".format(e))

rospy.init_node('test_move')

#===========Read csv==============
f = open('../csv_parser/states_and_vals.csv', 'rt')
reader = csv.reader(f)
row_count = sum(1 for row in reader)
f.seek(0)
Exemplo n.º 8
0
import tf2_geometry_msgs
from hsrb_interface import geometry
from geometry_msgs.msg import PoseStamped, Point
import geometry_msgs
from vision_msgs.srv import Yolov3_detector
from std_msgs.msg import Int32, Bool
from vision_msgs.msg import VisionObject, ObjectCoordinatesForDetection
import numpy as np
import os
from takeshi_tools.nav_tool_lib import nav_module

##################INITIA
robot = Robot()
#base = robot.try_get('omni_base')
base = nav_module("hsr")
tts = robot.try_get('default_tts')
whole_body = robot.try_get('whole_body')
gripper = robot.try_get('gripper')
collision = collision_world.CollisionWorld('global_collision_world')
#tf_broadcaster=tf.TransformBroadcaster()
tf_listener = tf.TransformListener()
tf_broadcaster = tf2_ros.StaticTransformBroadcaster()
#tf_buffer = robot._get_tf2_buffer()

global turn_rigth
turn_rigth = True


def detect_yolo_foo(timeout):
    objectCoordinates = ObjectCoordinatesForDetection()
    objectCoordinates.x_min = -1.0
Exemplo n.º 9
0
                                  GripperApplyEffortGoal)
from tmc_manipulation_msgs.srv import (SafeJointChange, SafeJointChangeRequest)

import matplotlib
matplotlib.use('Agg')
from matplotlib import pyplot as plt
import RunAve_fil
#import fuzzy
from std_msgs.msg import Bool
from hsr_handing.msg import grasp_key
from hsr_handing.srv import fuzzy
from hsr_handing.msg import pose_info
from hsrb_interface import geometry
from hsrb_interface import Robot
robot = Robot()
omni_base = robot.try_get('omni_base')
whole_body = robot.try_get('whole_body')
gripper = robot.get('gripper')

_CONNECTION_TIMEOUT = 10.0
# Move timeout[s]
_MOVE_TIMEOUT = 60.0
# Grasp force[N]
_GRASP_FORCE = 0.2
# TF name of the bottle
_BOTTLE_TF = 'ar_marker/150'
# TF name of the gripper
_HAND_TF = 'hand_palm_link'

# Posture that 0.04[m] front and rotate -1.57 around z-axis of the bottle maker
bottle_to_hand = geometry.pose(z=-0.04, ek=-1.57)
Exemplo n.º 10
0
from hsrb_interface import Robot
from hsrb_interface import geometry
import roslib
roslib.load_manifest('mechknownet')
import rospy
import numpy as np
from mechknownet.srv import *

robot = Robot()
whole_body = robot.try_get('whole_body')
base = robot.try_get('omni_base')
gripper = robot.try_get('gripper')
#whole_body.linear_weight = 30.0
whole_body.looking_hand_constraint = True


def demo():
    success = call_mechknownet_sys("drawer2", "", "grasp", [])
    print "success?", success
    step1_finish = False
    preposition = None
    if success:
        print "success, ready to go"
        pre_head_pan_joint = whole_body.joint_positions['head_pan_joint']
        pre_head_tilt_joint = whole_body.joint_positions['head_tilt_joint']
        preposition = base.pose
        gripper.command(1.0)
        rospy.sleep(3.0)
        whole_body.move_end_effector_pose(geometry.pose(z=-0.08), 'grasp_pose')
        rospy.sleep(1.0)
        whole_body.move_end_effector_pose(
Exemplo n.º 11
0
class ManipulationBridge(object):
    def __init__(self):

        # robot
        self.robot = Robot()
        self.whole_body = self.robot.try_get('whole_body')

        # server
        self.srv_manipulation_left = actionlib.SimpleActionServer(
            'left_arm/grasp_precompute',
            GraspPrecomputeAction,
            execute_cb=self.manipulation_srv_left,
            auto_start=False)
        self.srv_manipulation_right = actionlib.SimpleActionServer(
            'right_arm/grasp_precompute',
            GraspPrecomputeAction,
            execute_cb=self.manipulation_srv_right,
            auto_start=False)
        self.srv_manipulation_left.start()
        self.srv_manipulation_right.start()

    def manipulation_srv_left(self, action):
        success = self.manipulation_srv(action)
        if success:
            rospy.loginfo('Manipulation bridge: Succeeded')
            self.srv_manipulation_left.set_succeeded()

    def manipulation_srv_right(self, action):
        success = self.manipulation_srv(action)
        if success:
            rospy.loginfo('Manipulation bridge: Succeeded')
            self.srv_manipulation_right.set_succeeded()

    def manipulation_srv(self, action):
        """
        Here the grasp precompute action (TU/e) is translated to a PlanWithHandGoals (TMC) and send as goal to the robot
        :param action: the GraspPrecomputeAction type
        """
        success = True

        pose_quaternion = quaternion_from_euler(action.goal.roll,
                                                action.goal.pitch,
                                                action.goal.yaw)
        static_quaternion = quaternion_from_euler(3.14159265359,
                                                  -1.57079632679, 0)
        final_quaternion = quaternion_multiply(pose_quaternion,
                                               static_quaternion)
        pose = [action.goal.x, action.goal.y, action.goal.z], final_quaternion

        ################################################################################################################
        # This piece of code is partially copied from Toyota software, it also uses the private functions (we're very
        # sorry). Setting base_movement_type.val allows for adapting the allowed movements during manipulation.

        ref_frame_id = settings.get_frame('base')

        ref_to_hand_poses = [pose]

        odom_to_ref_pose = self.whole_body._lookup_odom_to_ref(ref_frame_id)
        odom_to_ref = geometry.pose_to_tuples(odom_to_ref_pose)
        odom_to_hand_poses = []
        for ref_to_hand in ref_to_hand_poses:
            odom_to_hand = geometry.multiply_tuples(odom_to_ref, ref_to_hand)
            odom_to_hand_poses.append(geometry.tuples_to_pose(odom_to_hand))

        req = self.whole_body._generate_planning_request(
            PlanWithHandGoalsRequest)
        req.origin_to_hand_goals = odom_to_hand_poses
        req.ref_frame_id = self.whole_body._end_effector_frame
        req.base_movement_type.val = BaseMovementType.ROTATION_Z

        service_name = self.whole_body._setting['plan_with_hand_goals_service']
        plan_service = rospy.ServiceProxy(service_name, PlanWithHandGoals)
        res = plan_service.call(req)
        if res.error_code.val != ArmManipulationErrorCodes.SUCCESS:
            rospy.logerr('Fail to plan move_endpoint')
            success = False
        else:
            res.base_solution.header.frame_id = settings.get_frame('odom')
            constrained_traj = self.whole_body._constrain_trajectories(
                res.solution, res.base_solution)
            self.whole_body._execute_trajectory(constrained_traj)

        return success
Exemplo n.º 12
0
end_pose = [5.004, 1.483, 0.766]

def set_initial_pose(x, y, phi):
	pub = rospy.Publisher('laser_2d_correct_pose', PoseWithCovarianceStamped, queue_size=1)
	rospy.sleep(0.5)

	initial_pose = PoseWithCovarianceStamped()

	initial_pose.header.frame_id = "map"

	initial_pose.pose.pose.position.x = x
	initial_pose.pose.pose.position.y = y
	initial_pose.pose.pose.position.z = 0.0
	initial_pose.pose.pose.orientation = Quaternion(*tf.transformations.quaternion_from_euler(0, 0, phi))
	initial_pose.pose.covariance = [0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.06853891945200942, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

	pub.publish(initial_pose)
	rospy.sleep(0.5)

# Preparation to use robot functions
print 'starting the rips'
robot = Robot()
omni_base = robot.try_get('omni_base')

set_initial_pose(*initial_pose)

omni_base.go(*mid_pose, relative=False)
omni_base.go(*end_pose, relative=False)

rospy.loginfo('finished')
Exemplo n.º 13
0
#!/usr/bin/env python

#from ImagePass.srv import *
import rospy
#import tf
from hsrb_interface import Robot
from hsrb_interface import geometry
import math

robot = Robot()
#omni_base = robot.try_get('omni_base')
#tts = robot.try_get('default_tts')
whole_body = robot.try_get('whole_body')
#gripper = robot.try_get('gripper')
collision_world = robot.try_get('global_collision_world')
#br=tf.TransformBroadcaster()

if __name__ == '__main__':
    #rospy.init_node('python_services')
    collision_world.remove_all()
    whole_body.collision_world = collision_world

    #kitchin-table
    collision_world.add_box(x=0.8,
                            y=0.7,
                            z=0.5,
                            pose=geometry.pose(x=-0.3, y=0.35, z=0.28),
                            frame_id='abstract_map')

    #desk
    collision_world.add_box(x=0.6,