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')
