class TestRobotArm(unittest.TestCase):
def setUp(self):
self.lengths = (
3,
2,
2,
)
self.destinations = (
(5, 4, 6, 4, 5),
(0, 2, 0.5, -2, -1),
)
self.theta = (
pi,
pi / 2,
0,
)
self.robot_arm = RobotArm(self.lengths, self.destinations, self.theta)
def test_init_all_arguments(self):
RobotArm(self.lengths, self.destinations, self.theta)
def test_init_without_theta(self):
RobotArm(self.lengths, self.destinations)
def test_wrong_lengths_type(self):
self.assertRaises(TypeError, RobotArm, np.array(self.lengths),
self.destinations, self.theta)
def test_wrong_destinations_type(self):
self.assertRaises(TypeError, RobotArm, self.lengths,
np.array(self.destinations), self.theta)
def test_wrong_theta_type(self):
self.assertRaises(TypeError, RobotArm, self.lengths, self.destinations,
np.array(self.theta))
def test_destinations_properties(self):
robot_arm = RobotArm(self.lengths, self.destinations, self.theta)
self.assertIsInstance(robot_arm.destinations, np.ndarray)
# Check if points are 2D
self.assertTrue(robot_arm.destinations.shape[0] == 2)
# Check if destinations are immutable
self.assertRaises(
ValueError,
robot_arm.destinations.__setitem__,
(
0,
0,
),
0,
)
def test_generate_initial_guess(self):
self.robot_arm.generate_initial_guess()
def main():
try:
print("============ Initialising Robot Arm software!")
with open('config.json') as config_file:
config = json.load(config_file)
trajectory_queue = Queue()
gripper_queue = Queue()
# Initialises the arm with the desired configuration
robot_arm = RobotArm(config)
# Creates the thread that will continuously publish the state of the robot to ROS
joint_publisher = JointStatePublisher(robot_arm)
joint_publisher.start()
# Runs the UI to manually control the arm directly through SPI # TODO Run as service with no UI
app = QApplication([])
station = ControlStation(robot_arm, trajectory_queue, gripper_queue)
station.show()
app.exec_()
# Cleanup methods after GUI exits
joint_publisher.stop()
joint_publisher.thread.join(timeout=5)
except KeyboardInterrupt:
print("Keyboard interrupt detected. Exiting properly.")
app.quit()
finally:
GPIO.cleanup()
def setUp(self):
self.lengths = (
3,
2,
2,
)
self.destinations = (
(5, 4, 6, 4, 5),
(0, 2, 0.5, -2, -1),
)
self.theta = (
pi,
pi / 2,
0,
)
self.robot_arm = RobotArm(self.lengths, self.destinations, self.theta)
def setUp(self):
lengths = (3, 2, 2,)
destinations = (
(5, 4, 6, 4, 5),
(0, 2, 0.5, -2, -1),
)
theta = (np.pi, np.pi / 2, 0,)
self.robot_arm = RobotArm(lengths, destinations, theta)
self.n = self.robot_arm.n
self.s = self.robot_arm.s
def setUp(self):
self.lengths = (
3,
2,
2,
)
self.destinations = (
(5, 4, 6, 4, 5),
(0, 2, 0.5, -2, -1),
)
self.theta = (
np.pi,
np.pi / 2,
0,
)
self.thetas = np.ones((3 * 5, ))
self.robot_arm = RobotArm(self.lengths, self.destinations, self.theta)
self.constraints_func = generate_constraints_function(self.robot_arm)
self.constraint_gradients_func = generate_constraint_gradients_function(
self.robot_arm)
def setUp(self):
self.thetas = np.array((
0.5,
4,
2,
5,
3,
6,
))
self.thetas.setflags(write=False)
lengths = (
1,
1,
)
destinations = ((0, 0, 0), (0, 0, 0))
self.robot_arm = RobotArm(lengths, destinations)
self.objective = generate_objective_function(self.robot_arm)
def test_destinations_properties(self):
robot_arm = RobotArm(self.lengths, self.destinations, self.theta)
self.assertIsInstance(robot_arm.destinations, np.ndarray)
# Check if points are 2D
self.assertTrue(robot_arm.destinations.shape[0] == 2)
# Check if destinations are immutable
self.assertRaises(
ValueError,
robot_arm.destinations.__setitem__,
(
0,
0,
),
0,
)
def test_objective_value2(self):
robot_arm = RobotArm(lengths=(
1,
2,
),
destinations=(
(
1,
-1,
),
(
1,
1,
),
))
thetas = np.array((
0,
np.pi / 2,
np.pi / 2,
np.pi / 4,
))
objective_func = generate_objective_function(robot_arm)
objective_value = objective_func(thetas)
self.assertEqual(objective_value, np.pi**2 * 5 / 16)
objective_gradient_func = generate_objective_gradient_function(
robot_arm)
objective_gradient = objective_gradient_func(thetas)
testing.assert_array_equal(
objective_gradient,
np.array((
-np.pi,
np.pi / 2,
np.pi,
-np.pi / 2,
)))
def test_constraint_values2(self):
robot_arm = RobotArm(lengths=(
1,
2,
),
destinations=(
(
1,
-1,
),
(
1,
1,
),
))
thetas = np.array((
0,
np.pi / 2,
np.pi / 2,
np.pi / 4,
))
constraints_func = generate_constraints_function(robot_arm)
constraint_values = constraints_func(thetas)
correct_constraint_values = np.array((
0,
1,
1 - np.sqrt(2),
np.sqrt(2),
))
testing.assert_array_almost_equal(constraint_values,
correct_constraint_values)
constraint_gradients_func = generate_constraint_gradients_function(
robot_arm)
correct = (
(
-2,
1,
0,
0,
),
(
-2,
0,
0,
0,
),
(
0,
0,
-1 - np.sqrt(2),
-np.sqrt(2),
),
(
0,
0,
-np.sqrt(2),
-np.sqrt(2),
),
)
constraint_grads = constraint_gradients_func(thetas)
testing.assert_array_almost_equal(constraint_grads, np.array(correct))
class TestConstraintFunctions(unittest.TestCase):
def setUp(self):
self.lengths = (
3,
2,
2,
)
self.destinations = (
(5, 4, 6, 4, 5),
(0, 2, 0.5, -2, -1),
)
self.theta = (
np.pi,
np.pi / 2,
0,
)
self.thetas = np.ones((3 * 5, ))
self.robot_arm = RobotArm(self.lengths, self.destinations, self.theta)
self.constraints_func = generate_constraints_function(self.robot_arm)
self.constraint_gradients_func = generate_constraint_gradients_function(
self.robot_arm)
def test_constraints_func_return_type(self):
constraints = self.constraints_func(self.thetas)
self.assertEqual(constraints.shape, (2 * 5, ))
def test_constraint_gradients_func_return_type(self):
constraint_gradients = self.constraint_gradients_func(self.thetas)
self.assertEqual(constraint_gradients.shape, (3 * 5, 2 * 5))
# print(np.array2string(constraint_gradients, max_line_width=np.inf))
def test_licq(self):
constraint_gradients = self.constraint_gradients_func(self.thetas)
rank = np.linalg.matrix_rank(constraint_gradients)
self.assertEqual(rank, 2 * 5)
def test_constraint_values(self):
initial_guess = self.robot_arm.generate_initial_guess()
constraints = self.constraints_func(initial_guess)
constraint_grads = self.constraint_gradients_func(initial_guess)
print(constraints)
print(np.array2string(constraint_grads, max_line_width=np.inf))
def test_constraint_values2(self):
robot_arm = RobotArm(lengths=(
1,
2,
),
destinations=(
(
1,
-1,
),
(
1,
1,
),
))
thetas = np.array((
0,
np.pi / 2,
np.pi / 2,
np.pi / 4,
))
constraints_func = generate_constraints_function(robot_arm)
constraint_values = constraints_func(thetas)
correct_constraint_values = np.array((
0,
1,
1 - np.sqrt(2),
np.sqrt(2),
))
testing.assert_array_almost_equal(constraint_values,
correct_constraint_values)
constraint_gradients_func = generate_constraint_gradients_function(
robot_arm)
correct = (
(
-2,
1,
0,
0,
),
(
-2,
0,
0,
0,
),
(
0,
0,
-1 - np.sqrt(2),
-np.sqrt(2),
),
(
0,
0,
-np.sqrt(2),
-np.sqrt(2),
),
)
constraint_grads = constraint_gradients_func(thetas)
testing.assert_array_almost_equal(constraint_grads, np.array(correct))
import numpy as np
from methods_project_old import gradient_descent, newtons_method, BFGS, MaximumIterationError
from robot_arm import RobotArm
if __name__ == '__main__':
lengths, theta0, p = np.load('initial_values_bug.npy')
WALL_E = RobotArm(lengths, destination=p, theta=theta0, precision=1e-3)
try:
WALL_E.move_to_destination()
except MaximumIterationError:
WALL_E.save_animation()
raise
except AssertionError:
WALL_E.save_animation()
raise
# WALL_E.save_animation()
WALL_E.plot()
print(WALL_E.position())
def run_test(lengths, theta0, p, plot_initial, plot_minimizer, animate):
WALL_E = RobotArm(lengths, p, theta0, precision=epsilon)
if plot_initial: WALL_E.plot()
try:
WALL_E.move_to_destination()
except MaximumIterationError:
np.save('initial_values_bug', (lengths, theta0, p))
WALL_E.save_animation()
raise
except AssertionError:
np.save('initial_values_bug', (lengths, theta0, p))
WALL_E.save_animation()
raise
if plot_minimizer: WALL_E.plot()
if animate: WALL_E.save_animation()
import os
import sys
from time import sleep
import bot.lib.lib as lib
from robot_arm import RobotArm
from QRCode2 import QRCode2
bot_config = lib.get_config()
arm_config = bot_config["dagu_arm"]
arm = RobotArm(arm_config)
while True:
print "Commands: "
print "1: Set Angles"
print "2: Move to hopper pos"
print "3: Set Hopper"
print "4: Empty hopper"
print "5: Calibrate rail"
print "6: Pinch "
print "7: Let go "
print "8: Reset the arm and rail"
print "9: Check color in bin"
print "10: Tier Grab "
print "11: Turn Light On "
print "12: Turn Light Off "
print "13: Grab QR "
print "14: Grab Color "
print "15: Find block with IR"
print "16: Check Hopper Colors"
def test_init_without_theta(self):
RobotArm(self.lengths, self.destinations)
def test_init_all_arguments(self):
RobotArm(self.lengths, self.destinations, self.theta)
# Adding the new thetas to iterates used for convergence analysis
if convergence_analysis is True:
iterates = np.concatenate(
(iterates, thetas.reshape(robot.n * robot.s, 1)), axis=1)
current_norm = np.linalg.norm(
augmented_lagrangian_objective_gradient(thetas))
if current_norm < global_tolerance:
path_figure(thetas.reshape((robot.n, robot.s), order='F'),
robot,
show=True)
print('The objective function evaluates to:',
objective_function(thetas))
print("Augmented lagrangian method successful")
if convergence_analysis is True:
return iterates, generate_objective_function(robot)
else:
return thetas
print("Augmented lagrangian method unsuccessful")
coordinates_tuple = ((5, 4, 6, 4, 5), (0, 2, 0.5, -2, -1))
lengths_tuple = (3, 2, 2)
robot = RobotArm(lengths_tuple, coordinates_tuple)
lambdas = np.zeros(2 * robot.s)
simple = generate_simple_augmented_lagrangian_function()
simple(robot)
import os
import sys
from time import sleep
import bot.lib.lib as lib
from robot_arm import RobotArm
from QRCode2 import QRCode2
bot_config = lib.get_config()
arm_config = bot_config["dagu_arm"]
arm = RobotArm(arm_config)
while True:
print "Commands: "
print "1: Set Angles"
print "2: Move to hopper pos"
print "3: Set Hopper"
print "4: Empty hopper"
print "5: Calibrate rail"
print "6: Pinch "
print "7: Let go "
print "8: Reset the arm and rail"
print "9: Check color in bin"
print "10: Tier Grab "
print "11: Turn Light On "
print "12: Turn Light Off "
print "13: Grab QR "
print "14: Grab Color "
print "15: Find block with IR"
print "16: Check Hopper Colors"
Command = input("Command: ")
show=True)
print("Augmented lagrangian method successful")
if convergence_analysis is True:
return iterates, generate_extended_objective_function
else:
return thetas_slack
path_figure(thetas_slack[:robot.n * robot.s].reshape((robot.n, robot.s),
order='F'),
robot,
show=True)
print("Augmented lagrangian method unsuccessful")
coordinates_tuple = ((-1, -3, -3, 0, 3), (5, 3, 4, 5, 2))
lengths_tuple = (3, 1, 1, 1, 1)
robot = RobotArm(lengths_tuple,
coordinates_tuple,
angular_constraint=np.pi / 2)
lagrange_multipliers = np.zeros(2 * robot.s * (robot.n + 1))
extended_augmented_lagrangian_method(lagrange_multipliers,
0.1,
1e-2,
5e-3,
4,
robot,
1e-6,
generate_initial_guess=True,
convergence_analysis=False)
import os, sys
from time import sleep
from robot_arm import RobotArm
arm = RobotArm("dagu_arm")
while True:
arm.servo_cape.transmit_block([1] + [0, 0, 0, 0, 0])
sleep(10)
#!/usr/bin/env python
import rospy
import time
from robot_arm import RobotArm
from ROSInterface import ROSInterface
from gripper import Gripper
from headController import FetchHeadController
from gripInterface import GripInterface
from originInterface import OriginInterface
if __name__ == "__main__":
# Setup clients
Ros = ROSInterface()
Arm = RobotArm()
GripRos = GripInterface()
GripHand = Gripper()
HeadTilt = FetchHeadController()
OriginROS = OriginInterface()
HeadTilt.look_at(0.7, 0, 0.7, "base_link")
print("Yes I looked")
while 1:
try:
Ros.Subscriber()
GripRos.GripSubscriber()
break
except:
rospy.loginfo('waiting for matlab')
import os import sys from time import sleep import pyDMCC from robot_arm import RobotArm from SeventhDOF import Rail_Mover arm = RobotArm() while True: sleep(5) arm.rail.DisplacementConverter(3.5) sleep(2) arm.SetPosition_1() sleep(1) arm.rail.Orientor(4) sleep(1) arm.rail.ResetToHome() sleep(1) arm.rail.DisplacementConverter(3.5) arm.SetPosition_2() sleep(5) arm.rail.ResetToHome()
"""
Fixtures for variables that need to be used across several tests
"""
import numpy as np
from robot_arm import RobotArm
lengths = (
3,
2,
2,
)
lengths2 = (
3,
1,
1,
1,
1,
)
destinations = (
(5, 4, 6, 4, 5),
(0, 2, 0.5, -2, -1),
)
theta = (
np.pi,
np.pi / 2,
0,
)
robot_arm1 = RobotArm(lengths, destinations, theta)
robot_arm2 = RobotArm(lengths2, destinations)