Develop Reference

Inconsistency in ODE solution using solve_ivp when adding events

I am using scipy.solve_ivp to solve an ODE in a loop, such that in every iteration I change some term in the ODE (which is like the force applied).
In terms of my problem - I just cant seem to understand why when I track events of zero crossing the solution sometimes stalls and essentially zeros out (as the figure below shows).
The code is a bit long, but only a small portion of it is actually relevant to my problem
(1) initialize some constants
MASS = 2.6e-4
WING_LENGTH = 0.07 # meters
GYRATION_RADIUS = 0.6 * WING_LENGTH
MoI = MASS * GYRATION_RADIUS ** 2 # Moment of Inertia
AIR_DENSITY = 1.2
WING_AREA = 0.5 * WING_LENGTH * (0.5 * WING_LENGTH) * np.pi # 1/2 ellipse with minor radios ~ 1/2 major = length/2
# drag coefficients from whitney & wood (JFM 2010):
C_D_MAX = 3.4
C_D_0 = 0.4
C_L_MAX = 1.8
ZERO_CROSSING = 1
RADIAN45 = np.pi / 4
RADIAN135 = 3 * RADIAN45
(2) Define a wrapper which is a RobotSimulation class - it's an easy interface that allows to easily change the values of the ODE. The important function is solve_dynamics (the others are not part of the problem I believe)
def phi_dot_zero_crossing_event(t, y):
"""
this event is given to solve_ivp to track if phi_dot == 0
:param t: unused time variable
:param y: a vector of [phi,phi_dot]
:return:
"""
return y[1]
class RobotSimulation:
def __init__(self, motor_torque=lambda x: 0, alpha=RADIAN45, phi0=0.0, phi_dot0=0.01, start_t=0, end_t=0.05, delta_t=0.001):
self.solution = 0
self.motor_torque = motor_torque
self.alpha = alpha # angle of attack
self.phi0 = phi0
self.phi_dot0 = phi_dot0
self.start_t = start_t
self.end_t = end_t
self.delta_t = delta_t
def set_time(self, new_start, new_end) -> None:
"""
we update the start time to be the end of the prev time and the end time
as the last end time + the window size we wish
"""
self.start_t = new_start
self.end_t = new_end
def set_init_cond(self, new_phi0, new_phi_dot0) -> None:
"""
we update the initial conditions to be the last value of previous iterations
"""
self.phi0 = new_phi0
self.phi_dot0 = new_phi_dot0
def set_motor_torque(self, new_motor_torque) -> None:
"""
sets the moment to the new value
"""
self.motor_torque = new_motor_torque
def flip_alpha(self) -> None:
"""
(Irrelevant for my question) changes the angle of attack's sign
:return:
"""
self.alpha = RADIAN135 if self.alpha == RADIAN45 else RADIAN45
def c_drag(self) -> float:
"""
calculates the drag coefficient based on the angle of attack
"""
return (C_D_MAX + C_D_0) / 2 - (C_D_MAX - C_D_0) / 2 * np.cos(2 * self.alpha)
def drag_torque(self, phi_dot):
"""
the drag moment
"""
return 0.5 * AIR_DENSITY * WING_AREA * self.c_drag() * GYRATION_RADIUS * phi_dot * np.abs(phi_dot)
def phi_derivatives(self, t, y):
"""
A function that defines the ODE that is to be solved: I * phi_ddot = tau_z - tau_drag.
We think of y as a vector y = [phi,phi_dot]. the ode solves dy/dt = f(y,t)
:return:
"""
phi, phi_dot = y[0], y[1]
dy_dt = [phi_dot, (self.motor_torque(t) - self.drag_torque(phi_dot)) / MoI]
return dy_dt
def solve_dynamics(self, phiarr, phidotarr, phiddotarr, angarr, torquearr):
"""
solves the ODE
:return:
"""
phi_0, phi_dot_0 = self.phi0, self.phi_dot0
start_t, end_t, delta_t = self.start_t, self.end_t, self.delta_t
phi_dot_zero_crossing_event.terminal = True
ang = []
times_between_zero_cross = []
sol_between_zero_cross = []
while start_t < end_t:
torquearr.append(self.motor_torque(start_t))
sol = solve_ivp(self.phi_derivatives, t_span=(start_t, end_t), y0=[phi_0,phi_dot_0],events=phi_dot_zero_crossing_event)
self.solution = sol.y
ang.append(self.alpha * np.ones(len(sol.t))) # set alpha for every t based on solution's size
times_between_zero_cross.append(sol.t)
sol_between_zero_cross.append(sol.y)
if sol.status == ZERO_CROSSING:
start_t = sol.t[-1] + delta_t
phi_0, phi_dot_0 = sol.y[0][-1], sol.y[1][-1] # last step is now initial value
self.flip_alpha()
else: # no zero crossing
break
time = np.concatenate(times_between_zero_cross)
phi, phi_dot = np.concatenate(sol_between_zero_cross, axis=1)
ang = np.concatenate(ang)
_, phi_ddot = self.phi_derivatives(time, [phi, phi_dot])
phiarr.append(phi)
phidotarr.append(phi_dot)
phiddotarr.append(phi_ddot)
angarr.append(ang)
(3) Finally the main function calls solve_dynamics sequentially for different values of actions and updates the time window and initial conditions:
phi_arr, phi_dot_arr, phi_ddot_arr, angarr, torquearr = [], [], [], [], []
phi0, phidot0 = 0, 0.01
start_t, end_t, delta_t = 0, 0.05, 0.001
sim = RobotSimulation()
for action in [1, -1, 1, -1, 1, -1]:
sim.set_motor_torque(lambda x: action)
sim.solve_dynamics(phi_arr, phi_dot_arr, phi_ddot_arr, angarr, torquearr)
phi0, phidot0 = sim.solution[0][-1], sim.solution[1][-1]
start_t = end_t
end_t += 0.05
sim.set_init_cond(phi0, phidot0)
sim.set_time(start_t, end_t)
phi_arr = np.concatenate(phi_arr)
phi_dot_arr = np.concatenate(phi_dot_arr)
phi_ddot_arr = np.concatenate(phi_ddot_arr)
angle_arr = np.concatenate(angarr)
Intuitively, as the actions I apply are just repeating +1 and -1, I cannot understand why the blue points in the figure (see below) act so different

