PyBullet - How to set global coordinates of robot joints? - pybullet

I am new to PyBullet. Is there a way to set the positions of my robot joints using cartesian coordinates as input? The function below requires angles as its input. I have looked through the documentation at https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA/edit# but cant seem to find anything relevant. Any suggestions ?
p.resetJointStatesMultiDof(self._humans[human], idxes, values)

Related

C++ How to calculate an arc between two 3D points

I read through the forum and as I am sure this question has been asked before, but I couldn't really find what I was looking for.
My problem is the following:
I have an AI-Character moving along a spline. Should that path be blocked, the character should move in an arc around it and then continue on it's path.
For arguments sake lets assume that the spline has a length of 7000 units.
Therefore, I have two 3D (x,y,z) vectors. The first vector is the current position of the AI-bot and the second vector the position past the obstacle. For the time being lets just say: current spline position + 400 units; later on I could do a line trace to get the dimension of the obstacle etc. but for now I don't care about it.
Now I would like to compute an alternative path to avoid aforementioned obstacle - hence compute the arc between these two points - How do I do this?
I am really terrible at maths but looked at projectile trajectory because I thought that it would be sort of the same, just was unable to really understand it :<
It doesn't have to be an arc. You can solve this problem recursively in a very simple way.
Consider you're at position A, and the obstacle is at position B. You can do the following moves:
From current position to A+V(B[x]+height(B),0,0)
From current position to A+V(0,B[y]+width(B),0)
From current position to A+V(B[x]-height(B),0,0)
where V is a vector with components V(x,y,z), width(B) is the width of the obstacle and B[x] is the x component of the position of B. This way you moved around it along a rectangle. You can now smoothen the path by subdividing that rectangle in halves. 3 subdivisions are enough to make this smooth enough. To subdivide, take the middle point the first path, and draw a line to the middle of the second path. The same you do from the second path to the third one, and now your rectangle becomes an octagon. If that's not smooth enough, do a few more steps. This will create a new spline that you can use.
I would look at a combination of splines and the EQS system. The spline defines the ideal path to follow. The EQS system finds locations near or on the path, while still doing obstacle avoidance. EQS can return all valid destinations so you can manually order them by custom critera.
Actors set on a spline do work, but there's a whole bunch o' mess when making them stop following a spline, creating a new one at the correct point, attaching the actor the new spline, and so on.
I arrived at this conclusion yesterday after exactly going the messy way of adding spline points etc. The only problem i see is that I find the EQS system very difficult to understand. Not following the examples as such, but modifying it in the way I need it. Lets see, i keep you posted.

Vortex flow field in array

