def sine_tens_sim(params):
sim = ntrtx.NTRTX(-9.8 / 1.0)
nodes, links = spr.loads('t_sphere_6mod.spr', scale=0.000005)
sim_nodes = []
sim_links = []
sim_link_modifiers = []
for node in nodes:
sim_node = sim.create_node(node["position"],
node["velocity"],
node["radius"],
color=node["color"],
mass=.005 * 1,
fixed=node["fixed"])
sim.add_node(sim_node)
sim_nodes.append(sim_node)
for i in range(len(links)):
link = links[i]
if link["elasticity"] > 200:
link["elasticity"] = 0 # is rod
link["mass"] = 0.035
else:
link["mass"] = 0.005
sim_link = sim.create_link(sim_nodes[link["nodes"][0]],
sim_nodes[link["nodes"][1]],
radius=link["radius"],
elasticity=link["elasticity"],
rest_length=link["length"],
color=link["color"],
mass=link["mass"],
fixed=link["fixed"])
sim_links.append(sim_link)
sim.add_link(sim_link)
if link["elasticity"] > 0:
modifier = link_modifiers.String(sim_link)
sim_link_modifiers.append(modifier)
rand_ids = range(len(sim_link_modifiers))
random.shuffle(rand_ids)
for i in range(24):
active_link_id = rand_ids[i]
print("active_link_id=", active_link_id)
#sim_link_modifiers.append(link_modifiers.SineString(sim_link_modifiers[active_link_id], 0.3, 1, 0.0, .5))
ampl = 0.0 + 0.1 * (params[i] - 0.5)
freq = 0.5 + 0.1 * (params[i + 1] - 0.5)
phase = 0.0 + 0.1 * (params[i + 2] - 0.5)
offset = 0.4 + 0.1 * (params[i + 3] - 0.5)
sim_link_modifiers[active_link_id] = link_modifiers.SineString(
sim_link_modifiers[active_link_id], ampl, freq, phase, offset)
sim.add_surface(sim.create_surface())
def idle_callback():
for link_modifier in sim_link_modifiers:
link_modifier.modify_link(0.005)
sim.step(.005)
vis = ntrtx_visualizer.ntrtx_visualizer(sim, idle_callback, -0.28)
def __init__(self, gravity, params):
sim = ntrtx.NTRTX(gravity)
nodes, links = spr.loads('t_sphere_6mod.spr', scale=0.000005)
sim_nodes = []
sim_links = []
sim_link_modifiers = []
for i in range(len(nodes)):
node = nodes[i]
sim_node = sim.create_node(str(i),
node["position"],
node["velocity"],
node["radius"],
color=node["color"],
mass=.005 * 1,
fixed=node["fixed"])
sim.add_node(sim_node)
sim_nodes.append(sim_node)
for i in range(len(links)):
link = links[i]
if link["elasticity"] > 200:
link["elasticity"] = 0 # is rod
link["mass"] = 0.035
else:
link["mass"] = 0.005
sim_link = sim.create_link(sim_nodes[link["nodes"][0]],
sim_nodes[link["nodes"][1]],
radius=link["radius"],
elasticity=link["elasticity"],
rest_length=link["length"],
color=link["color"],
mass=link["mass"],
fixed=link["fixed"])
sim_links.append(sim_link)
sim.add_link(sim_link)
if link["elasticity"] > 0:
modifier = link_modifiers.String(sim_link)
sim_link_modifiers.append(modifier)
rand_ids = range(len(sim_link_modifiers))
random.shuffle(rand_ids)
for i in range(24):
active_link_id = rand_ids[i]
print("active_link_id=", active_link_id)
#sim_link_modifiers.append(link_modifiers.SineString(sim_link_modifiers[active_link_id], 0.3, 1, 0.0, .5))
ampl = 0.0 + 0.1 * (params[i] - 0.5)
freq = .25 * 1.5 * params[i + 1]
phase = 0.0 + 1.75 * params[i + 2]
offset = 0.3 + 0.2 * params[i + 3]
sim_link_modifiers[active_link_id] = link_modifiers.SineString(
sim_link_modifiers[active_link_id], ampl, freq, phase, offset)
sim.add_surface(sim.create_surface())
self.sim = sim
self.sim_link_modifiers = sim_link_modifiers
self.nodes = sim_nodes
self.links = sim_links
self.initial_pos = self.get_position()
def test():
import ntrtx, spr, link_modifiers
sim = ntrtx.NTRTX(-9.8 / 6)
#nodes, links = spr.loads('spring_platform.spr', scale = 0.00001)
#nodes, links = spr.loads('t_sphere_6mod.spr', scale = 0.000005)
nodes, links = spr.loads('t_sphere_6payload.spr', scale=0.000005)
#nodes, links = spr.loads('t_sphere_32.spr', scale = 0.00001)
sim_nodes = []
sim_links = []
sim_link_modifiers = []
for node in nodes:
sim_node = sim.create_node(node["position"],
node["velocity"],
node["radius"],
color=node["color"],
mass=.005,
fixed=node["fixed"])
sim.add_node(sim_node)
sim_nodes.append(sim_node)
for i in range(len(links)):
link = links[i]
if link["elasticity"] > 200:
link["elasticity"] = 0 # is rod
link["mass"] = 0.035
else:
link["mass"] = 0.00005
sim_link = sim.create_link(sim_nodes[link["nodes"][0]],
sim_nodes[link["nodes"][1]],
radius=link["radius"],
elasticity=link["elasticity"],
rest_length=link["length"],
color=link["color"],
mass=link["mass"],
fixed=link["fixed"])
sim_links.append(sim_link)
sim.add_link(sim_link)
if link["elasticity"] > 0:
modifier = link_modifiers.String(sim_link)
#sim_link_modifiers.append(link_modifiers.SurfaceString(modifier))
sim_link_modifiers.append(modifier)
#sim.add_link_modifier(link_modifiers.SurfaceString(modifier))
# if i == 23:
# sim.add_link_modifier(link_modifiers.SineString(modifier))
# else:
# sim.add_link_modifier(modifier)
#
# if link["nodes"][0] == 0:
# motor_link = modifier
# modifier.set_string_force(100.0)
sim_link_modifiers.append(link_modifiers.RocketRod(sim_links[42]))
sim_link_modifiers[24].log = True
sim.add_surface(sim.create_surface())
# for i in range(10):
# sim.step(.04)
motor_id = 21
sim_links[motor_id].rest_length_delta = 0.01
def idle_callback(vis):
vis.origin = sim_nodes[10].body.getPosition()
for link_modifier in sim_link_modifiers:
link_modifier.modify_link()
#sim_nodes[19].add_force((0, 1.5, 0))
# pos1 = sim_nodes[12].body.getPosition()
# pos2 = sim_nodes[19].body.getPosition()
# dpos = [19 * (pos2[i] - pos1[i]) for i in range(len(pos1))]
# sim_nodes[19].add_force(dpos)
# print(dpos)
pass
# sim_links[motor_id].rest_length += sim_links[motor_id].rest_length_delta
# if sim_links[motor_id].rest_length < .05:
# sim_links[motor_id].rest_length_delta = 0.005
# if sim_links[motor_id].rest_length > .5:
# sim_links[motor_id].rest_length_delta = -0.005
#print("sim_links[1].rest_length", sim_links[23].rest_length)
#motor_link.set_string_force(50.0)
vis = ntrtx_visualizer.ntrtx_visualizer(sim, idle_callback, -0.28)
def test():
import ntrtx, spr, link_modifiers
import math, random
sim = ntrtx.NTRTX(-9.8 / 1.0)
#nodes, links = spr.loads('spring_platform.spr', scale = 0.00001)
nodes, links = spr.loads('t_sphere_6mod.spr', scale=0.000005)
#nodes, links = spr.loads('t_sphere_6payload.spr', scale = 0.000005)
#nodes, links = spr.loads('t_sphere_32.spr', scale = 0.000005)
sim_nodes = []
sim_links = []
sim_link_modifiers = []
for node in nodes:
sim_node = sim.create_node(node["position"],
node["velocity"],
node["radius"],
color=node["color"],
mass=.005 * 1,
fixed=node["fixed"])
sim.add_node(sim_node)
sim_nodes.append(sim_node)
for i in range(len(links)):
link = links[i]
if link["elasticity"] > 200:
link["elasticity"] = 0 # is rod
link["mass"] = 0.035
else:
link["mass"] = 0.005
sim_link = sim.create_link(sim_nodes[link["nodes"][0]],
sim_nodes[link["nodes"][1]],
radius=link["radius"],
elasticity=link["elasticity"],
rest_length=link["length"],
color=link["color"],
mass=link["mass"],
fixed=link["fixed"])
sim_links.append(sim_link)
sim.add_link(sim_link)
if link["elasticity"] > 0:
modifier = link_modifiers.String(sim_link)
sim_link_modifiers.append(modifier)
for i in range(3):
active_link_id = random.randrange(20)
print("active_link_id=", active_link_id)
sim_link_modifiers.append(
link_modifiers.SineString(sim_link_modifiers[active_link_id], 0.3,
1, 0.0, .5))
# sim_link_modifiers.append(link_modifiers.SineString(sim_link_modifiers[20], 0.3, 1, 0.0, .5))
# sim_link_modifiers[20].log = True
#
# sim_link_modifiers.append(link_modifiers.SineString(sim_link_modifiers[4], 0.3, 1, 0.0, .5))
# sim_link_modifiers.append(link_modifiers.SineString(sim_link_modifiers[12], 0.3, 1, 0.0, .5))
sine_modifiers = {
sim_link_modifiers[-1], sim_link_modifiers[-2], sim_link_modifiers[-3]
}
#sine_modifiers = {sim_link_modifiers[-1], sim_link_modifiers[-2]}
sim.add_surface(sim.create_surface())
sim_time = [0]
def get_ave_pos():
ave_pos = [0, 0, 0]
for i in range(len(sim_nodes)):
for j in range(len(ave_pos)):
ave_pos[j] += sim_nodes[i].body.getPosition()[j]
ave_pos = [x / len(sim_nodes) for x in ave_pos]
return ave_pos
def idle_callback():
for link_modifier in sim_link_modifiers:
if link_modifier in sine_modifiers:
link_modifier.modify_link(0.005)
else:
link_modifier.modify_link()
sim.step(.005)
#sim_time[0] += 0.005
#print("torque=", [node.body.getTorque() for node in sim_links[25].nodes])
#sim_nodes[19].add_force((0, 1.5, 0))
# pos1 = sim_nodes[12].body.getPosition()
# pos2 = sim_nodes[19].body.getPosition()
# dpos = [19 * (pos2[i] - pos1[i]) for i in range(len(pos1))]
# sim_nodes[19].add_force(dpos)
# print(dpos)
pass
# sim_links[motor_id].rest_length += sim_links[motor_id].rest_length_delta
# if sim_links[motor_id].rest_length < .05:
# sim_links[motor_id].rest_length_delta = 0.005
# if sim_links[motor_id].rest_length > .5:
# sim_links[motor_id].rest_length_delta = -0.005
#print("sim_links[1].rest_length", sim_links[23].rest_length)
#motor_link.set_string_force(50.0)
vis = ntrtx_visualizer.ntrtx_visualizer(sim, idle_callback, -0.28)
initial_pos = get_ave_pos()
max_dist = 0.0
for i in range(500):
idle_callback()
pos = get_ave_pos()
dist = math.sqrt(sum([(pos[i] - initial_pos[i])**2 for i in range(3)]))
print("ave_pos=", list(get_ave_pos()))
print("dist=", dist)
if dist > max_dist:
max_dist = dist
return max_dist