Deformable torus not showing in pydrake port of C++ deformable torus example - drake
I tried porting the C++ deformable torus example to python but the torus is not showing up in meshcat.
There are no errors and I see the "ground" but not the torus:
(I didn't include the parts related to the gripper, just trying to get the torus to show up for now)
def run_demo():
builder = DiagramBuilder()
plant_config = MultibodyPlantConfig()
plant_config.time_step = time_step
plant_config.discrete_contact_solver = "sap"
plant, scene_graph = AddMultibodyPlant(plant_config, builder)
rigid_proximity_props = ProximityProperties()
surface_friction = CoulombFriction(1.15, 1.15)
rigid_proximity_props.AddProperty("hydroelastic",
"resolution_hint", 1.0)
AddContactMaterial(friction = surface_friction,
properties = rigid_proximity_props)
ground = Box(4, 4, 4)
X_WG = RigidTransform(RotationMatrix(), [0, 0, -2])
plant.RegisterCollisionGeometry(plant.world_body(), X_WG, ground,
"ground_collision", rigid_proximity_props)
plant.RegisterVisualGeometry(plant.world_body(), X_WG, ground,
"ground_visual", [0.7, 0.5, 0.4, 0.8])
owned_deformable_model = DeformableModel(plant)
deformable_config = DeformableBodyConfig()
deformable_config.set_youngs_modulus(E)
deformable_config.set_poissons_ratio(nu)
deformable_config.set_mass_density(density)
deformable_config.set_stiffness_damping_coefficient(beta)
scale = 0.65;
torus_mesh = Mesh("./torus.vtk", scale)
kL = 0.09 * scale
X_WB = RigidTransform(RotationMatrix(), [0, 0, kL/2])
torus_instance = GeometryInstance(X_WB, torus_mesh, "deformable_torus")
deformable_proximity_props = ProximityProperties()
AddContactMaterial(friction = surface_friction,
properties = deformable_proximity_props)
torus_instance.set_proximity_properties(deformable_proximity_props)
unused_resolution_hint = 1.0
owned_deformable_model.RegisterDeformableBody(
torus_instance, deformable_config, unused_resolution_hint)
plant.AddPhysicalModel(owned_deformable_model)
deformable_model = owned_deformable_model
plant.Finalize();
builder.Connect(
deformable_model.vertex_positions_port(),
scene_graph.get_source_configuration_port(plant.get_source_id()));
MeshcatVisualizer.AddToBuilder(builder, scene_graph, meshcat)
diagram = builder.Build()
diagram_context = diagram.CreateDefaultContext()
simulator = Simulator(diagram)
simulator.Initialize()
simulator.set_target_realtime_rate(realtime_rate)
meshcat.AddButton("Stop Simulation", "Escape")
while meshcat.GetButtonClicks("Stop Simulation") < 1:
simulator.AdvanceTo(simulator.get_context().get_time() + 2.0)
meshcat.DeleteButton("Stop Simulation")
meshcat.DeleteAddedControls()
I'm thinking that the problem may be in the line: plant.AddPhysicalModel(owned_deformable_model)
or somewhere earlier in the process of creating the deformable model, but I'm not sure...
Full project here: DeepNote project
Thanks so much for your help!
The deformable body should have been added to the simulation already. You can verify this by checking deformable_model.num_bodies().
This should be a visualization issue. As the README of the C++ example states, Meldis is required to visualize deformable bodies. You should be able to visualize after launching Meldis and switching the MeshcatVisualizer to DrakeVisualizer in your diagram.
Related
Why does all my emission mu of HMM in pyro converge to the same number?
I'm trying to create a Gaussian HMM model in pyro to infer the parameters of a very simple Markov sequence. However, my model fails to infer the parameters and something wired happened during the training process. Using the same sequence, hmmlearn has successfully infer the true parameters.
Full code can be accessed in here:
https://colab.research.google.com/drive/1u_4J-dg9Y1CDLwByJ6FL4oMWMFUVnVNd#scrollTo=ZJ4PzdTUBgJi
My model is modified from the example in here:
https://github.com/pyro-ppl/pyro/blob/dev/examples/hmm.py
I manually created a first order Markov sequence where there are 3 states, the true means are [-10, 0, 10], sigmas are [1,2,1].
Here is my model
def model(observations, num_state):
assert not torch._C._get_tracing_state()
with poutine.mask(mask = True):
p_transition = pyro.sample("p_transition",
dist.Dirichlet((1 / num_state) * torch.ones(num_state, num_state)).to_event(1))
p_init = pyro.sample("p_init",
dist.Dirichlet((1 / num_state) * torch.ones(num_state)))
p_mu = pyro.param(name = "p_mu",
init_tensor = torch.randn(num_state),
constraint = constraints.real)
p_tau = pyro.param(name = "p_tau",
init_tensor = torch.ones(num_state),
constraint = constraints.positive)
current_state = pyro.sample("x_0",
dist.Categorical(p_init),
infer = {"enumerate" : "parallel"})
for t in pyro.markov(range(1, len(observations))):
current_state = pyro.sample("x_{}".format(t),
dist.Categorical(Vindex(p_transition)[current_state, :]),
infer = {"enumerate" : "parallel"})
pyro.sample("y_{}".format(t),
dist.Normal(Vindex(p_mu)[current_state], Vindex(p_tau)[current_state]),
obs = observations[t])
My model is compiled as
device = torch.device("cuda:0")
obs = torch.tensor(obs)
obs = obs.to(device)
torch.set_default_tensor_type("torch.cuda.FloatTensor")
guide = AutoDelta(poutine.block(model, expose_fn = lambda msg : msg["name"].startswith("p_")))
Elbo = Trace_ELBO
elbo = Elbo(max_plate_nesting = 1)
optim = Adam({"lr": 0.001})
svi = SVI(model, guide, optim, elbo)
As the training goes, the ELBO has decreased steadily as shown. However, the three means of the states converges.
I have tried to put the for loop of my model into a pyro.plate and switch pyro.param to pyro.sample and vice versa, but nothing worked for my model.
I have not tried this model, but I think it should be possible to solve the problem by modifying the model in the following way:
def model(observations, num_state):
assert not torch._C._get_tracing_state()
with poutine.mask(mask = True):
p_transition = pyro.sample("p_transition",
dist.Dirichlet((1 / num_state) * torch.ones(num_state, num_state)).to_event(1))
p_init = pyro.sample("p_init",
dist.Dirichlet((1 / num_state) * torch.ones(num_state)))
p_mu = pyro.sample("p_mu",
dist.Normal(torch.zeros(num_state), torch.ones(num_state)).to_event(1))
p_tau = pyro.sample("p_tau",
dist.HalfCauchy(torch.zeros(num_state)).to_event(1))
current_state = pyro.sample("x_0",
dist.Categorical(p_init),
infer = {"enumerate" : "parallel"})
for t in pyro.markov(range(1, len(observations))):
current_state = pyro.sample("x_{}".format(t),
dist.Categorical(Vindex(p_transition)[current_state, :]),
infer = {"enumerate" : "parallel"})
pyro.sample("y_{}".format(t),
dist.Normal(Vindex(p_mu)[current_state], Vindex(p_tau)[current_state]),
obs = observations[t])
The model would then be trained using MCMC:
# MCMC
hmc_kernel = NUTS(model, target_accept_prob = 0.9, max_tree_depth = 7)
mcmc = MCMC(hmc_kernel, num_samples = 1000, warmup_steps = 100, num_chains = 1)
mcmc.run(obs)
The results could then be analysed using:
mcmc.get_samples()
ojalgo BUG: constraint breaking weights while using the ConvexSolver in ojAlgo
I wanted to calculate the efficient frontier with equality and inequality constraints for given covariance matrix and expected returns. While doing so, the solver calculates weights that unfortunately seem to break the constraints. The overall risks and returns seem to be fine tho.
Primitive64Store tmpCOV = Primitive64Store.FACTORY.rows(new double[][]
{{0.04161599999999999,0.029532687342839998,0.027204649602,0.0077310803303999994,-1.1671155792E-4,-5.6919104880000004E-5,0.011540367260999999,-0.0011583596544,0.0016547567508,0.011887951467599998,-0.0021016779250799997,0.01217717900784,0.0354566417292},
{0.029532687342839998,0.034969,0.02882914496033,0.00673688572095,-1.5753286164E-4,-7.799020878E-5,0.008590460804089999,-0.00120903466574,0.0014109339056999999,0.0104683749489,-0.00296095338671,0.0075381371219,0.03405788034132},
{0.027204649602,0.02882914496033,0.028561000000000003,0.0095923333779,-1.4991438384E-4,-8.476063180000002E-5,0.009686536403049999,-0.0011563569438200001,9.994519899E-4,0.0082659675852,-3.1685615143000007E-4,0.00860292214782,0.03129653885267},
{0.0077310803303999994,0.00673688572095,0.0095923333779,0.011024999999999998,-5.7071784E-6,-1.6705374000000003E-5,0.0062936927109,-2.7820294949999996E-4,8.405088299999999E-5,6.61814874E-4,0.0037795989147,0.0055210432431,0.00810029447955},
{-1.1671155792E-4,-1.5753286164E-4,-1.4991438384E-4,-5.7071784E-6,1.6E-5,5.40282216E-6,-3.1099949760000004E-5,1.671549776E-5,-3.051312E-7,-7.517952359999999E-5,3.06077038E-5,1.3556312E-7,-2.345551502E-4},
{-5.6919104880000004E-5,-7.799020878E-5,-8.476063180000002E-5,-1.6705374000000003E-5,5.40282216E-6,4.0E-6,-1.7530565920000003E-5,3.13729976E-6,-4.841874E-6,-4.2240274199999995E-5,5.2011296E-6,3.44228808E-6,-1.3104418078000002E-4},
{0.011540367260999999,0.008590460804089999,0.009686536403049999,0.0062936927109,-3.1099949760000004E-5,-1.7530565920000003E-5,0.011881,4.4589363133999997E-4,9.499062894E-4,0.0036563291469,0.00251147898767,0.004586606807859999,0.0108024647181},
{-0.0011583596544,-0.00120903466574,-0.0011563569438200001,-2.7820294949999996E-4,1.671549776E-5,3.13729976E-6,4.4589363133999997E-4,0.001444,6.672796998E-4,-9.37965438E-5,0.00112122563298,-0.0011028714312,-0.0013360677697000002},
{0.0016547567508,0.0014109339056999999,9.994519899E-4,8.405088299999999E-5,-3.051312E-7,-4.841874E-6,9.499062894E-4,6.672796998E-4,9.0E-4,0.0013451552549999999,2.136913209E-4,4.8565227479999997E-4,0.0022035050261999998},
{0.011887951467599998,0.0104683749489,0.0082659675852,6.61814874E-4,-7.517952359999999E-5,-4.2240274199999995E-5,0.0036563291469,-9.37965438E-5,0.0013451552549999999,0.0081,-0.0018377539344,0.0026756272619999997,0.0138893569515},
{-0.0021016779250799997,-0.00296095338671,-3.1685615143000007E-4,0.0037795989147,3.06077038E-5,5.2011296E-6,0.00251147898767,0.00112122563298,2.136913209E-4,-0.0018377539344,0.004489000000000001,1.718834348E-4,-0.0025610649546900003},
{0.01217717900784,0.0075381371219,0.00860292214782,0.0055210432431,1.3556312E-7,3.44228808E-6,0.004586606807859999,-0.0011028714312,4.8565227479999997E-4,0.0026756272619999997,1.718834348E-4,0.023716,0.010939810005740002},
{0.0354566417292,0.03405788034132,0.03129653885267,0.00810029447955,-2.345551502E-4,-1.3104418078000002E-4,0.0108024647181,-0.0013360677697000002,0.0022035050261999998,0.0138893569515,-0.0025610649546900003,0.010939810005740002,0.051529000000000005}});
Primitive64Store tmpR = Primitive64Store.FACTORY.rows(new double[][] {{0.067},{0.045},{0.049},{0.01},{0.02},{1.0E-4},{0.034},{0.001},{0.006},{0.027},{0.005},{0.02},{0.04}});
Primitive64Store tmpEqualityMatrix = Primitive64Store.FACTORY.rows(new double[][]{{1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0}});
Primitive64Store tmpEqualityVector = Primitive64Store.FACTORY.rows(new double[][] {{1}});
Primitive64Store tmpInequalityMatrix = Primitive64Store.FACTORY.rows(new double[][] {{1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0},
{0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0},
{0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0},
{0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0},
{0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0},
{0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0},
{0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0},
{0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0},
{0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0},
{0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0},
{0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0},
{0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0},
{0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0},
{-1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0},
{0.0,-1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0},
{0.0,0.0,-1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0},
{0.0,0.0,0.0,-1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0},
{0.0,0.0,0.0,0.0,-1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0},
{0.0,0.0,0.0,0.0,0.0,-1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0},
{0.0,0.0,0.0,0.0,0.0,0.0,-1.0,0.0,0.0,0.0,0.0,0.0,0.0},
{0.0,0.0,0.0,0.0,0.0,0.0,0.0,-1.0,0.0,0.0,0.0,0.0,0.0},
{0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,-1.0,0.0,0.0,0.0,0.0},
{0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,-1.0,0.0,0.0,0.0},
{0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,-1.0,0.0,0.0},
{0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,-1.0,0.0},
{0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,-1.0},
{1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0},
{-1.0,-1.0,-1.0,-1.0,-1.0,-1.0,-1.0,-1.0,-1.0,-1.0,-1.0,-1.0,-1.0}});
Primitive64Store tmpInequalityVector = Primitive64Store.FACTORY.rows({{0.1},{0.2},{0.2},{0.1},{0.2},{0.1},{0.1},{1.0},{1.0},{1.0},{1.0},{0.1},{1.0},{0.0},{0.0},{0.0},{0.0},{0.0},{0.0},{0.0},{0.0},{0.0}{0.0}{0.0},{0.0},{0.0},{1.0},{0.0}});
//calculating the Portfolio with the maximum return, this always seems to work fine
ConvexSolver tmpSolverMinDiversification = new ConvexSolver.Builder(tmpCOV.multiply(0), tmpR)
.equalities(tmpEqualityMatrix, tmpEqualityVector).inequalities(tmpInequalityMatrix, tmpInequalityVector).build();
Optimisation.Result tmpResultMinDiversification = tmpSolverMinDiversification.solve();
//calculating the portfolio with minimal risk, this is where the error starts for me
ConvexSolver tmpSolverMaxDiversification = new ConvexSolver.Builder(tmpCOV, tmpR.multiply(0))
.equalities(tmpEqualityMatrix, tmpEqualityVector).inequalities(tmpInequalityMatrix, tmpInequalityVector).build();
Optimisation.Result tmpResultMaxDiversification = tmpSolverMaxDiversification.solve();
The Results im getting are as follows:
Weights of the portfolio with maximum return
OPTIMAL -0.0455 # { 0.1, 0.2, 0.2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.4999999999999999 }
Weights of the portfolio with minimal risk
OPTIMAL 1.9065360317211953E-4 # { -1.3215574127953212E-12, -3.0462153636587428E-12, 4.302357005267538E-12, 0.03119627351114846, 0.19999999989250858, 0.10000000045688691, -9.548402360338467E-13, 0.1370395162618518, 0.48451020053501254, 2.5326118583080594E-12, 0.03544670257704311, 0.011807307116290258, 6.086745301734428E-13 }
If i add an additional inequality (1 new row on both) constraint and run the calculation again
Primitive64Store tmpInequalityMatrix = Primitive64Store.FACTORY.rows(new double[][] {{1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0},
{0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0},
{0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0},
{0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0},
{0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0},
{0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0},
{0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0},
{0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0},
{0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0},
{0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0},
{0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0},
{0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0},
{0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,1.0},
{-1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0},
{0.0,-1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0},
{0.0,0.0,-1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0},
{0.0,0.0,0.0,-1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0},
{0.0,0.0,0.0,0.0,-1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0},
{0.0,0.0,0.0,0.0,0.0,-1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0},
{0.0,0.0,0.0,0.0,0.0,0.0,-1.0,0.0,0.0,0.0,0.0,0.0,0.0},
{0.0,0.0,0.0,0.0,0.0,0.0,0.0,-1.0,0.0,0.0,0.0,0.0,0.0},
{0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,-1.0,0.0,0.0,0.0,0.0},
{0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,-1.0,0.0,0.0,0.0},
{0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,-1.0,0.0,0.0},
{0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,-1.0,0.0},
{0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,-1.0},
{1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0,1.0},
{-1.0,-1.0,-1.0,-1.0,-1.0,-1.0,-1.0,-1.0,-1.0,-1.0,-1.0,-1.0,-1.0},
{-0.067,-0.045,-0.049,-0.01,-0.02,-1.0E-4,-0.034,-0.001,-0.006,-0.027,-0.005,-0.02,-0.04}});
Primitive64Store tmpInequalityVector = Primitive64Store.FACTORY.rows({{0.1},{0.2},{0.2},{0.1},{0.2},{0.1},{0.1},{1.0},{1.0},{1.0},{1.0},{0.1},{1.0},{0.0},{0.0},{0.0},{0.0},{0.0},{0.0},{0.0},{0.0},{0.0}{0.0}{0.0},{0.0},{0.0},{1.0},{0.0},{ -0.037349326902676616 }});
ConvexSolver tmpSolverMaxDiversification = new ConvexSolver.Builder(tmpCOV, tmpR.multiply(0))
.equalities(tmpEqualityMatrix, tmpEqualityVector).inequalities(tmpInequalityMatrix, tmpInequalityVector).build();
Optimisation.Result tmpResultEfficientPortfolio = tmpSolverMaxDiversification.solve();
i get the following result
OPTIMAL 0.004591387674786172 # { 0.10000000000095524, 0.0, 0.20000000000667748, -3.148187023285784E-12, 0.19999999988537426, -2.6699549475823382E-11, 0.09999999999830238, 3.2126828658623563E-12, 7.645985136245771E-12, 0.5204239501042672, -0.12042395010749231, 1.4422465593413845E-12, -3.1890111868219504E-12 }
Is there a way to avoid those constraint breaking weights? Should i have used an ExpressionBasedModel instead?
If you set debug(ConvexSolver.class) on an instance of Optimisation.Options and pass that to the build method of your ConvexSolver.Builder then you may get some info regarding what goes wrong. Optimisation.Options options = new Optimisation.Options(); options.debug(ConvexSolver.class); convexSolverBuilder.build(options);
PositionConstraint goal for robot arm: Unable to construct goal representation
I have a setting of ROS indigo, Gazebo under Ubuntu 14.04. Under ROS, moveit node is running. A robot arm IRB120 is simulated and standing in Gazebo. I have a node that uses moveit (move_group node) to plan a path (trajectory) for for the destination that Bob wants. The planned trajectory will be sent to Gazebo to be shown later.
There is two approaches that Bob can use to describe the destination:
Angles of each joints of the arm: using an array of six numbers (for six joints of the arm), the form of each joint and shin is defined. This approach works fine. It uses the JointConstraint class:
double goal_poses [] = {0.52, 0.50, 0.73, -0.02, 0.31, 6.83};
for(int i = 0 ; i < 6; i++) // iterate over joints of the arm.
{
moveit_msgs::JointConstraint jc;
jc.weight = 1.0;
jc.tolerance_above = 0.0001;
jc.tolerance_below = 0.0001;
jc.position = goal_poses[i];
jc.joint_name = names[i];
goal_constraint.joint_constraints.push_back(jc);
}
Define the location and direction of the end effector only. I can not use this approach. I have used the PositionConstraint class.
Problem in short: I can describe a destination using JointConstraint class, But I don't know how to describe it in PositionConstraint class. How to describe a goal, by just pointing out where the end effector should be?
How i describe the goal in PositionConstraint format: (I point out where the end effector should be and what it's orientation should be.)
moveit_msgs::PositionConstraint pc;
pc.weight = 1.0;
geometry_msgs::Pose p;
p.position.x = 0.3; // not sure if feasible position
p.position.y = 0.3; // not sure if feasible position
p.position.z = 0.3; // not sure if feasible position
pc.link_name="tool0";
p.orientation.x = 0;
p.orientation.y = 0;
p.orientation.z = 0;
p.orientation.w = 1;
pc.constraint_region.mesh_poses.push_back(p);
goal_constraint.position_constraints.push_back(pc);
But When the request is sent, server responds with:
[ERROR] [1527689581.951677797, 295.242000000]: Unable to construct goal representation
Note:
In both cases, I add the goal_constraint to the trajectory_request:
trajectory_request.goal.request.goal_constraints.push_back(goal_constraint);
// add other details to trajectory_request here...
trajectory_request is to be sent to the move_group. (by publishing the trajectory_request on the /move_group/goal topic)
A slightly different solution solved the problem of describing goal with end-effector orientation and location:
Instead of publishing the goal on a topic for another node to parse and read, we can use the moveit library function computeCartesianPath. (In this example the code to publish the trajectory is commented and partially missing)
void planTo(std::vector<double> coordinate, std::vector<double> orientation){
geometry_msgs::Pose p;
p.orientation.w = 1.0;
p.position.x = coordinate[0];
p.position.y = coordinate[1];
p.position.z = coordinate[2];
tf::Quaternion q = tf::createQuaternionFromRPY(
orientation[0],orientation[1],orientation[2]);
p.orientation.x = q.getX();
p.orientation.y = q.getY();
p.orientation.z = q.getZ();
p.orientation.w = q.getW();
std::vector<geometry_msgs::Pose> goals;
goals.push_back(p);
moveit::planning_interface::MoveGroup mg("manipulator");
mg.setStartStateToCurrentState();
// load the path in the `trajectory` variable:
moveit_msgs::RobotTrajectory trajectory;
mg.computeCartesianPath(goals, 0.01, 0.0, trajectory);
// publish to gazebo:
// trajectory.joint_trajectory.header.stamp = ros::Time::now();
// publisher.publish(trajectory.joint_trajectory);
}
I solved this a few months ago and unfortunately i do not remember the exact source/tutorial.
How to make a custom recurrent network in keras efficiently?
I have a function that takes an (image , hidden state) as an input, and feedback the output back to the input, This goes for the length of the sequence and the last output is of interest to me :- The recurrence function is as below:- def recurrence(): input_image = Input(shape=[dim1,dim2,dim3]) hidden_state= Input(shape=[4,4,4,128]) conv1a = Conv2D(filters=96, kernel_size=7, padding='same',input_shape=(dim1, dim2,dim3), kernel_initializer=k_init, bias_initializer=b_init)(input_image) conv1a = LeakyReLU(0.01)(conv1a) conv1b = Conv2D(filters=96, kernel_size=3, padding='same', kernel_initializer=k_init, bias_initializer=b_init)(conv1a) conv1b = LeakyReLU(0.01)(conv1b) conv1b = ZeroPadding2D(padding=(1, 1))(conv1b) pool1 = MaxPooling2D(2)(conv1b) flat6 = Flatten()(pool1) fc7 = Dense(units=1024, kernel_initializer=k_init, bias_initializer=b_init)(flat6) rect7 = LeakyReLU(0.01)(fc7) t_x_s_update_conv = Conv3D(128, 3, activation=None, padding='same', kernel_initializer=k_init, use_bias=False)(hidden_state) t_x_s_update_dense =Reshape((4,4,4,128))(Dense(units=8192)(rect7)) t_x_s_update = layers.add([t_x_s_update_conv, t_x_s_update_dense]) t_x_s_reset_conv = Conv3D(128, 3, activation=None, padding='same', kernel_initializer=k_init, use_bias=False)(hidden_state) t_x_s_reset_dense =Reshape((4,4,4,128))(Dense(units=8192)(rect7)) t_x_s_reset =layers.add([t_x_s_reset_conv, t_x_s_reset_dense]) update_gate = Activation(K.sigmoid)(t_x_s_update) comp_update_gate = Lambda(lambda x: 1 - x)(update_gate) reset_gate = Activation(K.sigmoid)(t_x_s_reset) rs = layers.multiply([reset_gate, hidden_state]) t_x_rs_conv = Conv3D(128, 3, activation=None, padding='same', kernel_initializer=k_init, use_bias=False)(rs) t_x_rs_dense = Reshape((4,4,4,128))(Dense(units=8192)(rect7)) t_x_rs = layers.add([t_x_rs_conv, t_x_rs_dense]) tanh_t_x_rs = Activation(K.tanh)(t_x_rs) gru_out = layers.add([layers.multiply([update_gate, hidden_state]), layers.multiply([comp_update_gate, tanh_t_x_rs])]) return Model(inputs=[input_image,hidden_state],outputs=gru_out) Currently i am achieving this using a loop and instantiating a model once and then reusing it like below:- step=recurrence() s_update = hidden_init for i in input_imgs: s_update = step([i, s_update])` But this method seems to be suffer from the following two disadvantages: i) It consumes a lot of GPU memory considering all variables are shared. ii) It doesn't work for an arbitrary length sequence without padding. Is there a better and efficient way to achieve this , I tried reading the code for SimpleRNN in the recurrent.py file , but couldn't understand how would i incorporate all these layers like Convolution3d into that framework? A dummy code would be really appreciated or any help you could provide. Thanks !
Neural network opencv 3.0
I am pretty new to neural networks in opencv. I read through the documentation and this is how i have implemented the training of the network
cv::Ptr<cv::ml::ANN_MLP> classifier = cv::ml::ANN_MLP::create();
ANN_MLP::Params params;
params.activateFunc = ANN_MLP::SIGMOID_SYM;
params.layerSizes = layers;
params.fparam1 = 0.6;
params.fparam2 = 1;
params.termCrit = TermCriteria(cv::TermCriteria::MAX_ITER+cv::TermCriteria::EPS, 1000, 0.000001);
params.trainMethod = ANN_MLP::Params::BACKPROP;
params.bpDWScale = 0.1;
params.bpMomentScale = 0.1;
params.rpDW0 = 0.1;
params.rpDWPlus = 1.2;
params.rpDWMinus = 0.5;
params.rpDWMin = FLT_EPSILON;
params.rpDWMax = 50.;
classifier->setParams(params);
int iterations = classifier->train(training_set,ROW_SAMPLE,training_set_classifications);
FileStorage fs("C:\\Users\\Hadi\\Downloads\\output\\param.xml", FileStorage::WRITE);
classifier->write(fs);
fs.release();
But on running of the code it is giving me abort error on training part. It is saying something like:
Expression Error : Vector subscript out of range
Can anybody please guide me to some tutorial for training and predicting of neural networks using opencv 3.0. Thank you.