We Keep Coding

Deterministic runs in Carla

I'd like to be able to run the exact same run, given I didn't change any parameters of the simulation, in the autonomous driving simulator Carla. Before I paste my code, my logic is that I have to set a specific seed for any Random operation to be repeatable, set a specific seed for the traffic manager, and in general to work in synchronous_mode=True so the lag in my computer won't interrupt(?). As you'll see, I log the x,y,z location of the ego vehicle, and run the simulation twice. It is similar, but not the same. What can I do to make it repeatable (not in recording mode, actual live runs)?
Additional info: Carla 0.9.14 on Ubuntu 20.04, Python 3.8.
import random
import numpy as np
import sys
import os
try:
sys.path.append(os.path.dirname(os.path.dirname(os.path.abspath(__file__))) + '/carla')
except IndexError:
pass
import carla
from agents.navigation.behavior_agent import BehaviorAgent # pylint: disable=import-error
seed = 123
N_vehicles = 50
camera = None
telemetry = []
random.seed(seed)
try:
# Connect the client and set up bp library and spawn points
client = carla.Client('localhost', 2000)
client.set_timeout(60.0)
world = client.get_world()
bp_lib = world.get_blueprint_library()
spawn_points = world.get_map().get_spawn_points()
settings = world.get_settings()
settings.synchronous_mode = True
settings.fixed_delta_seconds = 0.10
world.apply_settings(settings)
traffic_manager = client.get_trafficmanager()
traffic_manager.set_random_device_seed(seed)
traffic_manager.set_synchronous_mode(True)
# Spawn ego vehicle
vehicle_bp = bp_lib.find('vehicle.audi.a2')
# breakpoint()
vehicle = world.try_spawn_actor(vehicle_bp, random.choice(spawn_points))
# Move spectator behind vehicle to motion
spectator = world.get_spectator()
transform = carla.Transform(vehicle.get_transform().transform(carla.Location(x=-6,z=2.5)),vehicle.get_transform().rotation)
spectator.set_transform(transform)
world.tick()
# set the car's controls
agent = BehaviorAgent(vehicle, behavior="normal")
destination = random.choice(spawn_points).location
agent.set_destination(destination)
print('destination:')
print(destination)
print('current location:')
print(vehicle.get_location())
#Iterate this cell to find desired camera location
camera_bp = bp_lib.find('sensor.camera.rgb')
# Spawn camera
camera_init_trans = carla.Transform(carla.Location(z=2))
camera = world.spawn_actor(camera_bp, camera_init_trans, attach_to=vehicle)
# Callback stores sensor data in a dictionary for use outside callback
def camera_callback(image, data_dict):
data_dict['image'] = np.reshape(np.copy(image.raw_data), (image.height, image.width, 4))
# Get gamera dimensions and initialise dictionary
image_w = camera_bp.get_attribute("image_size_x").as_int()
image_h = camera_bp.get_attribute("image_size_y").as_int()
camera_data = {'image': np.zeros((image_h, image_w, 4))}
# Start camera recording
camera.listen(lambda image: camera_callback(image, camera_data))
# Add traffic to the simulation
SpawnActor = carla.command.SpawnActor
SetAutopilot = carla.command.SetAutopilot
FutureActor = carla.command.FutureActor
vehicles_list, batch = [], []
for i in range(N_vehicles):
ovehicle_bp = random.choice(bp_lib.filter('vehicle'))
npc = world.try_spawn_actor(ovehicle_bp, random.choice(spawn_points))
# add it if it was successful
if(npc):
vehicles_list.append(npc)
print(f'only {len(vehicles_list)} cars were spawned')
world.tick()
# Set the all vehicles in motion using the Traffic Manager
for idx, v in enumerate(vehicles_list):
try:
v.set_autopilot(True)
except:
pass
# Game loop
while True:
world.tick()
pose = vehicle.get_location()
telemetry.append([pose.x, pose.y, pose.z])
# keep following the car
transform = carla.Transform(vehicle.get_transform().transform(carla.Location(x=-6,z=2.5)),vehicle.get_transform().rotation)
spectator.set_transform(transform)
if agent.done():
print("The target has been reached, stopping the simulation")
break
control = agent.run_step()
control.manual_gear_shift = False
vehicle.apply_control(control)
finally:
# Stop the camera when we've recorded enough data
if(camera):
camera.stop()
camera.destroy()
settings = world.get_settings()
settings.synchronous_mode = False
settings.fixed_delta_seconds = None
world.apply_settings(settings)
traffic_manager.set_synchronous_mode(True)
if(vehicles_list):
client.apply_batch([carla.command.DestroyActor(v) for v in vehicles_list])
vehicle.destroy()
np.savetxt('telemetry.txt', np.array(telemetry), delimiter=',')
y-axis is the error between two runs, x-axis is the time index of the run

