import rospy
from arm_controller import ArmController
import argparse
import numpy as np
if __name__ == '__main__':
parser = argparse.ArgumentParser()
parser.add_argument('pose', type=float, nargs=6)
args = parser.parse_args()
pose = args.pose
pose[3:] = np.radians(pose[3:])
rospy.init_node('move_kinova_to')
controller = ArmController()
# moveit.move_to(0.37, -0.24, 0.1, math.pi , 0., 0.)
controller.move_to(*pose)
pose.pose.position.y = position[1]
pose.pose.position.z = position[2]
pose.pose.orientation.x = q[0]
pose.pose.orientation.y = q[1]
pose.pose.orientation.z = q[2]
pose.pose.orientation.w = q[3]
pose = listener.transformPose('m1n6s200_link_base', pose)
pub.publish(pose)
rospy.sleep(1)
p = pose.pose.position
x, y, z = (p.x, p.y, p.z)
p = pose.pose.orientation
quat = [p.x, p.y, p.z, p.w]
r, p, yw = euler_from_quaternion(quat)
if controller.plan_grasp(x, y, z, r, p, yw, width):
print 'Found successful grasp'
controller.grasp(x, y, z, r, p, yw, width)
controller.open_fingers()
controller.move_to(0, -0.2, .3, -np.pi, 0, 0)
break
print 'Grasp unsuccessful, mirroring approach'
print 'Mirrored approach unsuccessful, skipping'
break
