how to update bound of PositionConstraint? - drake

We want speed up the IK solve porocess, so we want to update the bound of IK constraints, instead of creating constraints every time solve IK problems.
We noticed that there are APIs like set_bounds() to update bounds in BoundingBoxConstraint, LinearConstraint, etc. But the bound of PositionConstraint cann't be updated this way.
Is this discouraged, or there are some way to update bounds of PositionConstraint?
Thanks!

The PR #16631 was just merged into Drake. Now you can use set_bounds, UpdateLowerBound and UpdateUpperBound in PositionConstraint.
PositionConstraint.set_bounds documentation
constraint = ik.PositionConstraint(
plant=variables.plant,
frameA=variables.body1_frame,
p_AQ_lower=[-0.1, -0.2, -0.3],
p_AQ_upper=[-0.05, -0.12, -0.28],
frameB=variables.body2_frame,
p_BQ=[0.2, 0.3, 0.5], plant_context=variables.plant_context)
constraint.UpdateLowerBound(new_lb=np.array([-2, -3, -0.5]))
constraint.UpdateUpperBound(new_ub=np.array([10., 0.5, 2.]))
constraint.set_bounds(new_lb=[-1, -2, -2.], new_ub=[1., 2., 3.])

Related

How to have different input and output sizes for a Drake system