I implemented a grid of vectors in 2D (in Obj-C actually, but I guess it's not really language dependent) which I'm able to fill with Simplex noise for example, to generate a flow field for particles.
I've been searching around and googling a lot, but as I'm not a mathematics expert I can't find a way to fill my grid with a Vortex (or at least a circular) flow field.
For the circle flow I thought about getting my vectors from tangents of circles getting from the outer grid lines to the inner ones.
But, for the vortex I just can't find any solution by myself.
The vortex is supposed to be symmetric and centered in my grid, but if there's an easy way to make it asymmetric in the same grid, well...
Still some questions marks on the field. But if i get you right it is a velocity field you want. In other words a velocity in each vertex.
For circular field you only need to take the orthogonal relative position vector to create a to get a circular field.
v_t = (Py-Ry,Rx-Px)
where P is the position of the vertex and R is the center of the field. The suffix x and y is just the corresponding coordinates.
To add a radial component to the velocity field just add some velocity in the radial direction. I can't tell if this will be stable when you simulate.
The radial direction is easily described as
v_r = (Rx-Px,Ry-Py)
and the use
v = a*v_r+(1-a)*v_t
with a good value for a, probably rather low.

How to project a spherical map onto a sphere / cube: "Equirectangular to cubic"

UPDATE:
I found that, http://os.ivrpa.org/panosalado/wiki , has an implementation in java. Anyone who has something similar in c or c++?
I have this panorama, an spherical map from google streetview, and want to map this on a sphere/cube. Below are some examples and illustrations, what i seek is a library that can do it, or some implementation guides.
I tried http://krpano.com/docu/tutorials/quickstart/#top that gives the results listed at the bottom. It illustrates what i want, but the rotation axis is off. I need to create the views of direct ahead and back, left and right. Ideal i would like to map it to the sphere and tell it what angles to extract (the orientation of the cube).
[Back,Down,Front,Left,Right,Up]
You could do this easily in POV-Ray putting the camera in the middle of a sphere mapped with your texture. See image_map map_type 1 and e.g this example.
But really this is very easy to implement yourself, assuming the input images are some sort of cylindrical equidistant or equirectangular projection: for each (x,y) in the output image you are rendering, just use the inverse formulas to compute a (longitude,latitude) in the input image and interpolate/copy over a pixel value.

How to get curve from intersection of point cloud and arbitrary plane?

I have various point clouds defining RT-STRUCTs called ROI from DICOM files. DICOM files are formed by tomographic scanners. Each ROI is formed by point cloud and it represents some 3D object.
The goal is to get 2D curve which is formed by plane, cutting ROI's cloud point. The problem is that I can't just use points which were intersected by plane. What I probably need is to intersect 3D concave hull with some plane and get resulting intersection contour.
Is there any libraries which have already implemented these operations? I've found PCL library and probably it should be able to solve my problem, but I can't figure out how to achieve it with PCL. In addition I can use Matlab as well - we use it through its runtime from C++.
Has anyone stumbled with this problem already?
P.S. As I've mentioned above, I need to use a solution from my C++ code - so it should be some library or matlab solution which I'll use through Matlab Runtime.
P.P.S. Accuracy in such kind of calculations is really important - it will be used in a medical software intended for work with brain tumors, so you can imagine consequences of an error (:
You first need to form a surface from the point set.
If it's possible to pick a 2d direction for the points (ie they form a convexhull in one view) you can use a simple 2D Delaunay triangluation in those 2 coordinates.
otherwise you need a full 3D surfacing function (marching cubes or Poisson)
Then once you have the triangles it's simple to calculate the contour line that a plane cuts them.
See links in Mesh generation from points with x, y and z coordinates
Perhaps you could just discard the points that are far from the plane and project the remaining ones onto the plane. You'll still need to reconstruct the curve in the plane but there are several good methods for that. See for instance http://www.cse.ohio-state.edu/~tamaldey/curverecon.htm and http://valis.cs.uiuc.edu/~sariel/research/CG/applets/Crust/Crust.html.

Finding object under mouse

I'm developing a game that basically has its entire terrain made out of AABB boxes. I know the verticies, minimum, and maximum of each box. I also set up my camera like this:
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
glRotatef(Camera.rotx,1,0,0);
glRotatef(Camera.roty,0,1,0);
glRotatef(Camera.rotz,0,0,1);
glTranslatef(-Camera.x,-Camera.y,-Camera.z);
What I'm trying to do is basically find the cube the mouse is on. I thought about giving the mouse position a forward directional vector and simply iterating through until the 'mouse bullet' hits something. However this envolves interating through all objects several times. Is there a way I could do it by only iterating through all the objects once?
Thanks
This is usually referred to as 'picking' This here looks like a good gl based link
If that is tldr, then a basic algorithm you could use
sort objects by z (or keep them sorted by z, or depth buffer tricks etc)
iterate and do a bounds test, stopping when you hit the first one.
This is called Ray Tracing (oops, my mistake, it's actually Ray Casting). Every Physics engine has this functionality. You can look at one of the simplest - ODE, or it's derivative - Bullet. They are open-source so you can take out what you don't need. They both have a handy math library that handles all oftenly needed matrix and vertex operations.
They all have demos on how to do exactly this task.
I suggest you consider looking at this issue from a bigger perspective.
The boxes are just points at a lower resolution. The trick is to reduce the resolution of the mouse to figure out which box it is on.
You may have to perform a 2d to 3d conversion (or vice versa). In most games, the mouse lives in a 2d coordinate world. The stuff "under" the mouse is a 2d projection of a 3d universe.
You want to use a 3D picking algorithm. The idea is that you draw a ray from the user's position in the virtual world in the direction of the click. This blog post explains very clearly how to implement such an algorithm. Essentially your screen coordinates need to be transformed from the screen space to the virtual world space. There's a website that has a very good description about the various transformations involved and I can't post the link due to my rank. Search for book of hook's mouse picking algorithm [I do not own the site and I haven't authored the document].
Once you get a ray in the desired direction, you need to perform tests for intersection with the geometries in the real world. Since you have AABB boxes entirely, you can use simple vector equations to check which geometry intersects the ray. I would say that approximating your boxes as a sphere would make life very easy since there is a very simple sphere-ray intersection test. So, your ray would be described by what you obtain from the first step (the ray drawn in the first step) and then you would need to use an intersection test. If you're ok with using spheres, the center of the sphere would be the point you draw your box and the diameter would be the width of your box.
Good Luck!