def toString(self, name, Body, salary):
print(name,
" ",
round(Body.toString(Body.getWeight(), Body.getHeight()), 2),
" ",
salary,
sep='')
def init_bodies(self):
self.body = Body(1., .1, 1., np.array([.0, .0, .0]),
np.array([0., 0., 0.]), 10)
self.ring_1 = Ring(np.array([0, 0, 10]), 10, 320)
self.ring_2 = Ring(np.array([0, 0, -10]), 10, 320)
self.E = np.array([0., 0., 0.])
self.F = (0., 0., 0.)
def planetAlignment(self, filename):
self.readInfo(filename)
sun = Body(True, 'experiments/innerSolarSystem/infoFiles/bodyInfo/sun')
mercury = Body(
True, 'experiments/innerSolarSystem/infoFiles/bodyInfo/mercury')
venus = Body(True,
'experiments/innerSolarSystem/infoFiles/bodyInfo/venus')
earth = Body(True,
'experiments/innerSolarSystem/infoFiles/bodyInfo/earth')
mars = Body(True,
'experiments/innerSolarSystem/infoFiles/bodyInfo/mars')
system = AnimatedSystem([sun, mercury, venus, earth, mars],
self.delta_t)
aligned = system.planetsAligned()
numIterToTry = 1000000
i = 0
print("Checking alignment")
while i < numIterToTry and not (aligned):
sys.stdout.write("\r" + str(100 * i / numIterToTry)[0:3] + "%")
system.updateSystemFull()
aligned = system.planetsAligned()
sys.stdout.flush()
i += 1
print("\n")
print(str(self.delta_t * i))
system.animateEveryNth(0.98 * self.delta_t * i, self.delta_t * i,
self.animateEveryNth, self.animationTimeStep)
def setup(self, reset=0):
self.start_state = demake(BLANK_STATE)
## Assiging Objects Start Positions
# Agents
a1x, a1y = 5, 3
if (not reset):
self.agent1 = Body("agent1", "A", [a1x, a1y])
self.start_state[a1x][a1y] = self.agent1.symbol
self.agent1.location = [a1x, a1y]
a2x, a2y = 7, 8
if (not reset):
self.agent2 = Body("agent2", "B", [a2x, a2y])
self.start_state[a2x][a2y] = self.agent2.symbol
self.agent2.location = [a2x, a2y]
# Non Agents
self.start_state[5][8] = "0"
self.start_state[10][3] = "*"
self.start_state[3][10] = "*"
##
self.state = self.start_state
class SKP:
def __init__(self):
super().__init__()
check_license_and_variables_exist()
self.body = Body()
self.confidence_threshold = 0.5
self.skp_output_path = "skp_output"
self.api = Api(default_license_dir())
self.model_path = os.path.join(
os.environ["CUBEMOS_SKEL_SDK"], "models", "skeleton-tracking", "fp32", "skeleton-tracking.cubemos"
)
self.api.load_model(CM_TargetComputeDevice.CM_CPU, self.model_path)
def get_skp_from_pic(self, pic_path):
try:
img = cv2.imread(pic_path)
skeletons = self.api.estimate_keypoints(img, 192)
file_name = os.path.basename(pic_path)
with open("{}/{}.json".format(self.skp_output_path, os.path.splitext(file_name)[0]), "w") as f:
skps = dict()
for i in skeletons:
self.body.set_body(i)
id = str(skeletons.index(i))
skps[id] = dict()
skps[id]['key_frame'] = int(os.path.splitext(file_name)[0].split('_')[-1])
skps[id]['head'] = self.body.get_head_coordinates()
skps[id]['angles'] = self.body.calculate_angles()
json.dump(skps, f)
# isSaved = cv2.imwrite("{}/{}".format(self.output_path, file_name), img)
# return True if isSaved else False
except Exception as ex:
print("Exception occured: \"{}\"".format(ex))
def meteorRisk(self, filename):
self.readInfo(filename)
sun = Body(True, 'experiments/innerSolarSystem/infoFiles/bodyInfo/sun')
mercury = Body(
True, 'experiments/innerSolarSystem/infoFiles/bodyInfo/mercury')
venus = Body(True,
'experiments/innerSolarSystem/infoFiles/bodyInfo/venus')
earth = Body(True,
'experiments/innerSolarSystem/infoFiles/bodyInfo/earth')
mars = Body(True,
'experiments/innerSolarSystem/infoFiles/bodyInfo/mars')
LeastSepToEarth = []
meteor = Meteor(
'experiments/innerSolarSystem/infoFiles/bodyInfo/meteor')
meteorSystem = AnimatedSystem(
[sun, mercury, venus, earth, mars, meteor], self.delta_t)
meteorSystem.iterateTimeInterval(self.timeInterval)
meteorSystem.animateEveryNth(0., self.timeInterval,
self.animateEveryNth,
self.animationTimeStep)
closestDisp, time = meteorSystem.closestApproach([3, 5])
closestDist = np.linalg.norm(closestDisp)
print("Closest approach to Earth: " + str(closestDist))
'''
def innerSystem(self, filename):
self.readInfo(filename)
sun = Body(True, 'experiments/innerSolarSystem/infoFiles/bodyInfo/sun')
mercury = Body(
True, 'experiments/innerSolarSystem/infoFiles/bodyInfo/mercury')
venus = Body(True,
'experiments/innerSolarSystem/infoFiles/bodyInfo/venus')
earth = Body(True,
'experiments/innerSolarSystem/infoFiles/bodyInfo/earth')
mars = Body(True,
'experiments/innerSolarSystem/infoFiles/bodyInfo/mars')
innerBodies = [sun, mercury, venus, earth, mars]
innerSystem = AnimatedSystem(innerBodies, self.delta_t)
innerSystem.iterateTimeInterval(self.timeInterval)
print("Orbital period [Earth days]: ")
#prints the number of earth days in an average orbit
print("Mercury: " + str(0.000072722 / innerSystem.averageAngVel(1)))
print("Venus: " + str(0.000072722 / innerSystem.averageAngVel(2)))
print("Earth: " + str(0.000072722 / innerSystem.averageAngVel(3)))
print("Mars: " + str(0.000072722 / innerSystem.averageAngVel(4)))
print("")
#shows that energy is conserved
innerSystem.printInfoTimeInterval(
'experiments/innerSolarSystem/outputData/innerSystemEnergyConservation.dat',
0., self.timeInterval, 1000, [-1], [-1, -2, -3])
innerSystem.animateEveryNth(0., self.timeInterval,
self.animateEveryNth,
self.animationTimeStep)
def energyConservationSolarSystem(self, filename):
self.readInfo(filename)
sun = Body(True, 'experiments/innerSolarSystem/infoFiles/bodyInfo/sun')
mercury = Body(
True, 'experiments/innerSolarSystem/infoFiles/bodyInfo/mercury')
venus = Body(True,
'experiments/innerSolarSystem/infoFiles/bodyInfo/venus')
earth = Body(True,
'experiments/innerSolarSystem/infoFiles/bodyInfo/earth')
mars = Body(True,
'experiments/innerSolarSystem/infoFiles/bodyInfo/mars')
systemOfBodies = [sun, mercury, venus, earth, mars]
energyData = []
for i in range(990, 1000):
delta_t = i * 1000.
system = AnimatedSystem(copy.deepcopy(systemOfBodies), delta_t)
system.iterateTimeInterval(self.timeInterval)
minEnergy, maxEnergy = system.mimMaxSystemEnergy()
energyDataElement = [delta_t, minEnergy, maxEnergy]
energyData.append(energyDataElement)
fileout = open(
'experiments/innerSolarSystem/outputData/minMaxEnergyData.dat',
'w')
for data in energyData:
fileout.write(
str(data[0]) + "\t" + str(data[1]) + "\t" + str(data[2]) +
"\n")
fileout.close()
def __init__(self):
super().__init__()
check_license_and_variables_exist()
self.body = Body()
self.confidence_threshold = 0.5
self.skp_output_path = "skp_output"
self.api = Api(default_license_dir())
self.model_path = os.path.join(
os.environ["CUBEMOS_SKEL_SDK"], "models", "skeleton-tracking", "fp32", "skeleton-tracking.cubemos"
)
self.api.load_model(CM_TargetComputeDevice.CM_CPU, self.model_path)
def jsonToSystem(jsonObjs):
sysObj = str(jsonObjs[0])
sysLoadStr = sysObj.replace("u'", '"').replace("'",
'"').replace("L,", ",")
systemDict = json.loads(sysLoadStr)
system = GravitySystem()
system.G = systemDict["gravityConstant"]
system.scale = systemDict["scale"]
system.timeScale = systemDict["timeScale"]
system.bodyScale = systemDict["bodyScale"]
system.centralBodyScale = systemDict["centralBodyScale"]
system.elasticity = systemDict["elasticity"]
system.systemRadius = systemDict["boundary"]
system.handMass = systemDict["handMass"]
centralBody = None
for i in range(len(jsonObjs) - 1):
bodyObj = str(jsonObjs[i + 1])
bodyLoadStr = bodyObj.replace("u'",
'"').replace("'",
'"').replace("L,", ",")
dict = json.loads(bodyLoadStr)
body = Body(dict["name"], dict["mass"], dict["radius"])
if (body.name == systemDict["centralBodyName"]):
centralBody = body
system.centralBody = body
centralBody.setScale(system.scale)
centralBody.setBodyScale(system.bodyScale *
system.centralBodyScale)
print "Central body found:", centralBody.name
if (centralBody != None):
body.centralBody = centralBody
else:
print body.name, "has no central body"
body.setLocation(
Vector(dict["locationX"], dict["locationY"],
dict["locationZ"]), False)
if (body.name != systemDict["centralBodyName"]):
body.orbitDirection = Vector(dict["directionX"],
dict["directionY"],
dict["directionZ"])
body.setSpeed(dict["orbitSpeed"])
body.setOrbitRadius(dict["orbitRadius"])
system.addBody(body)
return system
def test_check_collision(self):
unit1 = MagicMock()
unit1.body = Body.simple()
unit1.radius = 10
unit2 = MagicMock()
unit2.body = Body.simple()
unit2.radius = 10
self.space.add_unit(unit1)
self.space.add_unit(unit2)
self.space.check_collision()
self.assertTrue(unit1.collision.called)
self.assertTrue(unit2.collision.called)
def __init__(self, battle, stratName):
'''
Robots have arms, wheels, and a body by default. Sensors or add-ons can be appended later and are visible for attack
'''
self.body = Body()
self.wheels = Wheels()
self.addOns = [Arm(), Arm()]
self.stratName = stratName
self.battle = battle
p, m = 'robot.' + self.stratName, 'strat'
mod = import_module(p)
self.strategy = getattr(mod, m)
self.id = uuid.uuid4().hex
def threeBodies(self):
delta_t = 0.01
timeInterval = 5.0
timewarp = 1.
fps = 50
bodyA = Body(True, 'experiments/3Bodies/infoFiles/bodyA')
bodyB = Body(True, 'experiments/3Bodies/infoFiles/bodyB')
bodyC = Body(True, 'experiments/3Bodies/infoFiles/bodyC')
threeSystem = AnimatedSystem([bodyA, bodyB, bodyC], delta_t)
threeSystem.iterateTimeInterval(timeInterval)
#threeSystem.dataDump('experiments/3Bodies/data/3bodyDump.dat')
#threeSystem.printInfoTimeInterval('experiments/3Bodies/data/3bodydata.dat', 0., timeInterval, 0.1, [0,1,2], [0,1,8])
threeSystem.animateTimeInterval(0., timeInterval, fps, timewarp)
def simpleOrbit(self):
delta_t = 0.001
timeInterval = 5.
timewarp = 1.
fps = 50
mars = Body(True, 'experiments/simpleOrbit/infoFiles/marsInfo')
phobos = Body(True, 'experiments/simpleOrbit/infoFiles/phobosInfo')
marsAndMoon = AnimatedSystem([mars, phobos], delta_t)
marsAndMoon.iterateTimeInterval(timeInterval)
marsAndMoon.printInfoTimeInterval(
'experiments/simpleOrbit/data/test1.dat', 0., timeInterval, 0.01,
[1], [0, 1, 8])
marsAndMoon.animateTimeInterval(0., timeInterval, fps, timewarp)
def handle_sim_space_click(self, mouse_pos):
# Check if a body was clicked
pos = Render.convert_to_renderer(mouse_pos)
body_clicked = None
# check for a collision with any body
for body in self.allBodies:
if body.click_point(pos):
body_clicked = body
break
if body_clicked is not None:
# If a body is clicked, select/deselect it
if body_clicked in self.selected_bodies:
self.selected_bodies.remove(body_clicked)
else:
self.selected_bodies.append(body_clicked)
if len(self.selected_bodies) > 0:
self.details_bar.selected_objects = self.selected_bodies
else:
self.details_bar.selected_objects = []
else:
# If no body is clicked, either deselect all, or create new body
if len(self.selected_bodies) != 0:
self.selected_bodies = []
else:
o = Body(pos)
self.allBodies.append(o)
self.selected_bodies.append(o)
self.paths_calculated = False
self.do_when_selected_bodies_changed()
self.details_bar.update(True)
def add_body(self, obj_id, obj_mass, obj_position, obj_velocity):
"""
Add a Body Object to the list.
This method call Body() obtects to creater a new and append to this list.
Parameters
----------
obj_id : str
Body name
obj_mass : float
Body mass
obj_position : array_like
Position in x (float) and y (float). [x, y]
obj_velocity : array_like
velocity in x (float) and y (float). [V_x, V_y]
"""
# All dimensions are scaled
mass = float(obj_mass)*self.convert_m
position = [pos*self.convert_r for pos in obj_position]
velocity = [vel*self.convert_v for vel in obj_velocity]
new_Body = Body(obj_id,mass,position,velocity)
self.bodies.append( new_Body )
self.lookup[obj_id] = new_Body
print('*** Body = ',obj_id, obj_mass, obj_position, obj_velocity,' added ***',sep='\t')
def __init__(self, filename, timestep, maxIterations, probeExists):
# Recieves a datafile and generates a list of Body type objects.
self.system = []
datafile = open(filename, "r")
for line in datafile.readlines():
tokens = line.split(", ")
self.system.append(
Body(str(tokens[0]), float(tokens[1]), float(tokens[2]),
str(tokens[3]), [float(i) for i in tokens[4].split(",")],
[float(i) for i in tokens[5].split(",")],
[float(i) for i in tokens[6].split(",")]))
datafile.close()
# Sets some initial parameters for the simulation and constants energy and animation methods.
self.elapsedTime = 0.0
self.timestep = timestep
self.maxIterations = maxIterations
self.probeExists = probeExists
self.hasLaunched = False
def calc_exzentri_orbit(self, override_pos=True):
body = self.bd
s = body.orbit
s.calc_kb()
dist = (1 + s.exzentritaet) * s.ga
# for inkl = 0
# positioning on x-axis with inklination (2D -> 3D)
if body.orbit.inklination is not 0:
xp = dist * np.sin(np.pi / 2 + body.orbit.inklination)
yp = 0
zp = dist * np.cos(body.orbit.inklination)
npos = np.array([xp, yp, zp])
else:
npos = np.array(s.mother.pos) + np.array([1, 0]) * dist
sw = np.array(self.calc_schwerpunkt(s.mother, bbody2=Body(s.bd.mass, npos.tolist(), '')))
a = s.ga
b = s.kb
t = Symbol('t')
term = ""
if len(npos.tolist()) is 2:
term = sw + np.array([a * cos(t), b * sin(t)])
elif len(npos.tolist()) is 3:
c = np.dot((npos - sw), np.array([0, 0, 1])) # z-offset
term = sw + np.array([a * cos(t), b * sin(t), c * cos(t)])
# dy = np.array([[np.cos(body.orbit.inklination), 0, np.sin(body.orbit.inklination)], [0, 1, 0],
# [-np.sin(body.orbit.inklination), 0, np.cos(body.orbit.inklination)]])
s.track = term
if override_pos:
body.pos = npos.tolist()
body.orbit.sp = sw.tolist()
return term
def test_update(self):
black_hole = BlackHole.create(Body.simple())
space = MagicMock()
black_hole.update(space, 100)
self.assertFalse(black_hole.live)
def test_simple(self):
body = Body.simple()
self.assertEqual(Vector(0, 0), body.coordinates)
self.assertEqual(Vector(0, 0), body.velocity)
self.assertEqual(0, body.angle)
self.assertEqual(0, body.angel_velocity)
def walk(tree, parent):
for key in tree:
if key in System.reservedWords:
continue
if not self._checkSpec(key, tree[key]):
continue
dia = self._formatNumber(tree[key]["dia"])
mass = self._formatNumber(tree[key]["mass"])
rad = self._formatNumber(tree[key]["rad"])
if "color" in tree[key]:
color = self._evalExpr(tree[key]["color"])
else:
color = self._evalExpr("WHITE")
if "vel" in tree[key]:
vel = [
0,
self._evalExpr(tree[key]["vel"]) + parent.Velocity[1]
]
elif parent:
vel = [
0,
math.sqrt(G * (parent.Mass**2) /
(rad * (mass + parent.Mass))) +
parent.Velocity[1]
]
else:
vel = [0, 0]
b = Body(key, parent, dia, mass, vel, rad, color)
self._index[key] = b
if not parent:
self._roots.append(b)
walk(tree[key], b)
def test_constructor1(self):
b = Body("A", -2, [3, 2], [1.0, -1.0])
self.assertEqual(b.obj_id, "A")
# we have to protect the value of the mass from negative or zero
# In this case the mass is negative
self.assertEqual(b.obj_mass, -2)
self.assertEqual(b.obj_position, [3, 2])
self.assertEqual(b.obj_velocity, [1.0, -1.0])
def init_bodies(self):
Q = 10
self.l_bodies = []
# Body(self, b_id, b_radius, b_mass, b_pos, b_vel, b_charge):
# Init controlled body
self.controlled_body = Body(0, 0.1, 1., np.array([.0, .0, 0.]), np.array([0., 0., 0.]), Q * 0.01)
self.E = np.array([0., 0., 0.])
self.F = (0., 0., 0.)
# Init bodies
self.l_bodies.append(Body(2, .5, 1., np.array([4., 4., 4.]), np.array([0., 0., 0.]), Q))
self.l_bodies.append(Body(2, .5, 1., np.array([4., 4., -4.]), np.array([0., 0., 0.]), -Q))
self.l_bodies.append(Body(2, .5, 1., np.array([-4., -4., 4.]), np.array([0., 0., 0.]), Q))
self.l_bodies.append(Body(2, .5, 1., np.array([-4., -4., -4.]), np.array([0., 0., 0.]), -Q))
self.l_bodies.append(Body(2, .5, 1., np.array([4., -4., 4.]), np.array([0., 0., 0.]), -Q))
self.l_bodies.append(Body(2, .5, 1., np.array([4., -4., -4.]), np.array([0., 0., 0.]), Q))
self.l_bodies.append(Body(2, .5, 1., np.array([-4., 4., 4.]), np.array([0., 0., 0.]), -Q))
self.l_bodies.append(Body(2, .5, 1., np.array([-4., 4., -4.]), np.array([0., 0., 0.]), Q))
def __init__(self):
self.clock = pygame.time.Clock()
self.screen = pygame.display.set_mode(config.window_size)
Render.set_surface(pygame.display.get_surface())
self.allBodies = [
Body(name="Star_1",
position=(0, 0),
mass=500,
colour=(255, 255, 186)),
Body(name="Star_2",
position=(-40, 0),
mass=500,
velocity=(0, -4.84),
colour=(255, 255, 186)),
Body(name="Earth",
position=(-250, 0),
mass=10,
velocity=(0, -4.5),
colour=(186, 255, 255)),
Body(name="Moon",
position=(-265, 0),
mass=1,
velocity=(0, -5.39),
colour=(255, 255, 255))
]
self.KeyPressDict = {}
self.init_key_press_dict()
self.focus_point = None
self.focus_bodies = [self.allBodies[0], self.allBodies[1]]
self.set_focus_point()
self.paths_calculated = False
self.isPaused = True
self.details_bar = None
self.create_details_sidebar()
self.is_in_input_mode = False
self.selected_bodies = []
self.selected_input_obj = None
def again(self, space):
self.score = 0
body = Body.simple()
body.coordinates = Vector(space.size[0] / 2, space.size[1] / 2)
body.braking = True
self.player = Player(body)
self.play = True
space.add_unit(self.player)
def test_update(self):
body = Body.simple()
player = Player.create(body)
space = MagicMock()
player.update(space, 10)
self.assertTrue(player.live)
def test_collision(self):
black_hole = BlackHole.create(Body.simple())
other = MagicMock()
space = MagicMock()
black_hole.collision(other, space)
self.assertFalse(other.live)
def _calcReward(self, body: Body) -> float:
return -100 if not body.active else (
2 * abs(body.bones['torso'].linearVelocity.x) +
body.bones['torso'].linearVelocity.x + +0.1 *
(body.getRootPos().y > self.MinHeight) - sum([
joint.GetMotorTorque(1.0 / self.timestep)**2
for joint in body.joints
]) * 0.00001 + 0.1)
def __init__(self):
self.elements = [
Wheel("front left"),
Wheel("front right"),
Wheel("back left"),
Wheel("back right"),
Body(),
Engine()
]
def pickle(self, body):
from Body import Body
if isinstance(body, Body):
return body
handle = body.identify(self)
return Body(handle)
def __init__(self):
#self.b.setMotorConstants(self.Rho, self.K_v, self.K_t, self.K_tau, self.I_M, self.A_swept, self.A_xsec, self.Radius, self.C_D)
self.time = numpy.arange(0,5, self.dt)
self.t = 0
self.gyro = Vector()
self.angle = Vector()
self.quat = Quaternion()
self.stepResponse = [0, 0, 0]
self.waveResponse = [0, 0, 0]
self.flipResponse = [0, 0, 0]
self.moveType = 0
self.moveTimes = [0, 0, 0]
self.b = Body(self)
def test_output1(self):
b = Body("A",2.0,[3.0,5],[-1.0,3])
b.obj_position = [3.0,5]
self.assertEqual(str(b.obj_position),'[3.0, 5]')
def __init__(self, lev, score=0):
Body.__init__(self, lev)
self.__lives = 3
self.__score = score
self.__visited = [0] * 1001
self.__jflag = 0
def __init__(self, lev):
Body.__init__(self, lev)
class Simu(object):
Rho = 1.2250 #kg.m^-3
#K_v = 3000 #rpm.V^-1
K_v = 6.3E-5 #s.V.rad^-1
#K_v = K_v*2*pi/60 #rad.s^-1.V^-1
#K_t = K_v #N.m.A^-1
K_t = 6.3E-3 #N.m.A^-1
K_tau = 0.91
I_M = 104E-6 #
Radius = 0.5 #m
A_swept = pi*pow(Radius,2)
A_xsec = A_swept
C_D = 2
I = Params.I
K_d = 0.0013
time = None
dt = Params.dt
t = None
stepResponse = None
waveResponse = None
flipResponse = None
moveTimes = None
moveType = None
pltpq = DronePlot()
def __init__(self):
#self.b.setMotorConstants(self.Rho, self.K_v, self.K_t, self.K_tau, self.I_M, self.A_swept, self.A_xsec, self.Radius, self.C_D)
self.time = numpy.arange(0,5, self.dt)
self.t = 0
self.gyro = Vector()
self.angle = Vector()
self.quat = Quaternion()
self.stepResponse = [0, 0, 0]
self.waveResponse = [0, 0, 0]
self.flipResponse = [0, 0, 0]
self.moveType = 0
self.moveTimes = [0, 0, 0]
self.b = Body(self)
def initBody(self, local):
self.b.setModel(self.I, self.K_d, Params.L, Params.Mass)
self.b.setParameters(Params.TorqueIsSet, Params.MaxTorque, local)
if local==True:
self.b.initController()
self.b.setMotorConstants(self.Rho, self.K_v, self.K_t, self.K_tau, self.I_M, self.A_swept, self.A_xsec, self.Radius, self.C_D)
def deviate(self):
r = Vector([random(),random(), random()])
deviation = Params.MaxDeviation
self.b.setOmega(-deviation+(2*deviation*r))
def heavyside(self, t, t0):
if (t-t0)<0:
return 0
else:
return 1
def moveStep(self):
self.stepResponse[0] = self.heavyside(self.t, self.moveTimes[0])
self.stepResponse[1] = self.heavyside(self.t, self.moveTimes[1])
self.stepResponse[2] = self.heavyside(self.t, self.moveTimes[2])
return self.stepResponse
def moveWave(self):
self.waveResponse[0] = 0.5*sin(self.t-self.moveTimes[0])*self.heavyside(self.t, self.moveTimes[0])
self.waveResponse[1] = 0.5*sin(self.t-self.moveTimes[1])*self.heavyside(self.t, self.moveTimes[1])
self.waveResponse[2] = 0.5*sin(self.t-self.moveTimes[2])*self.heavyside(self.t, self.moveTimes[2])
return self.waveResponse
def moveFlip(self):
if self.t<self.moveTimes[0]:
t = 0
elif self.t<self.moveTimes[1]:
t = -(self.t-self.moveTimes[0])
elif self.t<self.moveTimes[2]:
t = self.moveTimes[2]-self.t
else:
t = 0
self.flipResponse[0] = t*pi
return self.flipResponse
def setMoveType(self, t):
self.moveType = t
if self.moveType==1:
self.moveTimes[0] = self.t + Params.stepTimes[0]
self.moveTimes[1] = self.t + Params.stepTimes[1]
self.moveTimes[2] = self.t + Params.stepTimes[2]
if self.moveType==2:
self.moveTimes[0] = self.t + Params.waveTimes[0]
self.moveTimes[1] = self.t + Params.waveTimes[1]
self.moveTimes[2] = self.t + Params.waveTimes[2]
if self.moveType==3:
self.moveTimes[0] = self.t + Params.flipTimes[0]
self.moveTimes[1] = self.t + Params.flipTimes[1]
self.moveTimes[2] = self.t + Params.flipTimes[2]
def getNextMove(self):
val = None
if self.moveType==0:
val = [0, 0, 0]
if self.moveType==1:
val = self.moveStep()
if self.moveType==2:
val = self.moveWave()
if self.moveType==3:
val = self.moveFlip()
return val
def nextStep(self):
self.singleStep(self.t)
self.t = self.t+self.dt
def singleStep(self, t):
self.b.nextStep(self.dt)
self.plot(t)
def mainLoop(self):
for t in self.time:
self.singleStep(t)
self.plot()
def plot(self, t):
self.pltpq.addTime(t)
self.pltpq.addQuaternion(self.b.Quat, self.b.ctrl.lastqRef)
self.pltpq.addTheta(self.b.Angles)
self.pltpq.addThetaDot(self.b.Omega)
self.pltpq.addTorque(self.b.Torque, self.b.ctrl.Torque)
self.pltpq.addX(self.b.Position)
self.pltpq.addXDot(self.b.Velocity, self.b.ctrl.lastvRef)
self.pltpq.addA(self.b.Acceleration)
self.pltpq.addMotor(self.b.CtrlInput)
self.pltpq.update()
def getI(self):
return self.I
def getTheta(self):
return self.theta
def getTime(self):
return self.t
def getdTime(self):
return self.dt
def setRequiredTorque(self, t):
self.b.setTorque(t)
def setReference(self, ref, v):
self.b.setReference(ref, v)
def calibration(self):
self.b.calibrate(self.dt)
def __init__(self):
Body.__init__(self, Firm.FORD, Material.STEEL)
def test_gfactor(self):
b1 = Body("A",4.0,[1,1],[1,1])
b2 = Body("B",4.0,[2,1],[2,1])
res =Body.gfactor(b1,b2)
self.assertEqual(res, 0.1496152)
def __init__(self):
Body.__init__(self, Firm.AUDI, Material.CARBON)
