print(static_lst) for i in range(len(static_lst)): #test pickStatic on the nearest obj (wrt. base frame): obj = static_lst[i] #test on only 1 static object print("= = = = = =") print("name: " + str(obj[0])) # print("pose: "+str(obj[1])) # print("twist: "+str(obj[2])) print("= = = = = =") name = obj[0] pose = obj[1] [qCurr, qd] = lynx.get_state() q_start = qCurr #q_target, q_reach, isReach = pickStatic(q_start, pose, name, color, map_struct) #TODO: update the static objects status after each run; perhaps not to use the current for loop structure res, qCurr = pickStatic(q_start, pose, name, color, map_struct) qClose = qCurr qClose[5] = 0 lynx.set_pos(qClose) sleep(5) qBack = [-0.70, 0, 0, 0, qClose[4], 0] lynx.set_pos(qBack) sleep(10)
# iterate over target waypoints for q in path: print("Goal:") print(q) lynx.set_pos(q) reached_target = False proximity_radius = 0.05 while not reached_target: # Check if robot is collided then wait collision = collision or lynx.is_collided() if collision: break pos, vel = lynx.get_state() if util.reachedTarget(pos, q, proximity_radius): reached_target = True sleep(0.05) print("Current Configuration:") pos, vel = lynx.get_state() print(pos) if collision: print("Robot collided during move") else: print("No collision detected") lynx.stop()
# Modify q as configuration that running in the Gazebo simulation q = [0, 0, -0.52, 0, 0, 0] # Please do not modify anything below this if __name__=='__main__': main = Main() [jointPostions, T0e] = main.forward(q) np.set_printoptions(precision=6, suppress=True) cont = ArmController() sleep(1) print("Setup complete") cont.set_state(q) sleep(3) print('') print('The joint values are',cont.get_state()[0]) print('') print('The joint velocities are', cont.get_state()[1]) pose = cont.get_poses() poses = np.zeros((6, 3)) for i in range (6): poses[i,0] = pose[i][0,3] poses[i,1] = pose[i][1,3] poses[i,2] = pose[i][2,3] print('') print('Simulation Joint Positions = ') print(poses) print('Predicted Joint Positions = ') print(np.around(jointPostions, decimals=3)) print('Simulation T0e =') print(pose[-1])
#!/usr/bin/python2 from time import sleep import numpy as np import rospy import sys from sys import path from os import getcwd path.append(getcwd() + "/../Core") from arm_controller import ArmController q = [.5,-0.5,-0.5,-0.1,1.2,0]; if __name__=='__main__': # set up ROS interface con = ArmController() sleep(1) rospy.loginfo("Setup complete !") # send command via controller con.set_state(q) sleep(3) rospy.loginfo("The current state is: ") print(con.get_state()) # shut down ROS interface con.stop()
#!/usr/bin/python from time import sleep import numpy as np import rospy import sys from arm_controller import ArmController if __name__=='__main__': cont = ArmController(5,False) sleep(1) cont.set_state([0,0,0,0,0,0]) sleep(5) rospy.loginfo("The current state is: ") print(cont.get_state()) cont.stop()
color = sys.argv[1] lynx = ArmController(color) sleep(2) # wait for setup map_struct = loadmap("./maps/final.txt") start = np.array([0, 0, 0, 0, 0, 0]) lynx.set_pos(start) sleep(5) # interact with simulator, such as... # get state of your robot [q, qd] = lynx.get_state() print(q) print(qd) # get state of scoreable objects [name, pose, twist] = lynx.get_object_state() names = np.array(name) poses = np.array(pose) twists = np.array(twist) start = time.time() dynamic_indices = [i for i, item in enumerate(names) if "dynamic" in item] static_indices = [i for i in range(len(names)) if not i in dynamic_indices] print(dynamic_indices) print(static_indices) d_pose = poses[dynamic_indices] s_pose = poses[static_indices]
def pickStatic(qstart, pose, name, color, map): isReach = True #1. get T_frame 0/world frame WRT frame 1/base frame (of blue/red) if (str(color).lower() == 'blue'): T01 = np.array([[-1, 0, 0, 200], [0, -1, 0, 200], [0, 0, 1, 0], [0, 0, 0, 1]]) elif (str(color).lower() == 'red'): T01 = np.array([[1, 0, 0, 200], [0, 1, 0, 200], [0, 0, 1, 0], [0, 0, 0, 1]]) else: print("incorrect color input") return # Tobj0 = np.array(pose) # T_obj wrt frame 0/world frame Tobj1 = np.matmul(T01, Tobj0) # T_obj wrt frame "1"/base frame # #get coords of object wrt base frame dobj_1 = Tobj1[0:3, 3] #stop at 30mm above the object dreach_1 = dobj_1 + np.array([-8, 12, 8]) #get coords of end eff wrt base frame fk = calculateFK_jz.calculateFK() jointPos, T0e = fk.forward(qstart) de_1 = jointPos[-1, :] #lin. v of end eff, in unit vector (gives dir. to move toward the obj) v_e = (dreach_1 - de_1) #/ 8 #/ np.linalg.norm(dreach_1 - de_1) #w_e = np.array([np.nan,np.nan,np.nan]) #keep ang. v 0s during the linear motion w_e = np.array([0, 0, 0]) #w_e = np.array([np.nan,np.nan,np.nan]) isReach = False qCurr = qstart deCurr = de_1 dt = tc * 0.1 lynx = ArmController(str(color)) sleep(1) cnt = 0 #if ran for 200 loops still can't reach: fails while (cnt < 230): print(cnt) if (checkReach(deCurr, dreach_1)): isReach = True #stop the robot lynx.set_vel([0, 0, 0, 0, 0, 0]) sleep(5) break #find joint vel. to create lin. motion toward the obj dq = IK_velocity(qCurr, v_e, w_e, 6) print("------------") print("dq") print(dq) print() lynx.set_vel(dq.reshape(6).tolist()) print("moving") sleep(dt) # lynx.set_vel([0,0,0,0,0,0]) # print("stopping") # sleep(1) #update qCurr [qCurr, qd] = lynx.get_state() #update deCurr jointPos, T0e = fk.forward(qCurr) deCurr = jointPos[-1, :] #update v_e v_e = (dreach_1 - deCurr) #/ 8 #/ np.linalg.norm(dreach_1 - deCurr) cnt += 1 if (cnt == 230): print("Failed") else: print("within reach") lynx.set_vel([0, 0, 0, 0, 0, 0]) # qClose = qCurr # qClose[5] = 0 # # # #lynx.set_pos(qClose) # #sleep(5) [qCurr, qd] = lynx.get_state() lynx.stop() return True, qCurr