how to create short circuit on different generators with python API

I am finding it hard to create a short circuit of different generator in the IEEE 9 bus system from python.
I tried to create a short circuit on generator 1 with the code below:
the code was executed successfully but short circuit event never happened
Here is the class function:
def create_short_circuit(self, target_name, time,
duration=None, name='short circuit'):
# get element where the short circuit will be applied
target = self.app.GetCalcRelevantObjects(target_name)[0]
# get the events folder from active study case
evt_folder = self.app.GetFromStudyCase('IntEvt')
# create an empty event of type EvtShc (short circuit)
evt_folder.CreateObject('EvtShc', name)
# get the newly created event
# sc = evt_folder.GetContents(name+'.EvtShc')[0][0]
sc = evt_folder.GetContents(name+'.EvtShc')[0]
# set time, target and type of short circuit (3-phase)
sc.time = time
sc.p_target = target
sc.i_shc = 0
# set clearing event if required
if duration is not None:
# create an empty event of type EvtShc (short circuit)
evt_folder.CreateObject('EvtShc', name+'_clear')
# get the newly created event
scc = evt_folder.GetContents(
name+'_clear'+'.EvtShc')[0]
# set time, target and type of event (clearing)
scc.time = time + duration
scc.p_target = target
scc.i_shc = 4
def delete_short_circuit(self, name='short circuit'):
# get the events folder from active study case
evt_folder = self.app.GetFromStudyCase('IntEvt')
# find the short circuit and clear event to delete
sc = evt_folder.GetContents(name+'.EvtShc')[0]
scc = evt_folder.GetContents(name+'_clear'+'.EvtShc')[0]
# delete short circuit and clear events if they exist
if sc:
sc.Delete()
if scc:
scc.Delete()
Here is the object code:
activate project and study case
sim = PowerFactorySimulation(FOLDER_NAME, PROJECT_NAME, STUDY_CASE_NAME)
# get all buses in network
buses = sim.app.GetCalcRelevantObjects('*.ElmTerm')
# create result dictionaries
t = {}
f = {}
for bus in buses:
# create short circuit on every bus
sim.create_short_circuit(
target_name=buses[1].loc_name+'*.ElmTerm',
time=2.0,
duration=0.15)
# prepare RMS simulation
sim.prepare_dynamic_sim(
monitored_variables=MONITORED_VARIABLES)
# run RMS simulation
sim.run_dynamic_sim()
# get and store generator response
t[bus.loc_name], f[bus.loc_name] = \
sim.get_dynamic_results('*.ElmSym', 's:xspeed')
# delete old short circuit before new one
sim.delete_short_circuit()

Pydrake: Creating a Trajectory Source for RigidTransformations

