I currently have an array of desired position vs. time of an object in my plant. I am using an inverse dynamics controller in order to drive the object to this desired position but I'm experiencing some difficulties. Here is how I am doing this:
I created the controller system
ID_cont = InverseDynamicsController(robot=controller_plant, kp=np.array([0.5]), ki=np.array([0.3]), kd=np.array([0.4]), has_reference_acceleration=False)
ID_controller = builder.AddSystem(ID_cont)
I got the controller input and output ports
control_estimated_state_input_port = ID_controller.get_input_port(0)
control_desired_state_input_port = ID_controller.get_input_port(1)
control_output_port = ID_controller.get_output_port(0)
I added a constant state source (likely wrong to do) and a state interpolator
constant_state_source = ConstantVectorSource(np.array([0.0]))
builder.AddSystem(constant_state_source)
position_to_state = StateInterpolatorWithDiscreteDerivative(controller_plant.num_positions(),
controller_plant.time_step())
builder.AddSystem(position_to_state)
I wired the controller to the plant
builder.Connect(constant_state_source.get_output_port(), position_to_state.get_input_port())
builder.Connect(position_to_state.get_output_port(), control_desired_state_input_port)
builder.Connect(plant.get_state_output_port(model_instance_1), control_estimated_state_input_port)
builder.Connect(control_output_port, plant.get_actuation_input_port(model_instance_1))
Next, I am trying to create a while loop that advances the simulation and changes the 'constant vector source' so I can feed in my position vs. time values but I'm unsure if the reason this isn't working out is because this is the complete wrong approach or if this is the right approach but I just have a few things wrong
diagram_context = diagram.CreateDefaultContext()
sim_time_temp = diagram_context.get_time()
time_step = 0.1
while sim_time_temp < duration:
ID_controller_context = diagram.GetMutableSubsystemContext(ID_controller, diagram_context)
simulator.AdvanceTo(sim_time_temp)
sim_time_temp = sim_time_temp + time_step
I added a constant state source (likely wrong to do) and a state interpolator
As you suspected, this is not the best way to go if you already have a desired sequence of positions and times that you want the system to track. Instead, you should use a TrajectorySource. Since you have a set of positions samples, positions (num_times x num_positions array), that you'd like the system to hit at specified times (num_times x 1 array), PiecewisePolynomial.CubicShapePreserving is a reasonable choice for building the trajectory.
desired_position_trajectory = PiecewisePolynomial.CubicShapePreserving(times, positions)
desired_state_source = TrajectorySource(desired_position_trajectory,
output_derivative_order=1)
builder.AddSystem(desired_state_source)
The output_derivative_order=1 argument makes desired_state_source output a [position, velocity] vector rather than just a position vector. You can connect desired_state_source directly to the controller, without an interpolator.
With this setup, you can advance the simulation all the way to duration without the need for a while loop.
Related
I've been using for loops for a script I've been working on, and I think the for loops are making the script run too slow. I think I could pick up a lot of speed with vectorization, but I'm having a little trouble figuring out how to vectorize my code. I'm copying some simple sample code that more or less mimics what I'm trying to accomplish in my actual code to make the question easier to follow. (Sorry if it's not the most elegant or technically sound. I'm still getting experience with Python.)
import numpy as np
def create_combo(input1,input2):
combo = input1 + input2
return combo
def another_combo(monitor,scalar_1,scalar_2):
add_more = monitor + scalar_1 + scalar_2
return add_more
# Initialize monitor
monitor = 0
# Initialize a threshold variable
threshold = 15
# Create input arrays
primary_1 = [1, 3, 5, 7, 9]
primary_2 = [2, 4, 6, 8, 10]
primary_1 = np.array(primary_1)
primary_2 = np.array(primary_2)
storage_vec = []
for i in range(5):
# Create input variables
scalar_1 = 0.5
scalar_2 = 2
# Call the create_combo function
combination = create_combo(primary_1[i],primary_2[i])
# call the another_combo function
add_on = another_combo(monitor,scalar_1,scalar_2)
monitor = combination + add_on
# Check if monitor exceeds threshold
if monitor > threshold:
# Store the index if so
storage_vec.append(i)
# Reset the variable, monitor
monitor = 0
I can understand how it would be easy to vectorize the function named create_combo. And I can also see that it would be simple to make the variable named monitor a vector as well.
What I'm having trouble wrapping my head around is how I could reset that monitor variable at specific points in the vector and continue doing computations with monitor after it has been reset. So, I've been working with monitor as a scalar and doing computations on each element of the inputs to the functions. However, this seems to be way too slow, and I know vectorizing typically speeds things up.
I read that perhaps the np.where() function could help, but I'm also not sure how to apply that in this situation.
Any guidance?
Your example if probably over complicated (or your example is not complete).
But if we want to create the storage_vec easily we can noticed that monitor increase by 21.5 after each step until it reach the threshold.
So the vector storage_vec can simply be computed with:
#Increase value:
inc = primary_1[-1]+primary_2[-1]+scalar_1+scalar_2 # = 21.5
#floor division
fd = threshold//inc
#ceil division
cd = -(-threshold//inc)
#storage_vec
storage_vec = np.r_[fd:10:cd]
I have been trying to get into python optimization, and I have found that pyomo is probably the way to go; I had some experience with GUROBI as a student, but of course that is no longer possible, so I have to look into the open source options.
I basically want to perform an non-linear mixed integer problem in which I will minimized a certain ratio. The problem itself is setting up a power purchase agreement (PPA) in a renewable energy scenario. Depending on the electricity generated, you will have to either buy or sell electricity acording to the PPA.
The only starting data is the generation; the PPA is the main decision variable, but I will need others. "buy", "sell", "b1" and "b2" are unknown without the PPA value. These are the equations:
Equations that rule the problem (by hand).
Using pyomo, I was trying to set up the problem as:
# Dataframe with my Generation information:
January = Data['Full_Data'][(Data['Full_Data']['Month'] == 1) & (Data['Full_Data']['Year'] == 2011)]
Gen = January['Producible (MWh)']
Time = len(Generacion)
M=100
# Model variables and definition:
m = ConcreteModel()
m.IDX = range(time)
m.PPA = Var(initialize = 2.0, bounds =(1,7))
m.compra = Var(m.IDX, bounds = (0, None))
m.venta = Var(m.IDX, bounds = (0, None))
m.b1 = Var(m.IDX, within = Binary)
m.b2 = Var(m.IDX, within = Binary)
And then, the constraint; only the first one, as I was already getting errors:
m.b1_rule = Constraint(
expr = (((Gen[i] - PPA)/M for i in m.IDX) <= m.b1[i])
)
which gives me the error:
---------------------------------------------------------------------------
AttributeError Traceback (most recent call last)
<ipython-input-5-5d5f5584ebca> in <module>
1 m.b1_rule = Constraint(
----> 2 expr = (((Generacion[i] - PPA)/M for i in m.IDX) <= m.b1[i])
3 )
pyomo\core\expr\numvalue.pyx in pyomo.core.expr.numvalue.NumericValue.__ge__()
pyomo\core\expr\logical_expr.pyx in pyomo.core.expr.logical_expr._generate_relational_expression()
AttributeError: 'generator' object has no attribute 'is_expression_type'
I honestly have no idea what this means. I feel like this should be a simple problem, but I am strugling with the syntax. I basically have to apply a constraint to each individual data from "Generation", there is no sum involved; all constraints are 1-to-1 contraints set so that the physical energy requirements make sense.
How do I set up the constraints like this?
Thank you very much
You have a couple things to fix. First, the error you are getting is because you have "extra parenthesis" around an expression that python is trying to convert to a generator. So, step 1 is to remove the outer parenthesis, but that will not solve your issue.
You said you want to generate this constraint "for each" value of your index. Any time you want to generate copies of a constraint "for each" you will need to either do that by making a constraint list and adding to it with some kind of loop, or use a function-rule combination. There are examples of each in the pyomo documentation and plenty on this site (I have posted a ton if you look at some of my posts.) I would suggest the function-rule combo and you should end up with something like:
def my_constr(m, i):
return m.Gen[i] - m.PPA <= m.b1[i] * M
m.C1 = Constraint(m.IDX, rule=my_constr)
I need to create a programmable filter using Paraview.
The idea is to create a vector called Speed equal to the speed in the non-rotating part equal to the speed+rotational speed in the rotational one.
The problem is that I can't accept the value of the speed in each single cell.
input0 = inputs[0]
radius=3
Speed1=input0.PointData["U"]
K=vtk.vtkDoubleArray()
X=input0.PointData["X"]
Y=input0.PointData["Y"]
Z=input0.PointData["Z"]
pdi = self.GetInput()
numPts = pdi.GetNumberOfPoints()
for i in range(0, numPts):
if X.getvalue(i)^2+Y.getvalue(i)^2<radius:
temp=U.getvalue(i)
else:
temp=U.getvalue(i)+rot
Speed.InsertNextValue(1)
output.PointData.append(Speed, "Speed")
The problem is that X.getvalue(i) is not working.
The correct syntax is
X[i]
The documentation can be found here
Is there a way in MEL or Python in Maya to get one object's position in the coordinate system of another object? I have a camera in a scene that may be rotated in any direction and am trying to measure the distance in its local Z axis to the vertices of various objects in the scene. This obviously needs to be fast, since it will likely be run thousands of times across the scene.
In Maxscript the command would be something like
" in coordsys $camera "
but I have yet to find something like this in Maya. If there's no direct command to do this, does anyone have a way to calculate it using matrix math?
There is no one liner similar to the MXS idiom -- and no easy way to do it in mel. However in Python you can do this fairly easily.
First you need to get the matrix for the coordinate system you want as an MMatrix, which is part of the OpenMaya api. Then get the position you want to check as an MPoint, which is another api class. Here's the cheap way to get them (there are faster methods but they're much wordier):
from maya.api.OpenMaya import MVector, MMatrix, MPoint
import maya.cmds as cmds
def world_matrix(obj):
"""'
convenience method to get the world matrix of <obj> as a matrix object
"""
return MMatrix( cmds.xform(obj, q=True, matrix=True, ws=True))
def world_pos(obj):
"""'
convenience method to get the world position of <obj> as an MPoint
"""
return MPoint( cmds.xform(obj, q=True, t=True, ws=True))
Once you have the matrix and the point, the relative position is simply point times the inverse of the matrix:
relative_position = world_pos('pSphere1') * world_matrix('pCube1').inverse()
print relative_position
# (0.756766, -0.0498943, 3.38499, 1)
The result will be an MPoint, which has 4 numbers (x, y, z and w); the 4th will always be 1 so you can just ignore it, although the math needs it to account for scales and shears.
Use this MEL script to calculate the distance from camera1 to nurbsSphere1 primitive:
vector $p1 = `getAttr camera1.translate`;
vector $p2 = `getAttr nurbsSphere1.translate`;
vector $result = $p1 - $p2;
print (mag($result))
Printed result must be like this:
# MEL 40.1965
Or use this Python script to calculate the distance from camera1 to nurbsSphere1 primitive:
import maya.cmds as cmds
import math
distance = math.sqrt(pow((float)(cmds.getAttr("nurbsSphere1.tx") - cmds.getAttr("camera1.tx")),2) +
pow((float)(cmds.getAttr("nurbsSphere1.ty") - cmds.getAttr("camera1.ty")),2) +
pow((float)(cmds.getAttr("nurbsSphere1.tz") - cmds.getAttr("camera1.tz")),2) )
print(distance)
Printed result must be like this:
# Python 40.1964998512
really not long ago I had my first dumb question answered here so... there I am again, with a hopefully less dumb and more interesting headscratcher. Keep in my mind I am still making my baby steps in scripting !
There it is : I need to rig a feathered wing, and I already have all the feathers in place. I thought of mimicking another rig I animated recently that had the feathers point-constrained to the arm and forearm, and orient-constrained to three other controllers on the arm : each and every feather was constrained to two of those controllers at a time, and the constraint's weights would shift as you went down the forearm towards the wrist, so that one feather perfectly at mid-distance between the elbow and the forearm would be equally constrained by both controllers... you get the picture.
My reasoning was as follows : let's make a loop that iterates over every feather, gets its world position, finds the distance from that feather to each of the orient controllers (through Pythagoras), normalize that and feed the values into the weight attribute of an orient constraint. I could even go the extra mile and pass the normalized distance through a sine function to get a nice easing into the feathers' silhouette.
My pseudo-code is ugly and broken, but it's a try. My issues are inlined.
Second try !
It works now, but only on active object, instead of the whole selection. What could be happening ?
import maya.cmds as cmds
# find world space position of targets
base_pos = cmds.xform('base',q=1,ws=1,rp=1)
tip_pos = cmds.xform('tip',q=1,ws=1,rp=1)
def relative_dist_from_pos(pos, ref):
# vector substract to get relative pos
pos_from_ref = [m - n for m, n in zip(pos, ref)]
# pythagoras to get distance from vector
dist_from_ref = (pos_from_ref[0]**2 + pos_from_ref[1]**2 + pos_from_ref[2]**2)**.5
return dist_from_ref
def weight_from_dist(dist_from_base, dist_to_tip):
normalize_fac = (1/(dist_from_base + dist_to_tip))
dist_from_base *= normalize_fac
dist_to_tip *= normalize_fac
return dist_from_base, dist_to_tip
sel = cmds.ls(selection=True)
for obj in sel:
# find world space pos of feather
feather_pos = cmds.xform(obj, q=1, ws=1, rp=1)
# call relative_dist_from_pos
dist_from_base = relative_dist_from_pos(feather_pos, base_pos)
dist_to_tip = relative_dist_from_pos(feather_pos, tip_pos)
# normalize distances
weight_from_dist(dist_from_base, dist_to_tip)
# constrain the feather - weights are inverted
# because the smaller the distance, the stronger the constraint
cmds.orientConstraint('base', obj, w=dist_to_tip)
cmds.orientConstraint('tip', obj, w=dist_from_base)
There you are. Any pointers are appreciated.
Have a good night,
Hadriscus