Daniel J. Gonzalez - [email protected] 2.12 Intro to Robotics Spring 2019 """ ##################################### import robot212_virtual as bot import kinematicsSolver as kin import time import numpy as np check_constraints = True pi = np.pi #3.1415927 bot.trajMoveRad((0, 0, 0)) deltaKin = kin.deltaSolver() if __name__ == "__main__": isRunning = True mainRunning = True inp = input("Press ENTER to begin or q+ENTER to quit...") if inp == 'q': mainRunning = False while mainRunning: tStart = time.time() while isRunning: t = time.time() - tStart #-400, 400 is the limits of xy plane on the plotter which you can see on kinematicsSolver.py #-900, 100 is the limits of the z axis on the plotter which you can see on kinematicsSolver.py density = 12 xvec = np.linspace(-400, 400, density)
def node(): LIMIT_SWITCH_INIT_VEL = -15000 # counts per second print("Node starting...") global bot, delta_kin, lim_subs, limit_flags rospy.init_node('inverse_kinematics', anonymous=False) out_of_the_way_pub = rospy.Publisher('delta_out_of_the_way', Bool, queue_size=10) out_of_the_way_pub.publish(Bool(False)) bot = Bot() #bot.full_init(reset=False, k_p=400, k_d=60) #bot.set_gains(400, 60, perm=True) for axis in bot.axes: axis.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL axis.controller.config.control_mode = CTRL_MODE_VELOCITY_CONTROL axis.controller.vel_setpoint = 0 # bot.full_init() # for i in range(3): # bot.test_one(i, mytime=.1) delta_kin = kin.deltaSolver(realBot=realBot) # Limit switch init begins here limit_flags = [False for _ in range(3)] lim_subs = [ rospy.Subscriber('lim%d' % i, Bool, callback_lim_init(i)) for i in range(3) ] print("Beginning limit switch initialization...") for axis in bot.axes: axis.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL axis.controller.config.control_mode = CTRL_MODE_VELOCITY_CONTROL axis.controller.vel_setpoint = LIMIT_SWITCH_INIT_VEL while not all(limit_flags): rospy.sleep(.001) print("Limit switch initialization complete.") for axis in bot.axes: axis.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL axis.controller.config.control_mode = CTRL_MODE_VELOCITY_CONTROL axis.controller.vel_setpoint = 0 for axis in bot.axes: axis.controller.config.control_mode = CTRL_MODE_POSITION_CONTROL pub = rospy.Publisher('delta_position', PointStamped, queue_size=10) moving_pub = rospy.Publisher('delta_moving', Bool, queue_size=10) degrees_off_pub = rospy.Publisher('degrees_off', Point, queue_size=10) rospy.Subscriber('desired_position', PointStamped, move_to_point) current_pubs = [ rospy.Publisher('current%d' % i, Float64, queue_size=100) for i, _ in enumerate(bot.axes) ] out_of_the_way_pub = rospy.Publisher('delta_out_of_the_way', Bool, queue_size=10) out_of_the_way_point = PointStamped() out_of_the_way_point.point = Point(165, 75, -810) out_of_the_way_point.header.stamp = rospy.Time.now() out_of_the_way_point.header.frame_id = "robot_base_plate" move_to_point(out_of_the_way_point) send_robot_transform() rate = rospy.Rate(10) rospy.sleep(1) out_of_the_way_pub.publish(Bool(True)) while not rospy.is_shutdown(): bot.queueHandle() thetas = bot.get_rad_all() x, y, z = delta_kin.FK(thetas) pos = PointStamped() pos.point = Point(x=x, y=y, z=z) pos.header.stamp = rospy.Time.now() pos.header.frame_id = "robot_base_plate" pub.publish(pos) moving_pub.publish(Bool(bot.moving())) degrees_off_pub.publish(Point(*bot.get_enc_deg_dif_all())) for i, p in enumerate(current_pubs): p.publish(Float64(bot.axes[i].motor.current_control.Iq_measured)) rate.sleep()