Eventually, the problem was with how solve_ivp finds zero crossing points. It seems that the solution kept on finding the same crossings from + to - crossing every time (Thanks Lutz), so I've added a restriction using the .direction variable:
before entering the while loop: phi_dot_zero_crossing_event.direction = -np.sign(phi_dot_0). This makes sure we start by finding a crossing in the direction of the initial velocity.
when identifying an event in if sol.status == 1: I changed the sign of the direction with phi_dot_zero_crossing_event.direction *= -1

Vectors in 3D 3 Body Problem, Error that vector needs x, y, dy, and dx

I used the skeleton of a code modeling the three body problem. However, I am trying to figure out why the vectors will not work. I have commented them out (lines 56-60) so that I can see the rest of the program working. I'll attach the code so you can see the error. It is an error telling me the specs for a vector, but I don't see why the input wouldn't work. Thanks !
import numpy as np
import matplotlib.pyplot as plt
from vpython import *
from IPython.display import display
scene = display(title = "Earth's Orbit", width = 500, height = 500, range = 3.e11)
# #
# scene.autoscale = 0 # Turn off auto scaling of display
#
# Define the Sun and the Earth objects.
#
sun = sphere(color = color.yellow)
earth = sphere(color = color.blue)
venus = sphere(color = color.red)
# Gravitational constant (Nm**2/kg**2)
G = 6.67 * 10 ** -11
sun.pos = vector(0, 0, 0) # Initial Sun position (m)
earth.pos = vector(0, -149.6 * 10 ** 7, 149.6 * 10 ** 9) # Initial Earth position (m)
venus.pos = vector(1.0820948 * 10 **11, -1.0820948 * 10 **11, 0) # Initial Venus position (m)
rhat = -norm(earth.pos) # Getting Magnitude, probably going touse normalized vectors for simplicity
sun.mass = 2 * 10 ** 30 # Mass of the Sun (kg)
earth.mass = 6 * 10 ** 24 # Mass of the Earth (kg)
venus.mass = 4.867 * 10 ** 24 # Mass of Venus (kg)
earth.velocity = vector(30 * 10 ** 3, 0, 0)
venus.velocity = vector(35.02 * 10 ** 3, 0, 0)
# Initial velocity in seconds (THIS IS WHERE WE CAN CHANGE THINGS UP BUT BE SENSIBLE)
dt = 86000
#
total = 0 #Initializes the totl elpsed time
#
# Scale factors to control how big the Earth and Sun are drawn in the display
#
sun.scale = 1e1
earth.scale = 5e2
venus.scale = earth.scale
#
sun.radius = 7.e8 * sun.scale
earth.radius = 6.4e6 * earth.scale
venus.radius = 6.052e6 * venus.scale
#
#Initialize the momentum and path of the Earth
#
earth.momentum = earth.mass * earth.velocity # momentum of Earth
venus.momentum = venus.mass * venus.velocity # momentum of Venus
earth.trail = curve(color = earth.color) # Defines Earth's path
# Set initial position of the Earth
earth.trail.append(pos = earth.pos)
#
# Define an arrow thata points from the origin to the Earth
#
##rearrow = arrow(pos = (0, 0, 0) ,axis = earth.pos,
## color = earth.color, shaftwidth = 1e6)
##momentumArrow = arrow(pos = earth.pos, axis = earth.momentum,
## color = earth.color, shaftwidth = 1e6)
#
tmax = 3600 * 24 * 364.25 # Number of seconds in a year
#
# Start of the loop structure
#
while(True):
#
rate(100) # limit the loop to a maximum of 100 times per second
#
# Fill in the next 3 lines with the correct expressions
earthToSun = -norm(earth.pos)
venusToSun = -norm(venus.pos)
earthToVenus = -norm(earth.pos - venus.pos)
# Compute the force that the Sun exerts on the Earth and added Venus's influence
earth.force = ((G * earth.mass * sun.mass) / (mag(earth.pos)) ** 2 * rhat
+ G * venus.mass * earth.mass / mag(earth.pos - venus.pos) ** 2 * earthToVenus)
earth.momentum = earth.momentum + earth.force * \
dt # Update Earth's momentum
# Let's updaate Earth's position
earth.pos = earth.pos + (earth.momentum / earth.mass) * dt
forceEarth = (G * earth.mass * venus.mass) / (mag(earth.pos - venus.pos)) ** 2 * earthToVenus
forceSun = G * venus.mass * sun.mass / (mag(venus.pos)) ** 2 * venusToSun
venus.force = forceEarth + forceSun
venus.momentum += venus.force * dt
venus.pos += (venus.momentum / venus.mass) * dt
momentumArrow.pos = earth.pos
momentumArrow.axis = earth.momentum * 10 ** -18
earth.trail.append(pos = earth.pos) # Updates Earth' trail
rearrow.axis = earth.pos # Move Earth's position arrow
total = total + dt #Increment the Time
#
# Print
#
print(earth.pos)

