We Keep Coding

Linearization of External System Dynamics in VectorSystem DoCalcVectorOutput Method

I am attempting to implement a custom linear MPC controller class in Drake, which inherits from the VectorSystem base class. This controller is supposed to linearize the dynamics of a given plant at its current operating point, and then perform a trajectory optimization.
I have defined the MPC controller class here.
class MpcController(VectorSystem):
def __init__(self, quadrotor_plant, init_state, target_state, time_horizon, max_time_samples, thrust_limit, obstacle_list):
# define this system as having 12 inputs and 4 outputs
VectorSystem.__init__(self, 12, 4)
self.quadrotor_plant = quadrotor_plant
self.init_state = init_state
self.target_state = target_state
self.time_horizon = time_horizon
self.max_time_samples = max_time_samples
self.thrust_limit = thrust_limit
self.obstacle_list = obstacle_list
self.current_step = 0
def DoCalcVectorOutput(self, context, inp, state, output):
quadrotor_context = self.quadrotor_plant.CreateDefaultContext()
# print("DoCalcVectorOutput quad input value", quad_input_port.HasValue(quadrotor_context))
# input into the controller is the state of the quadrotor
# set the context equal to the current state
quadrotor_context.SetContinuousState(inp)
current_state = inp
print(f"cur_quad_state = {current_state}")
print(f"quadrotor current input = {output}")
##################
# Linearize system dynamics - Take first order taylor series expansions of system
# around the operating point defined by quadrotor context
##################
eq_check_tolerance = 10e6 # we do not need to be at an equilibrium point
linear_quadrotor = Linearize(self.quadrotor_plant, quadrotor_context, \
equilibrium_check_tolerance = eq_check_tolerance )
I have setup my simulation like so:
# define the quadrotor plant using drake built in quadrotor plant class
# this is a System, not a MultiBodyPlant
quadrotor_plant = QuadrotorPlant()
# quadrotor_context = quadrotor_plant.CreateDefaultContext()
quadrotor_plant = builder.AddSystem(quadrotor_plant)
mpc_controller = MpcController(quadrotor_plant, initial_state, final_state, time_horizon, num_time_samples, thrust_limit, obstacles)
mpc_controller = builder.AddSystem(mpc_controller)
# connect the MPC controller to the quadrotor
builder.Connect(mpc_controller.get_output_port(0), quadrotor_plant.get_input_port(0))
builder.Connect(quadrotor_plant.get_output_port(0), mpc_controller.get_input_port(0))
# quad_context = quadrotor_plant.CreateDefaultContext()
# print(quadrotor_plant.get_input_port(0).HasValue(quad_context))
# Set up visualization in MeshCat
QuadrotorGeometry.AddToBuilder(builder, quadrotor_plant.get_output_port(0), scene_graph)
meshcat.Delete()
meshcat.ResetRenderMode()
meshcat.SetProperty('/Background','visible',False)
visualizer = MeshcatVisualizerCpp.AddToBuilder(builder, scene_graph, meshcat)
# end setup for visualization
################# Run Simulation ####################
#Set up a simulator to run this diagram
diagram = builder.Build()
quad_context = quadrotor_plant.CreateDefaultContext()
print(quadrotor_plant.get_input_port(0).HasValue(quad_context))
simulator = Simulator(diagram)
sim_context = simulator.get_mutable_context()
sim_context.SetContinuousState(initial_state)
# pass sim context into MpC controller to be used for linearization of dynamics
mpc_controller.set_sim_context(sim_context)
quadrotor_context = quadrotor_plant.GetMyMutableContextFromRoot(sim_context)
# quadrotor_plant.get_input_port(0).FixValue(quadrotor_context, [10.,10.,10.,10.])
end_time = 10.0
simulator.set_target_realtime_rate(1.0)
meshcat.AddButton('Stop Simulation')
while simulator.get_context().get_time() < end_time:
simulator.AdvanceTo(sim_context.get_time() + 0.1)
meshcat.DeleteAddedControls()
When I run this code I get the following runtime error:
RuntimeError: InputPort::Eval(): required InputPort[0] (propellor_force) of System ::_::drake/examples/quadrotor/QuadrotorPlant#00000000058fa1e0 (QuadrotorPlant) is not connected
However, I have clearly connected it.
To get around this, I have also tried passing the mutable context to the Linearize() method using:
quadrotor_context = quadrotor_plant.GetMyMutableContextFromRoot(sim_context)
However, when I do this I run into a different error:
SystemExit: Failure at bazel-out/k8-opt/bin/tools/install/libdrake/_virtual_includes/drake_shared_library/drake/systems/framework/vector_system.h:290 in DoCalcVectorOutput(): condition 'output->size() == 0' failed.
I believe this error is due to the fact that using the mutable context creates a scenario of infinite recursion, because Linearize() calls DoCalcVectorOutput for the MpcController, which in turn calls Linearize()...
It seems like neither of these approaches is correct. What is the best approach to linearizing the dynamics of an external plant inside of the DoCalcVectorOutput() method of a controller in Drake?

