本文整理汇总了Python中pr2_simple_arm_motions.GripUtils.grab_point方法的典型用法代码示例。如果您正苦于以下问题：Python GripUtils.grab_point方法的具体用法？Python GripUtils.grab_point怎么用？Python GripUtils.grab_point使用的例子？那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类pr2_simple_arm_motions.GripUtils的用法示例。
在下文中一共展示了GripUtils.grab_point方法的4个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的Python代码示例。
示例1: execute
# 需要导入模块: from pr2_simple_arm_motions import GripUtils [as 别名]
# 或者: from pr2_simple_arm_motions.GripUtils import grab_point [as 别名]
def execute(self, userdata):
process_mono = rospy.ServiceProxy("triangle_fitter_node/process_mono",ProcessMono)
resp = process_mono("wide_stereo/left")
pt_l = resp.pts3d[0]
pt_r = resp.pts3d[1]
pt_tl = resp.pts3d[2]
pt_tr = resp.pts3d[3]
#Compute approximate widths
towel_width = sqrt( (pt_l.point.x - pt_tr.point.x)**2 + (pt_l.point.y - pt_tr.point.y)**2 )
towel_height = sqrt( (pt_l.point.x - pt_tl.point.x)**2 + (pt_l.point.y - pt_tl.point.y)**2 )
userdata.cloth_width = towel_width
userdata.cloth_height = towel_height
if resp.params[resp.param_names.index("left")] == 0:
left = False
else:
left = True
print "Correct?"
tf = raw_input()
if len(tf) > 0 and (tf[0] == 'n' or tf[0] == 'f'):
left = not left
if left:
GripUtils.recall_arm("l")
if GripUtils.grab_point(pt_l,roll=pi/2,yaw=-pi/3,pitch=pi/4,arm="l",x_offset=0.005):
return SUCCESS
else:
return FAILURE
else:
GripUtils.recall_arm("r")
if GripUtils.grab_point(pt_r,roll=-pi/2,yaw=pi/3,pitch=pi/4,arm="r",x_offset=0.005):
return SUCCESS
else:
return FAILURE示例2: grab_far_right
# 需要导入模块: from pr2_simple_arm_motions import GripUtils [as 别名]
# 或者: from pr2_simple_arm_motions.GripUtils import grab_point [as 别名]
def grab_far_right(arm):
StanceUtils.call_stance("look_down",5.0)
rospy.sleep(2.5)
process_mono = rospy.ServiceProxy("far_right_finder_node/process_mono",ProcessMono)
resp = process_mono("wide_stereo/left")
pt = resp.pts3d[0]
GripUtils.grab_point(pt,roll=-pi/2,yaw=pi/2,arm=arm,z_offset = 0.02,x_offset = -0.01)
return pt示例3: sendTarget
# 需要导入模块: from pr2_simple_arm_motions import GripUtils [as 别名]
# 或者: from pr2_simple_arm_motions.GripUtils import grab_point [as 别名]
def sendTarget(self, dur, target1, target2=False,grab=False):
resp = False
if grab:
point = target1.point
pitch = target1.pitch
roll = target1.roll
yaw = target1.yaw
GripUtils.grab_point(point=point,pitch=pitch,roll=roll,yaw=yaw,arm=target1.arm,approach=False)
return
target1.point.header.stamp = rospy.Time.now()
if not target2:
try:
srv = rospy.ServiceProxy("move_one_arm",MoveOneArm)
resp = srv(MoveOneArmRequest(target=target1,dur=dur))
except rospy.ServiceException,e:
rospy.loginfo("Service Call Failed: %s"%e)示例4: fold_cloth
# 需要导入模块: from pr2_simple_arm_motions import GripUtils [as 别名]
# 或者: from pr2_simple_arm_motions.GripUtils import grab_point [as 别名]
def fold_cloth(dir='l'):
print "Folding cloth"
_cloth_tracker.scoot(-0.1)
GripUtils.recall_arm("b")
(waist_l_base,waist_r_base) = get_waist_points()
now = rospy.Time.now()
waist_l_base.header.stamp = now
waist_r_base.header.stamp = now
_listener.waitForTransform("table_height",waist_l_base.header.frame_id,now,rospy.Duration(5.0))
waist_l = _listener.transformPoint("table_height",waist_l_base)
waist_r= _listener.transformPoint("table_height",waist_r_base)
waist_l.point.z = 0
waist_r.point.z = 0
#Grab the waist point
smooth()
GripUtils.grab_point(waist_l,arm="l",yaw=-pi/2,x_offset=0.02)
#Fold over
ctr_x = (waist_l.point.x + waist_r.point.x)/2+0.02
ctr_y = (waist_l.point.y + waist_r.point.y)/2
ctr_z = waist_l.point.y - ctr_y
GripUtils.go_to( x=ctr_x, y=ctr_y, z=ctr_z,
roll=pi/2, pitch=pi/2, yaw=-pi/2,
arm="l", grip=True, frame=waist_l.header.frame_id)
GripUtils.go_to_pt(waist_r,arm="l",roll=pi/2,pitch=3*pi/4,yaw=-pi/2,grip=True,y_offset=0.01,x_offset=0.02)
GripUtils.go_to_pt(waist_r,arm="l",roll=pi/2,pitch=3*pi/4,yaw=-pi/2,grip=False,y_offset=-0.05,x_offset=0.02,dur=2.5)
GripUtils.go_to_pt(waist_r,arm="l",roll=pi/2,pitch=3*pi/4,yaw=-pi/2,grip=False,y_offset=-0.05,x_offset=0.02,z_offset=0.05,dur=2.5)
GripUtils.recall_arm("b")
#Grab waist
scoot_amt = 0.2
_cloth_tracker.scoot(-scoot_amt)
ctr_x = 0.25 * waist_l.point.x + 0.75 * waist_r.point.x + scoot_amt
ctr_y = 0.25 * waist_l.point.y + 0.75 * waist_r.point.y
ctr_z = waist_l.point.z
GripUtils.grab( x=ctr_x, y=ctr_y, z=ctr_z,
roll=pi/2, pitch=pi/4, yaw=0,
arm="r", frame=waist_l.header.frame_id)
sweep_drag_amount = 0.95*TABLE_WIDTH
sweep_lift_amount = 0.6
(x,y,z,r,p,yw) = sweep_cloth_with_scoot("r", sweep_drag_amount, TABLE_WIDTH, sweep_lift_amount,roll=pi/2)
print "y is %f"%y
print "(%f,%f,%f,%f,%f,%f)"%(x,y,z,r,p,y)
smooth("l")
GripUtils.go_to( x=x, y=y+PANTS_LENGTH/4-0.03, z=PANTS_LENGTH/4,
roll=r, pitch=3*pi/8, yaw=yw,
arm="r", grip=True, frame="table_height", dur=2.5)
GripUtils.go_to( x=x, y=y+PANTS_LENGTH/2-0.03, z=PANTS_LENGTH/2,
roll=r, pitch=pi/2, yaw=yw,
arm="r", grip=True, frame="table_height", dur=2.5)
GripUtils.go_to( x=x, y=y+3*PANTS_LENGTH/4-0.03, z=PANTS_LENGTH/4,
roll=r, pitch=5*pi/8, yaw=yw,
arm="r", grip=True, frame="table_height", dur=2.5)
GripUtils.go_to( x=x, y=y+PANTS_LENGTH-0.03, z=0.01,
roll=r, pitch=pi-p, yaw=yw,
arm="r", grip=True, frame="table_height", dur=2.5)
GripUtils.go_to( x=x, y=y+PANTS_LENGTH+0.05, z=0.01,
roll=r, pitch=pi-p, yaw=yw,
arm="r", grip=False, frame="table_height", dur=2.5)
GripUtils.go_to( x=x, y=y+PANTS_LENGTH+0.05, z=0.1,
roll=r, pitch=pi-p, yaw=yw,
arm="r", grip=False, frame="table_height", dur=2.5)
GripUtils.recall_arm("r")
smooth()
_cloth_tracker.scoot(scoot_amt+0.08)
return True