Develop Reference

Inverse Dynamics Controller Drake

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.

Rat with the randomize path in and 2-D array

The problem is similar to rat-maze problem. I have given an 2-d array MxN. each cell of an array is either 1 or 0 ,where 1 means blocked. I have given 2 points (starting point and ending point). I have to go from start index to end index. But the catch is 1) Path should be random. 2) There should be some parameter which allow me to decide how much random it can be. (i.e how crazily it should wander before reaching to its destination.) 3) Path should not intersect itself.(like a snake game).
This algorithm is needed to create population (randomly) which will used as input for genetic model for further optimize it.
For now i have used bfs and created one solution. But the problem is i cannot create any no of random path with this (which i will later use as population) + i'm unable to formalize the idea of how much random it should be.
This is my code that only produces min path by using bfs
def isSafe(x,y,length):
if ((x<length) and (x>-1) and (y<length) and (y>-1)):
return True;
return False;
def path(room,x1,y1,x2,y2,distance):
roomSize=len(room);
if ((x1==x2) and (y1==y2)):
room[x1][y1]=distance+1
return
queue=[[x1,y1]]
room[x1][y1]=0
start=0
end=0
while start<=end:
x,y=queue[start]
start+=1
distance=room[x][y]
for i in [-1,1]:
if isSafe(x+i,y,roomSize):
if room[x+i][y]=="O":
queue.append([x+i,y])
room[x+i][y]=distance+1
end+=1;
for i in [-1,1]:
if isSafe(x,y+i,roomSize):
if room[x][y+i]=="O":
queue.append([x,y+i])
room[x][y+i]=distance+1
end+=1;
def retrace(array,x1,y1,x2,y2):
roomSize=len(array)
if not (isSafe(x2,y2,roomSize)):
print("Wrong Traversing Point");
if type(array[x2][y2])==str:
print("##################No Pipe been installed due to path constrained################")
return [];
distance=array[x2][y2];
path=[[x2,y2]]
x=0
while not (array[x2][y2]==0):
if ((isSafe(x2+1,y2,roomSize)) and type(array[x2+1][y2])==int and array[x2+1][y2]==array[x2][y2]-1):
x2+=1;
path.append([x2,y2]);
elif ((isSafe(x2-1,y2,roomSize)) and type(array[x2-1][y2])==int and array[x2-1][y2]==array[x2][y2]-1):
x2-=1;
path.append([x2,y2])
elif ((isSafe(x2,y2+1,roomSize)) and type(array[x2][y2+1])==int and array[x2][y2+1]==array[x2][y2]-1):
y2+=1;
path.append([x2,y2]);
elif ((isSafe(x2,y2-1,roomSize)) and type(array[x2][y2-1])==int and array[x2][y2-1]==array[x2][y2]-1):
y2-=1;
path.append([x2,y2]);
return path;

Compute reachability of elements in a list of tuples

I have a list of tuples like this.
a = [(1,2),(1,3),(1,4),(2,5),(6,5),(7,8)]
In this list 1 relates to 2 and then 2 relates to 5 and 5 relates to 6 therefore 1 relates to 6. Similarly I need to find the relations between other elements in tuples. I need a function that takes the input values and outputs as follows:
input = (1,6) #output = True
input = (5,3) #output = True
input = (2,8) #output = False
I do not have knowledge of itertools or map functions. Can they be used to solve these types of problems?
And for the sake of curiosity and interest where can I find these types of questions to solve and where are these types of problems encountered in real life situations?

This can be easily done by considering the tuples as edges in a graph. The question is then reduced to checking if there is a path between the two nodes.
There exists lots of nice libraries for this, see e.g. networkx
import networkx as nx
a = [(1,2),(1,3),(1,4),(2,5),(6,5),(7,8)]
G = nx.Graph(a)
nx.has_path(G, 1, 6) # True
nx.has_path(G, 5, 3) # True
nx.has_path(G, 2, 8) # False

