def startupSeparateOne(self):
speed = random.random() * 45 + 5
numObjects = 50
kinematics = []
# Create the kinematics
for i in range(numObjects):
if i == 0:
targetColor = (0, 1, 0, 1)
else:
targetColor = (1, 1, 1, 1)
pandaObject = loadObject("ship.png", 2 * AVATAR_RAD, targetColor)
kinematic = Kinematic(Point2(0, 0), 0, speed, pandaObject,
WINDOW_SZ)
kinematics.append(kinematic)
self.steeringObjects = []
# Create the steering objects
for i in range(len(kinematics)):
thisKinematic = kinematics[i]
otherKinematics = kinematics[0:i] + kinematics[i + 1:]
if i == 0:
steering = SteeringSeparate(thisKinematic, otherKinematics,
100)
else:
steering = SteeringLinear(thisKinematic)
self.steeringObjects.append(
PlayerAndMovement(thisKinematic, steering))
def startupPerformMany(self):
speed = random.random() * 45 + 5
colors = [(1, 0, 0, 1), (0, 1, 0, 1), (0, 0, 1, 1), (1, 1, 0, 1),
(0, 1, 1, 1), (1, 0, 1, 1)]
numObjects = 50
kinematics = []
# Create the kinematics
for i in range(numObjects):
targetColor = colors[i % len(colors)]
pandaObject = loadObject("ship.png", 2 * AVATAR_RAD, targetColor)
kinematic = Kinematic(Point2(0, 0), 0, speed, pandaObject,
WINDOW_SZ)
kinematics.append(kinematic)
self.steeringObjects = []
# Create the steering objects
for i in range(len(kinematics)):
thisKinematic = kinematics[i]
otherKinematics = kinematics[0:i] + kinematics[i + 1:]
separate = SteeringSeparate(thisKinematic, otherKinematics, 100)
matchVelocity = SteeringMatchVelocity(thisKinematic,
otherKinematics)
self.steeringObjects.append(
PlayerAndMovement(thisKinematic, separate))
def __init__(self):
self.display = None
print("Booting SpotMicroAI")
self.display = RobotDisplay()
self.gyro = Gyro()
# self.servos=Servos()
self.Kinematic = Kinematic()
def __init__(self,config, ipAddress=None):
self.joints = 6
self.logging = True
if ipAddress == None:
self.ipAddress = config["ipAddress"]
else:
self.ipAddress = ipAddress
self.port = config["port"]
self.baseURL = "http://"+str(self.ipAddress)+":"+str(self.port)+"/"
self.error = False
self.timeout = 2 #Seconds
self.pollingStatus = False
#Values loaded from 'config.json'
for joint in config["joints"]:
self.jointMaxRotation.append(joint["maxRotation"])
self.jointMaxSpeed.append(joint["maxSpeed"])
self.jointMinSpeed.append(joint["minSpeed"])
self.jointPosDefault.append(joint["defaultPosition"])
self.jointSpeedDefault.append(joint["defaultSpeed"])
self.jointAccelDefault.append(joint["defaultAccel"])
#Status Flags
self.jointPosition = [None]*self.joints
self.switchState = [0]*self.joints
self.calibrationState = [0]*self.joints
self.movementFlag = [0]*self.joints
try:
self.session = requests.session()
self.clearLogs()
self.connected = True
except:
self.log("ERROR: Cannot create a session.")
self.connected = False
#Open a solver for kinematics
self.kin = Kinematic()
#Start capturing status packets
self.getStatus()
def __init__(self):
self.display = None
print("Booting SpotMicroAI")
# self.display=RobotDisplay()
self.gyro = Gyro()
self.servos = Servos()
self.Kinematic = Kinematic()
self.network = Network()
# For testing purposed
self.mode = ModeStandby(self.servos)
ControlVal = 0#set initial state
#=================================================
#define callback function to extract control actions
def controlCallBack(xboxControlId, value):
global ControlId, ControlVal
ControlId = xboxControlId
ControlVal = value
#setup xbox controller, set out the deadzone and scale, also invert the Y Axis (for some reason in Pygame negative is up - wierd!
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
arm = Arm(config)
#reconfigure for example
arm.resetEStop()
arm.calibrateArm()
while((not arm.armCalibrated()):
def __init__(self, useFixedBase=False, useStairs=True):
# Simulation Configuration
self.useMaximalCoordinates = False
self.useRealTime = True
self.debugLidar = False
self.debug = False
self.fixedTimeStep = 1. / 500
self.numSolverIterations = 200
self.useFixedBase = useFixedBase
self.useStairs = useStairs
self.init_oritentation = p.getQuaternionFromEuler([0, 0, 90.0])
self.init_position = [0, 0, 0.3]
self.reflection = False
self.state = RobotState.OFF
# Parameters for Servos - still wrong
self.kp = 0.045 #0.012
self.kd = .4 #.2
self.maxForce = 12.5
self.physId = p.connect(p.SHARED_MEMORY)
if (self.physId < 0):
p.connect(p.GUI)
self.angle = 90
self.oldTextId = 0
self.textId = 0
self.oldDebugInfo = []
self.rot = (0, 0, 0)
self.pos = (0, 0, 0)
self.t = 0
if self.reflection:
p.configureDebugVisualizer(p.COV_ENABLE_PLANAR_reflection, 1)
p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER, 1)
self.IDkp = p.addUserDebugParameter("Kp", 0, 0.05, self.kp) # 0.05
self.IDkd = p.addUserDebugParameter("Kd", 0, 1, self.kd) # 0.5
self.IDmaxForce = p.addUserDebugParameter("MaxForce", 0, 50, 12.5)
p.setRealTimeSimulation(self.useRealTime)
self.quadruped = self.loadModels()
self.changeDynamics(self.quadruped)
self.jointNameToId = self.getJointNames(self.quadruped)
replaceLines = True
self.numRays = 360
self.rayFrom = []
self.rayTo = []
self.rayIds = []
self.rayHitColor = [1, 0, 0]
self.rayMissColor = [0, 1, 0]
rayLen = 12
rayStartLen = 0.12
for i in range(self.numRays):
#rayFrom.append([0,0,0])
h = 0.045
self.rayFrom.append([
rayStartLen * math.sin(math.pi * 2 * float(i) / self.numRays),
rayStartLen * math.cos(math.pi * 2 * float(i) / self.numRays),
h
])
self.rayTo.append([
rayLen * math.sin(math.pi * 2 * float(i) / self.numRays),
rayLen * math.cos(math.pi * 2 * float(i) / self.numRays), h
])
if self.debugLidar:
if (replaceLines):
self.rayIds.append(
p.addUserDebugLine(
self.rayFrom[i],
self.rayTo[i],
self.rayMissColor,
parentObjectUniqueId=self.quadruped,
parentLinkIndex=self.jointNameToId["base_lidar"]))
else:
self.rayIds.append(-1)
self.L = 140
self.W = 75 + 5 + 40
self.dirs = [[-1, 1, 1], [1, 1, 1], [-1, 1, 1], [1, 1, 1]]
self.roll = 0
self.Lp = np.array([[120, -100, self.W / 2, 1],
[120, -100, -self.W / 2, 1],
[-50, -100, self.W / 2, 1],
[-50, -100, -self.W / 2, 1]])
self.kin = Kinematic()
p.setRealTimeSimulation(self.useRealTime)
self.ref_time = time.time()
p.getCameraImage(320, 200) #160,100)
p.resetDebugVisualizerCamera(1, 85.6, 0, [-0.61, 0.12, 0.25])
# Camera Settings
fov, aspect, nearplane, farplane = 90, 1.3, .0111, 100
self.projection_matrix = p.computeProjectionMatrixFOV(
fov, aspect, nearplane, farplane)
self.lastLidarTime = 0
class Robot:
def __init__(self, useFixedBase=False, useStairs=True):
# Simulation Configuration
self.useMaximalCoordinates = False
self.useRealTime = True
self.debugLidar = False
self.debug = False
self.fixedTimeStep = 1. / 500
self.numSolverIterations = 200
self.useFixedBase = useFixedBase
self.useStairs = useStairs
self.init_oritentation = p.getQuaternionFromEuler([0, 0, 90.0])
self.init_position = [0, 0, 0.3]
self.reflection = False
self.state = RobotState.OFF
# Parameters for Servos - still wrong
self.kp = 0.045 #0.012
self.kd = .4 #.2
self.maxForce = 12.5
self.physId = p.connect(p.SHARED_MEMORY)
if (self.physId < 0):
p.connect(p.GUI)
self.angle = 90
self.oldTextId = 0
self.textId = 0
self.oldDebugInfo = []
self.rot = (0, 0, 0)
self.pos = (0, 0, 0)
self.t = 0
if self.reflection:
p.configureDebugVisualizer(p.COV_ENABLE_PLANAR_reflection, 1)
p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER, 1)
self.IDkp = p.addUserDebugParameter("Kp", 0, 0.05, self.kp) # 0.05
self.IDkd = p.addUserDebugParameter("Kd", 0, 1, self.kd) # 0.5
self.IDmaxForce = p.addUserDebugParameter("MaxForce", 0, 50, 12.5)
p.setRealTimeSimulation(self.useRealTime)
self.quadruped = self.loadModels()
self.changeDynamics(self.quadruped)
self.jointNameToId = self.getJointNames(self.quadruped)
replaceLines = True
self.numRays = 360
self.rayFrom = []
self.rayTo = []
self.rayIds = []
self.rayHitColor = [1, 0, 0]
self.rayMissColor = [0, 1, 0]
rayLen = 12
rayStartLen = 0.12
for i in range(self.numRays):
#rayFrom.append([0,0,0])
h = 0.045
self.rayFrom.append([
rayStartLen * math.sin(math.pi * 2 * float(i) / self.numRays),
rayStartLen * math.cos(math.pi * 2 * float(i) / self.numRays),
h
])
self.rayTo.append([
rayLen * math.sin(math.pi * 2 * float(i) / self.numRays),
rayLen * math.cos(math.pi * 2 * float(i) / self.numRays), h
])
if self.debugLidar:
if (replaceLines):
self.rayIds.append(
p.addUserDebugLine(
self.rayFrom[i],
self.rayTo[i],
self.rayMissColor,
parentObjectUniqueId=self.quadruped,
parentLinkIndex=self.jointNameToId["base_lidar"]))
else:
self.rayIds.append(-1)
self.L = 140
self.W = 75 + 5 + 40
self.dirs = [[-1, 1, 1], [1, 1, 1], [-1, 1, 1], [1, 1, 1]]
self.roll = 0
self.Lp = np.array([[120, -100, self.W / 2, 1],
[120, -100, -self.W / 2, 1],
[-50, -100, self.W / 2, 1],
[-50, -100, -self.W / 2, 1]])
self.kin = Kinematic()
p.setRealTimeSimulation(self.useRealTime)
self.ref_time = time.time()
p.getCameraImage(320, 200) #160,100)
p.resetDebugVisualizerCamera(1, 85.6, 0, [-0.61, 0.12, 0.25])
# Camera Settings
fov, aspect, nearplane, farplane = 90, 1.3, .0111, 100
self.projection_matrix = p.computeProjectionMatrixFOV(
fov, aspect, nearplane, farplane)
self.lastLidarTime = 0
def loadModels(self):
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 0)
p.setGravity(0, 0, -9.81)
# p.setPhysicsEngineParameter(numSolverIterations=self.numSolverIterations)
#p.setTimeStep(self.fixedTimeStep)
orn = p.getQuaternionFromEuler([math.pi / 30 * 0, 0 * math.pi / 50, 0])
p.setAdditionalSearchPath(pybullet_data.getDataPath())
planeUid = p.loadURDF("plane_transparent.urdf", [0, 0, 0], orn)
p.changeDynamics(planeUid, -1, lateralFriction=1)
texUid = p.loadTexture("concrete.png")
p.changeVisualShape(planeUid, -1, textureUniqueId=texUid)
if self.useStairs:
stairsUid = p.loadURDF("../urdf/stairs_gen.urdf.xml", [0, -1, 0],
orn)
flags = p.URDF_USE_SELF_COLLISION
quadruped = p.loadURDF(
"../urdf/spotmicroai_gen.urdf.xml",
self.init_position,
self.init_oritentation,
useFixedBase=self.useFixedBase,
useMaximalCoordinates=self.useMaximalCoordinates,
flags=flags) #p.URDF_USE_IMPLICIT_CYLINDER)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING, 1)
p.changeDynamics(quadruped, -1, lateralFriction=0.8)
return quadruped
def changeDynamics(self, quadruped):
nJoints = p.getNumJoints(quadruped)
for i in range(nJoints):
p.changeDynamics(
quadruped,
i,
localInertiaDiagonal=[0.000001, 0.000001, 0.000001])
def getJointNames(self, quadruped):
nJoints = p.getNumJoints(quadruped)
jointNameToId = {}
for i in range(nJoints):
jointInfo = p.getJointInfo(quadruped, i)
jointNameToId[jointInfo[1].decode('UTF-8')] = jointInfo[0]
return jointNameToId
def addInfoText(self, bodyPos, bodyEuler, linearVel, angularVel):
# TODO: use replacementId instead of deleting the old views. seems to have memleak issues?
if not self.debug:
return
text = "Distance: {:.1f}m".format(
math.sqrt(bodyPos[0]**2 + bodyPos[1]**2))
text2 = "Roll/Pitch: {:.1f} / {:.1f}".format(
math.degrees(bodyEuler[0]), math.degrees(bodyEuler[1]))
text3 = "Vl: {:.1f} / {:.1f} / {:.1f} Va: {:.1f} / {:.1f} / {:.1f}".format(
linearVel[0], linearVel[1], linearVel[2], angularVel[0],
angularVel[1], angularVel[2])
x, y = bodyPos[0], bodyPos[1]
newDebugInfo = [
p.addUserDebugLine([x, y, 0], [x, y, 1], [0, 1, 0]),
p.addUserDebugText(text, [x + 0.03, y, 0.6],
textColorRGB=[1, 1, 1],
textSize=1.0),
p.addUserDebugText(text2, [x + 0.03, y, 0.5],
textColorRGB=[1, 1, 1],
textSize=1.0),
p.addUserDebugText(text3, [x + 0.03, y, 0.4],
textColorRGB=[1, 1, 1],
textSize=1.0)
]
p.addUserDebugLine([-0.3, 0, 0], [0.3, 0, 0], [0, 1, 0],
parentObjectUniqueId=self.quadruped,
parentLinkIndex=1),
p.addUserDebugLine([0, -0.2, 0], [0, 0.2, 0], [0, 1, 0],
parentObjectUniqueId=self.quadruped,
parentLinkIndex=1),
if len(self.oldDebugInfo) > 0:
for x in self.oldDebugInfo:
p.removeUserDebugItem(x)
self.oldDebugInfo = newDebugInfo
def handleCamera(self, cubePos, cubeOrn):
# Look forward/up
init_camera_vector = (-1, 0, 0)
init_up_vector = (0, 0, 1)
# rotate camera on y by 25degrees
orn = p.getMatrixFromQuaternion(
p.getQuaternionFromEuler([0, -math.pi / 180 * 25, 0]))
# Rotate vectors with body
orn = np.array(orn).reshape(3, 3)
rot_matrix = p.getMatrixFromQuaternion(cubeOrn)
rot_matrix = np.array(rot_matrix).reshape(3, 3)
camera_vector = rot_matrix.dot(orn.dot(init_camera_vector))
up_vector = rot_matrix.dot(orn.dot(init_up_vector))
# Camera origin is right at the end of the head watching forward (Body-Coordinates)
view_matrix = p.computeViewMatrix(cubePos + 0.17 * camera_vector,
cubePos + 3 * camera_vector,
up_vector)
img = p.getCameraImage(320, 200, view_matrix, self.projection_matrix)
#TODO: Use the camera image
def resetBody(self):
if len(self.oldDebugInfo) > 0:
for x in self.oldDebugInfo:
p.removeUserDebugItem(x)
p.resetBasePositionAndOrientation(self.quadruped, self.init_position,
[0, 0, 0, 1])
p.resetBaseVelocity(self.quadruped, [0, 0, 0], [0, 0, 0])
def checkSimulationReset(self, bodyOrn):
(xr, yr, _) = p.getEulerFromQuaternion(bodyOrn)
# If our Body rotated more than pi/3: reset
if (abs(xr) > math.pi / 3 or abs(yr) > math.pi / 3):
self.resetBody()
return True
return False
def bodyRotation(self, rot):
self.rot = rot
def bodyPosition(self, pos):
self.pos = pos
def feetPosition(self, Lp):
self.Lp = Lp
def getPos(self):
bodyPos, _ = p.getBasePositionAndOrientation(self.quadruped)
return bodyPos
def getIMU(self):
_, bodyOrn = p.getBasePositionAndOrientation(self.quadruped)
linearVel, angularVel = p.getBaseVelocity(self.quadruped)
return bodyOrn, linearVel, angularVel
def step(self):
quadruped = self.quadruped
bodyPos, bodyOrn = p.getBasePositionAndOrientation(quadruped)
linearVel, angularVel = p.getBaseVelocity(quadruped)
bodyEuler = p.getEulerFromQuaternion(bodyOrn)
kp = p.readUserDebugParameter(self.IDkp)
kd = p.readUserDebugParameter(self.IDkd)
maxForce = p.readUserDebugParameter(self.IDmaxForce)
self.handleCamera(bodyPos, bodyOrn)
self.addInfoText(bodyPos, bodyEuler, linearVel, angularVel)
if self.checkSimulationReset(bodyOrn):
return False
# Calculate Angles with the input of FeetPos,BodyRotation and BodyPosition
angles = self.kin.calcIK(self.Lp, self.rot, self.pos)
for lx, leg in enumerate(
['front_left', 'front_right', 'rear_left', 'rear_right']):
for px, part in enumerate(['shoulder', 'leg', 'foot']):
j = self.jointNameToId[leg + "_" + part]
p.setJointMotorControl2(bodyIndex=quadruped,
jointIndex=j,
controlMode=p.POSITION_CONTROL,
targetPosition=angles[lx][px] *
self.dirs[lx][px],
positionGain=kp,
velocityGain=kd,
force=maxForce)
nowLidarTime = time.time()
if (nowLidarTime - self.lastLidarTime > .2):
numThreads = 0
results = p.rayTestBatch(self.rayFrom,
self.rayTo,
numThreads,
parentObjectUniqueId=quadruped,
parentLinkIndex=0)
for i in range(self.numRays):
hitObjectUid = results[i][0]
hitFraction = results[i][2]
hitPosition = results[i][3]
if (hitFraction == 1.):
if self.debugLidar:
p.addUserDebugLine(self.rayFrom[i],
self.rayTo[i],
self.rayMissColor,
replaceItemUniqueId=self.rayIds[i],
parentObjectUniqueId=quadruped,
parentLinkIndex=0)
else:
localHitTo = [
self.rayFrom[i][0] + hitFraction *
(self.rayTo[i][0] - self.rayFrom[i][0]),
self.rayFrom[i][1] + hitFraction *
(self.rayTo[i][1] - self.rayFrom[i][1]),
self.rayFrom[i][2] + hitFraction *
(self.rayTo[i][2] - self.rayFrom[i][2])
]
dis = math.sqrt(localHitTo[0]**2 + localHitTo[1]**2 +
localHitTo[2]**2)
if self.debugLidar:
p.addUserDebugLine(self.rayFrom[i],
localHitTo,
self.rayHitColor,
replaceItemUniqueId=self.rayIds[i],
parentObjectUniqueId=quadruped,
parentLinkIndex=0)
lastLidarTime = nowLidarTime
# let the Simulator do the rest
if (self.useRealTime):
self.t = time.time() - self.ref_time
else:
self.t = self.t + self.fixedTimeStep
if (self.useRealTime == False):
p.stepSimulation()
time.sleep(self.fixedTimeStep)
#NAME: testForwardKinematic.py
#DATE: 05/06/2019
#AUTH: QLab Makerspace
#DESC: Script to demonstrate how to properly to interact with the Kinematic Class
#COPY: Copyright 2019, All Rights Reserved
import sys
sys.path.append('control/')
from kinematics import Kinematic
import time
#instantiate kinematic object
kin = Kinematic()
#input joint positions (work out why there is angles added and subtracted from 2 joints? caution when testing)
joint_positions = [0.01,-90,(90-90),0.01,0.01,(0.01+180)]
#[J0,J1,J2,J3,J4,J5]
print("INFO: Running Forward Kinematics")
startTime = time.time()
#perform forward kinematics and extract end effector pose
pose = kin.forwardKinematics(joint_positions)
#pose = [X,Y,Z,Yaw,Pitch,Roll]
print("INFO: Pose is '"+str(pose)+"'.")
print("INFO: Forward Kinematic Test complete. Processing took "+str(round(time.time()-startTime,2))+" seconds.")
class Arm:
debug = False
logFilePath = "logs/log.txt"
header = {'User-Agent':'Mozilla/5.0 (Windows NT 6.1; WOW64) AppleWebKit/537.36 (KHTML, like Gecko) Chrome/41.0.2272.101 Safari/537.36'}
#Config Variable Initialise
jointMaxRotation = []
jointMaxSpeed = []
jointMinSpeed = []
jointPosDefault = []
jointSpeedDefault = []
jointAccelDefault = []
def __init__(self,config, ipAddress=None):
self.joints = 6
self.logging = True
if ipAddress == None:
self.ipAddress = config["ipAddress"]
else:
self.ipAddress = ipAddress
self.port = config["port"]
self.baseURL = "http://"+str(self.ipAddress)+":"+str(self.port)+"/"
self.error = False
self.timeout = 2 #Seconds
self.pollingStatus = False
#Values loaded from 'config.json'
for joint in config["joints"]:
self.jointMaxRotation.append(joint["maxRotation"])
self.jointMaxSpeed.append(joint["maxSpeed"])
self.jointMinSpeed.append(joint["minSpeed"])
self.jointPosDefault.append(joint["defaultPosition"])
self.jointSpeedDefault.append(joint["defaultSpeed"])
self.jointAccelDefault.append(joint["defaultAccel"])
#Status Flags
self.jointPosition = [None]*self.joints
self.switchState = [0]*self.joints
self.calibrationState = [0]*self.joints
self.movementFlag = [0]*self.joints
try:
self.session = requests.session()
self.clearLogs()
self.connected = True
except:
self.log("ERROR: Cannot create a session.")
self.connected = False
#Open a solver for kinematics
self.kin = Kinematic()
#Start capturing status packets
self.getStatus()
#Logging Function
def log(self, entry):
currentDateTime = time.strftime("%d/%m/%Y %H:%M:%S")
logEntry = currentDateTime + ": " + entry
if self.logging == True:
#open a txt file to use for logging
logFile = open(self.logFilePath,"a+")
logFile.write(logEntry+"\n")
logFile.close()
print(logEntry)
#Send and Receive Messages with implemented logging
def sendCommand(self, command):
#Start Timing
start = time.time()
#combine with host address
message = self.baseURL + "send?command=" + command
message = message.encode('ascii')
if self.pollingStatus == False:
self.getStatus()
try:
if self.debug == True:
response = self.session.get(message,timeout=self.timeout)
status = response.content.decode("utf-8").split("\n")
self.log("INFO: Transmission response code is "+str(response.status_code))
end = time.time()
self.log("STATUS: Sending '"+str(command)+"' took "+str(round((end-start),2))+" seconds.")
self.log(status[0])
else:
#t1 = time.time()
self.session.get(message,timeout=self.timeout)
#t2 = time.time()
#print("in class ", t2 - t1)
self.connected = True
except:
self.log("ERROR: Could not access API.")
self.connected = False
@threaded
def getStatus(self):
while self.connected:
self.pollingStatus = True
try:
message = self.baseURL + "getLatest"
response = self.session.get(message,timeout=self.timeout)
status = str(response.text)
#Extract Joint Positions
if(status.find("STATUS:")!=-1):
if(status.find("MOVEMENT") != -1):
data = status.split(",")
self.movementFlag = list(map(int,data[1:]))
elif(status.find("CALIBRATION") != -1):
data = status.split(",")
self.calibrationState = list(map(int,data[1:]))
elif(status.find("POSITION") != -1):
data = status.split(",")
try:
self.jointPosition = list(map(float,data[1:]))
except:
pass
elif(status.find("SWITCH") != -1):
data = status.split(",")
self.switchState = list(map(int,data[1:]))
else:
self.log("FAILED TO PARSE: "+status)
elif(status !=""):
self.log(status)
except:
self.log("INFO: Did not receive status response from API.")
self.pollingStatus = False
def moveJointTo(self,joint,position):
if self.calibrationState[joint]:
if (position >= 0) and (position <= self.jointMaxRotation[joint]):
command = "p"+str(joint)+str(position)
self.sendCommand(command)
self.log("INFO: Joint "+str(joint)+" moved to "+str(position)+" degrees.")
else:
self.log("ERROR: Positon out of range.")
else:
self.log("ERROR: Joint "+str(joint)+" not calibrated.")
def moveJoint(self,motor,degrees):
#Check movement is within range allowed
if (int(self.jointPosition[motor])+degrees) > self.jointMaxRotation[motor]:
degrees = self.jointMaxRotation[motor] - int(self.jointPosition[motor])
if (int(self.jointPosition[motor])+degrees) < 0:
degrees = -self.jointMaxRotation[motor]
command = "m"+str(motor)+str(degrees)
self.sendCommand(command)
self.log("INFO: Command sent to adjust motor "+str(motor)+" "+str(degrees)+" degrees.")
def getPose(self):
pose = self.kin.forwardKinematics(self.jointPosition)
return pose
def getPositions(self):
return self.jointPosition
def getJointPosition(self,motor):
position = float(self.jointPosition[motor])
return position
def positionJoints(self,positions):
if self.armCalibrated():
if len(positions) == self.joints:
motor = 0
for position in positions:
self.moveJointTo(motor,position)
motor += 1
else:
self.log("ERROR: Invalid Joint Positions.")
else:
self.log("ERROR: Calibrate arm before continuing.")
def rest(self):
if self.armCalibrated():
self.log("INFO: Arm moving to a resting position.")
restPosition = [None]*self.joints
restPosition[0] = self.jointPosDefault[0]
restPosition[1] = 150
restPosition[2] = 175
restPosition[3] = self.jointPosDefault[3]
restPosition[4] = self.jointPosDefault[4]
restPosition[5] = self.jointPosDefault[5]
self.positionJoints(restPosition)
else:
self.log("ERROR: Calibrate arm before trying to rest.")
def standUp(self):
if self.armCalibrated():
self.log("INFO: Arm standing upright.")
self.positionJoints(self.jointPosDefault)
else:
self.log("ERROR: Calibrate arm before trying to stand.")
def setAccel(self,joint,accel):
command = "z"+str(joint)+str(int(accel))
self.sendCommand(command)
self.log("INFO: Joint "+str(joint)+" acceleration rate adjusted to "+str(int(accel))+" degrees per second squared.")
def setSpeed(self,joint,speed):
command = "s"+str(joint)+str(int(speed))
self.sendCommand(command)
self.log("INFO: Joint "+str(joint)+" speed adjusted to "+str(int(speed))+" degrees per second.")
def setMinSpeed(self,joint,minSpeed):
command = "d"+str(joint)+str(int(minSpeed))
self.sendCommand(command)
self.log("INFO: Joint "+str(joint)+" minimum speed adjusted to "+str(int(minSpeed))+" degrees per second.")
def calibrateArm(self):
command = "ca"
self.sendCommand(command)
self.log("INFO: Arm is Currently Calibrating.")
def calibrateJoint(self, joint):
command = "c"+joint
self.sendCommand(command)
self.log("INFO: Joint "+str(joint)+" is currently calibrating.")
def stop(self):
self.sendCommand("q")
self.log("INFO: Arm Emergency Stopped.")
def checkConnection(self):
self.sendCommand("test")
self.log("INFO: Testing the connection.")
def selectRandomPosition(self,motor):
randomPosition = random.randint(0,self.jointMaxRotation[motor])
return randomPosition
def waitToStationary(self):
time.sleep(0.2)
while(self.checkMovement()):
pass
def checkMovement(self):
time.sleep(0.2)
moving = False
for jointMoving in self.movementFlag:
if bool(jointMoving):
moving = jointMoving
return moving
def clearLogs(self):
url = self.baseURL + "clearLogs"
response = self.session.get(url,timeout=self.timeout)
if response.content.decode("utf-8"):
self.log(response.content.decode("utf-8"))
def resetArduino(self):
messages = ["disconnect","connect"]
for message in messages:
url = self.baseURL + message
response = self.session.get(url,timeout=self.timeout)
if response.content.decode("utf-8"):
self.log(response.content.decode("utf-8"))
time.sleep(1.5)
self.log("INFO: Arduino for Arm Reset.")
def resetEStop(self):
self.sendCommand("r")
time.sleep(1)
self.log("INFO: Emergency Stop Latch Reset.")
def stopDemos(self):
url = self.baseURL + "stopDemos"
response = self.session.get(url,timeout=self.timeout)
if response.content.decode("utf-8"):
self.log(response.content.decode("utf-8"))
time.sleep(1.5)
self.log("INFO: Raspberry Pi Demo Processes Terminated.")
def armCalibrated(self):
calibrated = True
for jointCalibrated in self.calibrationState:
calibrated &= int(jointCalibrated)
return calibrated
def setDefaults(self):
i = 0
for i in range(0,self.joints):
self.setSpeed(i,self.jointSpeedDefault[i])
self.setMinSpeed(i,(int(self.jointSpeedDefault[i])-10))
self.setAccel(i,self.jointAccelDefault[i])
self.log("INFO: Joint "+str(i)+" defaults set.")
rawDistance = direction.length()
normalizedDistance = direction.length() / self.threshold
if rawDistance < self.threshold:
strength = min(
decayCoefficient /
(normalizedDistance * normalizedDistance),
MAX_ACCELERATION)
direction.normalize()
linear += direction * strength
return SteeringOutput(linear, 0)
class SteeringMatchVelocity:
def __init__(self, character, others):
self.character = character
self.targets = others
def getSteering(self):
sumVelocities = self.character.velocity
for target in self.targets:
sumVelocities = sumVelocities + target.velocity
avgVelocity = sumVelocities / (len(self.targets) + 1.0)
N = 0
avgVelocity[0] += N * randomBinomial()
avgVelocity[1] += N * randomBinomial()
return SteeringOutput(avgVelocity, 0)
if __name__ == "__main__":
targetKinematic = Kinematic(Point2(0, 0), 0, 0, None, 0)
#NAME: testForwardKinematic.py
#DATE: 05/06/2019
#AUTH: QLab Makerspace
#DESC: Script to demonstrate how to properly to interact with the Kinematic Class
#COPY: Copyright 2019, All Rights Reserved
import sys
sys.path.append('control/')
from kinematics import Kinematic
#instantiate kinematic object
kin = Kinematic()
"""
#input joint positions (work out why there is angles added and subtracted from 2 joints? caution when testing)
# [J0,J1,J2,J3,J4,J5]
joint_positions = [0.01,-90,(90-90),0.01,0.01,(0.01+180)]
print("Running Forward Kinematics")
#perform forward kinematics and extract end effector pose
pose = kin.forwardKinematics(joint_positions)
#pose = [X,Y,Z,Yaw,Pitch,Roll]
print(pose)
print("Task complete")
"""
#insert a desired pose for inverse kinematics
current_joints = [30, 90, 90, 30, -30, 30]
desired_pose = [14.65766456, 43.51281973, 317.0026196, -163.0643134, 14.87094445, 106.9356866]
#inverse kinematics not finished
def __init__(self):
print("Initialising")
self.servos=Servos()
self.Kinematic=Kinematic()