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