(0,0,0) is not a vector, it's a Python tuple. You need to say vector(0,0,0) or vec(0,0,0).

I cannot get my 3 DOF bicycle model to work

I'm trying to simulate a vehicle using a dynamic bicycle model but I cannot seem to get it working. If I set a constant steering angle the lateral velocity grows exponentially and creates impossible results.
a = 0.34284
b = 0.40716
m = 155
I = 37.29
def f_DynBkMdl(x, y, delta, theta, dt, states):
dtheta = states[0]
vlat = states[1]
vlon = states[2]
if delta > math.radians(180):
delta = delta - math.radians(360)
if delta<0:
j = 1
else:
j = 0
if dtheta<0:
q = 1
else:
q = 0
dtheta = abs(dtheta)
delta = abs(delta)
sf = delta - (a*dtheta)/vlon
ff = 30.77*math.degrees(sf)
pf = 0
sr = (b*dtheta)/vlon
fr = 30.77*math.degrees(sr)
pr = 0
if j == 1:
fr = -fr
ff = -ff
if q == 1:
dtheta = -dtheta
theta = theta%math.radians(360)
ddtheta = (a*pf*delta + a*ff - b*fr)/I
dvlat = (pf*delta + ff + fr)/m - vlon*dtheta
dvlon = (pf + pr - ff*delta)/m - vlat*dtheta
dx = -vlat*np.sin(theta) + vlon*np.cos(theta)
dy = vlat*np.cos(theta) + vlon*np.sin(theta)
theta = theta + dtheta*dt + (1/2)*ddtheta*dt**2
dtheta = dtheta + ddtheta*dt
vlat = vlat + dvlat*dt
vlon = vlon + dvlon*dt
vabs = np.sqrt(vlat**2 + vlon**2)
x = x + dx*dt
y = y + dy*dt
states = [dtheta, vlat, vlon]
array = np.array([x, y, theta, vabs, states])
return array
With a and b being the distance between the front and rear axle to the vehicle's centre of gravity, m being the mass and I the inertia. x and y are the global position and theta is the heading with delta being the steering angle. I got these equations from this document. Please note I used a simplified tyre model to calculate the lateral forces and I assumed infinite friction so the friction circle was not needed.
Is there something I am missing to make this work?