I've been trying to create a TrajectorySource for RigidTransforms to pass into a DifferentialInverseKinematicsIntegrator which only takes in RigidTransforms in its input port.
def createTraj(time, pose):
times = []
poses = []
for step in time:
times.append(time[step])
poses.append(pose[step])
return PiecewisePose.MakeLinear(times, poses)
Initially, I tried to directly pass in the output from createTraj above into TrajectorySource but ran into the issue of my trajectory having more than one columns: Failure at systems/primitives/trajectory_source.cc:21 in TrajectorySource(): condition 'trajectory.cols() == 1' failed.
import matplotlib.pyplot as plt, mpld3
class DexterTest():
# Output from createTraj is passed as parameter: traj into constructor
def __init__(self, traj):
builder = DiagramBuilder()
self.station = DexterPPStation(1e-4, "/opt/drake/share/drake/manipulation/models/final_dexter_description/urdf/dexter.urdf")
self.station.CreateBins("/opt/drake/share/drake/examples/manipulation_station/models/bin.sdf", RigidTransform(np.array([0.5,0,0])), RigidTransform(np.array([0,0.5,0])))
self.station.CreateRandomPickingObjects(3)
self.station.AddDexter()
builder.AddSystem(self.station)
self.station.Finalize()
self.diff_ik = DifferentialInverseKinematicsIntegrator(self.station.controller_plant, self.station.plant.GetFrameByName("link6", self.station.dexter["instance"]), self.station.time_step, DifferentialInverseKinematicsParameters(7,7))
builder.AddSystem(self.diff_ik)
#=========================================== Likely Source of Error ===========================================
pose = builder.AddSystem(PoseSystem())
p_G_source = builder.AddSystem(TrajectorySource(traj.get_position_trajectory()))
w_G_source = builder.AddSystem(TrajectorySource(traj.get_orientation_trajectory()))
builder.Connect(p_G_source.get_output_port(), pose.GetInputPort("p_G"))
builder.Connect(w_G_source.get_output_port(), pose.GetInputPort("r_G"))
builder.Connect(pose.get_output_port(), self.diff_ik.get_input_port())
#======================================================================================
MeshcatVisualizerCpp.AddToBuilder(builder, self.station.GetOutputPort("query_object"), meshcat)
self.diagram = builder.Build()
self.simulator = Simulator(self.diagram)
self.diagram_context = self.simulator.get_mutable_context()
self.station_context = self.station.GetMyMutableContextFromRoot(self.diagram_context)
self.plant_context = self.station.GetSubsystemContext(self.station.plant, self.station_context)
self.station.SetRandomPoses(self.plant_context)
builder.Connect(self.diff_ik.get_output_port(), self.station.GetInputPort("dexter_position"))
def run(self):
self.simulator.set_target_realtime_rate(2.0)
self.simulator.AdvanceTo(1)
class PoseSystem(LeafSystem):
def __init__(self):
LeafSystem.__init__(self)
self.p_G = self.DeclareVectorInputPort("p_G", BasicVector(3))
self.r_G = self.DeclareVectorInputPort("r_G", BasicVector(4))
self.DeclareAbstractOutputPort("X_G", Value[RigidTransform], self.CalcOutput)
def CalcOutput(self, context, output):
pose = RigidTransform(Quaternion(self.r_G.Eval(context)), self.p_G.Eval(context))
output.set_value(pose)
Instead, I tried to break up my trajectory into its orientation and position parts, add them to the input ports of a custom system, and then reconstruct them together in the output port. However, this gives me the following RuntimeError once the run method is called: RuntimeError: This multibody element does not belong to the supplied MultibodyTree.
Any help would be greatly appreciated!

I think you are very close. The PoseSystem looks like it should be a solution to the problem you've articulated in your post. (The error about MultibodyTree must be coming from the other part of your code.
You don't actually need to break the RigidTransform up into orientation / translation to create your PoseSystem, your CalcOutput could just call output.set_value(poses.Eval(t)) if poses is a PiecewisePose trajectory.
I have an example of doing this in the PickAndPlaceTrajectory class in this notebook: https://github.com/RussTedrake/manipulation/blob/008cec6343dd39063705287e6664a3fee71a43b8/pose.ipynb

How to feed the data obtained from rospy.Subscriber data into a variable?

