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'
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')
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)
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!')
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)
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
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)
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(
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')
#!/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,