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