def main(args):
print "Running demo"
rospy.init_node("sock_demo_node")
PrimitiveUtils.call_primitive("init")
pt = pickup_clump("r")
pt = pickup_sock("l")
open_sock()
"""
def main(args):
print "Running demo"
rospy.init_node("sock_demo_node")
PrimitiveUtils.call_primitive("init")
pt = pickup_clump("r")
pt = pickup_sock("l")
open_sock()
"""
def init_stances(self):
my_stances = {}
folding_height_stance = Stance (set_torso_height=True,torso_height=0.3,arms="n")
my_stances["folding_height"] = folding_height_stance
viewing_height_stance = Stance (set_torso_height=True,torso_height=0.2,arms="n")
my_stances["viewing_height"] = viewing_height_stance
folding_stance = Stance(x_left=0.65,y_left=0.3,z_left=0.2,roll_left=pi/2,pitch_left=pi/4,yaw_left=0,grip_left=False,frame_left="torso_lift_link"
,x_right=0.65,y_right=-0.3,z_right=0.2,roll_right=pi/2,pitch_right=pi/4,yaw_right=0,grip_right=False,frame_right="torso_lift_link"
,arms="b")
my_stances["folding"] = folding_stance
folding_right_stance = Stance(x_left=0.65,y_left=0.3,z_left=0.2,roll_left=pi/2,pitch_left=pi/4,yaw_left=0,grip_left=False,frame_left="torso_lift_link"
,x_right=0.65,y_right=-0.3,z_right=0.2,roll_right=pi/2,pitch_right=pi/4,yaw_right=0,grip_right=False,frame_right="torso_lift_link"
,arms="r")
my_stances["folding_right"] = folding_right_stance
folding_left_stance = Stance(x_left=0.65,y_left=0.3,z_left=0.2,roll_left=pi/2,pitch_left=pi/4,yaw_left=0,grip_left=False,frame_left="torso_lift_link"
,x_right=0.65,y_right=-0.3,z_right=0.2,roll_right=pi/2,pitch_right=pi/4,yaw_right=0,grip_right=False,frame_right="torso_lift_link"
,arms="l")
my_stances["folding_left"] = folding_left_stance
viewing_stance = Stance (x_left=0.1,y_left=0.7,z_left=0.3,roll_left=pi/2,pitch_left=-pi/4,yaw_left=0,grip_left=False,frame_left='torso_lift_link'
,x_right=0.1,y_right=-0.7,z_right=0.3,roll_right=pi/2,pitch_right=-pi/4,yaw_right=0,grip_right=False,frame_right='torso_lift_link'
,arms="b")
my_stances["viewing"] = viewing_stance
viewing_stance_left = Stance (x_left=0.1,y_left=0.7,z_left=0.3,roll_left=pi/2,pitch_left=-pi/4,yaw_left=0,grip_left=False,frame_left='torso_lift_link'
,x_right=0.1,y_right=-0.7,z_right=0.3,roll_right=pi/2,pitch_right=-pi/4,yaw_right=0,grip_right=False,frame_right='torso_lift_link'
,arms="l")
my_stances["viewing_left"] = viewing_stance_left
viewing_stance_right = Stance (x_left=0.1,y_left=0.7,z_left=0.3,roll_left=pi/2,pitch_left=-pi/4,yaw_left=0,grip_left=False,frame_left='torso_lift_link'
,x_right=0.1,y_right=-0.7,z_right=0.3,roll_right=pi/2,pitch_right=-pi/4,yaw_right=0,grip_right=False,frame_right='torso_lift_link'
,arms="r")
my_stances["viewing_right"] = viewing_stance_right
#Head stances
my_stances["look_bottom_left"] = Stance(arms="n",set_head_angle=True,head_pan=0.6,head_tilt=1.25)
my_stances["look_top_left"] = Stance(arms="n",set_head_angle=True,head_pan=0.6,head_tilt=0.9)
my_stances["look_bottom_right"] = Stance(arms="n",set_head_angle=True,head_pan=-0.6,head_tilt=1.25)
my_stances["look_top_right"] = Stance(arms="n",set_head_angle=True,head_pan=-0.6,head_tilt=0.9)
my_stances["look_center"] = Stance(arms="n",set_head_angle=True,head_pan=0.0,head_tilt=1.0)
StanceUtils.add_stances(my_stances)
initialize = PrimitiveUtils.make_primitive(stances=["folding_height","viewing"],durs=[5.0,5.0],params=[[],[]])
PrimitiveUtils.add_primitive(primitive=initialize,name="initialize")
scan_cloth = PrimitiveUtils.make_primitive(
stances=["viewing","look_bottom_left","pause","look_top_left","pause","look_top_right","pause","look_bottom_right","pause"],
durs=[5.0,5.0,25.0,5.0,25.0,5.0,25.0,5.0,25.0],
params=[[],[],[],[],[],[],[],[],[]]
)
PrimitiveUtils.add_primitive(primitive=scan_cloth,name="scan_cloth")
def init_stances(self):
my_stances = {}
folding_height_stance = Stance(set_torso_height=True,
torso_height=0.3,
arms="n")
my_stances["folding_height"] = folding_height_stance
viewing_height_stance = Stance(set_torso_height=True,
torso_height=0.2,
arms="n")
my_stances["viewing_height"] = viewing_height_stance
folding_stance = Stance(x_left=0.65,
y_left=0.3,
z_left=0.2,
roll_left=pi / 2,
pitch_left=pi / 4,
yaw_left=0,
grip_left=False,
frame_left="torso_lift_link",
x_right=0.65,
y_right=-0.3,
z_right=0.2,
roll_right=pi / 2,
pitch_right=pi / 4,
yaw_right=0,
grip_right=False,
frame_right="torso_lift_link",
arms="b")
my_stances["folding"] = folding_stance
folding_right_stance = Stance(x_left=0.65,
y_left=0.3,
z_left=0.2,
roll_left=pi / 2,
pitch_left=pi / 4,
yaw_left=0,
grip_left=False,
frame_left="torso_lift_link",
x_right=0.65,
y_right=-0.3,
z_right=0.2,
roll_right=pi / 2,
pitch_right=pi / 4,
yaw_right=0,
grip_right=False,
frame_right="torso_lift_link",
arms="r")
my_stances["folding_right"] = folding_right_stance
folding_left_stance = Stance(x_left=0.65,
y_left=0.3,
z_left=0.2,
roll_left=pi / 2,
pitch_left=pi / 4,
yaw_left=0,
grip_left=False,
frame_left="torso_lift_link",
x_right=0.65,
y_right=-0.3,
z_right=0.2,
roll_right=pi / 2,
pitch_right=pi / 4,
yaw_right=0,
grip_right=False,
frame_right="torso_lift_link",
arms="l")
my_stances["folding_left"] = folding_left_stance
viewing_stance = Stance(x_left=0.1,
y_left=0.7,
z_left=0.3,
roll_left=pi / 2,
pitch_left=-pi / 4,
yaw_left=0,
grip_left=False,
frame_left='torso_lift_link',
x_right=0.1,
y_right=-0.7,
z_right=0.3,
roll_right=pi / 2,
pitch_right=-pi / 4,
yaw_right=0,
grip_right=False,
frame_right='torso_lift_link',
arms="b")
my_stances["viewing"] = viewing_stance
viewing_stance_left = Stance(x_left=0.1,
y_left=0.7,
z_left=0.3,
roll_left=pi / 2,
pitch_left=-pi / 4,
yaw_left=0,
grip_left=False,
frame_left='torso_lift_link',
x_right=0.1,
y_right=-0.7,
z_right=0.3,
roll_right=pi / 2,
pitch_right=-pi / 4,
yaw_right=0,
grip_right=False,
frame_right='torso_lift_link',
arms="l")
my_stances["viewing_left"] = viewing_stance_left
viewing_stance_right = Stance(x_left=0.1,
y_left=0.7,
z_left=0.3,
roll_left=pi / 2,
pitch_left=-pi / 4,
yaw_left=0,
grip_left=False,
frame_left='torso_lift_link',
x_right=0.1,
y_right=-0.7,
z_right=0.3,
roll_right=pi / 2,
pitch_right=-pi / 4,
yaw_right=0,
grip_right=False,
frame_right='torso_lift_link',
arms="r")
my_stances["viewing_right"] = viewing_stance_right
#Head stances
my_stances["look_bottom_left"] = Stance(arms="n",
set_head_angle=True,
head_pan=0.6,
head_tilt=1.25)
my_stances["look_top_left"] = Stance(arms="n",
set_head_angle=True,
head_pan=0.6,
head_tilt=0.9)
my_stances["look_bottom_right"] = Stance(arms="n",
set_head_angle=True,
head_pan=-0.6,
head_tilt=1.25)
my_stances["look_top_right"] = Stance(arms="n",
set_head_angle=True,
head_pan=-0.6,
head_tilt=0.9)
my_stances["look_center"] = Stance(arms="n",
set_head_angle=True,
head_pan=0.0,
head_tilt=1.0)
StanceUtils.add_stances(my_stances)
initialize = PrimitiveUtils.make_primitive(
stances=["folding_height", "viewing"],
durs=[5.0, 5.0],
params=[[], []])
PrimitiveUtils.add_primitive(primitive=initialize, name="initialize")
scan_cloth = PrimitiveUtils.make_primitive(
stances=[
"viewing", "look_bottom_left", "pause", "look_top_left",
"pause", "look_top_right", "pause", "look_bottom_right",
"pause"
],
durs=[5.0, 5.0, 25.0, 5.0, 25.0, 5.0, 25.0, 5.0, 25.0],
params=[[], [], [], [], [], [], [], [], []])
PrimitiveUtils.add_primitive(primitive=scan_cloth, name="scan_cloth")