How to implement a lane change manoeuver on Carla - python

I am trying to implement a simple lane change manoeuver for a self driving car on Carla simulator. Specifically a left lane change.
However when retrieving waypoints from the left lane ( using carla.Waypoint.get_left_lane() function ), the waypoints I get are oscillating between left and right lanes. Below is the script I used :
import sys
import glob
import os
try:
sys.path.append(glob.glob('../../carla/dist/carla-*%d.%d-%s.egg' % (
sys.version_info.major,
sys.version_info.minor,
'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0])
except IndexError:
pass
import carla
import time
import math
import numpy as np
import random
from agents.navigation.controller import VehiclePIDController
dt = 1.0 / 20.0
args_lateral_dict = {'K_P': 1.95, 'K_I': 0.05, 'K_D': 0.2, 'dt': dt}
args_longitudinal_dict = {'K_P': 1.0, 'K_I': 0.05, 'K_D': 0, 'dt': dt}
max_throt = 0.75
max_brake = 0.3
max_steer = 0.8
offset = 0
VEHICLE_VEL = 5
actorList = []
try:
client = carla.Client("localhost",2000)
client.set_timeout(10.0)
world = client.load_world("Town02")
spectator = world.get_spectator()
actorList.append(spectator)
settings = world.get_settings()
settings.synchronous_mode = True
settings.fixed_delta_seconds = 1/20
world.apply_settings(settings)
blueprint_library = world.get_blueprint_library()
vehicle_bp = blueprint_library.filter("cybertruck")[0]
vehicle = None
while(vehicle is None):
spawn_points = world.get_map().get_spawn_points()
spawn_point = random.choice(spawn_points) if spawn_points else carla.Transform()
vehicle = world.try_spawn_actor(vehicle_bp, spawn_point)
actorList.append(vehicle)
vehicle_controller = VehiclePIDController(vehicle,
args_lateral=args_lateral_dict,
args_longitudinal=args_longitudinal_dict,
offset=offset,
max_throttle=max_throt,
max_brake=max_brake,
max_steering=max_steer)
old_yaw = math.radians(vehicle.get_transform().rotation.yaw)
old_x = vehicle.get_transform().location.x
old_y = vehicle.get_transform().location.y
```
spectator_transform = vehicle.get_transform()
spectator_transform.location += carla.Location(x = 10, y= 0, z = 5.0)
control = carla.VehicleControl()
control.throttle = 1.0
vehicle.apply_control(control)
while True:
####### Im suspecting the bug is within this portion of code ########################
current_waypoint = world.get_map().get_waypoint(vehicle.get_location())
# if not turned_left :
left_waypoint = current_waypoint.get_left_lane()
if(left_waypoint is not None and left_waypoint.lane_type == carla.LaneType.Driving) :
target_waypoint = left_waypoint.previous(0.3)[0]
turned_left = True
else :
if(turned_left) :
target_waypoint = waypoint.previous(0.3)[0]
else : # I tryed commenting this else section but the bug is still present so I dont suspect the bug relates to this else part
target_waypoint = waypoint.next(0.3)[0]
################### End of suspected bug ############################################
world.debug.draw_string(target_waypoint.transform.location, 'O', draw_shadow=False,
color=carla.Color(r=255, g=0, b=0), life_time=120.0,
persistent_lines=True)
control_signal = vehicle_controller.run_step(VEHICLE_VEL,target_waypoint)
vehicle.apply_control(control_signal)
new_yaw = math.radians(vehicle.get_transform().rotation.yaw)
spectator_transform = vehicle.get_transform()
spectator_transform.location += carla.Location(x = -10*math.cos(new_yaw), y= -10*math.sin(new_yaw), z = 5.0)
spectator.set_transform(spectator_transform)
world.tick()
finally:
print("Destroying actors")
client.apply_batch([carla.command.DestroyActor(x) for x in actorList])

I just figured out the cause of the problem. The reason is that I was manipulating Carla waypoints as undirected points. However, in Carla, each waypoint is directed by the road direction.
In the scene, I was testing my code in, all the roads have two lanes, each one in an opposite direction. Hence, the left lane of each lane is the remaining lane.
The issue in the previous code was that I was not changing my view to match the direction of the lane. I was assuming a global reference frame, but in Carla, the waypoints are relative to the frames attached to their respective lanes. And since only one of the two coordinate frames (for each lane) was matching my imagined global reference frame, I was getting an oscillatory behavior.
Another issue was that I was updating the target waypoints to track too early. This caused the vehicle to move a very short distance without going through all the target waypoints. I changed this to keep tracking the same target waypoint until the distance separating it to my vehicle becomes too close before moving to the next waypoint. This helped to perform the lane change behavior.
Below is the script I used :
import sys
import glob
import os
#The added path depends on where the carla binaries are stored
try:
sys.path.append(glob.glob('../../carla/dist/carla-*%d.%d-%s.egg' % (
sys.version_info.major,
sys.version_info.minor,
'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0])
except IndexError:
pass
import carla
import time
import math
import numpy as np
import random
from agents.navigation.controller import VehiclePIDController
VEHICLE_VEL = 5
class Player():
def __init__(self, world, bp, vel_ref = VEHICLE_VEL, max_throt = 0.75, max_brake = 0.3, max_steer = 0.8):
self.world = world
self.max_throt = max_throt
self.max_brake = max_brake
self.max_steer = max_steer
self.vehicle = None
self.bp = bp
while(self.vehicle is None):
spawn_points = world.get_map().get_spawn_points()
spawn_point = random.choice(spawn_points) if spawn_points else carla.Transform()
self.vehicle = world.try_spawn_actor(vehicle_bp, spawn_point)
self.spectator = world.get_spectator()
dt = 1.0 / 20.0
args_lateral_dict = {'K_P': 1.95, 'K_I': 0.05, 'K_D': 0.2, 'dt': dt}
args_longitudinal_dict = {'K_P': 1.0, 'K_I': 0.05, 'K_D': 0, 'dt': dt}
offset = 0
self.controller = VehiclePIDController(self.vehicle,
args_lateral=args_lateral_dict,
args_longitudinal=args_longitudinal_dict,
offset=offset,
max_throttle=max_throt,
max_brake=max_brake,
max_steering=max_steer)
self.vel_ref = vel_ref
self.waypointsList = []
self.current_pos = self.vehicle.get_transform().location
self.past_pos = self.vehicle.get_transform().location
def dist2Waypoint(self, waypoint):
vehicle_transform = self.vehicle.get_transform()
vehicle_x = vehicle_transform.location.x
vehicle_y = vehicle_transform.location.y
waypoint_x = waypoint.transform.location.x
waypoint_y = waypoint.transform.location.y
return math.sqrt((vehicle_x - waypoint_x)**2 + (vehicle_y - waypoint_y)**2)
def go2Waypoint(self, waypoint, draw_waypoint = True, threshold = 0.3):
if draw_waypoint :
# print(" I draw")
self.world.debug.draw_string(waypoint.transform.location, 'O', draw_shadow=False,
color=carla.Color(r=255, g=0, b=0), life_time=10.0,
persistent_lines=True)
current_pos_np = np.array([self.current_pos.x,self.current_pos.y])
past_pos_np = np.array([self.past_pos.x,self.past_pos.y])
waypoint_np = np.array([waypoint.transform.location.x,waypoint.transform.location.y])
vec2wp = waypoint_np - current_pos_np
motion_vec = current_pos_np - past_pos_np
dot = np.dot(vec2wp,motion_vec)
if (dot >=0):
while(self.dist2Waypoint(waypoint) > threshold) :
control_signal = self.controller.run_step(self.vel_ref,waypoint)
self.vehicle.apply_control(control_signal)
self.update_spectator()
def getLeftLaneWaypoints(self, offset = 2*VEHICLE_VEL, separation = 0.3):
current_waypoint = self.world.get_map().get_waypoint(self.vehicle.get_location())
left_lane = current_waypoint.get_left_lane()
self.waypointsList = left_lane.previous(offset)[0].previous_until_lane_start(separation)
def getRightLaneWaypoints(self, offset = 2*VEHICLE_VEL, separation = 0.3):
current_waypoint = self.world.get_map().get_waypoint(self.vehicle.get_location())
right_lane = current_waypoint.get_left_lane()
self.waypointsList = right_lane.next(offset)[0].next_until_lane_end(separation)
def do_left_lane_change(self):
self.getLeftLaneWaypoints()
for i in range(len(self.waypointsList)-1):
self.current_pos = self.vehicle.get_location()
self.go2Waypoint(self.waypointsList[i])
self.past_pos = self.current_pos
self.update_spectator()
def do_right_lane_change(self):
self.getRightLaneWaypoints()
for i in range(len(self.waypointsList)-1)
self.current_pos = self.vehicle.get_location()
self.go2Waypoint(self.waypointsList[i])
self.past_pos = self.current_pos
self.update_spectator()
def update_spectator(self):
new_yaw = math.radians(self.vehicle.get_transform().rotation.yaw)
spectator_transform = self.vehicle.get_transform()
spectator_transform.location += carla.Location(x = -10*math.cos(new_yaw), y= -10*math.sin(new_yaw), z = 5.0)
self.spectator.set_transform(spectator_transform)
self.world.tick()
def is_waypoint_in_direction_of_motion(self,waypoint):
current_pos = self.vehicle.get_location()
def draw_waypoints(self):
for waypoint in self.waypointsList:
self.world.debug.draw_string(waypoint.transform.location, 'O', draw_shadow=False,
color=carla.Color(r=255, g=0, b=0), life_time=10.0,
persistent_lines=True)
dt = 1.0 / 20.0
args_lateral_dict = {'K_P': 1.95, 'K_I': 0.05, 'K_D': 0.2, 'dt': dt}
args_longitudinal_dict = {'K_P': 1.0, 'K_I': 0.05, 'K_D': 0, 'dt': dt}
offset = 0
VEHICLE_VEL = 10
actorList = []
try:
client = carla.Client("localhost",2000)
client.set_timeout(10.0)
world = client.load_world("Town02")
spectator = world.get_spectator()
actorList.append(spectator)
settings = world.get_settings()
settings.synchronous_mode = True
settings.fixed_delta_seconds = 1/20
world.apply_settings(settings)
blueprint_library = world.get_blueprint_library()
vehicle_bp = blueprint_library.filter("cybertruck")[0]
player = Player(world, vehicle_bp)
actorList.append(player.vehicle)
actorList.append(player.spectator)
while(True):
player.update_spectator()
manoeuver = input("Enter manoeuver: ")
if ( manoeuver == "l"): #Perform left lane change
player.do_left_lane_change()
elif (manoeuver == "r"): #Perform right lane change
player.do_right_lane_change()
elif (manoeuver == "d"): #Just draws the waypoints for debugging purpose
player.getLeftLaneWaypoints()
player.draw_waypoints()
finally:
print("Destroying actors")
client.apply_batch([carla.command.DestroyActor(x) for x in actorList])

Related

Why does translation matrix not affect rendering of 3d model in python?

I am very new to coding and decided to start with a 3D engine in python. I followed OLC's tutorial and adapted it for python. However, my translation matrix doesn't seem to be affecting how the model is rendered. Any advice at all, from code improvements, to telling me how bad I am would be much appreciated.
I have tried to re-do the vector multiplication in all sorts of ways, and all kinds of reformatting and re-trying, but I am not even sure what I am looking for in this case. Any help would be much appreciated
As I am new, I apologize for any poor etiquette I may have done on this site.
import math, numpy as np, pygame, sys, os, random, string
from pygame.locals import *
pygame.init()
width,height = 1600,900;cx,cy = width//2,height//2
screen = pygame.display.set_mode((width,height))
clock = pygame.time.Clock()
font = pygame.font.SysFont("Arial", 18)
pygame.display.set_caption('3D Graphics')
def loadObj(filename):
tris = []
verts = []
try:
fp = open(filename, "r")
except:
print("File: "+filename+" not found")
sys.exit(1)
for line in fp:
if line.startswith('#'): continue
values = line.split()
if not values: continue
if values[0] == 'v':
#v = vec3(float(values[1]), float(values[2]), float(values[3]))
#verts.append(vec3(int(values[1]), int(values[2]), int(values[3])))
verts.append(vec3(float(values[1]), float(values[2]), float(values[3]), 0 ))
#verts.append(v)
elif values[0] == 'f':
p = []
for v in values[1:]:
w = v.split("/")
p.append(int(w[0]))
#print(p)
#print(verts[-185])
triTemp = triangle(verts[p[0] - 1], verts[p[1] - 1], verts[p[2]- 1])
tris.append(triTemp)
fp.close()
return tris
class vec3(): #Possibly Obsolete
__slots__ = ['x','y','z','w']
def __init__(self, x, y, z, w):
self.x = x
self.y = y
self.z = z
self.w = w
class triangle():
__slots__ = ['vec1','vec2','vec3']
def __init__(self, vec1, vec2, vec3):
self.vec1 = vec1
self.vec2 = vec2
self.vec3 = vec3
def matrixMakeTranslation(x, y, z):
matrix = np.zeros((4,4))
matrix[0,0] = 1.0
matrix[1,1] = 1.0
matrix[2,2] = 1.0
matrix[3,3] = 1.0
matrix[3,0] = x
matrix[3,1] = y
matrix[3,2] = z
return matrix
def Matrix_MakeRotationX(fAngleRad):
matrix = np.zeros((4,4))
matrix[0,0] = 1.0
matrix[1,1] = (math.cos(fAngleRad * 0.5))
matrix[1,2] = (math.sin(fAngleRad * 0.5))
matrix[2,1] = (-math.sin(fAngleRad * 0.5))
matrix[2,2] = (math.cos(fAngleRad * 0.5))
matrix[3,3] = 1.0
return matrix
def Matrix_MakeRotationZ(fAngleRad):
matrix = np.zeros((4,4))
matrix[0,0] = (math.cos(fAngleRad))
matrix[0,1] = (math.sin(fAngleRad))
matrix[1,0] = (-math.sin(fAngleRad))
matrix[1,1] = (math.cos(fAngleRad))
matrix[2,2] = 1.0
matrix[3,3] = 1.0
return matrix
fNear = float(0.1) #Create the Projection Matrix
fFar = float(1000.0)
fFov = float(90.0)
fAspectRatio = float(height/width)
fFovRad = 1/math.tan(fFov * 0.5 / 180 * math.pi)
projectionMatrix = np.zeros((4,4))
projectionMatrix[0,0] = fAspectRatio * fFovRad
projectionMatrix[1,1] = fFovRad
projectionMatrix[2,2] = fFar / (fFar - fNear)
projectionMatrix[3,2] = float((-fFar * fNear) / (fFar - fNear))
projectionMatrix[2,3] = 1.0
projectionMatrix[3,3] = 0.0
meshname = "teapot.obj" #Load the mesh
tris = loadObj(meshname)
vCamera = np.array([0,0,0,0])
fAngleRad = 0
colour = (255,255,255)
colour2 = (0,0,0)
triProjected = triangle(np.array([0,0,0,0]),np.array([0,0,0,0]),np.array([0,0,0,0])) #These are used later
triTranslalted = triangle(np.array([0,0,0,0]),np.array([0,0,0,0]),np.array([0,0,0,0]))
triTransformed= triangle(np.array([0,0,0,0]),np.array([0,0,0,0]),np.array([0,0,0,0]))
while True: #Begin Loop
for event in pygame.event.get(): #Quit
if event.type == pygame.QUIT: pygame.quit(); sys.exit()
dt = clock.tick()/1000
pygame.display.set_caption('3D Graphics - FPS: %.2f'%int(dt))
print("fps:", clock.get_fps()) #Framerate and caption
pygame.display.update()
screen.fill((0,0,0))
fAngleRad += 0.1
matRotZ = Matrix_MakeRotationZ(fAngleRad * 0.5) #Set up matricies
matRotX = Matrix_MakeRotationX(fAngleRad)
matTrans = matrixMakeTranslation(0.0,0.0,50.0)
matWorld = np.identity(4)
matWorld = matRotZ # matRotX
matWorld = matWorld # matTrans #Seems to be broken. idk why.
for i in tris: #For triangle in all triangles
reDo1 = np.array([i.vec1.x, i.vec1.y, i.vec1.z, i.vec1.w])
reDo2 = np.array([i.vec2.x, i.vec2.y, i.vec2.z, i.vec2.w])
reDo3 = np.array([i.vec3.x, i.vec3.y, i.vec3.z, i.vec3.w])
triTransformed.vec1 = np.matmul(matWorld, reDo1)
triTransformed.vec2 = np.matmul(matWorld, reDo2)
triTransformed.vec3 = np.matmul(matWorld, reDo3)
triProjected.vec1 = np.matmul(projectionMatrix, triTransformed.vec1)
triProjected.vec2 = np.matmul(projectionMatrix, triTransformed.vec2)
triProjected.vec3 = np.matmul(projectionMatrix, triTransformed.vec3)
#Scale Into View
triProjected.vec1[0] += 1.0
triProjected.vec1[1] += 1.0
triProjected.vec2[0] += 1.0
triProjected.vec2[1] += 1.0
triProjected.vec3[0] += 1.0
triProjected.vec3[1] += 1.0
triProjected.vec1[0] *= 0.5 * width
triProjected.vec1[1] *= 0.5 * height
triProjected.vec2[0] *= 0.5 * width
triProjected.vec2[1] *= 0.5 * height
triProjected.vec3[0] *= 0.5 * width
triProjected.vec3[1] *= 0.5 * height
pygame.draw.polygon(screen, colour, [(triProjected.vec1[0], triProjected.vec1[1]),(triProjected.vec2[0], triProjected.vec2[1]),(triProjected.vec3[0], triProjected.vec3[1])])
You are using Homogenious Coordinates to represent the vertices of your model. So the W component must be 1.
When you load your model you are setting W to 0.
verts.append(vec3(float(values[1]), float(values[2]), float(values[3]), 0 ))
By setting W=0 you are creating a Homogenious Vector (a.k.a "point at infinity" or "ideal point") and by setting W=1 you are creating a Homogeneous Point.
Points can be translated but vectors cannot.
https://en.m.wikipedia.org/wiki/Homogeneous_coordinates

Using keyboard to move a ball, X and Z axis, in VPython

Doing a project for physics class. vPython documentation is not very good I think and it's a bit confusing. I need to move a ball, which currently is being moved by mouse, but I need to make this work with 2 players, so I need to use keyboard events.
Currently this is the code:
#GlowScript 2.7 VPython
scene.range = 30
scene.forward = vec(0,-0.25,-1)
#red side of the field
redField = box()
redField.size = vec(40,1,60)
redField.pos= vec(-20,0,0)
redField.color = color.red
#blue side of the field
redField = box()
redField.size = vec(40,1,60)
redField.pos= vec(20,0,0)
redField.color = color.blue
cueBall = sphere()
cueBall.radius = 1.5
cueBall.pos = vec(0,2,0)
cueBall.vel = vec(3,0,0) # velocity in m/s
cueBall.mass = 2.0
blueBall = sphere()
blueBall.radius = 1.5
blueBall.pos = vec(4,2,0)
blueBall.color = color.blue
blueBall.mass = 1.5
blueBall.vel = vec(0,0,0)
blueBall.force = arrow(axis = vec(0,0,0), color = color.blue)
rail1 = box()
rail1.texture = {'file': "https://i.imgur.com/ss5OcnL.jpg"}
rail1.size = vec(3,3,55)
rail1.pos = vec(-39,1,0)
rail2 = box()
rail2.texture = {'file': "https://i.imgur.com/ss5OcnL.jpg"}
rail2.size = vec(3,3,55)
rail2.pos = vec(39,1,0)
rail3 = box()
rail3.texture = {'file': "https://i.imgur.com/ss5OcnL.jpg"}
rail3.size = vec(80,3,3)
rail3.pos = vec(0,1,29)
rail4 = box()
rail4.texture = {'file': "https://i.imgur.com/ss5OcnL.jpg"}
rail4.size = vec(80,3,3)
rail4.pos = vec(0,1,-29)
redBall = sphere()
redBall.radius = 1.5
redBall.pos = vec(-3,2,0)
redBall.color = color.red
redBall.force = arrow(axis = vec(0,0,0), color = color.red)
redBall.vel = vec(0,0,0)
redBall.mass = 1.5
yBall = sphere()
yBall.radius = 1.5
yBall.pos = vec(-9,2,0)
yBall.color = color.yellow
yBall.vel = vec(0,0,0)
yBall.mass = 1.5
yBall.force = arrow(axis = vec(0,0,0), color = color.yellow)
rail = []
rail.append(rail1, rail2, rail3, rail4)
ball = []
ball.append(cueBall, blueBall, redBall, yBall)
ball[0].force = arrow(axis = vec(0,0,0), color = color.white)
drag = False
chosenObject = None
scene.bind("mousedown", down)
scene.bind("mousemove", move)
scene.bind("mouseup", up)
def down():
nonlocal drag, chosenObject
chosenObject = scene.mouse.pick()
drag = True
def move():
nonlocal drag, chosenObject
if (drag == True):
chosenObject.force.axis = scene.mouse.pos - chosenObject.pos
chosenObject.force.pos = chosenObject.pos
chosenObject.force.axis.y = 0
def up():
nonlocal drag, chosenObject
chosenObject.force.axis = vec(0,0,0)
chosenObject = None
drag = False
elasticConst = 500
dt = 0.01
time = 0
while (True):
rate(1/dt)
for i in range(len(ball)):
ball[i].vel = ball[i].vel + ( ball[i].force.axis * dt / ball[i].mass )
ball[i].force.pos = ball[i].pos
for i in range(len(ball)):
for j in range(len(ball)):
if (i == j): continue
separation = ball[i].pos - ball[j].pos
contactSeparation = separation.norm() * (ball[i].radius + ball[j].radius)
if (separation.mag < contactSeparation.mag):
elasticForce = - elasticConst * (separation - contactSeparation)
ball[i].vel = ball[i].vel + (elasticForce / ball[i].mass) * dt
for i in range(len(ball)):
if ( ball[i].pos.x < rail1.pos.x + 2):
ball[i].pos.x = rail1.pos.x + 2
ball[i].vel.x = - ball[i].vel.x
if ( ball[i].pos.x > rail2.pos.x - 2):
ball[i].pos.x = rail2.pos.x - 2
ball[i].vel.x = - ball[i].vel.x
if ( ball[i].pos.z > rail3.pos.z - 2):
ball[i].pos.z = rail3.pos.z - 2
ball[i].vel.z = - ball[i].vel.z
if ( ball[i].pos.z < rail4.pos.z + 2):
ball[i].pos.z = rail4.pos.z + 2
ball[i].vel.z = - ball[i].vel.z
## for i in range(len(ball)):
# for j in range(len(hole)):
# if ((ball[i].pos - hole[j].pos).mag < ball[i].radius + hole[j].radius):
# ball[i].visible = False
for i in range(len(ball)):
ball[i].pos = ball[i].pos + ball[i].vel * dt
If you could help me understand how I could increment the position variables on keyboard.

braidTool_Maya_script_error # NameError: name 'QtGui' is not defined

I found someone else's braid tool online and it seems to be working for other people but it doesn't for me. I can't reach that person for help so I am posting here instead.
There's 2 scripts, one is the actual making of the braid and the other is for loading UI. I keep getting The following error:
# Error: Traceback (most recent call last):
# File "<maya console>", line 2, in <module>
# File "/home/ykim/private/maya/2018/scripts/JUM/scripts/braid.py", line 12, in <module>
# list_form, list_base = load_ui_type(ui_file)
# File "/home/ykim/private/maya/2018/scripts/JUM/core/loadUIFile.py", line 30, in load_ui_type
# form_class = frame['Ui_{0}'.format(form_class)]
# File "<string>", line 1, in <module>
# NameError: name 'QtGui' is not defined
#
The following is the 1st script making the braid
import os
from JUM.core.loadUIFile import get_maya_window, load_ui_type
import random
import maya.cmds as cmds
ui_file = os.path.join(os.path.dirname(os.path.realpath(__file__)), 'UI','braid.ui')
list_form, list_base = load_ui_type(ui_file)
class espiral(list_form, list_base):
def __init__(self, parent = get_maya_window()):
self.__radius = 0.0
self.__side = 6
self.__variation = 0
self.__circle = ''
self.__pt_position_A = []
self.__pt_position_B = []
self.__pt_position_C = []
self.__path = ''
self.__quantidade = 0.0
self.clock = True
##########################################################
# #
# UI elements #
# #
##########################################################
self.window_name = 'makeSpiralWin'
if cmds.window(self.window_name, exists = True):
cmds.deleteUI(self.window_name)
super(espiral, self).__init__(parent)
self.setupUi(self)
self.btn_selectPath.clicked.connect(self.getPath)
self.btn_create.setDisabled(True)
self.btn_create.clicked.connect(self.create)
def __makeEight(self):
side = 16
offset = self.spin_offset.value()
eight = cmds.circle(nr=(0, 1, 0), c=(0, 0, 0), degree=3, sections=side)
cmds.select(eight[0])
lattice = cmds.lattice(dv = (3, 2, 3), objectCentered = True )
cmds.setAttr(lattice[0]+'.local', 0)
cmds.select(lattice[1]+'.pt[2][0:1][0]',lattice[1]+'.pt[2][0:1][1]',lattice[1]+'.pt[2][0:1][2]')
cmds.scale(1, 1, -1, pivot = (1.108194, 0 , 0), relative = True)
cmds.select(lattice[1]+'.pt[1][0:1][0]',lattice[1]+'.pt[1][0:1][1]',lattice[1]+'.pt[1][0:1][2]')
cmds.scale(0, 0, 0, pivot = (0, 0 , 0), relative = True)
cmds.select(lattice[1]+'.pt[0][0:1][0]',lattice[1]+'.pt[2][0:1][2]',lattice[1]+'.pt[2][0:1][0]',lattice[1]+'.pt[0][0:1][2]')
cmds.scale(1, 1, 1.455556, pivot = (0, 0 , 0), relative = True)
cmds.scale(0.929167, 1, 1, pivot = (0, 0 , 0), relative = True)
cmds.select(eight[0])
cmds.delete(ch = True)
cmds.rotate(0,offset,0,eight[0])
cmds.makeIdentity(eight[0],apply = True, t = True, r = True, s = True, n = 0, pn = True)
return eight
def __next(self, porcentagem,eight,scale):
#print porcentagem
curva = self.ln_path.text()
position = cmds.pointOnCurve(curva, top=True, pr=porcentagem, position=True)
tangent = cmds.pointOnCurve(curva, top=True, pr=porcentagem, tangent=True)
angle = cmds.angleBetween(er=True, v1=(0.0, 1.0, 0.0), v2=tangent)
cmds.scale((scale * random.uniform((1-self.spin_random.value()), 1.0)),
(scale * random.uniform((1-self.spin_random.value()), 1.0)),
(scale * random.uniform((1-self.spin_random.value()), 1.0)),
eight[0])
cmds.move(position[0],
position[1],
position[2],
eight[0])
cmds.rotate(angle[0],
angle[1],
angle[2],
eight[0])
def __voltas(self):
steps = 16 * float(self.spin_loops.value())
porcent = 1.0 / steps
return int(steps), porcent
def __makeMesh(self,curva):
scale_0 = self.spin_radius.value()
scale_1 = self.spin_radius_1.value()
scale = self.spin_radius.value()
if (scale_0 >= scale_1):
tempMaior = scale_0
tempMenor = scale_1
else:
tempMaior = scale_1
tempMenor = scale_0
scale_extrude = tempMenor/tempMaior
position = cmds.pointOnCurve(curva, top=True, pr=0, position=True)
tangent = cmds.pointOnCurve(curva, top=True, pr=0, normalizedTangent=True)
angle = cmds.angleBetween(er=True, v1=(0.0, 1.0, 0.0), v2=tangent)
circle = cmds.circle(nr=(0, 1, 0), c=(0, 0, 0), degree=3, sections=16, r = 0.5)
cmds.scale(tempMaior,
tempMaior,
tempMaior,
circle[0])
cmds.move(position[0],
position[1],
position[2],
circle[0])
cmds.rotate(angle[0],
angle[1],
angle[2],
circle[0])
extrude = cmds.extrude(circle[0],
curva,
constructionHistory = True,
range = True,
polygon = 0,
extrudeType = 2,
useComponentPivot = 0,
fixedPath = 0,
useProfileNormal = 1,
rotation = 0,
scale = scale_extrude,
reverseSurfaceIfPathReversed = 1)
poly = cmds.nurbsToPoly(extrude[0], matchNormalDir = True, constructionHistory = False, format = 2, polygonType = 1, uType = 3, uNumber = 1, vType = 3, vNumber = 3)
cmds.delete(circle, extrude[0])
print poly
return poly
def __curve(self):
curve_A = cmds.curve(p=self.__pt_position_A)
curve_B = cmds.curve(p=self.__pt_position_B)
curve_C = cmds.curve(p=self.__pt_position_C)
if (self.btn_makeMesh.isChecked()):
mesh_A = self.__makeMesh(curve_A)
mesh_B = self.__makeMesh(curve_B)
mesh_C = self.__makeMesh(curve_C)
cmds.delete(curve_A, curve_B, curve_C)
cmds.select(mesh_A,mesh_B,mesh_C)
else:
cmds.select(curve_A,curve_B,curve_C)
def __braid(self):
steps, porcent = self.__voltas()
increment = porcent
eight = self.__makeEight()
list = range(int(steps))
offset = self.spin_offset.value()
offset_normalize = offset/360.0
self.progress_Create.setRange(0,len(list))
scale_0 = self.spin_radius.value()
scale_1 = self.spin_radius_1.value()
if (scale_0 >= scale_1):
scale_maior = scale_0
scale_menor = scale_1
else:
scale_maior = scale_1
scale_menor = scale_0
diference = scale_maior-scale_menor
percent = diference/steps
scale = scale_maior
if (self.btn_reverse.isChecked()):
curva = self.ln_path.text()
cmds.reverseCurve(curva,ch = False, replaceOriginal = True)
for i in list:
self.progress_Create.setValue(i)
self.__next(porcent,eight,scale)
porcent += increment
_pos_A = (i*0.0625)%1.0 + offset_normalize
_pos_B = (i*0.0625+0.333333)%1.0 + offset_normalize
_pos_C = (i*0.0625+0.666666666)%1.0 + offset_normalize
self.__pt_position_A.append(cmds.pointOnCurve( eight[0],top = True, pr= _pos_A, p=True ))
self.__pt_position_B.append(cmds.pointOnCurve( eight[0],top = True, pr= _pos_B, p=True ))
self.__pt_position_C.append(cmds.pointOnCurve( eight[0],top = True, pr= _pos_C, p=True ))
scale -= percent
self.progress_Create.reset()
#cmds.delete(self.__circle[0])
# return self.__pt_position
self.__curve()
cmds.delete(eight[0])
def getPath(self):
path = cmds.ls(sl = True)
if path == []:
self.ln_path.setText('Nothing selected')
self.btn_create.setDisabled(True)
self.ln_path.setStyleSheet("background-color: rgb(110, 90, 90);")
else:
shape_path = cmds.listRelatives(path[0])
if (cmds.objectType(shape_path)== 'nurbsCurve'):
self.ln_path.setText(path[0])
self.btn_create.setEnabled(True)
self.ln_path.setStyleSheet("background-color: rgb(90, 150, 50);")
else:
self.ln_path.setText('Path not valid')
self.btn_create.setDisabled(True)
self.ln_path.setStyleSheet("background-color: rgb(110, 90, 90);")
def create(self):
self.__braid()
self.__pt_position_A = []
self.__pt_position_B = []
self.__pt_position_C = []
def run():
espira = espiral()
espira.show()
Another script is for loading UI design
import shiboken2
from PySide2 import QtWidgets
from PySide2 import QtGui
import maya.OpenMayaUI as apiUI
from cStringIO import StringIO
import pyside2uic
import xml.etree.ElementTree as xml
def get_maya_window():
ptr = apiUI.MQtUtil.mainWindow()
if ptr is not None:
return shiboken.wrapInstance(long(ptr), QtGui.QMainWindow)
def load_ui_type(ui_file):
parsed = xml.parse(ui_file)
widget_class = parsed.find('widget').get('class')
form_class = parsed.find('class').text
with open(ui_file,'r') as f:
o = StringIO()
frame = {}
pysideu2ic.compileUi(f, o, indent = 0)
pyc = compile(o.getvalue(), '<string>', 'exec')
exec pyc in frame
# Fetch the base_class and form class based on their type in the xml from design
form_class = frame['Ui_{0}'.format(form_class)]
base_class = eval('QtGui.{0}'.format(widget_class))
return form_class, base_class

Earth&Moon orbit system. My data is wrong

There is my code. I fixed it like this:
# Take 3 digits for significant figures in this code
import numpy as np
from math import *
from astropy.constants import *
import matplotlib.pyplot as plt
import time
start_time = time.time()
"""
G = Gravitational constant
g0 = Standard acceleration of gravity ( 9.8 m/s2)
M_sun = Solar mass
M_earth = Earth mass
R_sun = Solar darius
R_earth = Earth equatorial radius
au = Astronomical unit
Astropy.constants doesn't have any parameter of moon.
So I bring the data from wikipedia(https://en.wikipedia.org/wiki/Moon)
"""
M_moon = 7.342E22
R_moon = 1.737E6
M_earth = M_earth.value
R_earth = R_earth.value
G = G.value
perigee, apogee = 3.626E8, 4.054E8
position_E = np.array([0,0])
position_M = np.array([(perigee+apogee)/2.,0])
position_com = (M_earth*position_E+M_moon*position_M)/(M_earth+M_moon)
rel_pE = position_E - position_com
rel_pM = position_M - position_com
F = G*M_moon*M_earth/(position_M[0]**2)
p_E = {"x":rel_pE[0], "y":rel_pE[1],"v_x":0, "v_y":(float(F*rel_pE[0])/M_earth)**.5}
p_M = {"x":rel_pM[0], "y":rel_pM[1],"v_x":0, "v_y":(float(F*rel_pM[0])/M_moon)**.5}
print(p_E, p_M)
t = range(0,365)
data_E , data_M = [], []
def s(initial_velocity, acceleration, time):
result = initial_velocity*time + 0.5*acceleration*time**2
return result
def v(initial_velocity, acceleration, time):
result = initial_velocity + acceleration*time
return result
dist = float(sqrt((p_E["x"]-p_M['x'])**2 + (p_E["y"]-p_M["y"])**2))
xE=[]
yE=[]
xM=[]
yM=[]
data_E, data_M = [None]*len(t), [None]*len(t)
for i in range(1,366):
data_E[i-1] = p_E
data_M[i-1] = p_M
dist = ((p_E["x"]-p_M["x"])**2 + (p_E["y"]-p_M["y"])**2)**0.5
Fg = G*M_moon*M_earth/(dist**2)
theta_E = np.arctan(p_E["y"]/p_E["x"])
theta_M = theta_E + np.pi #np.arctan(data_M[i-1]["y"]/data_M[i-1]["x"])
Fx_E = Fg*np.cos(theta_E)
Fy_E = Fg*np.sin(theta_E)
Fx_M = Fg*np.cos(theta_M)
Fy_M = Fg*np.sin(theta_M)
a_E = Fg/M_earth
a_M = Fg/M_moon
v_E = (p_E["v_x"]**2+p_E["v_y"]**2)**.5
v_M = (p_M["v_x"]**2+p_M["v_y"]**2)**.5
p_E["v_x"] = v(p_E["v_x"], Fx_E/M_earth, 24*3600)
p_E["v_y"] = v(p_E["v_y"], Fy_E/M_earth, 24*3600)
p_E["x"] += s(p_E['v_x'], Fx_E/M_earth, 24*3600)
p_E["y"] += s(p_E['v_y'], Fy_E/M_earth, 24*3600)
p_M["v_x"] = v(p_M["v_x"], Fx_M/M_moon, 24*3600)
p_M["v_y"] = v(p_M["v_y"], Fy_M/M_moon, 24*3600)
p_M["x"] += s(p_M['v_x'], Fx_M/M_moon, 24*3600)
p_M["y"] += s(p_M['v_y'], Fy_M/M_moon, 24*3600)
for i in range(0,len(t)):
xE += data_E[i]["x"]
yE += data_E[i]["y"]
xM += data_M[i]["x"]
yM += data_M[i]["y"]
print("\n Run time \n --- %d seconds ---" %(time.time()-start_time))
after run this code i tried to print data_E and data_M.
Then I can get data but there is no difference. All of the data is the same.
But when I printed data step by step, it totally different.
I have wrong data problem and increase distance problem. Please help me this problem..
The code exits near line 45, where you are trying to assign p_E by pulling the square root of a negative number on the right hand side (as you've moved the [0] coordinate of the Earth to negative values while shifting Earth and Moon into the coordinate system of their center of mass). In line 45, the value of F*rel_pE[0]/M_earth is negative. So the code never reaches the end of the program using python 2.7.14. That bug needs to be solved before trying to discuss any further aspects.

Perspective projection and rotation in python

I've tried searching but none of the other questions seem to be like mine. I'm more or less experimenting with perspective projection and rotation in python, and have run into a snag. I'm sure my projection equations are accurate, as well as my rotation equations; however, when I run it, the rotation starts normal, but begins to swirl inwards until the vector is in the same position as the Z axis (the axis I am rotating over).
''' Imports '''
from tkinter import Tk, Canvas, TclError
from threading import Thread
from math import cos, sin, radians, ceil
from time import sleep
''' Points class '''
class pPoint:
def __init__(self, fPoint, wWC, wHC):
self.X = 0
self.Y = 0
self.Z = 0
self.xP = 0
self.yP = 0
self.fPoint = fPoint
self.wWC = wWC
self.wHC = wHC
def pProject(self):
self.xP = (self.fPoint * (self.X + self.wWC)) / (self.fPoint + self.Z)
self.yP = (self.fPoint * (self.Y + self.wHC)) / (self.fPoint + self.Z)
''' Main class '''
class Main:
def __init__(self):
''' Declarations '''
self.wWidth = 640
self.wHeight = 480
self.fPoint = 256
''' Generated declarations '''
self.wWC = self.wWidth / 2
self.wHC = self.wHeight / 2
''' Misc declarations '''
self.gWin = Tk()
self.vPoint = pPoint(self.fPoint, self.wWC, self.wHC)
self.vPoint.X = 50
self.vPoint.Y = 60
self.vPoint.Z = -25
self.vPoint.pProject()
self.ang = 0
def initWindow(self):
self.gWin.minsize(self.wWidth, self.wHeight)
self.gWin.maxsize(self.wWidth, self.wHeight)
''' Create canvas '''
self.gCan = Canvas(self.gWin, width = self.wWidth, height = self.wHeight, background = "black")
self.gCan.pack()
def setAxis(self):
''' Create axis points '''
self.pXax = pPoint(self.fPoint, self.wWC, self.wHC)
self.pXbx = pPoint(self.fPoint, self.wWC, self.wHC)
self.pYax = pPoint(self.fPoint, self.wWC, self.wHC)
self.pYbx = pPoint(self.fPoint, self.wWC, self.wHC)
self.pZax = pPoint(self.fPoint, self.wWC, self.wHC)
self.pZbx = pPoint(self.fPoint, self.wWC, self.wHC)
''' Set axis points '''
self.pXax.X = -(self.wWC)
self.pXax.Y = 0
self.pXax.Z = 1
self.pXbx.X = self.wWC
self.pXbx.Y = 0
self.pXbx.Z = 1
self.pYax.X = 0
self.pYax.Y = -(self.wHC)
self.pYax.Z = 1
self.pYbx.X = 0
self.pYbx.Y = self.wHC
self.pYbx.Z = 1
self.pZax.X = 0
self.pZax.Y = 0
self.pZax.Z = -(self.fPoint) / 2
self.pZbx.X = 0
self.pZbx.Y = 0
self.pZbx.Z = (self.fPoint * self.wWC) - self.fPoint
def projAxis(self):
''' Project the axis '''
self.pXax.pProject()
self.pXbx.pProject()
self.pYax.pProject()
self.pYbx.pProject()
self.pZax.pProject()
self.pZbx.pProject()
def drawAxis(self):
''' Draw the axis '''
self.gCan.create_line(self.pXax.xP, self.pXax.yP, self.pXbx.xP, self.pXbx.yP, fill = "white")
self.gCan.create_line(self.pYax.xP, self.pYax.yP, self.pYbx.xP, self.pYbx.yP, fill = "white")
self.gCan.create_line(self.pZax.xP, self.pZax.yP, self.pZbx.xP, self.pZbx.yP, fill = "white")
def prePaint(self):
self.vA = self.gCan.create_line(self.wWC, self.wHC, self.vPoint.xP, self.vPoint.yP, fill = "red")
def paintCanvas(self):
try:
while True:
self.ang += 1
if self.ang >= 361:
self.ang = 0
self.vPoint.X = (self.vPoint.X * cos(radians(self.ang))) - (self.vPoint.Y * sin(radians(self.ang)))
self.vPoint.Y = (self.vPoint.X * sin(radians(self.ang))) + (self.vPoint.Y * cos(radians(self.ang)))
self.vPoint.pProject()
self.gCan.coords(self.vA, self.wWC, self.wHC, self.vPoint.xP, self.vPoint.yP)
self.gWin.update_idletasks()
self.gWin.update()
sleep(0.1)
except TclError:
pass
mMain = Main()
mMain.initWindow()
mMain.setAxis()
mMain.projAxis()
mMain.drawAxis()
mMain.prePaint()
mMain.paintCanvas()
Thank you for any input :)
EDIT: Sorry, I just realized I forgot to put my question. I just want to know why it is gravitating inward, and not just rotating "normally"?
This section is wrong:
self.ang += 1
if self.ang >= 361:
self.ang = 0
self.vPoint.X = (self.vPoint.X * cos(radians(self.ang))
- self.vPoint.Y * sin(radians(self.ang)))
self.vPoint.Y = (self.vPoint.X * sin(radians(self.ang))
+ self.vPoint.Y * cos(radians(self.ang)))
self.vPoint.pProject()
For two reasons:
self.ang will take integers in the open range [0 - 360], which means the angle 360 (== 0) is repeated.
In each iteration, you rotate the point from the previous iteration by the angle. As a result, your first frame is at 1 degree, your second at 1+2 = 3, the third at 1 + 2 + 3... You should either be:
rotating the point from the previous iteration by a constant angle each time (1°). This suffers from the problem mentioned in my comment
rotating the initial point by the current angle of rotation each time
Not actualy related to your problem, but I strongly suggest you to use Numpy to perform geometric transformations, specially if it involves 3D points.
Below, I post a sample snippet, I hope it helps:
import numpy
from math import radians, cos, sin
## suppose you have a Nx3 cloudpoint (it might even be a single row of x,y,z coordinates)
cloudpoint = give_me_a_cloudpoint()
## this will be a rotation around Y azis:
yrot = radians(some_angle_in_degrees)
## let's create a rotation matrix using a numpy array
yrotmatrix = numpy.array([[cos(yrot), 0, -sin(yrot)],
[0, 1, 0],
[sin(yrot), 0, cos(yrot)]], dtype=float)
## apply the rotation via dot multiplication
rotatedcloud = numpy.dot(yrotmatrix, pointcloud.T).T # .T means transposition

Categories