def solve_ik(self,
target,
grasp_type,
fl_vec=[0, 0, 0],
frame="base_link"):
self.whole_body.linear_weight = 100.0
self.whole_body.angular_weight = 100.0
if grasp_type == "front_horizontal":
self.whole_body.move_end_effector_pose(geometry.pose(
x=(target.x + fl_vec[0]),
y=(target.y + fl_vec[1]),
z=(target.z + fl_vec[2]),
ei=-math.pi / 2,
ek=-math.pi / 2),
ref_frame_id=frame)
elif grasp_type == "front_vertical":
self.whole_body.move_end_effector_pose(geometry.pose(
x=(target.x + fl_vec[0]),
y=(target.y + fl_vec[1]),
z=(target.z + fl_vec[2]),
ej=math.pi / 2),
ref_frame_id=frame)
elif grasp_type == "top":
self.whole_body.move_end_effector_pose(geometry.pose(
x=(target.x + fl_vec[0]),
y=(target.y + fl_vec[1]),
z=(target.z + fl_vec[2]),
ei=-math.pi * 5 / 6,
ek=-math.pi / 2),
ref_frame_id=frame)
def manip(obj):
# go to kitchen
whole_body.move_to_go()
omni_base.go(5.90, 2.0, -3.14, 120, relative=False)
rospy.sleep(5)
# for i in [1, 2, 3]:
# try:
# whole_body.move_end_effector_pose(geometry.pose(ei=-1.10, y=-0.10, z=-0.05), 'recognized_object/4')
# break
# except:
# rospy.sleep(3)
# tts.say("うんち")
# rospy.loginfo("manip mistate no tf")
# rospy.sleep(3)
# omni_base.go(0.10, 0, 0, 10, relative=True)
whole_body.move_end_effector_pose(
geometry.pose(ei=-1.10, y=-0.10, z=-0.05), 'recognized_object/4')
whole_body.move_end_effector_pose(geometry.pose(z=0.05), 'hand_palm_link')
gripper.grasp(-0.10) # -0.01 is too weak
# go to table
whole_body.move_end_effector_pose(geometry.pose(z=-0.1), 'hand_palm_link')
omni_base.go(5.90, 4.25, -3.14, 120, relative=False)
omni_base.go(5.25, 4.25, -3.14, 120, relative=False) # approach to table
# omni_base.go(5.90, 4.4, -3.14, 120, relative=False)
# omni_base.go(5.25, 4.4, -3.14, 120, relative=False) # approach to table
# IK and release
whole_body.move_end_effector_pose(geometry.pose(z=0.090), 'hand_palm_link')
gripper.command(1.25)
whole_body.move_end_effector_pose(
geometry.pose(z=-0.20), 'hand_palm_link') # move arm far from table
whole_body.move_to_go()
def manip_d():
# IK and release
whole_body.move_end_effector_pose(geometry.pose(z=0.090), 'hand_palm_link')
gripper.command(1.25)
whole_body.move_end_effector_pose(
geometry.pose(z=-0.20), 'hand_palm_link') # move arm far from table
whole_body.move_to_go()
def execute_grasp(self, grasp_name, class_num=None):
"""
Picks up lego at target grasp
Delivers lego to target bin by color
To avoid collision errors, moves base in
front of bin before moving gripper forward to deposit lego
"""
if not (class_num is None) and class_num not in range(len(cfg.labels)):
raise ValueError("currently ony supports classes 0 to 7")
self.gripper.open_gripper()
self.whole_body.end_effector_frame = 'hand_palm_link'
#before lowering gripper, go directly above grasp position
self.whole_body.move_end_effector_pose(geometry.pose(z=-0.1),
grasp_name)
self.whole_body.move_end_effector_pose(geometry.pose(z=0.015),
grasp_name)
self.gripper.close_gripper()
self.whole_body.move_end_effector_pose(geometry.pose(z=-0.3),
grasp_name)
if class_num is None:
self.temp_bin_pose()
self.gripper.open_gripper()
if not (class_num is None):
color_name = class_num_to_name(class_num)
print("CLASS IS " + cfg.labels[class_num])
#AR numbers from 8 to 11
self.place_in_bin(class_num + 8)
def manip(obj):
# go to kitchen
whole_body.move_to_go()
omni_base.go(5.90, 2.0, -3.14, 120, relative=False)
rospy.sleep(5)
# for i in [1, 2, 3]:
# try:
# whole_body.move_end_effector_pose(geometry.pose(ei=-1.10, y=-0.10, z=-0.05), 'recognized_object/4')
# break
# except:
# rospy.sleep(3)
# tts.say("うんち")
# rospy.loginfo("manip mistate no tf")
# rospy.sleep(3)
# omni_base.go(0.10, 0, 0, 10, relative=True)
whole_body.move_end_effector_pose(geometry.pose(ei=-1.10, y=-0.10, z=-0.05), 'recognized_object/4')
whole_body.move_end_effector_pose(geometry.pose(z=0.05), 'hand_palm_link')
gripper.grasp(-0.10) # -0.01 is too weak
# go to table
whole_body.move_end_effector_pose(geometry.pose(z=-0.1), 'hand_palm_link')
omni_base.go(5.90, 4.25, -3.14, 120, relative=False)
omni_base.go(5.25, 4.25, -3.14, 120, relative=False) # approach to table
# omni_base.go(5.90, 4.4, -3.14, 120, relative=False)
# omni_base.go(5.25, 4.4, -3.14, 120, relative=False) # approach to table
# IK and release
whole_body.move_end_effector_pose(geometry.pose(z=0.090), 'hand_palm_link')
gripper.command(1.25)
whole_body.move_end_effector_pose(geometry.pose(z=-0.20), 'hand_palm_link') # move arm far from table
whole_body.move_to_go()
def opendoor(req):
# main(whole_body, gripper,wrist_wrench)
frame = req.handle_pose.header.frame_id
hanlde_pos = req.handle_pose.pose
# hanlde_pos=geometry_msgs.msg.PoseStamped()
res = OpendoorResponse()
cli = actionlib.SimpleActionClient('/hsrb/omni_base_controller/follow_joint_trajectory', control_msgs.msg.FollowJointTrajectoryAction)
vel_pub = rospy.Publisher('/hsrb/command_velocity', geometry_msgs.msg.Twist, queue_size=10)
armPub = rospy.Publisher('/hsrb/arm_trajectory_controller/command', JointTrajectory, queue_size=1)
robot = hsrb_interface.Robot()
whole_body = robot.get('whole_body')
gripper = robot.get('gripper')
wrist_wrench = robot.get('wrist_wrench')
base=robot.get('omni_base')
start_theta=base.pose[2]
# with hsrb_interface.Robot() as robot:
# whole_body = robot.get('whole_body')
# gripper = robot.get('gripper')
# wrist_wrench = robot.get('wrist_wrench')
try:
# Open the gripper
whole_body.move_to_neutral()
grasp_point_client()
global recog_pos
global is_found
print recog_pos.pose.position
print("Opening the gripper")
whole_body.move_to_neutral()
rospy.sleep(2.5)
switch = ImpedanceControlSwitch()
gripper.command(1.0)
# Approach to the door
print("Approach to the door")
grab_pose = geometry.multiply_tuples(geometry.pose(x=recog_pos.pose.position.x-HANDLE_TO_HAND_POS_X,
y=recog_pos.pose.position.y-HANDLE_TO_HAND_POS_Y,
z=recog_pos.pose.position.z,
ej=math.pi/2),
geometry.pose(ek=math.pi/2))
whole_body.move_end_effector_pose(grab_pose, _ORIGIN_TF)
# Close the gripper
wrist_wrench.reset()
switch.activate("grasping")
gripper.grasp(-0.01)
rospy.sleep(1.0)
switch.inactivate()
# Rotate the handle
whole_body.impedance_config = 'grasping'
traj = JointTrajectory()
# This controller requires that all joints have values
traj.joint_names = ["arm_lift_joint", "arm_flex_joint",
"arm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
def grasp():
"""
Grasping action.
"""
# Initial grasping setup
manipulation_setup()
# Command to open the gripper
print("Grasp: opening gripper.")
gripper.command(1.2)
# Grasp: phase 1 (approach)
print("Grasp: phase 1.")
whole_body.move_end_effector_pose(geometry.pose(x=0.13, z=-0.1),
OBJECT_TF_GRASP)
# Grasp: phase 2 (grasp)
print("Grasp: phase 2.")
whole_body.move_end_effector_pose(geometry.pose(x=0.13, z=-0.02),
OBJECT_TF_GRASP)
# Specify the force to grasp
print("Grasp: closing gripper now.")
gripper.apply_force(GRASP_FORCE)
# Wait time for simulator's grasp hack. Not needed on actual robot
print("Grasp: simulator hack.")
# Uncomment if working in simulation
# rospy.sleep(2.0)
# Transit to neutral position
print("Grasp: neutral pose transition.")
whole_body.move_to_neutral()
# Rotate 180 degrees
print("Grasp: 180 degree rotation.")
omni_base.go_rel(0.0, 0.0, 3.14)
# Release object
print("Grasp: object drop.")
gripper.command(1.2)
# Uncomment if working in simulation
# rospy.sleep(2.0)
# Rotate to initial pose
print("Grasp: 180 degree rotation.")
omni_base.go_rel(0.0, 0.0, 3.14)
# Close gripper
print("Grasp: closing gripper.")
gripper.command(0.0)
print("Grasp: done.")
def yakan_solve_ik(self):
yakan_point = self.get_bounding_box("yellow")
self.solve_ik(yakan_point, "front_horizontal", fl_vec=[-0.1, 0.0, 0.2])
self.whole_body.move_end_effector_pose(geometry.pose(y=0.02),
ref_frame_id="hand_palm_link")
self.whole_body.move_end_effector_pose(geometry.pose(z=0.1),
ref_frame_id="hand_palm_link")
self.whole_body.move_end_effector_pose(geometry.pose(x=-0.02),
ref_frame_id="hand_palm_link")
self.gripper.apply_force(1.7)
self.whole_body.move_end_effector_pose(geometry.pose(y=-0.05),
ref_frame_id="hand_palm_link")
def execute_grasp(self, cards, whole_body, direction):
whole_body.end_effector_frame = 'hand_palm_link'
nothing = True
#self.whole_body.move_to_neutral()
#whole_body.linear_weight = 99.0
whole_body.move_end_effector_pose(geometry.pose(), cards[0])
self.com.grip_squeeze(self.gripper)
whole_body.move_end_effector_pose(geometry.pose(z=-0.1), 'head_down')
self.com.grip_open(self.gripper)
def execute_grasp(self, grasp_name):
self.gripper.open_gripper()
self.whole_body.end_effector_frame = 'hand_palm_link'
self.whole_body.move_end_effector_pose(geometry.pose(), grasp_name)
self.gripper.close_gripper()
self.whole_body.move_end_effector_pose(geometry.pose(z=-0.1),
grasp_name)
self.whole_body.move_end_effector_pose(geometry.pose(z=-0.1),
'head_down')
self.gripper.open_gripper()
def get_hand_poses(self, tf_buffer, offset=geometry.pose()):
ref_to_hands = []
for object_to_hand in self._object_to_hands:
ref_to_hands.append(
geometry.multiply_tuples(self._ref_to_object, object_to_hand))
if self._approach_frame == _HAND_FRAME:
ref_to_hand_offsets = []
for ref_to_hand in ref_to_hands:
ref_to_hand_offsets.append(
geometry.multiply_tuples(ref_to_hand, offset))
return ref_to_hand_offsets
else:
hand_to_approach_tf = tf_buffer.lookup_transform(
_HAND_FRAME,
self._approach_frame,
# rospy.get_rostime(),
rospy.Time(0),
rospy.Duration(1.0))
hand_to_approach = geometry.transform_to_tuples(
hand_to_approach_tf.transform)
ref_to_hand_offsets = []
for ref_to_hand in ref_to_hands:
ref_to_approach = geometry.multiply_tuples(
ref_to_hand, hand_to_approach)
ref_to_approach_offset = geometry.multiply_tuples(
ref_to_approach, offset)
ref_to_hand_offset = geometry.multiply_tuples(
ref_to_approach_offset, _inverse_tuples(hand_to_approach))
ref_to_hand_offsets.append(ref_to_hand_offset)
return ref_to_hand_offsets
def place_in_bin(self, class_id):
self.move_to_home()
time.sleep(1)
nothing = True
i = 0
while nothing and i < 10:
try:
ar_name = 'ar_marker/' + str(class_id)
self.whole_body.move_end_effector_pose(
geometry.pose(y=0.08, z=-0.3), ar_name)
nothing = False
except:
rospy.logerr('continuing to search for AR marker')
i += 1
curr_tilt = -1 + (i * 1.0) / 5.0
self.whole_body.move_to_joint_positions(
{'head_pan_joint': curr_tilt})
if nothing:
print(
"Could not find AR marker- depositing object in default position."
)
self.temp_bin_pose()
self.gripper.open_gripper()
def force_pull(self, whole_body, direction):
"""Pull to the target `direction`, hopefully with the sheet!
I think this splits it into several motions, at most `cfg.MAX_PULLS`, so
this explains the occasional pauses with the HSR's motion. This is *per*
grasp+pull attempt, so it's different from the max attempts per side,
which I empirically set at 4. Actually he set his max pulls to 3, but
because of the delta <= 1.0 (including equality) this is 4 also.
Move the end-effector, then quickly read the torque data and look at the
wrench. The end-effector moves according to a _fraction_ that is
specified by `s = 1-delta`, so I _hope_ if there's too much tension
after that tiny bit of motion, that we'll exit. But maybe the
discretization into four smaller pulls is not fine enough?
"""
count = 0
is_pulling = False
t_o = self.get_translation(direction)
delta = 0.0
while delta <= 1.0:
s = 1.0 - delta
whole_body.move_end_effector_pose(
geometry.pose(x=t_o[0]*s, y=t_o[1]*s, z=t_o[2]*s),
direction
)
wrench = self.torque.read_data()
norm = np.abs(self.compute_bed_tension(wrench,direction))
print("FORCE NORM: {}".format(norm))
if norm > cfg.HIGH_FORCE:
is_pulling = True
if is_pulling and norm < cfg.LOW_FORCE:
break
if norm > cfg.FORCE_LIMT:
break
delta += 1.0/float(cfg.MAX_PULLS)
def handing():
try:
D = 2
#d = sub_face.center
#print('{},{}'.format(D,d))
pan = np.round(whole_body.joint_positions['head_pan_joint'], 2)
tilt = np.round(whole_body.joint_positions['head_tilt_joint'], 2)
print('pan, tilt = {}, {}'.format(pan, tilt))
#-----------head_pan_link からface への座標変換--------------#
i = np.array([[1, 0, 0], [0, math.cos(0), math.sin(0)],
[0, -math.sin(0), math.cos(0)]])
j = np.array([[math.cos(tilt), 0, -math.sin(tilt)], [0, 1, 0],
[math.sin(tilt), 0, math.cos(tilt)]])
k = np.array([[math.cos(pan), math.sin(pan), 0],
[-math.sin(pan), math.cos(pan), 0], [0, 0, 1]])
pan2tilt = np.array([0.02, 0, 0])
tilt2camera = np.dot(j, np.array([-0.0798, 0.022, 0.2152]))
pan2camera = pan2tilt + tilt2camera
#base2pan = np.dot(k,np.array([0,0,0.752]))
#base2tilt =
#base2camera = base2pan + pan2camera
print('pan2camera = {}'.format(pan2camera))
#print('base2camera = {}'.format(base2camera))
if tilt >= 0.52:
tilt = math.acos(d * math.cos(0.52) / D)
#x = D*math.cos(tilt)*math.cos(pan)
x = D * math.cos(tilt)
#y = D*math.cos(tilt)*math.sin(pan)
y = 0
z = D * math.sin(tilt)
camera2face = np.array([x, y, z])
print('cam2face = {}'.format(camera2face))
pan2face = pan2camera + camera2face
face2hand = np.array([-0.5, 0, -0.5])
#print(pan2face)
pan2hand = pan2face + face2hand
Hand_TF = np.round(pan2hand, 2)
#print(Hand_TF)
handing_point = geometry.pose(Hand_TF[0],
Hand_TF[1],
Hand_TF[2],
ei=1.57 * 2,
ej=-1.57)
#------------------------------------------------------#
#rospy.sleep(0.001)
#手渡し
#whole_body.move_end_effector_pose(handing_point, ref_frame_id='head_pan_link')
handing_END = True
except rospy.ROSException as wait_for_msg_exception:
rospy.logerr(wait_for_msg_exception)
def go_to_point(self, point, rot, c_img, d_img):
y, x = point
z_box = d_img[y - 20:y + 20, x - 20:x + 20]
z = self.gp.find_mean_depth(z_box)
print "singulation pose:", x, y, z
pose_name = self.gripper.get_grasp_pose(x, y, z, rot, c_img=c_img)
raw_input("Click enter to move to " + pose_name)
self.whole_body.move_end_effector_pose(geometry.pose(), pose_name)
def kitchen_collision(self):
collision_world.add_box(x=0.5,
y=1.0,
z=0.8,
pose=geometry.pose(x=3.8, y=0.0, z=0.4),
frame_id="map")
#collision_world.remove_all()
self.whole_body.collision_world = collision_world
def get_grasp_for_pose(object, distance_from_object_side=0.04):
MAX_GRASPABLE_WIDTH = 0.12
MAX_GRASPABLE_LENGTH = 0.12
MIN_SIDE_GRASPABLE_HEIGHT = 0.04
GRIPPER_TO_PALM_LENGTH = 0.08
MIN_TOP_GRASPABLE_HEIGHT= 0.03
TOP_GRASP_DIST_TO_OBJECT_ADJUST = -0.04
possible_styles = {"top", "side"}
if object.width > MAX_GRASPABLE_WIDTH:
rospy.loginfo("Too wide to top grasp")
possible_styles.discard("top")
if object.length > MAX_GRASPABLE_LENGTH:
rospy.loginfo("Too long to side grasp")
possible_styles.discard("side")
if object.height < MIN_SIDE_GRASPABLE_HEIGHT:
rospy.loginfo("Too high to side grasp")
possible_styles.discard("side")
if object.height < MIN_TOP_GRASPABLE_HEIGHT:
rospy.loginfo("Too low to top grasp")
possible_styles.discard("top")
if "side" in possible_styles:
style = "side"
# The robot approaches from the negative x direction
delta_x = -(object.width / 2 + distance_from_object_side)
delta_y = 0
target_pose = geometry.pose(x=delta_x, y=delta_y, z=object.height/4, ei=3.14, ej=-1.57, ek=0)
gripper_width = BasicGraspPlanner.gripper_width_for_object_dimension(object.width)
# Z axis extends from the palm
elif "top" in possible_styles:
style = "top"
delta_x = 0
delta_y = 0
delta_z = object.height / 2 + distance_from_object_side + TOP_GRASP_DIST_TO_OBJECT_ADJUST
target_pose = geometry.pose(x=delta_x, y=delta_y, z=delta_z, ei=3.14, ej=0., ek=1.57)
gripper_width = BasicGraspPlanner.gripper_width_for_object_dimension(object.length)
else:
return None, None, None
return style, target_pose, gripper_width
def execute(self, userdata):
rospy.loginfo('State : GRASP')
gripper.set_distance(.8, 1)
whole_body.move_end_effector_pose(
geometry.pose(x=-.51, y=-.51, z=.2, ei=3.1416, ek=1.57),
'my_frame_hole')
whole_body.move_end_effector_pose(
geometry.pose(x=-.51, y=-.51, z=-.2, ei=3.1416, ek=1.57),
'my_frame_hole')
whole_body.move_end_effector_pose(
geometry.pose(x=-.51, y=-.51, z=.2, ei=3.1416, ek=1.57),
'my_frame_hole')
whole_body.move_to_neutral()
base.go_rel(x=-.40, timeout=10)
return 'outcome1'
def execute(self, userdata):
rospy.loginfo('State : PLACE_TRASH_GRASPED_ABOVE')
base.go_rel(-0.35, 0, 0, 10, "pumas")
whole_body.move_to_go()
whole_body.move_end_effector_pose(
geometry.pose(ei=3.1416, ej=-1.57, ek=-1.57), 'trash')
whole_body.move_end_effector_pose(
geometry.pose(ei=3.1416, ej=-1.57, ek=-1.57), 'trash')
whole_body.move_end_effector_by_line((-1, 0, 0), .1)
gripper.set_distance(.8, .1)
whole_body.move_end_effector_by_line((1, 0, -1), .08)
gripper.set_distance(.01, .1)
whole_body.move_to_go()
#whole_body.move_end_effector_pose(geometry.pose(y=-.08,z=.19,ei=3.1416,ej=-1.57,ek=1.57), 'hole_reest')
return 'outcome1'
def stop_yakan_solve_ik(self):
self.whole_body.linear_weight = 100.0
self.whole_body.angular_weight = 100.0
#self.gripper.apply_force(1.0)
self.whole_body.move_end_effector_pose(geometry.pose(z=0.1,
ei=math.pi),
ref_frame_id="button2")
self.stop_yakan_tweak_flag = True
def go_to_centroid(self, whole_body):
whole_body.end_effector_frame = 'hand_l_finger_vacuum_frame'
nothing = True
#self.whole_body.move_to_neutral()
whole_body.move_end_effector_pose(geometry.pose(z=-0.02, ei=-0.785),
'ar_marker/11')
def execute_grasp(self):
self.com.grip_open(self.gripper)
self.whole_body.end_effector_frame = 'hand_palm_link'
nothing = True
try:
self.whole_body.move_end_effector_pose(geometry.pose(y=0.15,z=-0.09, ek=-1.57),'ar_marker/9')
except:
rospy.logerr('mustard bottle found')
self.com.grip_squeeze(self.gripper)
self.whole_body.move_end_effector_pose(geometry.pose(z=-0.9),'hand_palm_link')
self.com.grip_open(self.gripper)
self.com.grip_squeeze(self.gripper)
def singulate(self, waypoints, rot, c_img, d_img, expand=False):
self.gripper.close_gripper()
pose_names = [
self.get_pose(waypoint, rot, c_img, d_img)
for waypoint in waypoints
]
self.whole_body.move_end_effector_pose(geometry.pose(z=-0.05),
pose_names[0])
for pose_name in pose_names:
print "singulating", pose_name
self.whole_body.move_end_effector_pose(geometry.pose(z=0),
pose_name)
self.whole_body.move_end_effector_pose(geometry.pose(z=-0.05),
pose_names[-1])
def execute(self, userdata):
rospy.loginfo('State : go to table')
whole_body.move_end_effector_pose(
geometry.pose(x=-.05, z=.2, ei=3.1416, ek=1.57), 'my_frame_hole')
gripper.set_distance(.8, .5)
if self.visited:
return 'outcome2'
else:
self.visited = True
return 'outcome1'
def manip_e(obj):
for i in [1, 2, 3]:
try:
whole_body.move_end_effector_pose(geometry.pose(ei=-1.10, y=-0.10, z=-0.05), 'recognized_object/4')
break
except:
rospy.sleep(5)
tts.say("うんち")
rospy.loginfo("manip mistate no tf")
rospy.sleep(5)
omni_base.go(0.10, 0, 0, 10, relative=True)
def execute(self, userdata):
rospy.loginfo('State : reestimate_hole')
whole_body.move_end_effector_pose(
geometry.pose(z=.2, ei=3.1416, ek=1.57), 'chido')
if self.visited:
return 'outcome2'
else:
self.visited = True
return 'outcome1'
def get_grasp(object, style='side'):
# Planning wrt /base_link frame
thres = 0.05
if style == 'top':
target_pose = geometry.pose(x=object.bbox_pose.pose.position.x,
y=object.bbox_pose.pose.position.y,
z=object.bbox_pose.pose.position.z + object.bbox_height / 2 + thres,
ei=3.14,
ej=0.,
ek=0.)
else:
target_pose = geometry.pose(x=object.bbox_pose.pose.position.x - object.bbox_width / 2 - thres,
y=object.bbox_pose.pose.position.y,
z=object.bbox_pose.pose.position.z,
ei=3.14,
ej=-1.57,
ek=0.)
return target_pose
def execute_suction(self, grasp_name, class_num):
self.whole_body.end_effector_frame = "hand_l_finger_vacuum_frame"
self.whole_body.move_end_effector_pose(geometry.pose(z=-0.1),
grasp_name)
self.whole_body.move_end_effector_pose(geometry.pose(z=0.1),
grasp_name)
self.suction.start()
self.whole_body.move_end_effector_pose(geometry.pose(z=-0.1),
grasp_name)
color_name = class_num_to_name(class_num)
print("Identified lego: " + color_name)
lego_class_num = cfg.HUES_TO_BINS.index(color_name)
above_pose = "lego" + str(lego_class_num) + "above"
below_pose = "lego" + str(lego_class_num) + "below"
IPython.embed()
self.whole_body.end_effector_frame = 'hand_palm_link'
self.whole_body.move_end_effector_pose(geometry.pose(z=-0.1),
above_pose)
self.whole_body.move_end_effector_pose(geometry.pose(z=-0.1),
below_pose)
self.suction.stop()
self.whole_body.move_end_effector_pose(geometry.pose(z=-0.1),
above_pose)
def go_to_pose(self, results, c_img, d_img):
whole_body.end_effector_frame = 'hand_l_finger_vacuum_frame'
nothing = True
#self.whole_body.move_to_neutral()
#whole_body.linear_weight = 99.0
transforms = self.tl.getFrameStrings()
cards = []
for transform in transforms:
if 'belief' in transform:
f_p = self.tl.lookupTransform('head_rgbd_sensor_rgb_frame',
transform, rospy.Time(0))
cards.append(transform)
return True, cards
whole_body.move_end_effector_pose(geometry.pose(), cards[0])
self.base.move(geometry.pose(), 100.0, ref_frame_id='belief')
def manip_e(obj):
for i in [1, 2, 3]:
try:
whole_body.move_end_effector_pose(
geometry.pose(ei=-1.10, y=-0.10, z=-0.05),
'recognized_object/4')
break
except:
rospy.sleep(5)
tts.say("うんち")
rospy.loginfo("manip mistate no tf")
rospy.sleep(5)
omni_base.go(0.10, 0, 0, 10, relative=True)
def execute(self, userdata):
rospy.loginfo('STATE : initial state')
whole_body.linear_weight = 20
whole_body.move_to_go()
collision.remove_all()
collision.add_cylinder(radius=0.2,
length=.40,
pose=geometry.pose(x=1.0, z=.30))
whole_body.collision_world = collision
#base.go_rel(x=-.40, timeout=10)
#base.go_abs(x=0,y=0.3, timeout=10)
return 'outcome1'
def callback(data):
whole_body.move_to_neutral()
omni_base.go_abs(0, 0, 0)
omni_base.go_rel(x=-0.1, y=-0.08)
data = data.data.reshape(-1, 2)
dis = [(x - 0)**2 + (y - 240)**2 for (x, y) in data]
ob_to = data[np.argmax(dis)]
ob_from = data[np.argmin(dis)]
print "from:", ob_from, ", to", ob_to
omni_base.go_rel(y=-(ob_from[0] - 320.) / 306.)
gripper.set_distance(1.3)
omni_base.go_rel(x=0.1)
whole_body.move_end_effector_pose(geometry.pose(x=-0.18,
z=0.16,
ej=math.radians(-90.0)),
ref_frame_id='hand_palm_link')
gripper.apply_force(1.0)
whole_body.move_to_neutral()
omni_base.go_abs(0, 0, 0)
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)
import rospy
import sys
from hsrb_interface import geometry
# 移動のタイムアウト[s]
_MOVE_TIMEOUT=60.0
# ロボット機能を使うための準備
robot = hsrb_interface.Robot()
omni_base = robot.get('omni_base')
whole_body = robot.get('whole_body')
gripper = robot.get('gripper')
tts = robot.get('default', robot.Items.TEXT_TO_SPEECH)
# handを0.1[m]上に移動させる姿勢
hand_up = geometry.pose(x=0.1)
# handを0.5[m]手前に移動させる姿勢
hand_back = geometry.pose(z=-0.5)
def hello(msg):
"""
こんにちわ。
:param string msg: 出力するメッセージ
"""
print msg
def add(a, b):
"""
足し算ぐらいできます。
def manip_c():
# go to table
whole_body.move_end_effector_pose(geometry.pose(z=-0.1), 'hand_palm_link')
omni_base.go(5.90, 4.4, -3.14, 120, relative=False)
omni_base.go(5.25, 4.4, -3.14, 120, relative=False) # approach to table
def manip_b():
# IK and grasp
whole_body.move_end_effector_pose(geometry.pose(ei=-1.10, y=-0.10, z=-0.05), 'recognized_object/4')
whole_body.move_end_effector_pose(geometry.pose(z=0.05), 'hand_palm_link')
gripper.grasp(-0.10) # -0.01 is too weak
def manip_d():
# IK and release
whole_body.move_end_effector_pose(geometry.pose(z=0.090), 'hand_palm_link')
gripper.command(1.25)
whole_body.move_end_effector_pose(geometry.pose(z=-0.20), 'hand_palm_link') # move arm far from table
whole_body.move_to_go()