I have written a sample Subscriber. I want to feed the data that I have obtained from the rospy.Subscriber into another variable, so that I can use it later in the program for processing. At the moment I could see that the Subscriber is functioning as I can see the subscribed values being printed when I use rospy.loginfo() function. Although I donot know how to store this data into another varible. I have tried assigning it directly to a variable by using assignment operator '=', but I get error.
I have tried writing a callback function with rospy.loginfo to print the position data from the subscribed object. I have subscribed JointState and it containes, header, position, velocity and effort arrays. using rospy.loginfo I can verify that the subscriber is subscribing. But when i tried to assign it directly to a variable, I get an error.
I am displaying loginfo from a call back function as follows
def callback(data):
rospy.loginfo(data.position)
global listen
listen = rospy.Subscriber("joint_states", JointState,
callback)
rospy.spin()
and this works fine. But when i slightly modify the code to assign the subscribed values, I get following error i.e.
listen1 = rospy.Subscriber("joint_states", JointState,
callback=None)
listen = listen1.position
#rospy.loginfo(listen)
print(listen)
rospy.spin()```
The error is as follows,
```listen = listen1.position
AttributeError: 'Subscriber' object has no attribute 'position'
EDIT:
Here is my node I have defined in my program,
#rospy.loginfo(msg.data)
global tactile_states
tactile_states = data.data
def joint_callback(data):
#rospy.loginfo(data.position)
global g_joint_states
global g_position
global g_pos1
g_joint_states = data
#for i in len(data.position):
#g_position[i] = data.position[i]
g_position = data.position
if len(data.position) > 0:
print("jointstate more than 0")
g_pos1 = data.position[0]
#print(g_position)
def joint_modifier(*args):
#choice describes what the node is supposed to do whether act as publisher or subscribe to joint states or tactile sensors
rospy.init_node('joint_listener_publisher', anonymous=True)
pub1 = rospy.Publisher('joint_states', JointState, queue_size = 10)
if(len(args)>1):
choice = args[0]
joint_name = args[1]
position = args[2]
else:
choice = args[0]
if (choice == 1):
rate = rospy.Rate(1)
robot_configuration = JointState()
robot_configuration.header = Header()
robot_configuration.name = [joint_name]
robot_configuration.position = [position]
robot_configuration.velocity = [10]
robot_configuration.effort = [100]
while not rospy.is_shutdown():
robot_configuration.header.stamp = rospy.Time.now()
rospy.loginfo(robot_configuration)
break
pub1.publish(robot_configuration)
rospy.sleep(2)
if (choice == 2):
#rospy.Timer(rospy.Duration(2), joint_modifier)
listen = rospy.Subscriber("joint_states", JointState, joint_callback)
rospy.spin()
if (choice == 3):
#rospy.Timer(rospy.Duration(2), joint_modifier)
tactile_sub = rospy.Subscriber("/sr_tactile/touch/ff", Float64, tactile_callback)
rospy.spin()
This is how I am calling the node inside the main body of the program,
joint_modifier(2)
print("printing g_position")
print(g_position)#to check the format of g_position
print("printed g _position")
leg_1 = Leg_attribute(g_position[0], g_position[1], g_position[2], velocity1 = 10, velocity2 = 10, velocity3 = 10, effort1 = 100, effort2 = 100, effort3 = 100, acceleration=1)
When calling this way, the program is stuck at joint_modifier(2) as that function has rospy.spin().

