Exemplo n.º 1
0
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)
Exemplo n.º 2
0
            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