def run(planning_frame="/yumi_body"):
"""Starts the node
Runs to start the node and initialize everthing. Runs forever via Spin()
:returns: Nothing
:rtype: None
"""
#Start by connecting to ROS and MoveIt!
yumi.init_Moveit(planning_frame)
# Print current joint angles
yumi.print_current_joint_states(yumi.RIGHT)
yumi.print_current_joint_states(yumi.LEFT)
#sys.exit(0)
# Reset YuMi joints to "home" position
#yumi.reset_pose()
# Drive YuMi end effectors to a desired position (pose_ee), and perform a grasping task with a given effort (grip_effort)
# Gripper effort: opening if negative, closing if positive, static if zero
pose_ee = [0.25, 0.15, 0.2, 0.0, 3.14, 0.0]
pose_ee_t = [0.47185, -0.144102, 0.25, 0.0, 3.14, 0.0]
pose_ee_t = [0.45, 0.0, 0.1525, 0, 3.14, 0.0]
#yumi.open_grippers(yumi.LEFT)
yumi.open_grippers(yumi.RIGHT)
#yumi.move_global_planning(yumi.RIGHT, pose_ee_t)
yumi.move_local_planning(yumi.RIGHT, pose_ee_t)
rospy.sleep(2.0)
def run():
"""Starts the node
Runs to start the node and initialize everthing. Runs forever via Spin()
:returns: Nothing
:rtype: None
"""
rospy.init_node('arm_server')
#Start by connecting to ROS and MoveIt!
yumi.init_Moveit()
#yumi.move_point(yumi.BOTH, 0.35, -0.25)
#yumi.move_pick(yumi.BOTH, 0.35, -0.25)
#yumi.move_handover_ready(yumi.BOTH,0.55,-0.20)
#yumi.move_handover_give(yumi.BOTH,0.55,-0.20)
rospy.Service('arm/move_home', ArmInterface, handle_move_home)
rospy.Service('arm/open_gripper', ArmInterface, handle_open_gripper)
rospy.Service('arm/close_gripper', ArmInterface, handle_close_gripper)
rospy.Service('arm/move_simple', ArmInterface, handle_move_simple)
rospy.Service('arm/move_point', ArmInterface, handle_move_point)
#rospy.Service('arm/move_pick', ArmInterface, handle_move_pick)
#rospy.Service('arm/move_place', ArmInterface, handle_move_place)
rospy.Service('arm/move_point_pick', ArmInterface, handle_move_point_pick)
rospy.Service('arm/move_point_pick_handover', ArmInterface, handle_move_point_pick_handover)
rospy.Service('arm/move_point_place', ArmInterface, handle_move_point_place)
rospy.Service('arm/move_handover_ready', ArmInterface, handle_move_handover_ready)
#rospy.Service('arm/move_handover_give', ArmInterface, handle_move_handover_give)
#rospy.Service('arm/move_handover_take', ArmInterface, handle_move_handover_take)
print "Arm Server is running.. "
rospy.on_shutdown(yumi.kill_Moveit)
rospy.spin()
def run():
"""Starts the node
Runs to start the node and initialize everthing. Runs forever via Spin()
:returns: Nothing
:rtype: None
"""
rospy.init_node('yumi_moveit_demo')
#Start by connecting to ROS and MoveIt!
yumi.init_Moveit()
# Print current joint angles
yumi.print_current_joint_states(yumi.RIGHT)
yumi.print_current_joint_states(yumi.LEFT)
# Reset YuMi joints to "home" position
yumi.reset_pose()
# Drive YuMi end effectors to a desired position (pose_ee), and perform a grasping task with a given effort (grip_effort)
# Gripper effort: opening if negative, closing if positive, static if zero
pose_ee = [0.3, 0.15, 0.2, 0.0, 3.14, 3.14]
grip_effort = -10.0
move_and_grasp(yumi.LEFT, pose_ee, grip_effort)
pose_ee = [0.3, -0.15, 0.2, 0.0, 3.14, 3.14]
grip_effort = -10.0
move_and_grasp(yumi.RIGHT, pose_ee, grip_effort)
rospy.spin()
def __init__(self, args):
server_address = "localhost"
if args["--host"]:
server_address = args["--host"]
server_port = 5000
if args["--port"]:
server_port = int(args["--host"])
# Connect to ROS and MoveIt!
yumi.init_Moveit()
# Reset YuMi joints to "home" position
self.move_to_home()
# Connect to the server
args = {"-v":True, "-l":True}
self.logger = Logger("YuMiControl Client", args)
# Create a pipe to communicate to the client process
self.pipe_in_client, self.pipe_out_dia = os.pipe()
self.pipe_in_dia, self.pipe_out_client = os.pipe()
# Create a client object to communicate with the server
self.client = Client(client_type="yumi",
pipe_in=self.pipe_in_client,
pipe_out=self.pipe_out_client,
port=server_port,
host=server_address,
logger=self.logger)
self.client.start()
def __init__(self):
yumi.init_Moveit()
while not rospy.is_shutdown():
'''
pl = [0.3, 0.3, 0.2, -pi/4, pi, pi]
pr = [0.3, -0.3, 0.2, pi/4, pi, pi]
pol = [0.4, 0.3, 0.2, -pi/4, pi, pi]
por = [0.4, -0.3, 0.2, pi/4, pi, pi]
posel = []
poser = []
posel.append(pl)
posel.append(pol)
poser.append(pr)
poser.append(por)
yumi.plan_path_dual(posel,poser, 500)
'''
yumi.print_current_joint_states(LEFT)
yumi.print_current_joint_states(RIGHT)
print(pl)
#yumi.print_current_pose(LEFT)
rospy.signal_shutdown('stop')
def run():
"""Starts the node
Runs to start the node and initialize everthing. Runs forever via Spin()
:returns: Nothing
:rtype: None
"""
#Start by connecting to ROS and MoveIt!
yumi.init_Moveit()
# Print current joint angles
yumi.print_current_joint_states(yumi.RIGHT)
yumi.print_current_joint_states(yumi.LEFT)
pose_ee = [0.35, 0.25, 0.3, 0.0, 3.14, 0.0]
grip_effort = -10.0
move_and_grasp(yumi.LEFT, pose_ee, grip_effort)
pose_ee = [0.35, -0.25, 0.3, 0.0, 3.14, 0.0]
grip_effort = -10.0
move_and_grasp(yumi.RIGHT, pose_ee, grip_effort)
def __init__(self):
self._tfsub = tf.TransformListener()
self.state_lis = rospy.Subscriber('/state', String,
self.state_callback)
yumi.init_Moveit()
while not rospy.is_shutdown():
start = time.time()
if (self.need_adj == True):
try:
(trans_adr,
_) = self._tfsub.lookupTransform('/yumi_base_link',
'/adr', rospy.Time(0))
(trans_adrr,
_) = self._tfsub.lookupTransform('/yumi_base_link',
'/adrr', rospy.Time(0))
(trans_adl,
_) = self._tfsub.lookupTransform('/yumi_base_link',
'/adl', rospy.Time(0))
(trans_adll,
_) = self._tfsub.lookupTransform('/yumi_base_link',
'/adll', rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
yoffset = yoff - (gripperoff) * np.cos(pi / 4)
x_r = trans_adr[0] + xoff
y_r = trans_adr[1] + yoffset
y_rr = trans_adrr[1] + yoffset
x_l = trans_adl[0] + xoff
y_l = trans_adl[1] - yoffset
y_ll = trans_adll[1] - yoffset
self.need_adj = False
yumi.reset_arm_home(BOTH)
yumi.plan_and_move_dual(
yumi.create_pose_euler(x_l, y_l, 0.25, -pi / 4, pi, pi),
yumi.create_pose_euler(x_r, y_r, 0.25, pi / 4, pi, pi))
yumi.plan_and_move_dual(
yumi.create_pose_euler(x_l, y_l, 0.1, -pi / 4, pi, pi),
yumi.create_pose_euler(x_r, y_r, 0.1, pi / 4, pi, pi))
yumi.plan_and_move_dual(
yumi.create_pose_euler(x_l, y_ll, 0.1, -pi / 4, pi, pi),
yumi.create_pose_euler(x_r, y_rr, 0.1, pi / 4, pi, pi))
yumi.plan_and_move_dual(
yumi.create_pose_euler(x_l, y_l, 0.1, -pi / 4, pi, pi),
yumi.create_pose_euler(x_r, y_r, 0.1, pi / 4, pi, pi))
yumi.plan_and_move_dual(
yumi.create_pose_euler(x_l, y_l, 0.25, -pi / 4, pi, pi),
yumi.create_pose_euler(x_r, y_r, 0.25, pi / 4, pi, pi))
yumi.reset_arm_home(BOTH)
yumi.reset_arm_cal(BOTH)
rospy.sleep(3)
else:
try:
(trans,
_) = self._tfsub.lookupTransform('/yumi_base_link',
'/shoe_hole',
rospy.Time(0))
(trans_norm,
_) = self._tfsub.lookupTransform('/yumi_base_link',
'/pre_put',
rospy.Time(0))
(trans_pick,
_) = self._tfsub.lookupTransform('/yumi_base_link',
'/pick', rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
x = trans[0]
y = trans[1]
z = trans[2]
xn = trans_norm[0]
yn = trans_norm[1]
zn = trans_norm[2]
xp = trans_pick[0]
yp = trans_pick[1]
zp = trans_pick[2]
if (0 < xn <= x and zn >= z > 0 and 0 < x < xp):
a = np.arctan2((y - yn), (zn - z))
b = np.arctan2((x - xn), (zn - z))
c = np.arctan2((yp - y), (xp - x))
if (-0.5 * pi <= a <= 0.5 * pi and 0 <= b <= 0.5 * pi
and -0.5 * pi <= c <= 0.5 * pi):
zoffset = gripperoff * np.cos(a) * np.cos(b) + zoff
yoffset = yoff - (gripperoff) * np.sin(a)
xoffset = xoff - (gripperoff) * np.sin(b)
b = b + pi
c = 0.5 * pi + c - 0.10
#set up 6D pose----------------
x = x + xoffset
y = y + yoffset
z = z + zoffset
xn = xn + xoffset
yn = yn + yoffset
zn = zn + zoffset
xp = xp + xoff
yp = yp + yoff
zp = zp + zoff + gripperoff
print(x, y, z, xn, yn, zn, a, b)
if (np.isnan(a) == False and np.isnan(b) == False):
#'''
pose_norm = [xn, yn, zn, a, b, pi]
pose = [x, y, z, a, b, pi]
yumi.reset_arm(RIGHT)
yumi.move_and_grasp(yumi.RIGHT, pose_norm, 10.0)
yumi.move_and_grasp(yumi.RIGHT, pose, -10.0)
yumi.move_and_grasp(yumi.RIGHT, pose_norm, -10.0)
yumi.reset_arm(RIGHT)
yumi.reset_arm_cal(RIGHT)
#'''
pose_pick = [xp, yp, 0.18 + 0.08, 0, pi, c]
pose_grab = [xp, yp, 0.18, 0, pi, c]
yumi.reset_arm_home(LEFT)
yumi.move_and_grasp(yumi.LEFT, pose_pick, -10.0)
yumi.move_and_grasp(yumi.LEFT, pose_grab, 10.0)
yumi.move_and_grasp(yumi.LEFT, pose_pick, 10.0)
yumi.reset_arm_home(LEFT)
yumi.reset_arm_cal(LEFT)
#'''
print time.time() - start
rospy.on_shutdown(done)
if (direction == 1):
cup_from_left()
elif (direction == 2):
cup_from_top()
elif (direction == 3):
cup_from_front()
else:
continue
else:
continue
if __name__ == '__main__':
try:
# run()
rospy.init_node('yumi_moveit_demo')
#Start by connecting to ROS and MoveIt!
yumi.init_Moveit()
# yumi.go_to_simple(0.55, 0.2, 0.11, 1.57, 0, 1.57, yumi.LEFT)
yumi.go_to_simple(0.2, 0.2, 0.11, 1.57, 0, 1.57, yumi.LEFT)
yumi.go_to_joints(left_base_angle, yumi.LEFT)
# Giving joints: (0.67, 0.2, 0.3, 1.57, 0, 1.57, yumi.LEFT)
# yumi.go_to_joints(left_base_angle, yumi.LEFT)
# main_test()
# test_action_with_traverse()
print "#################################### Program finished ####################################"
except rospy.ROSInterruptException:
pass