The style which you're using is not very standard. I assume you've seen the example on ROS wiki, I've modified it to demonstrate standard usage below.
Chiefly, addressing the code you posted, you needed to make listen have global scope outside of the callback. This is to store the data you want, not the Subscriber object. The rospy.spin() never goes in a callback, only the main node function/section. The subscriber object, listen1, which is used infrequently, doesn't return anything, and doesn't store the data it acquires. That is, you need Subscriber() to have a non-None callback.
It's more of a bind, giving the data to the callback instead of returning it from Subscriber. That's why listen1 (Subscriber) has no attribute position (JointState).
import rospy
from sensor_msgs.msg import JointState
# Subscribers
# joint_sub (sensor_msgs/JointState): "joint_states"
# This is where you store all your data you recieve
g_joint_states = None
g_positions = None
g_pos1 = None
def timer_callback(event): # Type rospy.TimerEvent
print('timer_cb (' + str(event.current_real) + '): g_positions is')
print(str(None) if g_positions is None else str(g_positions))
def joint_callback(data): # data of type JointState
# Each subscriber gets 1 callback, and the callback either
# stores information and/or computes something and/or publishes
# It _does not!_ return anything
global g_joint_states, g_positions, g_pos1
rospy.loginfo(data.position)
g_joint_states = data
g_positions = data.position
if len(data.position) > 0:
g_pos1 = data.position[0]
print(g_positions)
# In your main function, only! here do you subscribe to topics
def joint_logger_node():
# Init ROS
rospy.init_node('joint_logger_node', anonymous=True)
# Subscribers
# Each subscriber has the topic, topic type, AND the callback!
rospy.Subscriber('joint_states', JointState, joint_callback)
# Rarely need to hold onto the object with a variable:
# joint_sub = rospy.Subscriber(...)
rospy.Timer(rospy.Duration(2), timer_callback)
# spin() simply keeps python from exiting until this node is stopped
# This is an infinite loop, the only code that gets ran are callbacks
rospy.spin()
# NO CODE GOES AFTER THIS, NONE! USE TIMER CALLBACKS!
# unless you need to clean up resource allocation, close(), etc when program dies
if __name__ == '__main__':
joint_logger_node()
Edit 1:
There seems to be some confusion on what Subscriber(), spin(), and _callback(s) do.
It's a bit obscured in the Python, but there is a master program that manages all nodes, and sending nodes between them. In each node, we register with that master program that the node exists, and what publishers and subscribers it has. By register, it means we tell the master program, "Hey, I want that topic!"; in your case, for your (undeclared) joint_sub Subscriber, "Hey, I want all the JointState msgs from the joint_states topic!" The master program will, every time it gets (from some publisher somewhere) a new joint_states JointState msg, send it to that subscriber.
The subscriber handles, deals with, and processes the msg (data) with a callback: when(!) I receive a message, run the callback.
So the master program receives a new joint_states JointState msg from some publisher. Then it, because we registered a subscriber to it, sends it to this node. rospy.spin() is an infinite loop waiting for that data. This is what it does (kinda-mostly):
def rospy.spin():
while rospy.ok():
for new_msg in get_new_messages from master():
if I have a subscriber to new_msg:
my_subscriber.callback(new_msg)
rospy.spin() is where your callback, joint_callback (and/or timer_callback, etc) actually get called, and executed. It only runs when there is data for it.
More fundamentally, I think because of this confusion, your program structure is flawed; your functions don't do what you think they do. This is how you should make your node.
Make your math-portion (all the real non-ros code), the one doing the NN, into a separate module, and make a function to run it.
If you only want to run it when you receive data, run it in the callback. If you want to publish the result, publish in the callback.
Don't call the main function! The if __name__ == '__main__': my_main_function() should be the only place it gets called, and this will call your code. I repeat: the main function, declaring subscribers/publishers/init/timers/parameters, is only run in if __name__ ..., and this function runs your code. To have it run your code, place your code in a callback. Timer callbacks are handy for this.
I hope this code sample clarifies:
import rospy
from std_msgs.msg import Header
from sensor_msgs.msg import JointState
import my_nn as nn # nn.run(data)
# Subscribers
# joint_sub (sensor_msgs/JointState): "joint_states"
# Publishers
# joint_pub (sensor_msgs/JointState): "target_joint_states"
joint_pub = None
def joint_callback(data): # data of type JointState
pub_msg = JointState() # Make a new msg to publish results
pub_msg.header = Header()
pub_msg.name = data.name
pub_msg.velocity = [10] * len(data.name)
pub_msg.effort = [100] * len(data.name)
# This next line might not be quite right for what you want to do,
# But basically, run the "real code" on the data, and get the
# result to publish back out
pub_msg.position = nn.run(data.position) # Run NN on data, store results
joint_pub.publish(pub_msg) # Send it when ready!
if __name__ == '__main__':
# Init ROS
rospy.init_node('joint_logger_node', anonymous=True)
# Subscribers
rospy.Subscriber('joint_states', JointState, joint_callback)
# Publishers
joint_pub = rospy.Publisher('target_joint_states', JointState, queue_size = 10)
# Spin
rospy.spin()
# No more code! This is not a function to call, but its
# own program! This is an executable! Run your code in
# a callback!
Notice that a python module we design to be a ros node, has no functions to be called. It has a defined structure of callbacks and global data shared between them, all initialized and registered in the main function / if __name__ == '__main__'.

