class arm1(arm):
def __init__(self):
self.ac = ArmController()
seg = list()
seg.append(s.segment(0, .012, 0, 0))
seg.append(s.segment(0, 0, .077, (np.pi / 2)))
seg.append(s.segment((np.pi / 2) - acos(.128 / .13), .130, 0, 0))
seg.append(s.segment(acos(.128 / .13) - (np.pi / 2), .124, 0, 0))
seg.append(s.segment(0, .126, 0, -(np.pi / 2)))
self.segments = seg
super(arm1, self).__init__(seg)
def move(self, u1, u2, u4):
print('moving to ' + str(u1) + ',' + str(u2) + ',' + str(u4))
super(arm1, self).move([0, u1, -u2, 0, -u4])
self.ac.set_joints([u1, u2, 0, u4])
def pose(self):
T = self.matrix()
x = T[0, 3]
y = T[1, 3]
z = T[2, 3]
print("x = ", x)
print("y = ", y)
print("z = ", z)
phi = atan2(-T[2, 0], sqrt(T[0, 0] * T[0, 0] + T[1, 0] * T[1, 0]))
theta = -atan2(T[1, 0] / cos(phi), T[0, 0] / cos(phi))
psi = atan2(T[2, 1] / cos(phi), T[2, 2] / cos(phi))
print("roll =", psi)
print("pitch =", phi)
print("yaw =", theta)
print(self.ac.get_pose())
return (x, y, z, psi, phi, theta)
def __init__(self):
self.ac = ArmController()
seg = list()
seg.append(s.segment(0, .012, 0, 0))
seg.append(s.segment(0, 0, .077, (np.pi / 2)))
seg.append(s.segment((np.pi / 2) - acos(.128 / .13), .130, 0, 0))
seg.append(s.segment(acos(.128 / .13) - (np.pi / 2), .124, 0, 0))
seg.append(s.segment(0, .126, 0, -(np.pi / 2)))
self.segments = seg
super(arm1, self).__init__(seg)
def __init__(self):
self.recovery_mode = False
"""
Setup Baxter and the surrounding environment
"""
print("========= Initialise pick and place system =========")
self.grasping_arm = ArmController("left")
self.non_grasping_arm = ArmController("right")
self.poses = create_poses()
def main():
# get X, Y, Z, roll, pitch, and yaw at end of arm
posX = input("Arm's X value? ")
posY = input("Arm's Y value? ")
posZ = input("Arm's Z value? ")
psi = input("Arm's Roll? ")
phi = input("Arm's Pitch? ")
theta = input("Arm's Yaw? ")
# Matrix = T
matrix = getTransMatrix(posX, posY, posZ, psi, phi, theta)
# We now need to use the values we got after multiplying all of the matrices together to solve for cos1, sin1, cos2, sin2, and cos3, sin3.
cos1 = matrix[1][1]
sin1 = -matrix[0][1]
cos23 = matrix[2][2]
sin23 = -matrix[2][0]
# Next we do jointAngle1 = arctan2(sin1, cos1)
# jointAngle2 = arctan2(sin2, cos2)
# jointAngle3 = 0 (default)
# jointAngle4 = arctan2(sin3, cos3)
jointAngle1 = np.arctan2(sin1, cos1)
jointAngle2 = math.acos((matrix[2][3] - 0.077) / -0.126) + 0.713
sin2 = sin(jointAngle2)
cos2 = cos(jointAngle2)
jointAngle3 = np.arctan2(sin23, cos23) - np.arctan2(sin2, cos2)
# Print joint angles to reach user's desired position
print("Joint angles required to reach the desired position:")
print("Joint Angle 1:")
print(str(jointAngle1))
print("Joint Angle 2:")
print(str(jointAngle2))
print("Joint Angle 3:")
print(str(jointAngle3))
# Move the arm to the desired position
ac = ArmController()
ac.set_joints([jointAngle1, jointAngle2, 0, jointAngle3])
def __init__(self):
self.book_data = DatabaseReader()
self.arm_controller = ArmController()
self.location_driver = NavigationManager()
self.torso = fetch_api.Torso()
self.head = fetch_api.Head()
self.cmdline = False
self.cmdline_grab_tray = False
self.cmdline_grab_book = False
# home for sim. could be refactored better
# returnPose = Pose()
# returnPose.position.x = 0.3548
# returnPose.position.y = 0.6489
# returnPose.position.z = 0.0
# returnPose.orientation.x = 0.0
# returnPose.orientation.y = 0.0
# returnPose.orientation.z = 0.14559
# returnPose.orientation.w = .989
# home for real robot as negative book indices
# self.home_pose = returnPose
self.home_pose = self.book_data.library[-1].pose
self.delivery_pose = self.book_data.library[-2].pose
# Target 3
#T0e = np.array([ [-0.3409003, -0.1074855, 0.9339346, 282.96],[0.7842780, -0.5802868, 0.2194888, -48.302],[0.5183581, 0.8072881, 0.2821184, 235.071 ], [0,0,0,1]])
# Target 4
# T0e = np.array([[ 0.5054096, -0.8370580, -0.2095115, -45],[-0.0305796, 0.2252773, -0.9738147,-300],[0.8623375, 0.4985821, 0.0882604, 63 ],[0,0,0,1]])
# Impossible orientation
T0e = np.array([[ 1, 0, 0, 225.325],[0, -1, 0, 0],[0, 0, -1, 222.25],[0,0,0,1]])
# Please do not modify anything below this
if __name__=='__main__':
main = Main()
[q, isPos] = main.inverse(deepcopy(T0e))
np.set_printoptions(precision=6, suppress=True)
cont = ArmController()
sleep(1)
print("Setup complete")
if q.size != 0:
cont.set_state(np.append(np.ravel(q)[range(5)],8))
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]
#logic for determining if move jumped a piece
if (abs(move1 - move2) >= 14):
val = (move2 - move1) / 2
pickup = move1 + val
#waits til human says to continue gameplay
stop = input("tell when to go")
#pickup and trash jumped piece
arm.move_j([pickup, 0, 1])
arm.move_j([pickup, 1, 0])
# initialize game object
game = Game.Game()
# set ip and port
arm = ArmController('192.168.125.1',
3000) #'130.215.217.14',3000) #192.168.100.100
#attempt to connect
arm.connect()
game.consecutive_noncapture_move_limit = 50
while True:
#human player turn
if game.whose_turn() == 1:
#breaks loop if game is over
if game.is_over():
break
print("\nYour possible moves: ")
move_num = input(game.get_possible_moves()) #prints possible moves
print(game.get_possible_moves()[int(move_num[:1])])
move = game.get_possible_moves()[int(move_num)]
from pickDynamic import pickDynamic
from loadmap import loadmap
from pickStatic import pickStatic
from drop import drop_object
if __name__ == '__main__':
map_struct = loadmap("maps/final.txt")
if len(sys.argv) < 2:
print('usage: python final.py <color>')
sys.exit()
color = str(sys.argv[1])
lynx = ArmController(str(color))
sleep(1)
# Wait for Start Gun to be fired
lynx.wait_for_start()
############################################
# Main Code
############################################
while True:
# value indicates whether the dynamic function has picked an object 1 = yes, 0 = no
# dyn_target_ind indicates the id of that dynamic object picked, id = -1 if value = 0
value, dyn_target_ind = pickDynamic(lynx, color)
import json
import time
import threading
import RPi.GPIO as GPIO
from mqtt_service import MqttService
from nrf24l01 import NRF24L01
from arm_controller import ArmController
from config import *
rf24 = NRF24L01()
mqtt = MqttService()
arm = ArmController()
robot = False
trashbin = False
def button_press():
global trashbin
trashbin = not trashbin
if trashbin:
for _ in range(3):
rf24.write(b'11')
rf24.write(b'21')
rf24.write(b'31')
time.sleep(0.1)
else:
import rospy
import sys
import math
from os import getcwd
sys.path.append(getcwd() + "/../Core")
from arm_controller import ArmController
from loadmap import loadmap
from pickStatic_jz import pickStatic
import operator
if __name__ == '__main__':
color = sys.argv[1]
lynx = ArmController(str(color))
sleep(1)
#############################################
# Testing Static
#############################################
if len(sys.argv) < 2:
print('usage: python final.py <color>')
sys.exit()
#color = sys.argv[1]
map_struct = loadmap("maps/final.txt")
q_start = np.array([-0.70, -0., 0, 0.1, 1.5, 30])
lynx.set_pos(q_start)
#adjustable, depending on rtf
from os import getcwd
path.append(getcwd() + "/../Core")
from arm_controller import ArmController
from calculateFK import Main
# from calculateFK import Main
# 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('')
class PickAndPlace:
def __init__(self):
self.recovery_mode = False
"""
Setup Baxter and the surrounding environment
"""
print("========= Initialise pick and place system =========")
self.grasping_arm = ArmController("left")
self.non_grasping_arm = ArmController("right")
self.poses = create_poses()
def pick(self):
print("========= Pick object =========")
print("Open grippers")
self.grasping_arm.gripper.open(True)
print("Move above object")
target_pose = deepcopy(self.poses["object"])
target_pose.position.z += 0.3
b = self.grasping_arm.move_arm(target_pose)
print(" ### Could I move?: ", b, " ###")
if not b and self.recovery_mode:
self.recover()
return False
print("Grasp object")
target_pose = deepcopy(self.poses["object"])
b = self.grasping_arm.move_arm(target_pose)
self.grasping_arm.gripper.close(True)
# if b and not self.recovery_mode:
# self.grasping_arm.gripper.close(True)
# else:
# self.recover()
# return False
print(" ### Could I move?: ", b, " ###")
print("Lift object")
target_pose = deepcopy(self.poses["object"])
target_pose.position.z += 0.3
b = self.grasping_arm.move_arm(target_pose)
print(" ### Could I move?: ", b, " ###")
if not b and not self.recovery_mode:
self.recover()
return False
return True
def place(self):
print("========= Inspect object =========")
target_pose = deepcopy(self.poses["inspect"])
b = self.grasping_arm.move_arm(target_pose)
print("Determine object colour")
object_colour = inspect_object()
print("Detected_colour: {}".format(object_colour))
if not b and not self.recovery_mode:
self.recover()
return object_colour
print("========= Place object =========")
print("Move to above bin")
target_pose = deepcopy(self.poses["tote_{}".format(object_colour)])
target_pose.position.z += 0.3
b = self.grasping_arm.move_arm(target_pose)
print(" ### Could I move?: ", b, " ###")
if not b and not self.recovery_mode:
self.recover()
return object_colour
print("Place object in bin")
target_pose = deepcopy(self.poses["tote_{}".format(object_colour)])
b = self.grasping_arm.move_arm(target_pose)
if b and not self.recovery_mode:
self.grasping_arm.gripper.open(True)
else:
self.recover()
return object_colour
print(" ### Could I move?: ", b, " ###")
#We don't need to recover if we are going to the neutral position
print("Move back to neutral position")
target_pose = deepcopy(self.poses["tote_{}".format(object_colour)])
target_pose.position.z += 0.3
b = self.grasping_arm.move_arm(target_pose)
self.grasping_arm.reset_arm()
print(" ### Could I move?: ", b, " ###")
return object_colour
def recover(self):
# Change the flPickAndPlaceag status to true: now we are in recovery mode
# self.recovery_mode = True
rospy.loginfo("AAAAAAAAAAAH SAVE ME")
print("Move back to neutral position")
target_pose = deepcopy(self.poses["object"])
target_pose.position.z += 0.3
b = self.grasping_arm.move_arm(target_pose)
print(" ### Could I move?: ", b, " ###")
print(" Oh God this is too high. Oh god.")
target_pose = deepcopy(self.poses["object"])
# target_pose.position.z += 0.3
b = self.grasping_arm.move_arm(target_pose)
print(" ### Could I move?: ", b, " ###")
self.grasping_arm.gripper.open(True)
if self.recovery_mode:
rospy.loginfo("**** it, I GIVE UP")
self.grasping_arm.reset_arm()
else:
rospy.loginfo("Alright i'm gonna try one more ******* time...")
self.recovery_mode = True
repickSuccess = self.pick()
if repickSuccess:
self.place()
else:
rospy.loginfo("You know what? I'm done... I'm ******* done...")
import time
from os import getcwd
sys.path.append(getcwd() + "/../Core")
from arm_controller import ArmController
from astar import Astar
from loadmap import loadmap
if __name__ == '__main__':
if len(sys.argv) < 2:
print('usage: python final.py <color>')
sys.exit()
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)
#!/usr/bin/python
from time import sleep
import numpy as np
import sys
from arm_controller import ArmController
if __name__ == '__main__':
lynx = ArmController()
while True:
try: # show we're alive but don't do anything
lynx.set_pos([0,0,0,0,.4,30])
sleep(2)
lynx.set_pos([0,0,0,0,-.4,30])
sleep(2)
except KeyboardInterrupt:
lynx.stop()
break
if __name__ == '__main__':
rospy.init_node('experiment_0')
listener = tf.TransformListener()
pub = rospy.Publisher('grasp_pose', PoseStamped, latch=True, queue_size=1)
cloud_data = rospy.wait_for_message('/camera/depth/points', PointCloud2)
xyzs = ros_numpy.point_cloud2.get_xyz_points(ros_numpy.numpify(cloud_data))
rospy.sleep(1)
base_platform = PointStamped(Header(frame_id='m1n6s200_link_base'), Point())
base_platform = listener.transformPoint('camera_depth_optical_frame', base_platform)
base_platform = np.array([base_platform.point.x, base_platform.point.y, base_platform.point.z])
onet = OrthoNet()
controller = ArmController()
while not rospy.is_shutdown():
positions, zs, ys, widths = onet.predict(xyzs, roi=[-2, 1, -.15, .25, 0, 0.2], predictor=OrthoNet.manual_predictor)
for idx in range(len(positions)):
position = positions[idx]
z = zs[idx]
y = ys[idx]
width = widths[idx]
position = position.squeeze()
z = z.squeeze()
# Force z to be the closest direction to base platform
if z[2]
if np.abs(np.linalg.norm(position - base_platform)) < np.abs(np.linalg.norm(position + z - base_platform)):
print 'z should be inverted'
#!/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()
from arm_controller import ArmController ac = ArmController() ac.set_joints([.2, -.5, .3, -.4]) ac.set_joints([-.3, -.6, .7, .9]) ac.set_joints([0, 0, 0, 0]) print(ac.get_pose())
import numpy as np
import rospy
import sys
from random import random as rand
from sys import path
from os import getcwd
path.append(getcwd() + "/../Core")
from arm_controller import ArmController
dq = [.5, -0.5, -0.5, -0.1, 1.2, 0]
if __name__ == '__main__':
lynx = ArmController()
sleep(1) # wait for setup
print("Returning to Start Position:")
lynx.set_pos([0, 0, 0, 0, 0, 0])
sleep(5)
print("Target velocity:")
print(dq)
lynx.set_vel(dq)
sleep(2)
lynx.set_vel([0, 0, 0, 0, 0, 0])
import rospy
from arm_controller import ArmController
import numpy as np
if __name__ == '__main__':
rospy.init_node('width test')
controller = ArmController()
controller.set_fingers_width(0.11)
# controller.close_fingers()
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)
if verbose:
print("\n[[ FINAL SCORES ]]\n")
print "[RED]: ", RedScore, " [BLUE]: ", BlueScore, " [WINNER]: ", Winner
return RedScore, BlueScore, Winner
if __name__ == '__main__':
try:
pause_physics_client = rospy.ServiceProxy('/gazebo/pause_physics',
EmptySrv)
# connect to a robot for scoring purposes
lynx = ArmController('red') # doesn't matter red or blue
sleep(1)
# fire start gun
start_pub = rospy.Publisher("/start",
EmptyMsg,
queue_size=1,
latch=True)
start_pub.publish(EmptyMsg())
# match clock initialization
start = rospy.Time.now()
allotted = rospy.Duration(60)
remaining = allotted
while remaining.to_sec() > 0:
class BookServer(object):
def __init__(self):
self.book_data = DatabaseReader()
self.arm_controller = ArmController()
self.location_driver = NavigationManager()
self.torso = fetch_api.Torso()
self.head = fetch_api.Head()
self.cmdline = False
self.cmdline_grab_tray = False
self.cmdline_grab_book = False
# home for sim. could be refactored better
# returnPose = Pose()
# returnPose.position.x = 0.3548
# returnPose.position.y = 0.6489
# returnPose.position.z = 0.0
# returnPose.orientation.x = 0.0
# returnPose.orientation.y = 0.0
# returnPose.orientation.z = 0.14559
# returnPose.orientation.w = .989
# home for real robot as negative book indices
# self.home_pose = returnPose
self.home_pose = self.book_data.library[-1].pose
self.delivery_pose = self.book_data.library[-2].pose
# Call this before doing the callback from cmdline
def set_home(self, homeIndx):
self.home_pose = self.book_data.library[homeIndx].pose
# Call this before doing the callback from cmdline
def set_bookshelf(self, bookshelfIndx):
self.bookshelfIndex = bookshelfIndx
self.bookshelf = self.book_data.library[bookshelfIndx].pose
def book_request_callback(self, data):
# get book information
bookID = None
if self.cmdline:
bookID = self.bookshelfIndex
else:
bookID = data.book_id
book_info = self.book_data.library[bookID]
print book_info.pose
target_id = book_info.fiducial_number
# Go home before anywhere else
self.location_driver.goto(self.home_pose)
# Temp code to publish the shelf location
marker_pub = rospy.Publisher("visualization_marker",
Marker,
queue_size=10)
rospy.sleep(0.5)
box_marker = Marker()
box_marker.type = Marker.ARROW
box_marker.header.frame_id = "map"
box_marker.pose = book_info.pose
box_marker.scale.x = 0.5
box_marker.scale.y = 0.1
box_marker.scale.z = 0.1
box_marker.color.r = 0.0
box_marker.color.g = 0.5
box_marker.color.b = 0.5
box_marker.color.a = 1.0
marker_pub.publish(box_marker)
# navigate to book
self.location_driver.goto(book_info.pose)
# move torso
self.torso.set_height(book_info.torso_height)
# move head (pan and tilt)
self.head.pan_tilt(book_info.head_pan, book_info.head_tilt)
# execute grasping procedure
self.arm_controller.add_bounding_box()
# finding marker code
target_marker = self.arm_controller.find_marker(target_id, self.head)
print "Target Marker is..."
print target_marker
# t/f if grab tray
grab_tray_success = None
closest_pose = None
grab_book_success = None
if not self.cmdline or (self.cmdline and self.cmdline_grab_tray):
grab_tray_success = self.arm_controller.grab_tray(
target_id, target_marker)
print "Did the grab tray succeed: ", grab_tray_success
temp = 0
while grab_tray_success is False and temp < 3:
print "Grab tray failed. Retrying process......"
# Here we can try to move to bookself pose again, or raise the torso if need be
self.arm_controller.remove_bounding_box()
self.arm_controller.add_bounding_box()
target_marker = self.arm_controller.find_marker(
target_id, self.head)
print "Target Marker is..."
print target_marker
grab_tray_success = self.arm_controller.grab_tray(
target_id, target_marker)
temp += 1
if grab_tray_success is False:
print "Grab Tray Critical fail"
# If we failed, we want to robot to drive back home
self.arm_controller.remove_bounding_box()
self.location_driver.goto(self.home_pose)
success_msg = RequestBookResponse()
success_msg.success = int(False)
return success_msg
target_marker = self.arm_controller.find_marker(
target_id, self.head)
print "Target Marker is..."
print target_marker
closest_pose = self.arm_controller.find_grasp_pose(
target_id, target_marker)
temp = 0
while closest_pose == None and temp < 3:
print "Failed to find closest pose, trying again....."
target_marker = self.arm_controller.find_marker(
target_id, self.head)
print "Target Marker is..."
print target_marker
self.head.pan_tilt(0.0, 0.6)
closest_pose = self.arm_controller.find_grasp_pose(
target_id, target_marker)
temp += 1
if closest_pose == None:
print "Critical Pose-finding failure"
# If we failed, we want to robot to drive back home
self.arm_controller.remove_bounding_box()
self.location_driver.goto(self.home_pose)
success_msg = RequestBookResponse()
success_msg.success = int(False)
return success_msg
# t/f if grab book
if not self.cmdline or (self.cmdline and self.cmdline_grab_book):
# target_marker = self.arm_controller.find_marker(target_id, self.head)
# print "Target Marker is..."
# print target_marker
grab_book_success = self.arm_controller.grab_book(closest_pose)
temp = 0
while grab_book_success is False and temp < 3:
print "Grab Book Failed. Retrying process..........."
self.arm_controller.remove_bounding_box()
self.arm_controller.add_bounding_box()
# target_marker = self.arm_controller.find_marker(target_id, self.head)
# print "Target Marker is..."
# print target_marker
grab_book_success = self.arm_controller.grab_book(closest_pose)
temp += 1
if grab_book_success is False:
print "Grab Book Critical fail"
# If we failed, we want to robot to drive back home
self.arm_controller.remove_bounding_box()
self.location_driver.goto(self.home_pose)
success_msg = RequestBookResponse()
success_msg.success = int(False)
return success_msg
self.arm_controller.remove_bounding_box()
# move head (pan and tilt)
self.head.pan_tilt(0.0, 0.0)
# move torso
self.torso.set_height(0.0)
print "Driving to Delivery"
self.location_driver.goto(self.delivery_pose)
print "Arrived at delivery"
self.head.pan_tilt(0.0, 0.65)
self.arm_controller.add_delivery_bounding_box()
self.torso.set_height(0.4)
#rospy.sleep(3.0)
self.arm_controller.delivery()
self.torso.set_height(0.255) #0.25
self.arm_controller.open_gripper()
self.torso.set_height(0.4)
self.arm_controller.curl_arm()
print "Before height lowering"
self.torso.set_height(
0.1
) # 0.0 may-or-may-not have accidentily caused the robot to hit it's own killswitch.....
print "after lowering height"
self.head.pan_tilt(0.0, 0.0)
self.arm_controller.remove_bounding_box()
#self.torso.set_height(1.0)
# navigate back home
self.location_driver.goto(self.home_pose)
# TODO: set response?
if not self.cmdline:
success_msg = RequestBookResponse()
# success_msg.book_id_response = bookID
success_msg.success = int(
True) # int(grab_book_success and grab_tray_success)
return success_msg
#!/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()
max_iter = 3000
# or run rrt code
path, cost = rrt(deepcopy(map_struct),
deepcopy(start),
deepcopy(goal),
max_nodes,
max_iter,
stepsize=0.1,
neighbour_radius=0.15,
bias_ratio=5,
bias_radius=0.075,
optimize=True)
# start ROS
lynx = ArmController()
sleep(1) # wait for setup
collision = False
# Take out
lynx.set_pos(start)
sleep(5)
# iterate over target waypoints
for q in path:
print("Goal:")
print(q)
lynx.set_pos(q)
reached_target = False
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