Your analysis of the error is correct. The quadrotor_context that you have in the MpcController does not have the input port connected, which is why you get the error when you call Linearize. You need to set or connect the input port inside that method in order to specify which input you are linearizing around.
One solution would be to use something like self.quadrotor_plant.get_input_port().FixValue(quadrotor_context, u0) in your MpcController DoCalcVectorOutput method.

Using same data in different states of SMACH Concurrent-container

Let's say we have a concurrent SMACH container sm_con which includes two state machines SM1 and SM2. I need to find a way for SM1 to continuously update some data and for SM2 to access (and eventually also modify) the same data. I thought about solving this by passing the userdata of sm_con to SM1 and SM2 as input- and output-keys hoping that if SM1 modifies the data it would automatically overwrite sm_cons userdata (kind of like working with pointers in c++) but this doesn't work.
The corresponding code would look like this:
import smach
import smach_ros
import rospy
class st1(smach.State):
def __init__(self, outcomes=['successful', 'preempted']):
smach.State.__init__(self, outcomes, input_keys=['in_test'], output_keys=['out_test'])
def execute(self, userdata):
if self.preempt_requested():
self.service_preempt()
return 'preempted'
rospy.logerr('test 1: '+str(userdata.in_test))
userdata.out_test=userdata.in_test+1
return 'successful'
class st2(smach.State):
def __init__(self, outcomes=['successful', 'preempted']):
smach.State.__init__(self, outcomes, input_keys=['in_test'])
def execute(self, userdata):
#time.sleep(2)
if self.preempt_requested():
self.service_preempt()
return 'preempted'
rospy.logerr('test 2: ' + str(userdata.in_test))
return 'successful'
if __name__=="__main__":
rospy.init_node('test_state_machine')
sm_con = smach.Concurrence(outcomes=['success'],
default_outcome='success'
)
sm_con.userdata.testdata = 0
with sm_con:
sm_1 = smach.StateMachine(outcomes=['success', 'preempted'], input_keys=['testdata'], output_keys=['testdata'])
with sm_1:
smach.StateMachine.add('ST1', st1(),
remapping={'in_test': 'testdata', 'out_test': 'testdata'},
transitions={'successful': 'ST1'})
sm_2 = smach.StateMachine(outcomes=['success', 'preempted'], input_keys=['testdata'])
with sm_2:
smach.StateMachine.add('ST2', st2(),
remapping={'in_test':'testdata'},
transitions={'successful': 'ST2'})
smach.Concurrence.add('SM1', sm_1)
smach.Concurrence.add('SM2', sm_2)
# Execute SMACH plan
outcome = sm_con.execute()
print('exit-outcome:' + outcome)
# Wait for ctrl-c to stop the application
rospy.spin()
Running this code, the output 'test 1: ...' shows that inside SM1 the userdata gets incremented while the output 'test 2: ...' shows that SM2 doesn't access the incremented data as the output remains 0.
How can I modify some data in SM1 and access the modified data in SM2?