Read-Out of two channels of National Instrument USB 6211 with python

I'm trying to read out two channels simultaneously if an USB 6211 with python. To that end, I tried to adapt the example from http://www.scipy.org/Cookbook/Data_Acquisition_with_NIDAQmx by changing the line
CHK(nidaq.DAQmxCreateAIVoltageChan(
taskHandle,
"Dev1/ai0",
"",
DAQmx_Val_Cfg_Default,
float64(-10.0),
float64(10.0),
DAQmx_Val_Volts,
None))
to
CHK(nidaq.DAQmxCreateAIVoltageChan(
taskHandle,
"Dev1/ai0:1",
"",
DAQmx_Val_Cfg_Default,
float64(-10.0),
float64(10.0),
DAQmx_Val_Volts,
None))
But then, I keep receiving the error message that "nidaq call failed with error -200229: 'Buffer is too small to fit read data". Adding the line CHK(nidaq.DAQmxCfgInputBuffer(taskHandle, uInt32(10000000))) or increasing the length of the data array did not help...
Could someone point me to the right variable to change?

I found an answer here: http://www.physics.oregonstate.edu/~hetheriw/whiki/py/topics/ni/files/ni-daq_ctypes_multichannel_adc_usb_6008.txt
In short, the arguments of nidaq.DAQmxReadAnalogF64() need the additional argument "-1" after taskHandle. The line should then look like this:
CHK(nidaq.DAQmxReadAnalogF64(taskHandle, -1,float64(1.0),
DAQmx_Val_GroupByScanNumber,#DAQmx_Val_GroupByChannel,#DAQmx_Val_GroupByScanNumber
data.ctypes.data,max_num_samples,
ctypes.byref(read),None))

Here is an object I use to do A to D with a USB-6009. Note: at the bottom is an example of the calling procedure.
#-------------------------------------------------------------------------------
# Name: This is a object that takes data from the AtoD board
# Purpose:
#
# Author: Carl Houtman
#
# Created: 12/10/2012
# Copyright: (c) Carl Houtman 2012
# Licence: none
#-------------------------------------------------------------------------------
from PyDAQmx import *
import numpy
class DAQInput:
def __init__(self, num_data, num_chan, channel, high, low):
""" This is init function that opens the channel"""
# Declare variables passed by reference
taskHandle = TaskHandle()
read = int32()
data = numpy.zeros((10000,),dtype=numpy.float64)
sumi = [0,0,0,0,0,0,0,0,0,0]
#Get the passed variables
self.num_data = num_data
self.channel = channel
self.high = high
self.low = low
self.num_chan = num_chan
# Create a task and configure a channel
DAQmxCreateTask(b"",byref(self.taskHandle))
DAQmxCreateAIVoltageChan(self.taskHandle,self.channel,b"",DAQmx_Val_Cfg_Default,
self.low,self.high,DAQmx_Val_Volts,None)
# Start the task
DAQmxStartTask(self.taskHandle)
def getData(self):
""" This function gets the data from the board and calculates the average"""
DAQmxReadAnalogF64(self.taskHandle,self.num_data,10.0,DAQmx_Val_GroupByChannel,
self.data,10000,byref(self.read),None)
# Calculate the average of the values in data (could be several channels)
i = self.read.value
for j in range(self.num_chan):
self.sumi[j] = numpy.sum(self.data[j*i:(j+1)*i])/self.read.value
return self.sumi
def killTask(self):
""" This function kills the tasks"""
# If the task is still alive kill it
if self.taskHandle != 0:
DAQmxStopTask(self.taskHandle)
DAQmxClearTask(self.taskHandle)
if __name__ == '__main__':
myDaq = DAQInput(100, 2, b"Dev1/ai0:1", 10.0, -10.0)
result = myDaq.getData()
print ("the average readings were {:.4f} and {:.4f} volts".format(result[0], result[1]))
myDaq.killTask()