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')
def __init__(self): self.rob = Robot() self.whole_body = self.rob.get('whole_body') self.omni_base = self.rob.get('omni_base') self.gripper = self.rob.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): print("Getting robot infos") self.rob = Robot() try: self.whole_body = self.rob.get('whole_body') self.omni_base = self.rob.get('omni_base') self.gripper = self.rob.get('gripper') self.whole_body.end_effector_frame = u'hand_palm_link' print("Infos received") except Exception as e: print("Fail")
def __init__(self): s = rospy.Service('grab_the_bag', Grabbag, go_to) self.rob = Robot() self.whole_body = self.rob.get('whole_body') self.omni_base = self.rob.get('omni_base') self.gripper = self.rob.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' rospy.spin()
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): # robot self.robot = Robot() self.whole_body = self.robot.try_get('whole_body') # server self.srv_manipulation = actionlib.SimpleActionServer( 'arm_center/grasp_precompute', GraspPrecomputeAction, execute_cb=self.manipulation_srv_inst, auto_start=False) self.srv_manipulation.start()
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' self.g_planner = grasp_planner()
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()
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)
omni_base.go_rel(x=-0.1, y=-0.08) omni_base.go_rel(y=-(ob_to[0] - 320.) / 306.) omni_base.go_rel(x=0.1) whole_body.move_end_effector_pose(geometry.pose(x=-0.10, z=0.16, ej=math.radians(-90.0)), ref_frame_id='hand_palm_link') gripper.set_distance(1.3) whole_body.move_to_neutral() omni_base.go_abs(0, 0, 0) with Robot() as robot: whole_body = robot.try_get('whole_body') omni_base = robot.try_get('omni_base') # collision_world = robot.try_get('global_collision_world') # suction = robot.try_get('suction') gripper = robot.try_get('gripper') # wrist_wrench = robot.try_get('wrist_wrench') # marker = robot.try_get('marker') # battery = robot.try_get('battery') # tts = robot.try_get('default_tts') # shell(LOGO) # rospy.init_node('listener') omni_base.go_abs(0, 0, 0) whole_body.move_to_neutral()
#!/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,
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")
rospy.loginfo("give pose") self.body.move_to_joint_positions({ "arm_lift_joint": 0.0, "arm_flex_joint": 0.0, "arm_roll_joint": -1.57, "wrist_roll_joint": 0.00, "wrist_flex_joint": -1.57 }) # self.body.move_to_neutral() rospy.loginfo("home_poseaction finished") rospy.sleep(3) self.open_gripper() # rospy.sleep(2) self._as.set_succeeded() def open_gripper(self, to_width=1.2): self.gripper.command(to_width) def close_gripper(self, to_width=-0.01): # self.gripper.command(to_width) self.gripper.grasp(to_width) if __name__ == '__main__': robot = Robot() # rospy.init_node('handover_action') rospy.loginfo("Initializing givepose server...") server = HomePoseAction('homepose_action', robot) rospy.loginfo("homepose_action server created") rospy.spin()
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/env python import rospy from hsrb_interface import Robot, exceptions, geometry from std_srvs.srv import Empty import math rospy.loginfo('Getting robot resources') print('Starting') robot = None while not rospy.is_shutdown(): try: robot = Robot() whole_body = robot.try_get('whole_body') omni_base = robot.try_get('omni_base') print('got resources') break except (exceptions.ResourceNotFoundError, exceptions.RobotConnectionError) as e: rospy.logerr_throttle( 1, 'Failed to obtain resource: {}\nRetrying...'.format(e)) whole_body.gaze_point(point=geometry.Vector3(x=5, y=5, z=0), ref_frame_id='map') whole_body.gaze_point(point=geometry.Vector3(x=-5, y=5, z=0), ref_frame_id='map') rospy.sleep(5)
#!/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!')
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)
from tmc_control_msgs.msg import (GripperApplyEffortAction, 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
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
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')