I found a workaround for this using mutable objects like described here.
Applied on the code above, it would look like the following:
import smach
import smach_ros
import rospy
class st1(smach.State):
def __init__(self, outcomes=['successful', 'preempted']):
smach.State.__init__(self, outcomes, input_keys=['in_test'])
def execute(self, userdata):
if self.preempt_requested():
self.service_preempt()
return 'preempted'
rospy.logerr('test 1: '+str(userdata.in_test))
userdata.in_test[0]=userdata.in_test[0]+1
return 'successful'
class st2(smach.State):
def __init__(self, outcomes=['successful', 'preempted']):
smach.State.__init__(self, outcomes, input_keys=['in_test'])
def execute(self, userdata):
#time.sleep(2)
if self.preempt_requested():
self.service_preempt()
return 'preempted'
rospy.logerr('test 2: ' + str(userdata.in_test[0]))
return 'successful'
if __name__=="__main__":
rospy.init_node('test_state_machine')
sm_con = smach.Concurrence(outcomes=['success'],
default_outcome='success'
)
sm_con.userdata.testdata = [0]
with sm_con:
sm_1 = smach.StateMachine(outcomes=['success', 'preempted'], input_keys=['testdata'])
with sm_1:
smach.StateMachine.add('ST1', st1(),
remapping={'in_test': 'testdata'},
transitions={'successful': 'ST1'})
sm_2 = smach.StateMachine(outcomes=['success', 'preempted'], input_keys=['testdata'])
with sm_2:
smach.StateMachine.add('ST2', st2(),
remapping={'in_test':'testdata'},
transitions={'successful': 'ST2'})
smach.Concurrence.add('SM1', sm_1)
smach.Concurrence.add('SM2', sm_2)
# Execute SMACH plan
outcome = sm_con.execute()
print('exit-outcome:' + outcome)
# Wait for ctrl-c to stop the application
rospy.spin()
Since this is only a workaround, refer to my corresponding issue-post here for more information.

Optimize output of a script by varying input parameters

I have a written a script that uses the code below and I would like to optimize rsi_high and rsi_low to get the best sharpe_ratio:
#
import numpy
import talib as ta
global rsi_high, rsi_low
rsi_high = 63
rsi_low = 41
def myTradingSystem(DATE, OPEN, HIGH, LOW, CLOSE, VOL, exposure, equity, settings):
''' This system uses trend following techniques to allocate capital into the desired equities'''
nMarkets = CLOSE.shape[1] # SHAPE OF NUMPY ARRAY
result, rsi_pos = numpy.apply_along_axis(rsicalc, axis=0, arr=CLOSE)
pos = numpy.asarray(rsi_pos, dtype=numpy.float64)
return pos, settings
def rsicalc(num):
# print rsi_high
try:
rsival = ta.RSI(numpy.array(num,dtype='f8'),timeperiod=14)
if rsival[14] > rsi_high: pos_rsi = 1
elif rsival[14] < rsi_low: pos_rsi = -1
else: pos_rsi = 0
except:
rsival = 0
pos_rsi = 0
return rsival, pos_rsi
def mySettings():
''' Define your trading system settings here '''
settings = {}
# Futures Contracts
settings['markets'] = ['CASH','F_AD', 'F_BO', 'F_BP', 'F_C', 'F_CC', 'F_CD',
'F_CL', 'F_CT', 'F_DX', 'F_EC', 'F_ED', 'F_ES', 'F_FC', 'F_FV', 'F_GC',
'F_HG', 'F_HO', 'F_JY', 'F_KC', 'F_LB', 'F_LC', 'F_LN', 'F_MD', 'F_MP',
'F_NG', 'F_NQ', 'F_NR', 'F_O', 'F_OJ', 'F_PA', 'F_PL', 'F_RB', 'F_RU',
'F_S', 'F_SB', 'F_SF', 'F_SI', 'F_SM', 'F_TU', 'F_TY', 'F_US', 'F_W',
'F_XX', 'F_YM']
settings['slippage'] = 0.05
settings['budget'] = 1000000
settings['beginInSample'] = '19900101'
settings['endInSample'] = '19931231'
settings['lookback'] = 504
return settings
# Evaluate trading system defined in current file.
if __name__ == '__main__':
import quantiacsToolbox
results = quantiacsToolbox.runts(__file__, plotEquity=False)
sharpe_ratio = results['stats']['sharpe']
I suspect that using something like scipy minimize function would do the trick, but I am having trouble understanding how to package my script so that it can be in a usable form.
I have tried putting everything in a function and then running all the code through a number of loops, each time incrementing values but there must be a more elegant way of doing this.
Apologies for posting all my code but I thought it would help if the responder wanted to reproduce my setup and for anyone who is new to quantiacs to see a real example who is faced with the same issue.
Thanks for your help in advance!