This answer here nicely states your problem as a graph problem, where every time you need to run your algorithm you need to check for the existence of a path between your input vertices. The time complexity for every query then depends on the size, order, diameter, degree of the underlying graph.
However, if you intend to run this algorithm many times with the same array a, it may be worth doing some preprocessing on the input graph to find the connected components (Wikipedia : connected components) first. In that case you can get constant time for every query. Here is the code I suggest :
# NOTE : tested using python 3.6.1
# WARNING : no input sanitization
a = [(1,2),(1,3),(1,4),(2,5),(6,5),(7,8)]
n = 8 # order of the underlying graph
# prepare graph as lists of neighbors for every vertex, i.e. adjacency lists (extra unused vertex '0', just to match the value range of the problem)
graph = [[] for i in range(n+1)]
for edge in a:
graph[edge[0]].append(edge[1])
graph[edge[1]].append(edge[0])
print( "graph : " + str(graph) )
# list of unprocessed vertices : contains all of them at the beginning
unprocessed_vertices = {i for i in range(1,n+1)}
# subroutine to discover the connected component of a vertex
def build_component():
component = [] # current connected component
curr_vertices = {unprocessed_vertices.pop()} # locally unprocessed vertices, initialize with one of the globally unprocessed vertices
while len(curr_vertices) > 0:
curr_vertex = curr_vertices.pop() # vertex to be processed
# add unprocessed neighbours of current vertex to the set of vertices to process
for neighbour in graph[curr_vertex]:
if neighbour in unprocessed_vertices:
curr_vertices.add(neighbour)
unprocessed_vertices.remove(neighbour)
component.append(curr_vertex)
return component
# main algorithm : graph traversal on multiple connected components
components = []
while len(unprocessed_vertices) > 0:
components.append( build_component() )
print( "components : " + str(components) )
# assign a number to each component
component_numbers = [None] * (n+1)
curr_number = 1
for comp in components:
for vertex in comp:
component_numbers[vertex] = curr_number
curr_number += 1
print( "component_numbers : " + str(component_numbers) )
# main functionality
def is_connected( pair ):
return component_numbers[pair[0]] == component_numbers[pair[1]]
# run main functionnality on inputs : every call is executed in constant time now, regardless of the size of the graph
print( is_connected( (1,6) ) )
print( is_connected( (5,3) ) )
print( is_connected( (2,8) ) )
I don't really know about the most likely situations where this problem could be encountered, but I suppose it can have application is some clustering tasks, or maybe if you want to know if it is possible to go from one place to another. If the edges of the graph represent dependencies between modules, this problem would tell you if two parts depend on each other, so maybe some potential applications in compiling or the managment of large projects. The underlying problem is a "Connected component" problem which is among the problems we know polynomial algorithms for.
It is generally very useful to model these kind of problems with graphs as these objects have a very simple structure, and most of the time we can reduce the original problem to a well known problem on graphs.

recursion to iteration in python

We are trying to make a cluster analysis for a big amount of data. We are kind of new to python and found out that an iterative function is way more efficient than an recursive one. Now we are trying to change that but it is way harder than we thought.
This code underneath is the heart of our clustering function. This takes over 90 percent of the time. Can you help us to change that into a recursive one?
Some extra information: The taunach function gets neighbours of our point which will later form the clusters. The problem is that we have many many points.
def taunach(tau,delta, i,s,nach,anz):
dis=tabelle[s].dist
#delta=tau
x=data[i]
y=Skalarprodukt(data[tabelle[s].index]-x)
a=tau-abs(dis)
#LA.norm(data[tabelle[s].index]-x)
if y<a*abs(a):
nach.update({item.index for item in tabelle[tabelle[s].inner:tabelle[s].outer-1]})
anz = anzahl(delta, i, tabelle[s].inner, anz)
if dis>-1:
b=dis-tau
if y>=b*abs(b):#*(1-0.001):
nach,anz=taunach(tau,delta, i,tabelle[s].outer,nach,anz)
else:
if y<tau**2:
nach.add(tabelle[s].index)
if y < delta:
anz += 1
if tabelle[s].dist>-4:
b = dis - tau
if y>=b*abs(b):#*(1-0.001)):
nach,anz=taunach(tau,delta, i,tabelle[s].outer,nach,anz)
if tabelle[s].dist > -1:
if y<=(dis+tau)**2:
nach,anz=taunach(tau,delta, i,tabelle[s].inner,nach,anz)
return nach,anz

Batch-constraining objects (feathers to a wing)

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