Printing only the starting position and final position in ROS

I'm new to ROS and I was given some code to "play with".
I want my turtle to go straight one meter and then turn in a 45 degrees angle.
I'm getting the right result (Or at least it looks that way...) but I also want
to print the starting and final location of my turtle. I added some code that
prints the log in a non stop fashion, meaning every iteration I get the x,y position of my turtle, but I only want it in the beginning and the end, plus I want to add an angle theta that will represent the angle that my turtle is at.
This is my code:
import sys, rospy
from geometry_msgs.msg import Twist
from turtlesim.msg import Pose
PI = 3.1415926535897
theta = 0
def pose_callback(pose_msg):
rospy.loginfo("x: %.2f, y: %.2f" % (pose_msg.x, pose_msg.y))
def move():
msg.linear.x = FORWARD_SPEED_IN_MPS
t0 = rospy.Time.now().to_sec()
current_distance = 0
# Move turtle as wanted distance
while current_distance < DISTANCE_IN_METERS:
pub.publish(msg)
# Get current time.
t1 = rospy.Time.now().to_sec()
# Calc how much distance our turtle moved.
current_distance = FORWARD_SPEED_IN_MPS * (t1 - t0)
msg.linear.x = 0
def turn():
current_angle = 0
angular_speed = ROUND_SPEED * 2 * PI / 360
relative_angle = 45 * 2 * PI / 360
t0 = rospy.Time.now().to_sec()
msg.angular.z = abs(angular_speed)
while current_angle < relative_angle:
pub.publish(msg)
t1 = rospy.Time.now().to_sec()
current_angle = angular_speed * (t1 - t0)
if __name__ == "__main__":
robot_name = sys.argv[1]
FORWARD_SPEED_IN_MPS = 0.5
DISTANCE_IN_METERS = 1
ROUND_SPEED = 5
# Initialize the node
rospy.init_node("move_turtle")
# A publisher for the movement data
pub = rospy.Publisher(robot_name+"/cmd_vel", Twist, queue_size=10)
# A listener for pose
sub = rospy.Subscriber(robot_name+"/pose", Pose, pose_callback, None, 10)
# The default constructor will set all commands to 0
msg = Twist()
pose = Pose()
# Loop at 10Hz, publishing movement commands until we shut down
rate = rospy.Rate(10)
# Drive forward at a given speed. The robot points up the x-axis.
move()
# Turn counter-clockwise at a given speed.
turn()
Thanks for your help.