I am trying to write a system that takes as input the state of a free body and gives as output the desired state of a revolute joint.
Hence the input port should take a vector of size 13 and the output port should give a vector of size 2.
For now, I just want to extract one value from the input state, so I tried something like this:
ball_state = Variable("ball_state")
desired_theta_system = builder.AddSystem(SymbolicVectorSystem(input=[ball_state], state=[], dynamics=[], output=[ball_state[6], 0]))
However, this did not work, as the ball_state variable is not subscriptable.
How can I do this? Do I need to derive LeafSystem?
Thanks!
You could certainly write a small LeafSystem, but you could accomplish this one with a MatrixGain system (e.g. with D =
[0, ..., 0, 1, 0, ...] ;
[0, ... 0].

How to set up a Histogram counter in Prometheus to watch 256 values?

I am making a mоnіtоrіng sуstem and using Prоmеthеus, prometheus-cpp lib and Grаfаnа. I need to watch a function that returns one of 256 values:
int result function();
result = [ 0...255]
The Hіstоgrаm counter seems to work best for this. But for this counter, I need to set a vector that defines the boundaries, that is:
std::vector<double> bucket_sec = {0.0, 1.0, 1.1, 2.0, 2.1, 3.0, 3.1, 4.0, .....254.1, 255.0}
Is this the right way?
Do I need to make such a long border vector?
Or is there another way that is more correct?

How do I apply multiple linear transformations?

I'm trying to use LinearTransformationScene's apply_matrix multiple times:
from manim import *
class LT(LinearTransformationScene):
def __init__(self):
super().__init__(
self,
show_coordinates=True,
leave_ghost_vectors=True,
)
def construct(self):
P = [[1, 1], [1, -1]];
D = [[2, 0], [0, 0.5]];
P_inv = [[0.5, 0.5], [0.5, 0.5]];
self.apply_matrix(P);
self.wait();
self.apply_matrix(D);
self.wait();
self.apply_matrix(P_inv);
self.wait();
But I get this error: submobjects must be of type VMobject.
I'm hoping to create an animation that:
Applies the matrix P
Pauses briefly
Applies another matrix D
Pauses briefly again
And finally, applies the inverse of P, P_inv.
How do I accomplish this? There were similar questions posted, but no one posted about this specific error.
These specialized Scene classes are unfortunately not very well maintained, this is a known issue. There is a simple workaround though: after calling self.apply_matrix, add
self.moving_mobjects = []
and the next application of apply_matrix will work as intended again.

Constructing discrete table-based CPDs in tensorflow-probablity?

I'm trying to construct the simplest example of Bayesian network with several discrete random variables and conditional probabilities (the "Student Network" from Koller's book, see 1)
Although a bit unwieldy, I managed to build this network using pymc3. Especially, creating the CPDs is not that straightforward in pymc3, see the snippet below:
import pymc3 as pm
...
with pm.Model() as basic_model:
# parameters for categorical are indexed as [0, 1, 2, ...]
difficulty = pm.Categorical(name='difficulty', p=[0.6, 0.4])
intelligence = pm.Categorical(name='intelligence', p=[0.7, 0.3])
grade = pm.Categorical(name='grade',
p=pm.math.switch(
theano.tensor.eq(intelligence, 0),
pm.math.switch(
theano.tensor.eq(difficulty, 0),
[0.3, 0.4, 0.3], # I=0, D=0
[0.05, 0.25, 0.7] # I=0, D=1
),
pm.math.switch(
theano.tensor.eq(difficulty, 0),
[0.9, 0.08, 0.02], # I=1, D=0
[0.5, 0.3, 0.2] # I=1, D=1
)
)
)
letter = pm.Categorical(name='letter', p=pm.math.switch(
...
But I have no idea how to build this network using tensoflow-probability (versions: tfp-nightly==0.7.0.dev20190517, tf-nightly-2.0-preview==2.0.0.dev20190517)
For the unconditioned binary variables, one can use categorical distribution, such as
from tensorflow_probability import distributions as tfd
from tensorflow_probability import edward2 as ed
difficulty = ed.RandomVariable(
tfd.Categorical(
probs=[0.6, 0.4],
name='difficulty'
)
)
But how to construct the CPDs?
There are few classes/methods in tensorflow-probability that might be relevant (in tensorflow_probability/python/distributions/deterministic.py or the deprecated ConditionalDistribution) but the documentation is rather sparse (one needs deep understanding of tfp).
--- Updated question ---
Chris' answer is a good starting point. However, things are still a bit unclear even for a very simple two-variable model.
This works nicely:
jdn = tfd.JointDistributionNamed(dict(
dist_x=tfd.Categorical([0.2, 0.8], validate_args=True),
dist_y=lambda dist_x: tfd.Bernoulli(probs=tf.gather([0.1, 0.9], indices=dist_x), validate_args=True)
))
print(jdn.sample(10))
but this one fails
jdn = tfd.JointDistributionNamed(dict(
dist_x=tfd.Categorical([0.2, 0.8], validate_args=True),
dist_y=lambda dist_x: tfd.Categorical(probs=tf.gather_nd([[0.1, 0.9], [0.5, 0.5]], indices=[dist_x]))
))
print(jdn.sample(10))
(I'm trying to model categorical explicitly in the second example just for learning purposes)
-- Update: solved ---
Obviously, the last example wrongly used tf.gather_nd instead of tf.gather as we only wanted to select the first or the second row based on the dist_x outome. This code works now:
jdn = tfd.JointDistributionNamed(dict(
dist_x=tfd.Categorical([0.2, 0.8], validate_args=True),
dist_y=lambda dist_x: tfd.Categorical(probs=tf.gather([[0.1, 0.9], [0.5, 0.5]], indices=[dist_x]))
))
print(jdn.sample(10))
The tricky thing about this, and presumably the reason it's subtler than expected in PyMC, is -- as with almost everything in vectorized programming -- handling shapes.
In TF/TFP, the (IMO) nicest way to solve this is with one of the new TFP JointDistribution{Sequential,Named,Coroutine} classes. These let you naturally represent hierarchical PGM models, and then sample from them, evaluate log probs, etc.
I whipped up a colab notebook demoing all 3 approaches, for the full student network: https://colab.research.google.com/drive/1D2VZ3OE6tp5pHTsnOAf_7nZZZ74GTeex
Note the crucial use of tf.gather and tf.gather_nd to manage the vectorization of the various binary and categorical switching.
Have a look and let me know if you have any questions!

Is there a way to parse multiple co-ordinates script data incorrectly set wrong by 180 degrees in LSL (Linden Scripting Language)?

I am using LSL (Linden Scripting Language) in Second Life. I have imported a virtual (mesh) aircraft object and spent hours animating various parts such as canopy and undercarriage.
I have now discovered that I should have imported the object to face east when all axis are set to zero (mine was facing west). I have now re-imported the object with the correct orientation, however as the main mesh object was my 'root prim' and the frames storing the positions of every animated part was relative to that, all the animated parts (child prims) are now reversed by exactly 180 degrees.
Does anyone know of a way I could parse the script data to find and automatically add a correction of 180 degrees?
I spent hours on the animations and have pages of data, so an automated solution would be extremely preferable and any help gratefully received.
A snippet of the code I need to parse is reproduced below:
link_message(integer n, integer c, string m, key id){
vector lSize = llList2Vector(llGetLinkPrimitiveParams(1,[7]),0);
if(m == lAnimName + "|0"){// Frame 0.
if(lLeg3t)
llSetLinkPrimitiveParamsFast(lLeg3t,[
33, <0.245628*lSize.x, -0.183868*lSize.y, -0.184195*lSize.z>, 29, <-0.500000, 0.000000, -0.707107, 0.500000>
]);
if(lWire3t)
llSetLinkPrimitiveParamsFast(lWire3t,[
33, <0.259854*lSize.x, -0.187642*lSize.y, -0.196354*lSize.z>, 29, <-0.500000, 0.000000, -0.707107, 0.500000>
]);
if(lWire3b)
llSetLinkPrimitiveParamsFast(lWire3b,[
33, <0.244813*lSize.x, -0.194661*lSize.y, -0.171052*lSize.z>, 29, <0.073912, -0.549525, -0.444997, 0.703233>
]);
if(lFoot3)
llSetLinkPrimitiveParamsFast(lFoot3,[
33, <0.261851*lSize.x, -0.180508*lSize.y, -0.157508*lSize.z>, 29, <-0.270598, -0.270598, -0.653282, 0.653282>
]);
if(lLeg3b)
llSetLinkPrimitiveParamsFast(lLeg3b,[
33, <0.247470*lSize.x, -0.200321*lSize.y, -0.190136*lSize.z>, 29, <0.073912, -0.549525, -0.444997, 0.703233>
]);
if(lSled3)
llSetLinkPrimitiveParamsFast(lSled3,[
33, <0.251954*lSize.x, -0.184123*lSize.y, -0.169543*lSize.z>, 29, <0.000000, 0.000000, -0.707107, 0.707107>
]);
if(lWire2t)
llSetLinkPrimitiveParamsFast(lWire2t,[
33, <0.268535*lSize.x, 0.190722*lSize.y, -0.196969*lSize.z>, 29, <-0.061628, 0.541675, -0.454520, 0.704416>
]);
if(lLeg2t)
llSetLinkPrimitiveParamsFast(lLeg2t,[
33, <0.255244*lSize.x, 0.185132*lSize.y, -0.176223*lSize.z>, 29, <-0.061628, 0.541675, -0.454520, 0.704416>
]);
if(lWire2b)
llSetLinkPrimitiveParamsFast(lWire2b,[
33, <0.237334*lSize.x, 0.180499*lSize.y, -0.159385*lSize.z>, 29, <0.517145, -0.024678, -0.706676, 0.482246>
]);
I'm no scripter but, I think I kinda get what's going on....
Try this - import your vehicle in Blender. Select all the parts, making sure your vehicle is facing the correct direction. If it's not, hit R, then Z, then the number of the degrees you want it to rotate, then Enter.
Once the vehicle is facing the correct direction. Hit Ctrl-A and select 'Rotation' in the pop up menu. This resets the rotation data all to 0. Export your object and upload it. You'll find when you rezz it that the rotation of all the parts are now set to 0.
If I understand your quandry correctly that should resolve the rotation issue of the child prims?
If not...see what happens when scripting noobs try to be helpful? =/
My recollection is if you flip the sign on any single non-zero quaternion component it will flip the rotation 180 degrees.
So...
if(lLeg3b)
llSetLinkPrimitiveParamsFast(lLeg3b,[
33, <0.247470*lSize.x, -0.200321*lSize.y, -0.190136*lSize.z>, 29, <0.073912, -0.549525, -0.444997, 0.703233>
]);
becomes:
if(lLeg3b)
llSetLinkPrimitiveParamsFast(lLeg3b,[
33, <0.247470*lSize.x, -0.200321*lSize.y, -0.190136*lSize.z>, 29, <-0.073912, -0.549525, -0.444997, 0.703233>
]);

Resources