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