You can get the position, orientation, and velocity from Turtlesim Pose Message and I added a condition which checking the robot velocities:
import rospy
import time
from geometry_msgs.msg import Twist
from turtlesim.msg import Pose
PI = 3.1415926535897
theta = 0
def pose_callback(msg):
if msg.linear_velocity == 0 and msg.angular_velocity == 0:
rospy.loginfo("x: %.2f, y: %.2f" % (msg.x, msg.y))
rospy.loginfo('Orientation in euler - theta:{}'.format(msg.theta))
def move():
msg.linear.x = FORWARD_SPEED_IN_MPS
t0 = rospy.Time.now().to_sec()
current_distance = 0
while current_distance < DISTANCE_IN_METERS:
pub.publish(msg)
t1 = rospy.Time.now().to_sec()
current_distance = FORWARD_SPEED_IN_MPS * (t1 - t0)
msg.linear.x = 0
def turn():
current_angle = 0
angular_speed = ROUND_SPEED * 2 * PI / 360
relative_angle = 45 * 2 * PI / 360
t0 = rospy.Time.now().to_sec()
msg.angular.z = abs(angular_speed)
while current_angle < relative_angle:
pub.publish(msg)
t1 = rospy.Time.now().to_sec()
current_angle = angular_speed * (t1 - t0)
if __name__ == "__main__":
FORWARD_SPEED_IN_MPS = 0.5
DISTANCE_IN_METERS = 1
ROUND_SPEED = 5
rospy.init_node("move_turtle")
pub = rospy.Publisher("turtle1/cmd_vel", Twist, queue_size=10)
sub = rospy.Subscriber("turtle1/pose", Pose, pose_callback)
msg = Twist()
rate = rospy.Rate(100)
move()
turn()
time.sleep(2)
[NOTE]:
Orientation default message in turtlesim is euler type, but in most of ROS nodes type is quaternion, so if you want to get the quaternion orientation type, you must convert it:
from tf.transformations import quaternion_from_euler
euler = (0, 0, pose.z)
quaternion = quaternion_from_euler(euler)
x = quaternion[0]
y = quaternion[1]
z = quaternion[2]
w = quaternion[3]

python - calculate landmarks within distance from a latitude and longitude and append in loop

Given a 2d list of lats and longs and landmark features
how do I find those landmarks within say 10km of the user input current location lat long?
I then want to append these distances (stationDist) in a new 2d list (distanceList) as I go in a loop.
How do I write this in Python code? I have no idea!! Please help
import math
def main():
DISTANCE_CONSTANT = 111120.0
latOne = 55.9669
lonOne = 114.5967
latTwo = 55.9622
lonTwo = 114.5889
coLat = math.fabs(lonOne - lonTwo)
alpha = 90 - latTwo
beta = 90 - latOne
cosAlpha = math.cos(math.radians(alpha))
cosBeta = math.cos(math.radians(beta))
sinAlpha = math.sin(math.radians(alpha))
sinBeta = math.sin(math.radians(beta))
cosC = math.cos(math.radians(coLat))
cos_of_angle_a = (cosAlpha * cosBeta)
cos_of_angle_b = (sinAlpha * sinBeta * cosC)
cos_of_angle_c = cos_of_angle_a + cos_of_angle_b
angle = math.degrees(math.acos(cos_of_angle_c))
distance = angle * DISTANCE_CONSTANT
print '\nThe distance is: ', distance

Here is one way to set it up. I have converted your meters to kilometers...
import math
def calcdist(p1,p2):
latOne,lonOne=p1
latTwo,lonTwo=p2
DISTANCE_CONSTANT = 111.1200
coLat = math.fabs(lonOne - lonTwo)
alpha = 90 - latTwo
beta = 90 - latOne
cosAlpha = math.cos(math.radians(alpha))
cosBeta = math.cos(math.radians(beta))
sinAlpha = math.sin(math.radians(alpha))
sinBeta = math.sin(math.radians(beta))
cosC = math.cos(math.radians(coLat))
cos_of_angle_a = (cosAlpha * cosBeta)
cos_of_angle_b = (sinAlpha * sinBeta * cosC)
cos_of_angle_c = cos_of_angle_a + cos_of_angle_b
angle = math.degrees(math.acos(cos_of_angle_c))
distance = angle * DISTANCE_CONSTANT
return distance
ref = [36.0,-122.0]
names = ['close','far','kindaclose','ratherfar']
points = [[36.1,-122.0],[89.0,-123.0],[39.0,-122.0],[36.0,123.0]]
closelist=[]
threshold =12 #distance in km
for n,p in zip(names,points):
d = calcdist(ref,p)
print '{} : {:.1f}'.format(n,d)
if d < threshold:
closelist.append([n,d])
print 'These are within {} km:'.format(threshold) , closelist
Output:
close : 11.1
far : 5889.4
kindaclose : 333.4
ratherfar : 9561.9
These are within 12 km: [['close',11.11200]]