Create UV Set with openmaya API 2.0

Im writing a custom reader node for maya (in python with openmaya API 2.0) and I would like to send my uv sets to a regular maya mesh node.
Im wondering what would be the best way to push the uv sets in a mesh node? I wasn't able to find what data I have to create and how to send them to the mesh node.
My reader is a OpenMaya.MPxNode who push custom data to a OpenMaya.MPxSurfaceShape. The shape is linked by out mesh / in mesh plugs to a regular maya mesh. I attempt to fill the uvSet plug of this shape using compute but without success. I expect the UVSets to be sent to the mesh.
The following code sample is a limited test where my only goal is to create a new UVSet and attach it to the mesh.
Any ideas or documentation which could help?
I tried several things but I always get the error below.
The error:
// Error: (kFailure): Object does not exist
# Traceback (most recent call last):
# File "/path/to/maya/plug-ins/test_create_uv_set.py", line 60, in compute
# mesh.createUVSet("toto")
# RuntimeError: (kFailure): Object does not exist //
The running code:
"""
usage
import maya.cmds as cmds
import pymel.core as pm
cmds.loadPlugin("/path/to/maya/plug-ins/test_create_uv_set.py")
transform_node = pm.polySphere(n='transform1', ch=1, o=1, r=4)[0]
mesh1_node = transform_node.getShape()
pm.setAttr(mesh1_node + ".visibility", False)
uv_set_mod_node = pm.createNode("uvSetModifier", name="uvsetmodifier1")
mesh2_node = pm.createNode("mesh", name="mesh2", parent=transform_node)
pm.hyperShade(assign="initialShadingGroup")
pm.Attribute.connect(mesh1_node.attr("outMesh"), uv_set_mod_node.attr("inMesh"))
pm.Attribute.connect(uv_set_mod_node.attr("outMesh"), mesh2_node.attr("inMesh"))
"""
import sys
import maya.api.OpenMaya as OpenMaya
def maya_useNewAPI():
pass
class uvSetModifier(OpenMaya.MPxNode):
typeName = "uvSetModifier"
id = OpenMaya.MTypeId(0xCCCCC)
inMesh = None
outMesh = None
#staticmethod
def creator():
return uvSetModifier()
#staticmethod
def initialize():
typedAttr = OpenMaya.MFnTypedAttribute()
uvSetModifier.inMesh = typedAttr.create("inMesh", "im", OpenMaya.MFnData.kMesh)
typedAttr.writable = True
OpenMaya.MPxNode.addAttribute(uvSetModifier.inMesh)
uvSetModifier.outMesh = typedAttr.create("outMesh", "om", OpenMaya.MFnData.kMesh)
typedAttr.writable = True
OpenMaya.MPxNode.addAttribute(uvSetModifier.outMesh)
def __init__(self):
OpenMaya.MPxNode.__init__(self)
def compute(self, plug, datablock):
if plug == uvSetModifier.outMesh:
inputData = datablock.inputValue(uvSetModifier.inMesh)
outputData = datablock.outputValue(uvSetModifier.outMesh)
outputData.setMObject(inputData.asMesh())
mesh = OpenMaya.MFnMesh(inputData.asMesh())
mesh.createUVSet("toto")
datablock.setClean(plug)
def initializePlugin(obj):
plugin = OpenMaya.MFnPlugin(obj, "Autodesk", "3.0", "Any")
try:
plugin.registerNode(uvSetModifier.typeName, uvSetModifier.id, uvSetModifier.creator, uvSetModifier.initialize)
except:
sys.stderr.write("Failed to register node\n")
raise
def uninitializePlugin(obj):
plugin = OpenMaya.MFnPlugin(obj)
try:
plugin.deregisterNode(uvSetModifier.id)
except:
sys.stderr.write("Failed to deregister node\n")
pass
UPDATE 1: THEODOX input
I added the following line (59)
if inputData.asMesh() is not None:
print "test"
mesh = OpenMaya.MFnMesh(inputData.asMesh())
mesh.createUVSet("toto")
results: I still get the same error message

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()