def __init__(self, torso_center, lower_body_flag):
self._body_center = torso_center
self._lower_body_flag = lower_body_flag
self.head = Head(Point(torso_center.x, torso_center.y+15, torso_center.z))
self.torso = Torso(Point(torso_center.x, torso_center.y, torso_center.z))
self.left_arm = Arm(Point(torso_center.x+6.6, torso_center.y+8, torso_center.z))
self.right_arm = Arm(Point(torso_center.x-6.6, torso_center.y+8, torso_center.z))
self.right_arm.set_slocal_rotate_angle(180, 0, 0)
if self._lower_body_flag:
self.left_leg = Leg(Point(torso_center.x+4, torso_center.y-10.6, torso_center.z))
self.right_leg = Leg(Point(torso_center.x-4, torso_center.y-10.6, torso_center.z))
self.right_leg.set_hlocal_rotate_angle(180, 0, 0)
def __init__(self): self.arm = Arm(self.ARM_BASE, 100, 160, 200, 1, -20) # TODO: Initialze arm object #self.paperPlot = PaperPlot() #self.forcePlot = ForcePlot() self.paper = Paper(self.PAPER_BASE) # Initialize paper self.armPlot = ArmPlot() # Initialize the 3D plot self.armPlot.plotPaper(self.paper) self.armPlot.plotArm(self.arm) self.controller = DeltaController(self.arm, self.paper)
class Actuators: def __init__(self,tamp): self.tamp=tamp #motor left self.motorL = Motor(self.tamp, 2, 4) self.motorL.write(1,0) #motor right self.motorR = Motor(self.tamp,1, 3) self.motorR.write(1,0) #arm servo self.arm = Arm(self.tamp) #sorting servo self.sorter = Sorter(self.tamp) def update(self): self.arm.update() self.sorter.update()
def __init__(self,tamp): self.tamp=tamp #motor left self.motorL = Motor(self.tamp, 2, 4) self.motorL.write(1,0) #motor right self.motorR = Motor(self.tamp,1, 3) self.motorR.write(1,0) #arm servo self.arm = Arm(self.tamp) #sorting servo self.sorter = Sorter(self.tamp)
def __init__(self,obs_size,n_actions,n_hidden_channels=50):
super().__init__()
with self.init_scope():
self.l0=L.Linear(obs_size,n_hidden_channels)
self.l1=L.Linear(n_hidden_channels,n_hidden_channels*2)
self.l2=L.Linear(n_hidden_channels*2,n_hidden_channels*2)
self.l3=L.Linear(n_hidden_channels*2,n_actions)
def __call__(self,x,test=False):
h=F.tanh(self.l0(x))
h=F.tanh(self.l1(h))
h=F.tanh(self.l2(h))
return chainerrl.action_value.DiscreteActionValue(self.l3(h))
if __name__ == "__main__":
env=Arm(4,[1.,1.,1.,1.])
print('observation space:',env.state)
print('action space:',env.actions)
obs=env.reset()
#env.render()
print('initial observation:',obs)
action=env.random_action()
print(action)
state,r,done=env.step(action)
print('next observation:',state)
print('reward:',r)
env.actions.shape[0]
obs_size=env.state.shape[0]
def __init__(self,
robot_config_file,
initRobotConfig,
timeStep,
odometryTimer=0.1,
defaultLogging=30):
"""
Creates the main bot class.
"""
if not os.path.exists(robot_config_file):
raise ValueError(f'Cannot find configuration file '
f'"{robot_config_file}"')
with open(robot_config_file, 'r') as f:
self.robotConfiguration = json.load(f)
# Adjust JSON just read
self.__adjustConfigDict(self.robotConfiguration)
# Exception Queue for inter threads exception communications
self.exceptionQueue = queue.Queue()
self.mqttMoveQueue = queue.Queue()
self.baseMovementLock = asyncio.locks.Lock()
self.timeStep = timeStep
self.powerSupply = PowerSupply()
# region sensor Init
self.sensors = {}
for sensor in self.robotConfiguration["sensors"]:
sensorDict = self.robotConfiguration["sensors"][sensor]
legoPort = LegoPort(sensorDict["input"])
# Applies to both I2C and SDK controled sensors
if legoPort.mode != sensorDict["mode"]:
legoPort.mode = sensorDict["mode"]
# Only applies to sensors controled by the python SDK
if "set_device" in sensorDict:
legoPort.set_device = sensorDict["set_device"]
time.sleep(1)
address = sensorDict["input"]
self.sensors[sensor] = sensorDict["sensorType"](address=address)
# endregion
# base Init
self.base = DiffDriveBase(self.robotConfiguration["base"],
initRobotConfig, odometryTimer)
# arm/endeffector Init
self.arm = Arm(armLinkDefinitions=self.robotConfiguration["arm"])
# Other matrix init
self.M0e = mr.RpToTrans(
R=np.array(
self.robotConfiguration["arm"]["arm1"]["manipulatorConfiguration"]["M0e"]["rotationMatrix"]).T, # noqa F501
p=self.robotConfiguration["arm"]["arm1"]["manipulatorConfiguration"]["M0e"]["p"]) # noqa F501
self.Toe_grip = mr.RpToTrans(
R=np.array(
self.robotConfiguration["arm"]["arm1"]["manipulatorConfiguration"]["Toe_grip"]["rotationMatrix"]).T, # noqa F501
p=self.robotConfiguration["arm"]["arm1"]["manipulatorConfiguration"]["Toe_grip"]["p"]) # noqa F501
self.Tbc = mr.RpToTrans(
R=np.array(
self.robotConfiguration["cameraConfiguration"]["Tbc"]["rotationMatrix"]).T, # noqa F501
p=self.robotConfiguration["cameraConfiguration"]["Tbc"]["p"])
self.Tcb = mr.TransInv(self.Tbc)
# Reset all motors at init
self.urgentStop = False
log.info(LOGGER_DDR_MAIN, 'Done main robot init.')
from time import sleep
import SocketServer
# from OmegaArmHandler import OmegaArmHandler # creates an Arm instance in the handler script
# some constants for testing
reference_position = [-75, 150, 300]
reference_vec56 = [0, 1, 0]
reference_vec67 = [1, 0, 0]
reference_end_effector = 90 # open servo gripper
upright = [0, 160, 200] # some upright position
# main function
# most of the stuff is done in the individual files
if __name__ == '__main__':
# hard coded test values
arm = Arm('COM6', 115200) # initialize the arm
# test setServoAngles
print "Controlling the robot via angles."
servoAngles = [90, 115, 65, 90, 90, 90, 90] # robot reaches up to its left
arm.setServoAngles(servoAngles)
print "Angle command sent."
sleep(2)
print "Controlling the robot via angles."
servoAngles = [90, 115, 110, 90, 90, 0, 0] # robot reaches up to its left
arm.setServoAngles(servoAngles)
print "Angle command sent."
sleep(2)
print "Controlling the robot via angles."
sim.timeoflastreset = 0 # time when arm was last reseted
# train/test params
sim.trainTime = 1 * 1e3
sim.testTime = 1 * 1e3
sim.cfg.duration = sim.trainTime + sim.testTime
sim.numTrials = ceil(sim.cfg.duration / 1e3)
sim.numTargets = 1
sim.targetid = 3 # target to train+test
sim.trialTargets = [
sim.targetid
] * sim.numTrials #[i%sim.numTargets for i in range(int(sim.numTrials+1))] # set target for each trial
# create Arm class and setup
if sim.useArm:
sim.arm = Arm(sim.animArm, sim.graphsArm)
sim.arm.targetid = 0
sim.arm.setup(sim) # pass framework as argument
# Function to run at intervals during simulation
def runArm(t):
# turn off RL and explor movs for last testing trial
if t >= sim.trainTime:
sim.useRL = False
sim.explorMovs = False
if sim.useArm:
sim.arm.run(
t, sim
) # run virtual arm apparatus (calculate command, move arm, feedback)
def recluster(self, cluster_positions, cluster_fitnesses, labels):
k = max(labels) + 1
# Compare ranks to find unchanged arms
old_arms_ranks = self.get_ranks()
new_arms_ranks = self.get_ranks(cluster_fitnesses)
if self.verbose:
print('\nold ranks:')
for ranks in old_arms_ranks:
print(ranks)
print('new ranks:')
for ranks in new_arms_ranks:
print(ranks)
# Generate new arms
new_arms = []
matrices = [
Matrix(positions, fitnesses, self.min_bounds,
self.max_bounds) for positions, fitnesses in zip(
cluster_positions, cluster_fitnesses)
]
trans_samples = np.random.uniform(0,
1,
size=(self.n_samples,
self.dimension))
for new_index, new_ranks in enumerate(new_arms_ranks):
new_set = set(new_ranks)
arm = None
# Get best positions for matrix optimization
argmax = np.argmax(cluster_fitnesses[new_index])
best = cluster_positions[new_index][argmax]
# Get exclude positions for matrix optimization
exclude = []
for j in range(k):
if new_index != j:
samples = matrices[j].inverse_transform(trans_samples)
exclude.extend(samples)
exclude = np.array(exclude)
# Case 1: Copy old arm with more than 50% similarities
for old_index, old_ranks in enumerate(old_arms_ranks):
intersection = new_set.intersection(old_ranks)
duplicate_ratio = len(intersection) / max(
len(old_ranks), len(new_ranks))
#if len(intersection) > max( len(old_ranks)/2, len(new_ranks)/2 ):
if duplicate_ratio > 0.5:
# Optimize matrix
if self.verbose:
print('\nOptimizing matrix for new arm %d' % new_index)
matrices[new_index].optimize(cluster_positions[new_index],
cluster_fitnesses[new_index],
exclude)
# Copy unchanged arm to new arms
arm = self.arms[old_index]
# Transform Algorithm to new subspace
arm.transform(matrices[new_index])
# Find update particles
insert_ranks = new_set.difference(intersection)
remove = set(old_ranks).difference(new_set)
resize_ranks = sorted(deepcopy(new_ranks))
resize_ranks = resize_ranks[:self.n_points]
if self.verbose:
print('Copy arm %d -> %d: %.1f%% duplicate' %
(old_index, new_index, 100 * duplicate_ratio))
#print('intersection:', list(intersection))
#print('insert_ranks:', list(insert_ranks))
#print('remove_ranks:', list(remove))
#print('resize_ranks:', resize_ranks)
#print('\noriginal:')
#print(self.arms[old_index].get_positions())
#print(self.arms[old_index].get_fitnesses())
#print(old_ranks)
#print('\nnew:')
#print(cluster_positions[new_index])
#print(cluster_fitnesses[new_index])
#print(new_ranks)
replace_indices, replace_positions, replace_fitnesses = [], [], []
for i, rank in enumerate(old_ranks):
if rank not in resize_ranks:
# insert set is not empty
if insert_ranks:
insert_rank = insert_ranks.pop()
index = new_ranks.index(insert_rank)
replace_position = cluster_positions[
new_index][index]
subspace_position = arm.matrix.transform(
[replace_position])[0]
replace_fitness = cluster_fitnesses[new_index][
index]
#if self.verbose:
#if self.verbose:
# print('old_rank[%d] = %d is replaced by rank %d, f = %f, pos='
# % (i, rank, insert_rank, replace_fitness), replace_position)
# Replace particle with a random sample
else:
subspace_position = np.random.uniform(
0, 1, size=(self.dimension))
replace_position = arm.matrix.inverse_transform(
[subspace_position])[0]
replace_fitness = self.obj(replace_position)
#if self.verbose:
# print('old_rank[%d] = %d is replaced by sample point f = %f, pos='
# % (i, rank, replace_fitness), replace_position)
replace_indices.append(i)
replace_positions.append(subspace_position)
replace_fitnesses.append(replace_fitness)
try:
arm.algo.replace(replace_indices, replace_positions,
replace_fitnesses)
except AssertionError as e:
arm = None
for i, p, f in zip(replace_indices, replace_positions,
replace_fitnesses):
print(i, p, f)
input()
break
arm.update_model()
break
# Case 2: Create new arm
if arm is None:
arm = Arm(self.obj,
self.n_points,
cluster_positions[new_index],
cluster_fitnesses[new_index],
algo_type=self.algo_type,
exclude=exclude,
min_bounds=self.min_bounds,
max_bounds=self.max_bounds)
# Append new_arm and update matrix
new_arms.append(arm)
matrices[new_index] = new_arms[new_index].matrix
self.arms = new_arms
self.config_ax.set_ylim(-0.1, 2*pi + 0.1)
self.op_ax.set_xlim(-10.1, 10.1)
self.op_ax.set_ylim(-5.1, 15.1)
self.config_ax.axvline(self.arm.control_links[0].min_angle)
self.config_ax.axvline(self.arm.control_links[0].max_angle)
self.config_ax.axhline(self.arm.control_links[1].min_angle)
self.config_ax.axhline(self.arm.control_links[1].max_angle)
self.config_ax.set_xlabel('$\Theta_1$')
self.config_ax.set_ylabel('$\Theta_2$')
self.op_ax.set_xlabel('X')
self.op_ax.set_ylabel('Y')
self.fig.canvas.draw()
if __name__ == '__main__':
l1 = FixedLink(length = 5, angle = pi/2)
l2 = FixedLink(length = 0, angle = -pi/2)
l3 = Link(length = 6, max_angle=2*pi)
l4 = FixedLink(length = 0, angle = -pi/2)
l5 = Link(length = 4, max_angle=2*pi)
l6 = FixedLink(length = 0, angle = -pi/2)
l7 = Link(length = 4, max_angle=2*pi)
arm = Arm([l1, l3, l5])
print(arm)
gui = GUI(arm)
def __init__(self):
self.arm = Arm()
self.hand = Hand()
io_reader.Start()
arm_config = ArmConfig()
arm_config.ApplyTares(bank, io_reader)
# while True:
# s = "\n------\n"
# for i in range(22, 32, 2):
# s += " PIN %d = %d\n" % (i, io_reader.GetPin(i))
# logging.info("RPS = %f %s", io_reader.ReadsPerSec(), s)
# time.sleep(0.2)
bank.Release()
bank.Rezero()
time.sleep(5.0)
hand = Hand(bank, io_reader, arm_config)
arm = Arm(bank, io_reader, arm_config)
#hand.Dispense()
# if True:
# hand.Grab()
# bank.WaitDone()
# time.sleep(5)
# logging.info("Release now.")
# hand.Release()
# bank.WaitDone()
# while True:
# logging.info("Wait here forever.")
# time.sleep(10)
hand.Calibrate()
#bank.WaitDone()
bank.base_motor.SetDisableAfterMoving(False)
arm.Rotate(-math.pi / 2)
PORT = 8765
RIGHT_ARM_INDEX = 0
LEFT_ARM_INDEX = 50
decoder = json.JSONDecoder()
robot = piarm.PiArm()
robot.connect("/dev/ttyAMA0")
DEFAULT_RESPONSE = "ack".encode()
CLIENT_ERROR = "CLIENT error".encode()
SERVER_ERROR = "SERVER error".encode()
ARMS = {
'Right': Arm(robot, RIGHT_ARM_INDEX),
'Left': Arm(robot, LEFT_ARM_INDEX)
}
def client_thread(conn, robot_arm):
# conn.send(":smile:")
while True:
try:
data = conn.recv(1024) # buffer size is 1024 bytes
except socket.timeout:
print('Socket timed out')
break
# This check will ensure that empty streams, generally caused by
# unorderly terminating a socket connection at the client side
class ArmSim(object):
# ===== Paper location input for simulation =====
Rp = Rrpy(pi/2,0,0) # Roll-pitch-yaw rotation parameterization
PAPER_BASE = np.identity(4) # Paper frame rigid body transform
PAPER_BASE[0:3,0:3] = Rp # Paper rotation matrix
PAPER_BASE[0:3,3] = np.asfarray([-Paper.X_SIZE/2,-50,0]) # Paper origin
# Arm Location
Ra = Rrpy(-pi/2-0.05,pi/2,0) # Arm Roll-Pitch-Yaw base orientation parameterization
ARM_BASE = np.identity(4) # Arm fixed-base rigid body transform
ARM_BASE[0:3,3] = np.asfarray([250,-50,0]) # Arm origin
ARM_BASE[0:3,0:3] = Ra # Set rotation
INITIAL_CONFIG = np.asfarray([pi/4,pi/4,0]) # Initial arm joint configuration
# ===== Set of waypoints on paper =====
WAYPOINTS = [[[10,0],[10,30]],[[10,15],[20,15]],[[20,0],[20,30]]]
def __init__(self):
self.arm = Arm(self.ARM_BASE, 100, 160, 200, 1, -20) # TODO: Initialze arm object
#self.paperPlot = PaperPlot()
#self.forcePlot = ForcePlot()
self.paper = Paper(self.PAPER_BASE) # Initialize paper
self.armPlot = ArmPlot() # Initialize the 3D plot
self.armPlot.plotPaper(self.paper)
self.armPlot.plotArm(self.arm)
self.controller = DeltaController(self.arm, self.paper)
# Run the simulation
def run(self, strokes, initialConfig, minStep=100):
initialConfig = np.asfarray(initialConfig)
current = initialConfig # Current configuration
self.worldWaypoints = self.paper.strokesToWorld(strokes)
# Loop over waypoints
#for i in range(0,len(strokes)):
'''
ikConfig = np.append(self.arm.planarIK(strokes[i][0]),[50])# Compute IK
print str(ikConfig)
nsteps = minStep
configs = interpolateLinear(current, ikConfig, nsteps) # Interpolate trajectory
print configs.shape'''
configs = self.controller.generateTrajectory(strokes)
# Wait for replay
# Loop over interpolated configurations
while(True):
for k in range(0, len(configs)):
print 'Step', k
#print str(configs[k])
self.arm.setConfiguration(self.rx64RoundConfig(configs[k])) # Update arm position
self.armPlot.clear()
self.armPlot.plotArm(self.arm) # Plot
self.armPlot.plotPaper(self.paper) # Plot paper again
self.armPlot.plotIdealPath(self.worldWaypoints)
#print 'Arm Position', str(self.arm.eePosition())
self.arm.printEEPosition()
draw()
self.armPlot.fig.show()
sleep(0.0001)
c = input('Enter some string to continue')
#current = ikConfig
# Round the configuration to RX64 angles
def rx64RoundConfig(self, config, randomness=0):
nbits = 10
rx64Range = 5.0*pi/3.0
dConfig = (config/rx64Range) * pow(2.0, nbits)
#print 'Discrete configuration:', str(dConfig)
dConfig = np.round(dConfig)
roundedConfig = dConfig/pow(2.0, nbits) * rx64Range
#print 'Rounded Configuration', str(roundedConfig)
# Generate Gaussian
return roundedConfig
class Robot_Interface(object):
"""For usage with the Fetch robot."""
def __init__(self, simulation=True):
"""Initializes various aspects of the Fetch.
TODOs: get things working, also use `simulation` flag to change ROS
topic names if necessary (especially for the cameras!). UPDATE: actually
I don't think this is necessary now, they have the same topic names.
"""
rospy.init_node("fetch")
self.arm = Arm()
self.arm_joints = ArmJoints()
self.base = Base()
self.camera = RGBD()
self.head = Head()
self.gripper = Gripper(self.camera)
self.torso = Torso()
self.joint_reader = JointStateReader()
# Tucked arm starting joint angle configuration
self.names = ArmJoints().names()
self.tucked = [1.3200, 1.3999, -0.1998, 1.7199, 0.0, 1.6600, 0.0]
self.tucked_list = [(x,y) for (x,y) in zip(self.names, self.tucked)]
# Initial (x,y,yaw) position of the robot wrt map origin. We keep this
# fixed so that we can reset to this position as needed. The HSR's
# `omni_base.pose` (i.e., the start pose) returns (x,y,yaw) where yaw is
# the rotation about that axis (intuitively, the z axis). For the base,
# `base.odom` supplies both `position` and `orientation` attributes.
start = copy.deepcopy(self.base.odom.position)
yaw = Base._yaw_from_quaternion(self.base.odom.orientation)
self.start_pose = np.array([start.x, start.y, yaw])
self.TURN_SPEED = 0.3
self.num_restarts = 0
def body_start_pose(self, start_height=0.10, end_height=0.10, velocity_factor=None):
"""Sets the robot's body to some initial configuration.
The HSR uses `whole_body.move_to_go()` which initializes an appropriate
posture so that the hand doesn't collide with movement. For the Fetch,
we should probably make the torso extend, so the arms can extend more
easily without collisions. Use `move_to_joint_goal` since that uses
motion planning. Do NOT directly set the joints without planning!!
"""
self.torso.set_height(start_height)
self.arm.move_to_joint_goal(self.tucked_list, velocity_factor=velocity_factor)
self.torso.set_height(end_height)
# Specific to the siemens challenge (actually a lot of this stuff is ...)
if self.num_restarts == 0:
self.base.turn(angular_distance=45*DEG_TO_RAD)
self.num_restarts += 1
def head_start_pose(self):
"""Hard-coded starting pose for the robot's head.
These values are from the HSR. The Fetch needs a different pan and tilt.
Positive pan means rotating counterclockwise when looking at robot from
an aerial view.
"""
self.head.pan_tilt(pan=0.0, tilt=0.8)
def position_start_pose(self, offsets=None, do_something=False):
"""Assigns the robot's base to some starting pose.
Mainly to "reset" the robot to the original starting position (and also,
rotation about base axis) after it has moved, usually w/no offsets.
Ugly workaround: we have target (x,y), and compute the distance to the
point and the angle. We turn the Fetch according to that angle, and go
forward. Finally, we do a second turn which corresponds to the target
yaw at the end. This turns w.r.t. the current angle, so we undo the
effect of the first turn. See `examples/test_position_start_pose.py`
for tests.
Args:
offsets: a list of length 3, indicating offsets in the x, y, and
yaws, respectively, to be added onto the starting pose.
"""
# Causing problems during my tests of the Siemens demo.
if not do_something:
return
current_pos = copy.deepcopy(self.base.odom.position)
current_theta = Base._yaw_from_quaternion(self.base.odom.orientation) # [-pi, pi]
ss = np.array([current_pos.x, current_pos.y, current_theta])
# Absolute target position and orientation specified with `pp`.
pp = np.copy(self.start_pose)
if offsets:
pp += np.array(offsets)
# Get distance to travel, critically assumes `pp` is starting position.
dist = np.sqrt( (ss[0]-pp[0])**2 + (ss[1]-pp[1])**2 )
rel_x = ss[0] - pp[0]
rel_y = ss[1] - pp[1]
assert -1 <= rel_x / dist <= 1
assert -1 <= rel_y / dist <= 1
# But we also need to be *facing* the correct direction, w/input [-1,1].
# First, get the opposite view (facing "outwards"), then flip by 180.
desired_facing = np.arctan2(rel_y, rel_x) # [-pi, pi], facing outward
desired_theta = math.pi + desired_facing # [0, 2*pi], flip by 180
if desired_theta > math.pi:
desired_theta -= 2*math.pi # [-pi, pi]
# Reconcile with the current theta. Got this by basically trial/error
angle = desired_theta - current_theta # [-2*pi, 2*pi]
if angle > math.pi:
angle -= 2*math.pi
elif angle < -math.pi:
angle += 2*math.pi
self.base.turn(angular_distance=angle, speed=self.TURN_SPEED)
self.base.go_forward(distance=dist, speed=0.2)
# Back at the start x, y, but now need to consider the _second_ turn.
# The robot is facing at `desired_theta` rads, but wants `pp[2]` rads.
final_angle = pp[2] - desired_theta
if final_angle > math.pi:
final_angle -= 2*math.pi
elif final_angle < -math.pi:
final_angle += 2*math.pi
self.base.turn(angular_distance=final_angle, speed=self.TURN_SPEED)
def get_img_data(self):
"""Obtain camera and depth image.
Returns:
Tuple containing RGB camera image and corresponding depth image.
"""
c_img = self.camera.read_color_data()
d_img = self.camera.read_depth_data()
return (c_img, d_img)
def get_depth(self, point, d_img):
"""Compute mean depth near grasp point.
NOTE: assumes that we have a simlar `cfg.ZRANGE` as with the HSR. I'm
not sure where exactly this comes from.
"""
y, x = int(point[0]), int(point[1])
z_box = d_img[y-ZRANGE:y+ZRANGE, x-ZRANGE:x+ZRANGE]
indx = np.nonzero(z_box)
z = np.mean(z_box[indx])
return z
def get_rot(self, direction):
"""Compute rotation of gripper such that given vector is grasped.
Currently this directly follows the HSR code as there's nothing
Fetch-dependent.
"""
dy, dx = direction[0], direction[1]
dx *= -1
if dy < 0:
dx *= -1
dy *= -1
rot = np.arctan2(dy, dx)
rot = np.pi - rot
return rot
def create_grasp_pose(self, x, y, z, rot):
""" If `intuitive=True` then x,y,z,rot interpreted wrt some link in the
world, e.g., 'odom' for the Fetch. It's False by default to maintain
backwards compatibility w/Siemens-based code.
"""
pose_name = self.gripper.create_grasp_pose(x, y, z, rot)
return pose_name
def open_gripper(self):
self.gripper.open()
def close_gripper(self):
self.gripper.close()
def move_to_pose(self, pose_name, z_offset, velocity_factor=None):
"""Moves to a pose.
In the HSR, moved the `hand_palm_link` to the frame named `pose_name` at
the correct pose. For the Fetch we should be able to extract the pose
from `pose_name` and then call the Arm's `move_to_pose` method.
Args:
pose_name: A string name for the pose to go
z_offset: Scalar offset in z-direction, offset is wrt the pose
specified by `pose_name`.
velocity_factor: controls the speed, closer to 0 means slower,
closer to 1 means faster. (If 0.0, then it turns into 1.0 for
some reason.) Values greater than 1.0 are cut to 1.0.
"""
# See:
# http://wiki.ros.org/tf/Tutorials/Writing%20a%20tf%20listener%20%28Python%29
# https://answers.ros.org/question/256354/does-tftransformlistenerlookuptransform-return-quaternion-position-or-translation-and-rotation/
# First frame should be the reference frame, use `base_link`, not `odom`.
point, quat = self.gripper.tl.lookupTransform('base_link', pose_name, rospy.Time(0))
z_point = point[2] + z_offset
# See:
# https://github.com/cse481wi18/cse481wi18/blob/indigo-devel/applications/scripts/cart_arm_demo.py
# https://github.com/cse481wi18/cse481wi18/wiki/Lab-19%3A-Cartesian-space-manipulation
ps = PoseStamped()
ps.header.frame_id = 'base_link'
ps.pose = Pose(
Point(point[0], point[1], z_point),
Quaternion(quat[0], quat[1], quat[2], quat[3])
)
# See `arm.py` written by Justin Huang
error = self.arm.move_to_pose(pose_stamped=ps, velocity_factor=velocity_factor)
if error is not None:
rospy.logerr(error)
def find_ar(self, ar_number, velocity_factor=None):
try:
ar_name = 'ar_marker/' + str(ar_number)
# HSR code, with two hard-coded offsets?
#self.whole_body.move_end_effector_pose(geometry.pose(y=0.08, z=-0.3), ar_name)
# Fetch 'translation'. Note the `ar_name` for pose name.
point, quat = self.gripper.tl.lookupTransform('base_link', ar_name, rospy.Time(0))
y_point = point[1] + 0.08
z_point = point[2] - 0.3
ps = PoseStamped()
ps.header.frame_id = 'base_link'
ps.pose = Pose(
Point(point[0], y_point, z_point),
Quaternion(quat[0], quat[1], quat[2], quat[3])
)
error = self.arm.move_to_pose(pose_stamped=ps, velocity_factor=velocity_factor)
if error is not None:
rospy.logerr(error)
return True
except:
return False
def pan_head(self, tilt):
"""Adjusts tilt of the robot, AND set pan at zero.
Args:
tilt: Value in radians, positive means looking downwards.
"""
self.head.pan_tilt(pan=0, tilt=tilt)
class EyeInHand:
def __init__(self, limb_name):
self.limb_name = limb_name
self.arm = Arm(limb_name)
self.available_tread = True
if limb_name == 'left':
self.other_limb_name = 'right'
else:
self.other_limb_name = 'left'
self.otherArm = Arm(self.other_limb_name)
#self.otherArm.tuck_arm()
self.cards = np.array([])
self.bridge = CvBridge()
self.cards = Cards()
#rospy.init_node(limb_name, anonymous=True)
self.display_pub = rospy.Publisher('/robot/xdisplay', Image)
head_camera = baxter_interface.CameraController('head_camera')
head_camera.close()
self.other_camera = baxter_interface.CameraController(
self.other_limb_name + '_hand_camera')
self.other_camera.close()
self.camera = baxter_interface.CameraController(self.limb_name +
'_hand_camera')
self.camera.open()
self.arm.calibrate()
resp = 'n'
while resp is not 'y':
resp = str(
raw_input(
'Please place the gripper arm at the center of the card holder, press "y" to continue: '
))
self.arm.go_home()
if self.available_tread:
cam_thread = threading.Thread(target=self.get_camera)
choices_thread = threading.Thread(target=self.pick_choices)
cam_thread.start()
choices_thread.start()
rospy.spin()
cv2.destroyAllWindows()
def get_camera(self):
self.available_tread = False
sub = rospy.Subscriber(
'/cameras/' + self.limb_name + '_hand_camera/image', Image,
self.callback, None, 1)
def callback(self, msg):
self.cards.img = self.bridge.imgmsg_to_cv2(msg)
cv2.imshow('main', self.cards.img)
cv2.waitKey(1)
self.corners, self.contours, self.hierarchy = self.cards.get_features(
(5, 5))
for iterations, corner in enumerate(self.corners):
card_img = self.cards.extract_card(corner)
self.cards.append(card_img)
cv2.imshow(str(iterations), card_img)
def pick_choices(self):
while not rospy.is_shutdown():
card_response = 100
while (card_response < 0
or card_response > 3) and not rospy.is_shutdown():
card_response = int(
input(
'please pick a card index, where the left most corner is 0 and the right most corner is 3: '
))
self.arm.choose_card(card_response)
if card_response < 0 or card_response > 3:
print(
'index out of range! please choose a number between 0 and 3!'
)
from math import pi
from stepper_motors.stepper import Stepper
from arm import Arm
from link import Link, FixedLink
from arm_astar import ArmAStar
from arm_gui import ArmGUI
if __name__ == '__main__':
"""
Script to run the A* code on the physical workshop arm.
"""
lf1 = FixedLink(length=0, angle=pi / 2)
l1 = Link(length=11.8, angle=0, min_angle=-1e4, max_angle=1e4)
lf2 = FixedLink(length=.12, angle=pi / 2)
lf3 = FixedLink(length=0, angle=-pi / 2)
l2 = Link(length=11.8, angle=0, min_angle=-1e4, max_angle=1e4)
s2 = Stepper(2, 3, 4, 17, delay=1e-2)
s1 = Stepper(27, 22, 10, 9, delay=1e-2)
s1.reverse()
arm = Arm([lf1, l1, lf2, lf3, l2], [s1, s2])
print(arm)
astar = ArmAStar(arm, discretization=3 * pi / 180, min_dist=0.5)
gui = ArmGUI(arm, astar)
xboxCont = XboxController(controlCallBack, deadzone = 30, scale = 100, invertYAxis = True)
#start the xbox controller
xboxCont.start()
#=================================================
kin = Kinematic()
#Clear Log File
open('logs/log.txt', 'w').close()
with open('config/config.json') as json_file:
config = json.load(json_file)
#Create instance of Arm class for each arm
arm1 = Arm(config)
arm2 = Arm(config, "192.168.0.106")
arm = []
arm.append(arm1)
arm.append(arm2)
arm[0].resetEStop()
arm[1].resetEStop()
#calibrate both arms
arm[0].calibrateArm()
arm[1].calibrateArm()
while(not (arm[0].armCalibrated() and arm[1].armCalibrated())):
time.sleep(1)
# -*- coding: utf-8 -*
from car import Car
from arm import Arm
import time
if __name__ == "__main__":
# car = Car()
# car.capture()
# time.sleep(1)
# car.unload()
arm = Arm()
# for i in range(2500, 0, -10):
arm.servo_angle(arm.s1, 2500)
time.sleep(0.01)
import numpy as np
from arm.arm_directions import ArmDirections
from arm.move_arm import move_arm
import arm.Arm
default_arm_positions = [np.pi / 2, np.pi / 2, 0]
## create the robot arm
arm = Arm.Arm3Link(q=default_arm_positions, L=np.array([90, 60, 48]))
print('the old angles are:')
print(arm.q)
print(arm.get_xy())
old_angles = default_arm_positions
# x, y = -100, 80
x, y = -70, 0
arm.q = arm.inv_kin([x, y])
new_angles = arm.q
print('new angle is:')
print(arm.q)
print(arm.get_xy())
move_arm(old_angles[0] - new_angles[0], ArmDirections.up(),
ArmDirections.down())
from room2 import Room
from rotplate import RotationPlate
from connection_cylinder import ConnectionCylinder
from topcylinder import TopCylinder
from arm import Arm
from arm2 import Arm2
from camerabody import CameraRoom
from topmotor_holder import TopMotorHolder
from wire_conductor import WireConductor
if __name__ == "__main__":
room = Room()
plate = RotationPlate()
concyl = ConnectionCylinder()
topcylinder = TopCylinder()
arm0 = Arm()
arm1 = Arm2()
camera = CameraRoom()
wire_conductor = WireConductor()
topmotor_holder = TopMotorHolder()
concyl.set_color(0, 1, 0, 0)
topcylinder.rotateZ(deg(90))
room.socket.link(plate)
plate.socket.link(concyl)
concyl.socket.link(topcylinder)
topcylinder.socket0.link(arm0)
class Application:
def __init__(self, configfile, map_name, human=True, fps=DEFAULT_FPS):
self.running = False
self.displaySurface = None
self.config = configparser.ConfigParser()
self.config.read(configfile)
self.fps = fps
self.__human = human
self.clock = pygame.time.Clock()
self.trajectory = []
# Parse config file
self.windowTitle = "CS440 MP2 Robotic Arm"
self.window = eval(self.config.get(map_name, 'Window'))
armBase = eval(self.config.get(map_name, 'ArmBase'))
armLinks = eval(self.config.get(map_name, 'ArmLinks'))
self.armLimits = [(0, 0), (0, 0), (0, 0)]
for i in range(len(armLinks)):
self.armLimits[i] = armLinks[i][-1]
self.arm = Arm(armBase, armLinks)
self.obstacles = eval(self.config.get(map_name, 'Obstacles'))
self.goals = eval(self.config.get(map_name, 'Goals'))
# Initializes the pygame context and certain properties of the maze
def initialize(self):
pygame.init()
self.displaySurface = pygame.display.set_mode(
(self.window[0], self.window[1]), pygame.HWSURFACE)
self.displaySurface.fill(WHITE)
pygame.display.flip()
pygame.display.set_caption(self.windowTitle)
self.running = True
# Once the application is initiated, execute is in charge of drawing the game and dealing with the game loop
def execute(self, searchMethod, granularity, trajectory, saveImage,
saveMaze):
self.initialize()
if not self.running:
print("Program init failed")
raise SystemExit
currAngle = [0, 0, 0]
for i in range(len(self.arm.getArmAngle())):
currAngle[i] = self.arm.getArmAngle()[i]
self.gameLoop()
if not self.__human:
print("Transforming a map configuration to a maze...")
maze = transformToMaze(self.arm, self.goals, self.obstacles,
self.window, granularity)
#Save testfile
maze.saveToFile('TestTransform.txt')
print("Done!")
print("Searching the path...")
path = search(maze, searchMethod)
if path is None:
print("No path found!")
else:
for i in range(len(path)):
self.arm.setArmAngle(path[i])
if (trajectory > 0) and (i % trajectory == 0):
self.trajectory.append(self.arm.getArmPos())
self.gameLoop()
print("Done!")
self.drawTrajectory()
while self.running:
pygame.event.pump()
keys = pygame.key.get_pressed()
if (keys[K_ESCAPE]):
self.running = False
if self.__human:
alpha, beta, gamma = currAngle
if (keys[K_z]):
alpha += granularity if isValueInBetween(
self.armLimits[ALPHA], alpha + granularity) else 0
if (keys[K_x]):
alpha -= granularity if isValueInBetween(
self.armLimits[ALPHA], alpha - granularity) else 0
if (keys[K_a]):
beta += granularity if isValueInBetween(
self.armLimits[BETA], beta + granularity) else 0
if (keys[K_s]):
beta -= granularity if isValueInBetween(
self.armLimits[BETA], beta - granularity) else 0
if (keys[K_q]):
gamma += granularity if isValueInBetween(
self.armLimits[GAMMA], gamma + granularity) else 0
if (keys[K_w]):
gamma -= granularity if isValueInBetween(
self.armLimits[GAMMA], gamma - granularity) else 0
newAngle = (alpha, beta, gamma)
tempArm = copy.deepcopy(self.arm)
tempArm.setArmAngle(newAngle)
armEnd = tempArm.getEnd()
armPos = tempArm.getArmPos()
armPosDist = tempArm.getArmPosDist()
print("doesArmTouchObjects",
doesArmTouchObjects(armPosDist, self.obstacles))
# print(armPosDist)
if doesArmTouchObjects(
armPosDist, self.obstacles) or not isArmWithinWindow(
armPos, self.window):
continue
if not doesArmTipTouchGoals(
armEnd, self.goals) and doesArmTouchObjects(
armPosDist, self.goals, isGoal=True):
continue
self.arm.setArmAngle(newAngle)
self.gameLoop()
currAngle = copy.deepcopy(newAngle)
if doesArmTipTouchGoals(armEnd, self.goals):
self.gameLoop()
print("SUCCESS")
raise SystemExit
if saveImage:
pygame.image.save(self.displaySurface, saveImage)
if saveMaze and not self.__human:
maze.saveToFile(saveMaze)
def gameLoop(self):
self.clock.tick(self.fps)
self.displaySurface.fill(WHITE)
self.drawTrajectory()
self.drawArm()
self.drawObstacles()
self.drawGoal()
pygame.display.flip()
def drawTrajectory(self):
cnt = 1
for armPos in self.trajectory:
x = (255 - 255 / len(self.trajectory) * cnt)
color = (x, x, x)
cnt += 1
for i in range(len(armPos)):
pygame.draw.line(self.displaySurface, color, armPos[i][0],
armPos[i][1], ARM_LINKS_WIDTH[i])
def drawArm(self):
armPos = self.arm.getArmPos()
for i in range(len(armPos)):
pygame.draw.line(self.displaySurface, BLACK, armPos[i][0],
armPos[i][1], ARM_LINKS_WIDTH[i])
def drawObstacles(self):
for obstacle in self.obstacles:
pygame.draw.circle(self.displaySurface, RED,
(obstacle[0], obstacle[1]), obstacle[2])
def drawGoal(self):
for goal in self.goals:
pygame.draw.circle(self.displaySurface, BLUE, (goal[0], goal[1]),
goal[2])
from __future__ import division
import numpy as np
from arm import Arm
import math
###############################################################################
# CONSTANTS AND FUNCTIONS
num_arms = 3
time_horizon = 50
###############################################################################
all_arms = [Arm(0, 1), Arm(1, 1), Arm(1.1, 1)]
valid_indices = set(range(num_arms))
delta = 1
num_rounds = int(math.floor(0.5 * math.log(time_horizon / math.exp(1), 2)))
print num_rounds
iteration = 0
reward = 0
for m in range(num_rounds):
counts = [0] * num_arms
n_m = int(math.ceil(2 * math.log(time_horizon * delta**2) / (delta**2)))
goal = [n_m if i in valid_indices else 0 for i in range(num_arms)]
while counts != goal:
i = np.random.choice(list(valid_indices))
if counts[i] >= n_m:
continue
new_reward = all_arms[i].sample()
counts[i] += 1
reward += new_reward
print "\nIteration: %d" % iteration
class Body:
def __init__(self, torso_center, lower_body_flag):
self._body_center = torso_center
self._lower_body_flag = lower_body_flag
self.head = Head(Point(torso_center.x, torso_center.y+15, torso_center.z))
self.torso = Torso(Point(torso_center.x, torso_center.y, torso_center.z))
self.left_arm = Arm(Point(torso_center.x+6.6, torso_center.y+8, torso_center.z))
self.right_arm = Arm(Point(torso_center.x-6.6, torso_center.y+8, torso_center.z))
self.right_arm.set_slocal_rotate_angle(180, 0, 0)
if self._lower_body_flag:
self.left_leg = Leg(Point(torso_center.x+4, torso_center.y-10.6, torso_center.z))
self.right_leg = Leg(Point(torso_center.x-4, torso_center.y-10.6, torso_center.z))
self.right_leg.set_hlocal_rotate_angle(180, 0, 0)
def horiz_move(self, gx, gy, gz):
movement = tf.translate(Vector(gx, gy, gz)) # Affine matrix for horizontal movement.
self._body_center = movement * self._init_torso_center
self.head.horiz_move(x, y, z)
self.torso.horiz_move(x, y, z)
self.left_arm.horiz_move(x, y, z)
self.right_arm.horiz_move(x, y, z)
if self._lower_body_flag:
self.left_leg.horiz_move(x, y, z)
self.right_leg.horiz_move(x, y, z)
def calc_body_center(self):
return self._body_center
def get_property_list(self):
property_list = []
property_list.extend(self.head.get_property_list())
property_list.extend(self.torso.get_property_list())
property_list.extend(self.left_arm.get_property_list())
property_list.extend(self.right_arm.get_property_list())
if self._lower_body_flag:
property_list.extend(self.left_leg.get_property_list())
property_list.extend(self.right_leg.get_property_list())
return property_list
def constr_json_data(self):
# Construcat JSON data
json_data_head = self.head.constr_json_data()
json_data_torso = self.torso.constr_json_data()
json_data_left_arm = self.left_arm.constr_json_data()
json_data_right_arm = self.right_arm.constr_json_data()
if self._lower_body_flag:
json_data_left_leg = self.left_leg.constr_json_data()
json_data_right_leg= self.right_leg.constr_json_data()
json_data = {
'joint_angle': {
'left_shoulder': json_data_left_arm['joint_angle']['shoulder'],
'left_elbow': json_data_left_arm['joint_angle']['elbow'],
'right_shoulder': json_data_right_arm['joint_angle']['shoulder'],
'right_elbow': json_data_right_arm['joint_angle']['elbow']
},
'true_position': {
'head': json_data_head['true_position']['head'],
'torso': json_data_torso['true_position']['torso'],
'left_shouler': json_data_left_arm['true_position']['shoulder'],
'left_elbow': json_data_left_arm['true_position']['elbow'],
'left_hand': json_data_left_arm['true_position']['hand'],
'right_shoulder': json_data_right_arm['true_position']['shoulder'],
'right_elbow': json_data_right_arm['true_position']['elbow'],
'right_hand': json_data_right_arm['true_position']['hand']
}
}
if self._lower_body_flag:
json_data['joint_angle']['left_hip'] = json_data_left_leg['joint_angle']['hip']
json_data['joint_angle']['left_knee'] = json_data_left_leg['joint_angle']['knee']
json_data['joint_angle']['right_hip'] = json_data_right_leg['joint_angle']['hip']
json_data['joint_angle']['right_knee'] = json_data_right_leg['joint_angle']['knee']
json_data['true_position']['left_hip'] = json_data_left_leg['true_position']['hip']
json_data['true_position']['left_knee'] = json_data_left_leg['true_position']['knee']
json_data['true_position']['left_foot'] = json_data_left_leg['true_position']['foot']
json_data['true_position']['right_hip'] = json_data_right_leg['true_position']['hip']
json_data['true_position']['right_knee'] = json_data_right_leg['true_position']['knee']
json_data['true_position']['right_foot'] = json_data_right_leg['true_position']['foot']
return json_data
def set_from_json_data(self, json_data):
# Set angle parameters from json data (not json file!)
self.left_arm.set_from_json_data(json_data['joint_angle']['left_shoulder'], json_data['joint_angle']['left_elbow'])
self.right_arm.set_from_json_data(json_data['joint_angle']['right_shoulder'], json_data['joint_angle']['right_elbow'])
if self._lower_body_flag:
self.left_leg.set_from_json_data(json_data['joint_angle']['left_hip'], json_data['joint_angle']['left_knee'])
self.right_leg.set_from_json_data(json_data['joint_angle']['right_hip'], json_data['joint_angle']['right_knee'])
def is_collision_body_parts(self):
def collision_detection_generator():
# left arm vs. cranicm
yield self.left_arm.is_collision_to_sphere(self.head.calc_cranicm_point(), self.head.get_cranicm_radius())
# left arm vs. torso
yield self.left_arm.is_collision_to_capsule(self.torso.calc_torso_upper_point(), self.torso.calc_torso_lower_point(), self.torso.get_torso_radius())
# left arm vs. right upperarm
yield self.left_arm.is_collision_to_capsule(self.right_arm.calc_shoulder_point(), self.right_arm.calc_upperarm_lower_point(), self.right_arm.get_upperarm_radius())
# left arm vs. right elbow
yield self.left_arm.is_collision_to_sphere(self.right_arm.calc_elbow_point(), self.right_arm.get_elbow_radius())
# left arm vs. right forearm
yield self.left_arm.is_collision_to_capsule(self.right_arm.calc_forearm_upper_point(), self.right_arm.calc_hand_point(), self.right_arm.get_forearm_radius())
# right arm vs. cranicm
yield self.right_arm.is_collision_to_sphere(self.head.calc_cranicm_point(), self.head.get_cranicm_radius())
# right arm vs. torso
yield self.right_arm.is_collision_to_capsule(self.torso.calc_torso_upper_point(), self.torso.calc_torso_lower_point(), self.torso.get_torso_radius())
if self._lower_body_flag:
# left arm vs. left thigh
yield self.left_arm.is_collision_to_capsule(self.left_leg.calc_hip_point(), self.left_leg.calc_thigh_lower_point(), self.left_leg.get_thigh_radius())
# left arm vs. left knee
yield self.left_arm.is_collision_to_sphere(self.left_leg.calc_knee_point(), self.left_leg.get_knee_radius())
# left arm vs. left calf
yield self.left_arm.is_collision_to_capsule(self.left_leg.calc_calf_upper_point(), self.left_leg.calc_foot_point(), self.left_leg.get_calf_radius())
# left arm vs. right thigh
yield self.left_arm.is_collision_to_capsule(self.right_leg.calc_hip_point(), self.right_leg.calc_thigh_lower_point(), self.right_leg.get_thigh_radius())
# left arm vs. right knee
yield self.left_arm.is_collision_to_sphere(self.right_leg.calc_knee_point(), self.right_leg.get_knee_radius())
# left arm vs. right calf
yield self.left_arm.is_collision_to_capsule(self.right_leg.calc_calf_upper_point(), self.right_leg.calc_foot_point(), self.right_leg.get_calf_radius())
# right arm vs. left thigh
yield self.right_arm.is_collision_to_capsule(self.left_leg.calc_hip_point(), self.left_leg.calc_thigh_lower_point(), self.left_leg.get_thigh_radius())
# right arm vs. left thigh
yield self.right_arm.is_collision_to_capsule(self.left_leg.calc_hip_point(), self.left_leg.calc_thigh_lower_point(), self.left_leg.get_thigh_radius())
# right arm vs. left knee
yield self.right_arm.is_collision_to_sphere(self.left_leg.calc_knee_point(), self.left_leg.get_knee_radius())
# right arm vs. left calf
yield self.right_arm.is_collision_to_capsule(self.left_leg.calc_calf_upper_point(), self.left_leg.calc_foot_point(), self.left_leg.get_calf_radius())
# right arm vs. right thigh
yield self.right_arm.is_collision_to_capsule(self.right_leg.calc_hip_point(), self.right_leg.calc_thigh_lower_point(), self.right_leg.get_thigh_radius())
# right arm vs. right knee
yield self.right_arm.is_collision_to_sphere(self.right_leg.calc_knee_point(), self.right_leg.get_knee_radius())
# right arm vs. right calf
yield self.right_arm.is_collision_to_capsule(self.right_leg.calc_calf_upper_point(), self.right_leg.calc_foot_point(), self.right_leg.get_calf_radius())
# left leg vs. cranicm
yield self.left_leg.is_collision_to_sphere(self.head.calc_cranicm_point(), self.head.get_cranicm_radius())
# left leg vs. right thigh
yield self.left_leg.is_collision_to_capsule(self.right_leg.calc_hip_point(), self.right_leg.calc_thigh_lower_point(), self.right_leg.get_thigh_radius())
# left leg vs. right knee
yield self.left_leg.is_collision_to_sphere(self.right_leg.calc_knee_point(), self.right_leg.get_knee_radius())
# left leg vs. right calf
yield self.left_leg.is_collision_to_capsule(self.right_leg.calc_calf_upper_point(), self.right_leg.calc_foot_point(), self.right_leg.get_calf_radius())
# right leg vs. cranicm
yield self.right_leg.is_collision_to_sphere(self.head.calc_cranicm_point(), self.head.get_cranicm_radius())
# torso vs. left knee
yield self.torso.is_collision_to_sphere(self.left_leg.calc_knee_point(), self.left_leg.get_knee_radius())
# torso vs. left calf
yield self.torso.is_collision_to_capsule(self.left_leg.calc_calf_upper_point(), self.left_leg.calc_foot_point(), self.left_leg.get_calf_radius())
# torso vs. right knee
yield self.torso.is_collision_to_sphere(self.right_leg.calc_knee_point(), self.right_leg.get_knee_radius())
# torso vs. right calf
yield self.torso.is_collision_to_capsule(self.right_leg.calc_calf_upper_point(), self.right_leg.calc_foot_point(), self.right_leg.get_calf_radius())
for detection in collision_detection_generator():
if detection:
return True
return False
import sys
sys.path.append("..");
from arm import Arm;
if __name__=="__main__":
arm = Arm();
angles = {1:95, 2:65};
arm.setAngle(angles);
del arm
def init_arms(self, cluster_positions, cluster_fitnesses):
# Default matrix that translate and shrink search space to [0,1]^D
matrices = [
Matrix(positions, fitnesses, self.min_bounds,
self.max_bounds) for positions, fitnesses in zip(
cluster_positions, cluster_fitnesses)
]
if self.plot > 0:
draw_optimization(function_id - 1,
cluster_positions,
matrices,
fig_name='initial.png',
**self.fig_config)
# Random sample n_samples to represent search space for non-overlapped optimization
trans_samples = np.random.uniform(0,
1,
size=(self.n_samples,
self.dimension))
k = len(cluster_positions)
for i in range(k):
exclude = []
for j in range(k):
if i != j:
samples = matrices[j].inverse_transform(trans_samples)
exclude.extend(samples)
exclude = np.array(exclude)
if self.verbose: print('optimizing matrix for arm %d' % i)
arm = Arm(self.obj,
self.n_points,
cluster_positions[i],
cluster_fitnesses[i],
algo_type=self.algo_type,
exclude=exclude,
min_bounds=self.min_bounds,
max_bounds=self.max_bounds)
self.arms.append(arm)
matrices[i] = self.arms[i].matrix
if self.plot > 0:
opt_points = []
for j in range(k):
if i == j:
opt_points.append(cluster_positions[i])
else:
samples = matrices[j].inverse_transform(trans_samples)
opt_points.append(samples)
draw_optimization(function_id - 1,
opt_points,
matrices,
fig_name='initial_optimize_%d.png' % i,
**self.fig_config)
if self.plot > 0:
opt_points = [arm.get_positions() for arm in self.arms]
opt_matrices = [arm.matrix for arm in self.arms]
draw_optimization(function_id - 1,
opt_points,
opt_matrices,
fig_name='initial_optimized.png',
**self.fig_config)
from arm import Arm
import rpyc
from rpyc.utils.server import ThreadedServer
ARM = Arm() # thread-safe instance
class ArmService(rpyc.Service):
def _rpyc_getattr(self, name):
# expose everything of the ARM instance
return getattr(ARM, name)
def main():
ARM.open()
RPC_PORT = 18861
t = ThreadedServer(ArmService, port=RPC_PORT)
t.start()
if __name__ == '__main__':
main()
def train_network_scenario1(prototypes1, prototypes2, origin, train_data='train_data.p', val_data='validation_data.p'):
"""
Combines the networks for the arm and the eye. Arm is dominant over the eye, as in scenario 2, so the eye recieves its input from the arm and its target.
Also plots a lot of information about loss and accuracy.
Saves the weights of the network
Input: prototypes for the arm, prototypes for the eye, point of origin of both models, trainingdata for the arm, validationdata for the eye
Output: -
"""
epochs = 150 # number of epochs
print 'network1'
network1, train_fn1, val_fn1 = create_network(prototypes1)
print 'network2'
network2, train_fn2, val_fn2 = create_network(prototypes2, n_inputs=4)
print 'Networks done'
eyes = Eyes(origin=origin, visualize=False)
arm = Arm(origin=origin, visualize=False)
print 'moare stuff'
# Arrays for saving performance after each epoch
means_arm = np.zeros(epochs)
stds_arm = np.zeros(epochs)
train_losses_arm = np.zeros(epochs)
val_losses_arm = np.zeros(epochs)
means_eye = np.zeros(epochs)
stds_eye = np.zeros(epochs)
train_losses_eye = np.zeros(epochs)
val_losses_eye = np.zeros(epochs)
dists_eye = np.zeros(epochs)
dists_arm = np.zeros(epochs)
print "Train network"
for e in tqdm(range(epochs)):
total_mean_arm = 0
total_std_arm = 0
total_mean_eye = 0
total_std_eye = 0
total_error_arm = 0
total_error_eye = 0
train_loss_arm = 0
val_loss_arm = 0
train_loss_eye = 0
val_loss_eye = 0
# training epoch
i = 0
for input_batch, output_batch in iterate_data(data_file=train_data):
pred1, train_loss1 = train_fn1(input_batch, output_batch)
arm_angles = np.array([arm.calculate_angles(x, y) for [x, y] in input_batch], dtype='float32') # same targets as arm
eye_positions = [calc_intersect(left, right) for [left, right] in pred1] # get x,y from predicted eye angels
arm_input = np.hstack((input_batch, eye_positions)).astype('float32') # first the eye coordinates, take care when combining prototypes
pred2, train_loss2 = train_fn2(arm_input, arm_angles)
train_loss_arm += train_loss2
train_loss_eye += train_loss1
i += 1
# Take average loss of this epoch
train_loss_arm = train_loss_arm / i
train_loss_eye = train_loss_eye / i
n = 0
# Validation Epoch
for inp_val, out_val in iterate_data(data_file=val_data):
predictions_eye, loss_eye = val_fn1(inp_val, out_val)
dist_eye, mean_eye, std_eye = evaluate(predictions_eye, out_val) # dist_arm is for debugging
arm_angles = np.array([arm.calculate_angles(x, y) for [x, y] in inp_val], dtype='float32')
eye_positions = [calc_intersect(left, right) for [left, right] in predictions_eye]
arm_input = np.hstack((inp_val, eye_positions)).astype('float32')
prediction_arm, loss_arm = val_fn2(arm_input, arm_angles)
dist_arm, mean_arm, std_arm = evaluate(prediction_arm, inp_val)
arm_positions = np.array([arm.move_arm(shoulder, shoulder) for [shoulder, elbow] in prediction_arm])
arm_error_dist, mean_arm_error, std_arm_error = evaluate(arm_positions, inp_val)
eye_error_dist, mean_eye_error, std_eye_error = evaluate(eye_positions, inp_val)
total_error_arm += mean_arm_error
total_error_eye += mean_eye_error
n += 1
total_mean_arm += mean_arm
total_std_arm += std_arm
total_mean_eye += mean_eye
total_std_eye += std_eye
val_loss_arm += loss_arm
val_loss_eye += loss_eye
# Save epoch data
means_arm[e] = total_mean_arm / n
stds_arm[e] = total_std_arm / n
train_losses_arm[e] = train_loss_arm
val_losses_arm[e] = val_loss_arm / n
means_eye[e] = total_mean_eye / n
stds_eye[e] = total_std_eye / n
train_losses_eye[e] = train_loss_eye
val_losses_eye[e] = val_loss_eye / n
dists_eye[e] = total_error_eye / n
dists_arm[e] = total_error_arm / n
# Plots
# Plot mean and std
plt.figure()
meanplot_arm, = plt.plot(means_arm, label='mean arm')
stdplot_arm, = plt.plot(stds_arm, label='std arm')
meanplot_eye, = plt.plot(means_eye, label='mean eye')
stdplot_eye, = plt.plot(stds_eye, label='std eye')
plt.legend(handles=[meanplot_arm, stdplot_arm, meanplot_eye, stdplot_eye])
plt.savefig('../images/scenario1/accuracy_combined.png')
plt.show()
# Plot just the means
plt.figure()
meanplot_arm, = plt.plot(means_arm, label='mean arm')
meanplot_eye, = plt.plot(means_eye, label='mean eye')
plt.legend(handles=[meanplot_arm, meanplot_eye])
plt.savefig('../images/scenario1/accuracy_combined_arm.png')
# Ploot the train and validations losses
plt.figure()
trainplot_arm, = plt.plot(train_losses_arm, label='train loss arm')
valplot_arm, = plt.plot(val_losses_arm, label='val loss arm')
trainplot_eye, = plt.plot(train_losses_eye, label='train loss eye')
valplot_eye, = plt.plot(val_losses_eye, label='val loss eye')
plt.legend(handles=[trainplot_arm, valplot_arm, trainplot_eye, valplot_eye])
plt.savefig('../images/scenario1/loss_combined.png')
plt.show()
# Plot distance errors
plt.figure()
distsplot_arm, = plt.plot(dists_arm, label='Distance Error arm')
plt.legend(handles=[distsplot_arm])
plt.savefig('../images/scenario1/distance_error_arm.png')
plt.show()
np.save('../images/scenario1/distance_arm', dists_arm)
# Plot Distance error ot the eye
plt.figure()
distsplot_eye, = plt.plot(dists_eye, label='Distance Error eye')
plt.legend(handles=[distsplot_eye])
plt.savefig('../images/scenario1/distance_error_eye.png')
plt.show()
np.save('../images/scenario1/distance_eye', dists_eye)
# Save the weights
np.save('network_arm_s1', layers.get_all_param_values(network1))
np.save('network_eye_s1', layers.get_all_param_values(network2))
return # network, predictions
#eyes.attended_points = np.array()
if __name__ == '__main__':
"""
Create arm model
Create arm prototypes
Create dataset
Create eye model
Create eye prototypes
Combine prototypes (for the eye network)
Train the network
"""
origin = 0 #make sure the eye and the arm have the same origin!
arm = Arm(visualize = False, origin = origin)
print 'Create arm proto'
proto_arm = arm.create_prototypes(shape=(10, 10))
eyes = Eyes(visualize = False, origin = origin)
# eyes.create_dataset(n_datapoints=100000, train_file='train_data_eyes.p', val_file='validation_data_eyes.p', test_file= 'test_data_eyes.p')
print 'create eye proto'
proto_eye = eyes.create_prototypes(shape = (10, 10))
print 'combine prototypes'
proto_arm = combine_prototypes(proto_arm, proto_eye)
print 'To the network!'
train_network_scenario1(proto_eye, proto_arm, origin = origin, train_data = 'train_data_eyes.p', val_data = 'validation_data_eyes.p' )
"""
origin = 0
arm = Arm(visualize= False, origin = origin)
class DiffDriveManipulatorRobot(object):
"""
DiffDrive main robot class. Launching a few threads to handle mechanics
and odometry.
"""
__slots__ = [
'_urgentStop',
'timeStep',
'logger',
'_current_V',
'baseMovementLock',
'armMovementLock',
'powerSupply',
'eventLoop',
'eventLoopExecutors',
'eventLoopArmExecutors',
'eventLoopMainExecutors',
'eventLoopBaseExecutors',
'sensors',
'gyroSensorConfiguration',
'robotConfiguration',
'arm',
'base',
'mqtt_topics',
'mqtt_connect_mid',
'threadOdometry',
'eventLoopThread',
'threadMQTT',
'mqttMoveQueue',
'mqttClient',
'exceptionQueue',
'asyncVoltageCheckLoopTask',
'asyncOdometryReportingTask',
'M0e',
'Toe_grip',
'Tbc',
'Tcb',
'asyncImplMoveLoopTask'
]
def __init__(self,
robot_config_file,
initRobotConfig,
timeStep,
odometryTimer=0.1,
defaultLogging=30):
"""
Creates the main bot class.
"""
if not os.path.exists(robot_config_file):
raise ValueError(f'Cannot find configuration file '
f'"{robot_config_file}"')
with open(robot_config_file, 'r') as f:
self.robotConfiguration = json.load(f)
# Adjust JSON just read
self.__adjustConfigDict(self.robotConfiguration)
# Exception Queue for inter threads exception communications
self.exceptionQueue = queue.Queue()
self.mqttMoveQueue = queue.Queue()
self.baseMovementLock = asyncio.locks.Lock()
self.timeStep = timeStep
self.powerSupply = PowerSupply()
# region sensor Init
self.sensors = {}
for sensor in self.robotConfiguration["sensors"]:
sensorDict = self.robotConfiguration["sensors"][sensor]
legoPort = LegoPort(sensorDict["input"])
# Applies to both I2C and SDK controled sensors
if legoPort.mode != sensorDict["mode"]:
legoPort.mode = sensorDict["mode"]
# Only applies to sensors controled by the python SDK
if "set_device" in sensorDict:
legoPort.set_device = sensorDict["set_device"]
time.sleep(1)
address = sensorDict["input"]
self.sensors[sensor] = sensorDict["sensorType"](address=address)
# endregion
# base Init
self.base = DiffDriveBase(self.robotConfiguration["base"],
initRobotConfig, odometryTimer)
# arm/endeffector Init
self.arm = Arm(armLinkDefinitions=self.robotConfiguration["arm"])
# Other matrix init
self.M0e = mr.RpToTrans(
R=np.array(
self.robotConfiguration["arm"]["arm1"]["manipulatorConfiguration"]["M0e"]["rotationMatrix"]).T, # noqa F501
p=self.robotConfiguration["arm"]["arm1"]["manipulatorConfiguration"]["M0e"]["p"]) # noqa F501
self.Toe_grip = mr.RpToTrans(
R=np.array(
self.robotConfiguration["arm"]["arm1"]["manipulatorConfiguration"]["Toe_grip"]["rotationMatrix"]).T, # noqa F501
p=self.robotConfiguration["arm"]["arm1"]["manipulatorConfiguration"]["Toe_grip"]["p"]) # noqa F501
self.Tbc = mr.RpToTrans(
R=np.array(
self.robotConfiguration["cameraConfiguration"]["Tbc"]["rotationMatrix"]).T, # noqa F501
p=self.robotConfiguration["cameraConfiguration"]["Tbc"]["p"])
self.Tcb = mr.TransInv(self.Tbc)
# Reset all motors at init
self.urgentStop = False
log.info(LOGGER_DDR_MAIN, 'Done main robot init.')
# log.setLevel(LOGGER_DDR_MAIN)
def __del__(self):
""" Destructor """
loggerName = 'ddrmain'
log.info(loggerName,
msg=f'Deleting robot ; opening gripper '
f'and shutting down motors.')
# Open the gripper
self.arm.openEndEffector()
# Shut down the motors
self.urgentStop = True
# Clear the event loop and shut it down
self.asyncVoltageCheckLoopTask.cancel()
self.asyncImplMoveLoopTask.cancel()
# Cancel running tasks in the event loop
if self.eventLoop.is_running():
self.eventLoop.stop()
# Stop threads
self.base.stopThreadOdometry()
# Will release motors that are on hold before shutting down
self.base.resetBodyConfiguration()
# Stop the mqtt client loop
self.mqttClient.loop_stop()
self.arm.killThread = True
self.arm.openEndEffector()
# Unload the drivers for sensors
for sensor in self.robotConfiguration["sensors"]:
LegoPort(
self.robotConfiguration["sensors"][sensor]['input']).mode = \
'none'
log.debug(loggerName, 'Done the __del__ function')
# Unload the sensor drivers
# region properties
@property
def currentVoltage(self):
return self.powerSupply.measured_volts
@property
def urgentStop(self):
return self._urgentStop
@urgentStop.setter
def urgentStop(self, value):
if value is True:
log.info(LOGGER_DDR_MAIN, "Need to immediately stop the bot.")
self.mqttMoveQueue = queue.Queue()
self._urgentStop = value
if self.base:
self.base.urgentStop = value
if self.arm:
self.arm.urgentStop = value
@property # should stay on main robot
def currentConfigVector(self):
return np.r_[self.base.currentConfigVector,
self.arm.currentConfigVector,
self.arm.endEffectorStatus]
# endregion
def run(self):
"""
run the main event loop for the robot and the required threads
"""
try:
# region Start Base Odometry Thread Init
self.base.run()
# endregion
# region MQTTConnect Thread Init
self.threadMQTT = threading.Thread(
target=self.threadImplMQTT,
name="threadImplMQTT")
self.threadMQTT.start()
# endregion
# Launch main event loop in a separate thread
self.eventLoopThread = threading.Thread(
target=self.threadImplEventLoop,
name="threadImplEventLoop")
self.eventLoopThread.start()
# Error handling for threads
while True:
try:
exc = self.exceptionQueue.get(block=True)
except queue.Empty:
pass
else:
log.error(LOGGER_DDR_RUN,
msg=f'Exception handled in one of the '
f'spawned process : {exc}')
raise exc
self.eventLoopThread.join(0.2)
if self.eventLoopThread.isAlive():
continue
else:
break
except SystemExit:
# raise the exception up the stack
raise
except:
log.error(LOGGER_DDR_RUN, f'Error : {traceback.print_exc()}')
def threadImplEventLoop(self):
"""
Main event asyncio eventloop launched in a thread
"""
try:
log.warning(LOGGER_DDR_THREADIMPLEVENTLOOP,
msg=f'Launching main event loop.')
self.eventLoop = asyncio.new_event_loop()
self.eventLoopArmExecutors = \
concurrent.futures.ThreadPoolExecutor(
max_workers=MAX_WORKER_THREADS,
thread_name_prefix='ArmThreadPool')
self.eventLoopMainExecutors = \
concurrent.futures.ThreadPoolExecutor(
max_workers=MAX_WORKER_THREADS,
thread_name_prefix='MainThreadPool')
self.eventLoopBaseExecutors = \
concurrent.futures.ThreadPoolExecutor(
max_workers=MAX_WORKER_THREADS,
thread_name_prefix='BaseThreadPool')
self.asyncImplMoveLoopTask = \
self.eventLoop.create_task(
self.asyncProcessMQTTMessages(0.25))
self.asyncVoltageCheckLoopTask = \
self.eventLoop.create_task(self.asyncVoltageCheckLoop(10))
self.asyncOdometryReportingTask = \
self.eventLoop.create_task(self.asyncOdometryReporting(1))
self.eventLoop.run_in_executor(
self.eventLoopMainExecutors,
self.asyncExecutorImplKillSwitch)
self.eventLoop.run_forever()
except:
log.error(LOGGER_DDR_THREADIMPLEVENTLOOP,
msg=f'Error : {traceback.print_exc()}')
finally:
self.eventLoop.stop()
self.eventLoop.close()
async def asyncProcessMQTTMessages(self, loopDelay):
"""
This function receives the messages from MQTT to move the robot.
It is implemented in another thread so that the killswitch can kill
the thread without knowing which specific
"""
while True:
try:
await asyncio.sleep(loopDelay)
if self.mqttMoveQueue.empty() is False:
try:
if self.currentVoltage < LOW_VOLTAGE_THRESHOLD:
raise LowVoltageException
except LowVoltageException:
log.warning(LOGGER_DDR_THREADIMPLMQTT,
msg=f'Low voltage threshold '
f'{float(LOW_VOLTAGE_THRESHOLD):.2} '
f'reached. Current voltage = '
f'{self.currentVoltage:.2f} .'
f'Robot will not move, but MQTT '
f'message will attempt to process. '
f'Consider charging the battery, '
f'or find some kind of power supply')
# Remove the first in the list, will pause until there
# is something
currentMQTTMoveMessage = self.mqttMoveQueue.get()
# Decode message received
msgdict = json.loads(
currentMQTTMoveMessage.payload.decode('utf-8'))
# Verify if need to urgently overried current movement
# and queue
if 'override' in msgdict:
if msgdict['override']:
log.debug(LOGGER_DDR_IMPLMOVELOOP,
msg=f'Overriding all motion - calling '
f'killswitch to empty queue')
# Urgently stop the current movement (and empty
# movement queue - done in killSwitch)
self.killSwitch()
# Default motion configuration
pid = False if 'pid' not in msgdict else msgdict["pid"]
velocity_factor = 0.1 if 'velocity_factor' not \
in msgdict else msgdict["velocity_factor"]
dryrun = False if 'dryrun' not in msgdict else \
msgdict["dryrun"]
inter_wheel_distance = None if 'inter_wheel_distance' not\
in msgdict else msgdict["inter_wheel_distance"]
wheel_radius = None if 'wheel_radius' not in msgdict \
else msgdict["wheel_radius"]
time_scaling_method = MOVEMENT_TIME_SCALING_METHOD_DEFAULT
if currentMQTTMoveMessage.topic == 'bot/killSwitch':
self.killSwitch()
elif currentMQTTMoveMessage.topic == \
'bot/base/reset/position':
log.warning(
LOGGER_DDR_IMPLMOVELOOP,
msg=f'Invoking reset body configuration')
self.base.resetBodyConfiguration()
elif currentMQTTMoveMessage.topic == \
'bot/base/move/xyphi':
self.eventLoop.run_in_executor(
self.eventLoopBaseExecutors,
functools.partial(self.base.moveBaseXYPhi,
endBaseConfig=(msgdict["x"],
msgdict["y"],
msgdict["phi"]),
pid=pid,
velocity_factor=velocity_factor,
dryrun=dryrun))
elif currentMQTTMoveMessage.topic == \
'bot/base/move/x':
self.eventLoop.run_in_executor(
self.eventLoopBaseExecutors,
functools.partial(self.base.moveBaseX,
distance=msgdict["distance"],
pid=pid,
velocity_factor=velocity_factor,
wheel_radius=wheel_radius,
dryrun=dryrun))
elif currentMQTTMoveMessage.topic == \
'bot/base/turn/phi':
self.eventLoop.run_in_executor(
self.eventLoopBaseExecutors,
functools.partial(
self.base.rotateBasePhi,
phi=msgdict["phi"],
pid=pid,
velocity_factor=velocity_factor,
inter_wheel_distance=inter_wheel_distance,
dryrun=dryrun))
elif currentMQTTMoveMessage.topic == \
'bot/base/move/home':
# Calculate where to go from there (not avoiding
# obstacles...)
_, (x, y, _) = mr.TransToRp(
mr.TransInv(self.base.Tsb))
# pi - self.base.currentPhi
phi = - 1 * self.base.currentPhi
self.base.moveBaseXYPhi(
endBaseConfig=(x, y, phi),
pid=pid,
velocity_factor=velocity_factor,
time_scaling_method=time_scaling_method,
dryrun=dryrun)
elif currentMQTTMoveMessage.topic == \
'bot/arm/move/rest':
self.eventLoop.run_in_executor(
self.eventLoopArmExecutors,
functools.partial(
self.arm.moveToAngle,
armConfigVector=self.arm.armRestPosition,
dryrun=dryrun))
elif currentMQTTMoveMessage.topic == \
'bot/arm/move/zero':
self.eventLoop.run_in_executor(
self.eventLoopArmExecutors,
functools.partial(
self.arm.moveToAngle,
armConfigVector=self.arm.armZeroPosition,
dryrun=dryrun))
elif currentMQTTMoveMessage.topic == \
'bot/arm/move/theta':
self.eventLoop.run_in_executor(
self.eventLoopArmExecutors,
functools.partial(
self.arm.moveToAngle,
armConfigVector=[msgdict["theta1"],
msgdict["theta2"]],
dryrun=dryrun))
elif currentMQTTMoveMessage.topic == \
'bot/gripper/open':
# Already an async call (non blocking)
self.arm.openEndEffector()
elif currentMQTTMoveMessage.topic == \
'bot/gripper/close':
# Already an async call (non blocking)
self.arm.closeEndEffector()
elif currentMQTTMoveMessage.topic == \
'bot/gripper/position':
# Call function to move this thing
Pco = np.array(msgdict["P"])
angle = msgdict["grip_angle"]
Toe_grip = mr.RpToTrans(
R=np.array([[cos(angle), 0, sin(angle)],
[0, 1, 0],
[-sin(angle), 0, cos(angle)]]),
p=[0, 0, 0])
Rco, _ = mr.TransToRp(Toe_grip) # Rco=camera-object
# Rco, _ = mr.TransToRp(self.Toe_grip)
Tco_desired = mr.RpToTrans(R=Rco, p=Pco)
Tbo_desired = self.Tbc @ Tco_desired
# Initial guess - based on the angle received
thetalist0 = (0, 0, pi / 8, pi / 4) # pi / 4, pi / 2)
log.debug(LOGGER_DDR_IMPLMOVELOOP,
msg=f'Running inverse kinematics to get '
f'required joint angles.')
thetalist, success = mr.IKinBody(
Blist=self.arm.bList,
M=self.M0e,
T=Tbo_desired,
thetalist0=thetalist0,
eomg=0.02, # 0.08 rad = 5 deg uncerainty
ev=0.025) # 4cm error?
if success:
thetalist = thetalist.round(6)
log.debug(LOGGER_DDR_IMPLMOVELOOP,
msg=f'IK Solved with joint thetas '
f'found {thetalist}')
# 0. Open gripper
log.debug(LOGGER_DDR_IMPLMOVELOOP,
msg=f'Opening or ensuring end effector '
f'is opened.')
self.arm.openEndEffector()
# 1. Move arm to 0 position - launch async.
log.debug(LOGGER_DDR_IMPLMOVELOOP,
msg=f'Moving arm to driving'
f'position')
self.arm.moveToAngle(
armConfigVector=self.arm.armDrivePosition,
dryrun=dryrun)
# 2. Move base by X first
log.debug(LOGGER_DDR_IMPLMOVELOOP,
msg=f'Moving body by X '
f'{thetalist[0]:.2f}')
self.base.moveBaseX(
distance=thetalist[0],
pid=pid,
velocity_factor=velocity_factor,
dryrun=dryrun)
# 3. Rotate base by Phi
log.debug(LOGGER_DDR_IMPLMOVELOOP,
msg=f'Rotating base by phi '
f'{thetalist[1]:.2f}')
self.base.rotateBasePhi(
phi=thetalist[1],
pid=pid,
velocity_factor=velocity_factor,
dryrun=dryrun)
# 4a. Move to standoff position
log.debug(LOGGER_DDR_IMPLMOVELOOP,
msg=f'Moving arm joints to standoff '
f'position')
# 4. Move other joint in position - need to block
log.debug(LOGGER_DDR_IMPLMOVELOOP,
msg=f'Moving arm joints into position '
f'for picking up object j3 = '
f'{degrees(thetalist[2]):.2f}, '
f'j4 = {degrees(thetalist[3]):.2f}')
self.arm.moveToAngle(armConfigVector=thetalist[2:],
dryrun=dryrun)
# 5. Grab object
log.debug(LOGGER_DDR_IMPLMOVELOOP,
msg=f'Closing end effector')
self.arm.closeEndEffector()
# Rest b/c the previous call is non blocking,
# and blocking causes issues on the wait
log.debug(LOGGER_DDR_IMPLMOVELOOP,
msg=f'Async Sleep for 1 second...')
await asyncio.sleep(1)
# 6. Set to driving position
log.debug(LOGGER_DDR_IMPLMOVELOOP,
msg=f'Moving arm to driving position.')
self.arm.moveToAngle(
armConfigVector=self.arm.armDrivePosition,
dryrun=dryrun)
log.debug(LOGGER_DDR_IMPLMOVELOOP, f'Done!')
else:
log.warning(LOGGER_DDR_IMPLMOVELOOP,
msg=f'Unable to calculate joint and '
f'wheel angles to achieve the '
f'required position.')
elif currentMQTTMoveMessage.topic == \
'bot/logger':
# Changing the logging level on the fly...
log.setLevel(msgdict['logger'], lvl=msgdict['level'])
elif currentMQTTMoveMessage.topic == \
'bot/logger/multiple':
# Changing the logging level on the fly for multiple loggers at a time
for logger, level in msgdict.items():
log.setLevel(logger, level)
else:
raise NotImplementedError
except NotImplementedError:
log.warning(LOGGER_DDR_IMPLMOVELOOP,
msg=f'MQTT message not implemented.')
except asyncio.futures.CancelledError:
log.warning(LOGGER_DDR_IMPLMOVELOOP,
msg=f'Cancelled the MQTT dequeing task.')
except:
log.error(LOGGER_DDR_IMPLMOVELOOP,
msg=f'Error : {traceback.print_exc()}')
async def asyncVoltageCheckLoop(self, loopDelay):
"""
run the voltage loop check
"""
while True:
try:
await asyncio.sleep(loopDelay)
if self.currentVoltage < LOW_VOLTAGE_THRESHOLD:
raise LowVoltageException
except asyncio.futures.CancelledError:
log.warning(LOGGER_DDR_VOLTAGECHECK,
msg=f'Cancelled the Voltage Check task.')
except LowVoltageException:
log.critical(LOGGER_DDR_VOLTAGECHECK,
msg=f'Voltage low on device '
f'({self.currentVoltage:.2f}V - '
f'threshold = {LOW_VOLTAGE_THRESHOLD:.2f}V)'
f' - consider charging battery or shutting '
f'down.')
async def asyncOdometryReporting(self, loopDelay):
"""
run the odometry loop and printout
"""
try:
while True:
await asyncio.sleep(loopDelay)
log.info(LOGGER_DDR_ODOMETRY,
msg=f'Joint theta (degrees) = {self.arm}')
log.info(LOGGER_DDR_ODOMETRY,
msg=f'Latest Vb6 = {self.base.Vb6} ; '
f'Phi(degrees) = '
f'{degrees(self.base.currentPhi):.2f}')
log.info(LOGGER_DDR_ODOMETRY,
msg=f'Latest Base SE3 (self.base.Tsb) = \n'
f'{self.base.Tsb}')
# Don't read actual position, there's a race condition
left = self.base.oldLeftPhi
right = self.base.oldRightPhi
log.debug(LOGGER_DDR_ODOMETRY,
msg=f'Left/Right Position:{left:.2f}/{right:.2f}')
theoretical_dist = radians((left + right) / 2) * \
self.base.wheelRadius
log.debug(LOGGER_DDR_ODOMETRY,
msg=f'Theoretical distance:{theoretical_dist:.4f}')
except asyncio.futures.CancelledError:
log.warning(LOGGER_DDR_ODOMETRY,
msg=f'Cancelled the Voltage Check task.')
def asyncExecutorImplKillSwitch(self):
""" Detects click and double clicks"""
try:
button = self.sensors["killSwitch"]
log.info(LOGGER_DDR_EXECUTORKILLSWITCH,
msg=f'Initialized async process for kill switch.')
while True:
firstClick = time.time()
button.wait_for_bump()
secondClick = time.time()
if secondClick - firstClick < DOUBLECLICK:
log.warning(LOGGER_DDR_EXECUTORKILLSWITCH,
msg=f'DoubleClick caught.')
self.exceptionQueue.put(SystemExit)
# Return is there to end this thread and return it
# to the pool (run_in_executor).
return
else:
self.killSwitch()
log.warning(LOGGER_DDR_EXECUTORKILLSWITCH,
msg=f'Button pressed. Urgent Stop switch '
f'flipped. Threads using motors should '
f'stop.')
except:
log.warning(LOGGER_DDR_EXECUTORKILLSWITCH,
msg=f'Error in the kill switch routing : '
f'{traceback.print_exc()}')
def __adjustConfigDict(self, confDict):
'''
adjustConfigDict :
param:confDict:parameters read in the config file for the robot
param:confDict:type:parameter dictionary
returns : modified confDict
'''
for key, value in confDict.items():
if not isinstance(value, dict):
if not isinstance(value, list):
if value in LEGO_ROBOT_DEFINITIONS:
confDict[key] = LEGO_ROBOT_DEFINITIONS[value]
else:
confDict[key] = self.__adjustConfigDict(confDict[key])
return confDict
def killSwitch(self):
# Empty movement queue
try:
log.warning(LOGGER_DDR_KILLSWITCH, 'Killing all motor motion.')
self.urgentStop = True
time.sleep(1)
self.urgentStop = False
except:
pass
def threadImplMQTT(self):
"""
MQTT Thread launching the loop and subscripbing to the right topics
"""
mqtt_default_qos = 2
self.mqtt_topics = [
(topic, mqtt_default_qos) for topic in
self.robotConfiguration["mqtt"]["subscribedTopics"]]
def on_connect(client, userdata, flags, rc):
log.warning(LOGGER_DDR_THREADIMPLMQTT,
msg=f'Connected to MQTT broker. '
f'Result code {rc}')
mqtt_connect_result, self.mqtt_connect_mid = \
client.subscribe(self.mqtt_topics)
if mqtt_connect_result == mqtt.MQTT_ERR_SUCCESS:
log.warning(LOGGER_DDR_THREADIMPLMQTT,
msg=f'Successfully subscribed '
f'to {self.mqtt_topics}')
else:
log.error(LOGGER_DDR_THREADIMPLMQTT,
msg=f'MQTT Broker subscription problem.')
def on_message(client, userdata, message):
""" callback function used for the mqtt client (called when
a new message is publisehd to one of the queues we subscribe to)
"""
log.info(LOGGER_DDR_THREADIMPLMQTT,
msg=f'Received MID {message.mid} : '
f'{str(message.payload)} on topic '
f'{message.topic} with QoS {message.qos}')
self.mqttMoveQueue.put_nowait(message)
def on_disconnect(client, userdata, rc=0):
"""callback for handling disconnects
"""
log.info(LOGGER_DDR_THREADIMPLMQTT,
msg=f'Disconnected MQTT result code = {rc}.'
f'Should automatically re-connect to broker')
def on_subscribe(client, userdata, mid, granted_qos):
if mid == self.mqtt_connect_mid:
log.warning(LOGGER_DDR_THREADIMPLMQTT,
msg=f'Subscribed to topics. Granted QOS '
f'= {granted_qos}')
else:
log.error(LOGGER_DDR_THREADIMPLMQTT,
msg=f'Strange... MID doesn\'t match '
f'self.mqtt_connect_mid')
self.mqttClient = mqtt.Client(
client_id="mybot",
clean_session=True,
transport=self.robotConfiguration["mqtt"]["brokerProto"])
self.mqttClient.enable_logger(
logger=RoboLogger.getSpecificLogger(LOGGER_DDR_THREADIMPLMQTT))
self.mqttClient.on_subscribe = on_subscribe
self.mqttClient.on_connect = on_connect
self.mqttClient.on_disconnect = on_disconnect
self.mqttClient.on_message = on_message
self.mqttClient.connect(
host=self.robotConfiguration["mqtt"]["brokerIP"],
port=self.robotConfiguration["mqtt"]["brokerPort"])
self.mqttClient.loop_start()
def train_network(prototypes, train_data = 'train_data.p', val_data = 'validation_data.p'):
"""
Legacy Code, doesnt work anymore
Trains a single network (eithers arm of eye model)
Also plots some of information about loss and accuracy.
Input: prototypes of the model, train data, validation data
Output: -
"""
network, train_fn, val_fn = create_network(prototypes)
epochs = 150
means = np.zeros(epochs)
stds = np.zeros(epochs)
train_losses = np.zeros(epochs)
val_losses = np.zeros(epochs)
dists = np.zeros(epochs)
arm = Arm(origin=0, visualize=False)
print "Train network"
for e in tqdm(range(epochs)):
#Train epoch
for input_batch, output_batch in iterate_data(data_file = train_data):
pred, train_loss = train_fn(input_batch, output_batch)
total_mean = 0
total_std = 0
total_dist = 0
n = 0
for inp_val, out_val in iterate_data(data_file = val_data):
#validation epoch
predictions, loss = val_fn(inp_val, out_val)
dist, mean, std = evaluate(predictions, out_val)
arm_positions = np.array([arm.move_arm(shoudler, elbow) for [shoudler, elbow] in predictions])
eye_error_dist, mean_eye_error, std_eye_error = evaluate(arm_positions, inp_val)
n += 1
total_mean += mean
total_std += std
total_dist += mean_eye_error
means[e] = total_mean/n
stds[e] = total_std/n
train_losses[e] = train_loss
val_losses[e] = loss
dists[e] = total_dist/n
np.save('network_epoch' + str(e), layers.get_all_param_values(network))
#Plots
plt.figure()
distplot, = plt.plot(dists, label = 'arm distance error')
plt.legend(handles = [distplot])
plt.savefig('../images/arm_error.png')
plt.show()
plt.figure()
meanplot, = plt.plot(means, label = 'mean')
stdplot, = plt.plot(stds, label = 'std')
plt.legend(handles = [meanplot, stdplot])
plt.savefig('../images/arm_angles.png')
plt.show()
plt.figure()
trainplot, = plt.plot(train_losses, label = 'train loss')
valplot, = plt.plot(val_losses, label = 'val loss')
plt.legend(handles = [trainplot, valplot])
plt.savefig('../images/arm_losses.png')
plt.show()
print "saving network"
np.save('network_arm', layers.get_all_param_values(network))
print "done saving"
return network, predictions
def main(argv):
rclpy.init(args=argv)
arm = Arm()
arm.move_to(+0.2000, +0.0000, +0.2400, accelerate=2.0)
arm.move_joints(joint4=+0.7000, path_time=1.0)
time.sleep(2.0)
arm.move_to(+0.2800, +0.0000, +0.2400, accelerate=0.3)
time.sleep(2.0)
arm.open()
arm.close()
arm.open()
arm.close()
time.sleep(3.0)
arm.move_to(+0.1796, +0.0000, +0.1500, accelerate=0.3)
time.sleep(3.0)
arm.open()
arm.close()
time.sleep(3.0)
# return to neutral
arm.move_to(+0.1360, +0.0000, +0.2324, accelerate=0.5)