How to Rotate a object3D in PyOpenGL? - opengl

I am trying to rotate one object in the X-axis, but I don't get.
I have a class Object 3D it's:
class Object3D():
implements( IRenderizable )
def __init__(self, parameters={} ):
self.parameters = parameters
self.ID= Engine().addObject3D()
self.parent = None
self.childrens =[]
self.position = (parameters.get('POSITION') is None ) is True and Vector4() or parameters.get('POSITION')
self.rotation = (parameters.get('ROTATION') is None ) is True and Quaternion() or parameters.get('ROTATION')
self.direction = Vector4()
self.scale = Vector3(1,1,1)
self.matrix = Matrix4()
self.matrixLocal = Matrix4()
self.matrixWorld = Matrix4()
self.matrixRotationWorld = Matrix4()
self.up = Vector3(0,1,0 )
self.parameters =parameters
self.rotationAngle= 10.
self.currentMatrix = None
self.initCurrentMatrix()
def initCurrentMatrix(self):
glPushMatrix()
glLoadIdentity()
self.currentMatrix = glGetDoublev(GL_MODELVIEW_MATRIX)
glPopMatrix()
return
def setID(self, Id ):
self.ID = Id
def moveTo( self, x,y,z ):
v=Vector4(x,y,z)
#self.position.addSelf( self.rotation.rotateVector(v) )
self.position.addSelf( v )
glPushMatrix()
glLoadIdentity()
glTranslatef( float(self.position.x),float(self.position.y),float(self.position.z) )
self.currentMatrix = glGetDoublev(GL_MODELVIEW_MATRIX)
glPopMatrix()
return self
def render(self):
pass
In this chunk of code you see how to implements the rotation:
def rotateX(self, angle=2 ):
glPushMatrix()
glRotated( angle, 0,1,0)
glPopMatrix()
return self
when Vector4, Vector3 , Quaternion, Matrix4 are my own classes.
what this my mistake? and How to make a Rotation?

I don't know, whether it helps, but the general workflow for moving or rotating an object is like the following:
draw static stuff
push actual matrix on stack (glPushMatrix)
load identity matrix (initial matrix) (glLoadIdentity)
use your own matrix and load and set it as the actual matrix
transform the actual matrix via glRotate / gl....
save the actual matrix as your own matrix
draw your object
pop matrix from stack (glPopMatrix)
draw rest of the static stuff
In step 5/6 you have updated your transformation matrix. This is necessary, because glRotate is like a makro for multiply an rotation matrix onto the actual transformation matrix. If you always load the identity matrix and then do an glRotate then it transforms the identity matrix by only the given degree --> your object will be drawn rotated by this degree and then never do something else - i guess, this is your fault...
if you use the 9 steps above, the matrix for your object will be multiplied by the rotation matrix and the result is used for the next multiply in the next iteration step.
I hope, this will help in general understanding =)

In your rotate func, it should be degrees not angle:
glRotatef(degrees, 0, 0, -1)
Does that work?

what this my mistake?
You mistake OpenGL for a math library, which it is not. Do not use OpenGL for doing math on matrices you keep around in some variable. Actually, don't use OpenGL matrix functions at all. They're part of the fixed function pipeline and have been removed from later versions of OpenGL.

Related

Rotation between two frame similar to interactive markers

What do I want to do?
I work with a Franka Emika Panda and use the "cartesian_impedance_example_controller" with its "equilibrium_pose" topic to move the panda arm.
I want to use a command to rotate the arm along its axes of the "panda_rightfinger" joint axes (axis of interactive marker seen in picture). The roation only happens around the axis and happens by pressing a specific button.
(Right finger frame with the interactive marker around it and panda_link0 frame on the left)
How do I do it?
The rotation quaternion gets created by a function that uses following script:
axis = {
"roll": 0,
"pitch": 0,
"yaw": 0
}
def pyr_producer(self, gesture_msg):
global axis
axis[gesture_msg.cls] += 1 * 0.01
return list(axis.values())
def get_quaternion(self, gesture_msg):
roll, pitch, yaw = pyr_producer(gesture_msg)
q_rot = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
return Quaternion(*q_rot)
Afterwards, this rotation quaterion will be used by another script and gets published to the corresponding equilibrium_pose topic.
This part of the script calculates the rotation:
eq_pose: the new pose that will be used for the topic
current_goal_pose: the pose that contains the actual rotation
last_goal_pose: the pose that contains the last rotation
eq_pose.pose.position = last_goal_pose.pose.position
eq_pose.pose.orientation = orientation_producer.get_quaternion(goal_pose.gesture)
# calculate the relative quaternion from the last pose to the new pose
# (see http://wiki.ros.org/tf2/Tutorials/Quaternions)
# add relative rotation quaternion to the new equilibrium orientation by multiplying
q_equilibrium = [eq_pose.pose.orientation.x, eq_pose.pose.orientation.y,
eq_pose.pose.orientation.z, eq_pose.pose.orientation.w]
q_2 = [current_goal_pose.pose.orientation.x, current_goal_pose.pose.orientation.y,
current_goal_pose.pose.orientation.z, current_goal_pose.pose.orientation.w]
# Negate w value for inverse
q_1_inv = [last_goal_pose.pose.orientation.x, last_goal_pose.pose.orientation.y,
last_goal_pose.pose.orientation.z, (-1)*last_goal_pose.pose.orientation.w]
q_relative = tf.transformations.quaternion_multiply(q_2, q_1_inv)
q_equilibrium = tf.transformations.quaternion_multiply(q_relative, q_equilibrium)
eq_pose.pose.orientation.x = q_equilibrium[0]
eq_pose.pose.orientation.y = q_equilibrium[1]
eq_pose.pose.orientation.z = q_equilibrium[2]
eq_pose.pose.orientation.w = q_equilibrium[3]
# update last pose
last_goal_pose = current_goal_pose
# Only publish poses when there is an interaction
eq_publisher.publish(eq_pose)
The eq_pose gets generated by this part:
def franka_state_callback(msg):
global eq_pose
global initial_eq_pose_found
# the initial pose has to be retrieved only once
if initial_eq_pose_found:
return
initial_quaternion = \
tf.transformations.quaternion_from_matrix(
np.transpose(np.reshape(msg.O_T_EE,
(4, 4))))
initial_quaternion = initial_quaternion / np.linalg.norm(initial_quaternion)
eq_pose.pose.orientation.x = initial_quaternion[0]
eq_pose.pose.orientation.y = initial_quaternion[1]
eq_pose.pose.orientation.z = initial_quaternion[2]
eq_pose.pose.orientation.w = initial_quaternion[3]
eq_pose.pose.position.x = msg.O_T_EE[12]
eq_pose.pose.position.y = msg.O_T_EE[13]
eq_pose.pose.position.z = msg.O_T_EE[14]
initial_eq_pose_found = True
rospy.loginfo("Initial panda pose found: " + str(initial_eq_pose_found))
rospy.loginfo("Initial panda pose: " + str(eq_pose))
if __name__ == "__main__":
state_sub = rospy.Subscriber("/panda/franka_state_controller/franka_states", FrankaState, franka_state_callback)
while not initial_eq_pose_found:
rospy.sleep(1)
state_sub.unregister()
What actually happens
The rotation itself works, but only happens around the "panda_link0" axis, which is the fixed position of the panda foot. The rotation should be the same like the one around the interactive marker in the interactive marker example.
Final Question
So I want to know, how to calculate the quaternions for this rotation?
I am quite new to robotics and hope my description was clear.
Okay, I just found my mistake, as expected, it was very easy:
The multiplication of quaternions is not cummutative. With respect to that, I just had to change the calculation of the quaternion from
q_equilibrium = tf.transformations.quaternion_multiply(q_relative, q_equilibrium)
to
q_equilibrium = tf.transformations.quaternion_multiply(q_equilibrium,q_relative)

c++ quaternion clarification

I'm working on a flight simulator. I've read a tutorial about quaternions, ( this one : http://www.opengl-tutorial.org/intermediate-tutorials/tutorial-17-quaternions/ ), so It it very new for me.
From what I've understand, quaternions should rotate an object using a direction vector and make the object's orientation match the direction, and rotate the quaternion should not move the vector, but just make him turn around himself. Am I true?
If yes, this is my code :
void Plane::Update()
{
m_matrix = GLM_GTX_quaternion::toMat4( GLM_GTX_quaternion::angleAxis( radians( m_angle ), normalize( m_planeDirection ) ) );
}
When my plane model's is pointing to x vector, my plane will rotate correctly around the x vector with the angle equal to 0, but if I change the angle, it will not rotate correctly. So how can I find the angle?
Yes, you are correct - Quaternion rotates an object around a directional vector. You should also use the glm::quat typedef when working with quaternions.
#include <glm/gtc/quaternion.hpp>
//...
glm::mat4 m = glm::mat4_cast(glm::angleAxis(glm::radians(m_angle), glm::normalize(m_planeDirection)));
glm::rotate function also works with quaternions
glm::mat4 m = glm::mat4_cast(glm::rotate(glm::quat(), 45.0f, glm::vec3(1.0f)));

Transformation hierarchy causes object to jump in space?

I have two objects that I want to parent together so that Tri is a child of Torus. When I do so and multiply the matricies together by adding the parents modelView to the childs, the child jumps in space initially, over to the right and up by a few units. Where do I insert an offset into this, and how do I calculate it?
obj = make_shared<Object>(*this);
obj->rename("tri");
obj->type->val_s = "tri";
obj->t->val_3 = glm::vec3(-4.f, 1.5f, 0.f);
allObj.push_back(obj);
obj = make_shared<Object>(*this);
obj->rename("torus");
obj->type->val_s = "obj";
obj->t->val_3 = glm::vec3(3.f, 2.f, 0.f);
allObj.push_back(obj);
//Matrix
scaleM = glm::scale(glm::mat4(), s->val_3);
rotationM = glm::toMat4(r_quat);
glm::vec3 usablePivot = t->val_3 - pivot->val_3;
glm::mat4 localAxis1M = glm::translate(glm::mat4(), usablePivot);
glm::mat4 localAxis2M = glm::translate(glm::mat4(), -usablePivot);
translationM = glm::translate(glm::mat4(), t->val_3);
modelM = translationM * localAxis2M * rotationM * scaleM * localAxis1M;
//View
usableView = myGL->ViewM;
//Projection
usableProjection = myGL->ProjectionM;
//MVP
if (parent == "world") { MVP = usableProjection * usableView * modelM; }
else { MVP = usableProjection * usableView * parentTo->modelM * modelM; }
Matrix order in mine OpenGL apps:
sub-object matrices
the lowest in ownership hierarchy are first
they are local to their parents
their have offset to point (0,0,0) of parent space
if not then and they are local to world then this is the jump reason
in that case the sub-objects are not really sub objects
and handle them as normal object without parent
object to world
is transform matrix of objects without parent
they are local to your scene world
then goes camera view
it is inverse of camera space
where Z-axis is view direction
last is projection
like gluPerspective(...)
this one is usually stored in GL_PROJECTION matrix on fixed pipeline
instead of GL_MODELVIEW
Clipping is done by OpenGL separately via glViewport
(your usable view I think)
Look for more info here
you can check the content of your matrices
look at positions 12,13,14 where the offset vector is stored
do not forget that this vector is local to parent coordinate system!!!
so it is more likely rotated by it ...
also a good idea is to draw axis lines for each tested matrix in its parent space
to see if they are correct
I use red,gree,blue lines for x,y,z - axis
just extract origin of coordinate system [12,13,14]
and draw line from it to the same point + a*axis vector
a is line length (big enough so you see a line not a point)
axis vectors are at positions x=[0,1,2], y=[4,5,6], z=[8,9,10]
do not forget to set matrices to parent coordinate system !!!
if you handle matrices your self via GLSL then do not forget that
direction vectors like Normals are transformed without offset
so get the whole transform matrix (all multilicated together without projection and sometimes also camera)
set the offset to zero [12,13,14]=(0.0,0.0,0.0)
and multiply by this matrix

Rotating the LookAt vector of gluLookAt

I'm a student new to opengl. Currently, I'm doing a project that creates a scene.
Right now, my team is using gluLookAt() for my camera. What I want to accomplish is to try and rotate the LookAt vector around a certain point, namely where the camera is looking at.
This accomplishes a sort of "swaying in a circle". I need this because I am making a dart game for the scene, and my camera stay still, but I need it to move in a circle, but still allow the user's mouse to influence it. I also need it to create a drunken movement. That is why I am not considering rotating the Up or Eye vectors.
Currently, my look at code is like this.
int deltax = x - mouse.mX;
int deltay = y - mouse.mY;
cameradart.mYaw -= ((deltax/360.0) * 3.142) * 0.5;
cameradart.mPitch -= deltay * 0.02;
mouse.mX = x;
mouse.mY = y;
cameradart.lookAt.x = sin (cameradart.mYaw);
cameradart.lookAt.y = cameradart.mPitch ;
cameradart.lookAt.z = cos (cameradart.mYaw);
gluLookAt (cameradart.eye.x, cameradart.eye.y, cameradart.eye.z,
cameradart.eye.x + cameradart.lookAt.x, cameradart.eye.y + cameradart.lookAt.y,
cameradart.eye.z + cameradart.lookAt.z,
cameradart.up.x, cameradart.up.y, cameradart.up.z);
I know that it could be done easier using a different camera, but I really don't want to mess with my team's code by not using gluLookAt().
There's a couple of solutions in my mind, I'll tell you the easiest to understand/implement as a new graphics student
Assuming at first you're looking at (0,0,1) -store that vector-:
Think of a point that's drawing a circle and you're looking at it,
-Do it first to turn right and left (2D on X & Z)
-Let HDiff be the horizontal difference between old mouse position and the new one
-Update the x = cos(HDiff)
-Update the z = sin(HDiff)
*I didn't try it but it should work :)
If you want to be able to manipulate the camera, using a camera matrix is a much more effective mechanism than working with gluLookat. GLM is a good library for matrix and vector math and includes a lookat mechanism you can use to initialize the matrix, or you can just initialize it with a series of operations. However, remember that lookat produces a view matrix, and the view matrix is the inverse of the camera matrix.
This piece of code has a demonstration of what I'm talking about. Specifically look at the player member variable and how it's manipulated
glm::mat4 player;
...
glm::vec3 playerPosition(0, eyeHeight, ipd * 4.0f);
player = glm::inverse(glm::lookAt(playerPosition, glm::vec3(0, eyeHeight, 0), GlUtils::Y_AXIS));
This approach lets you apply changes like rotation and translation directly to the player matrix
// Rotate on the Y axis
player = glm::rotate(player, angle, glm::vec3(0, 1, 0));
This is much more intuitive than manipulating the view matrix, since changes to the view matrix always have to be the inverse of what you'd do to the player matrix.
When you're ready to render you need to convert the player matrix to a view matrix by taking it's inverse. In my example it's done like this:
gl::Stacks::modelview().top() = riftOrientation * glm::inverse(player);
This is because I'm using an application based modelview matrix stack that gets applied to the Shader programs I'm running.
For an OpenGL 1.x program, you'd instead use LoadMatrix
glMatrixMode(GL_MODELVIEW);
glm::mat4 modelview = glm::inverse(player);
glLoadMatrixf(&modelview);

rotating model while color updating

In OpenGL, I draw tree(composed of lines) like structure(neurons), and based on activity, i apply a color on each of the branches of the tree. Each branch portion may have a different color at an instance. I keep record of the color at an instance corresponding to the branch.
I am able to rotate (by changing the modelview matrix) the tree. I can see the changing color (activity) on the branches when updating.
However, i am not able to rotate the tree while the color change is happening. (the rotation is seen after the complete updating) This is also true with translating the model, not able to translate while updating.
How should i be doing this to see them simultaneously?
i call the updateViz function to update the colors.
def render(self):
glMatrixMode(GL_MODELVIEW)
glLoadIdentity()
self.renderAxis() #draws 3 axes at origin
for obj in self.vizObjects:
glPushMatrix()
glColor(obj.r, obj.g, obj.b)
glLineWidth(2)
glBegin(GL_LINES)
glVertex3f(obj.l_coords[0],obj.l_coords[1],obj.l_coords[2])
glVertex3f(obj.l_coords[3],obj.l_coords[4],obj.l_coords[5])
glEnd()
glPopMatrix()
def updateViz(self,vals):
inds = digitize(vals,colrMapVals)
for i in range(0,len(self.vizObjects)):
self.vizObjects[i].r,self.vizObjects[i].g,self.vizObjects[i].b= colorMap[inds[i]-1]
def translate(self, _trans):
self.makeCurrent()
glMatrixMode(GL_MODELVIEW)
glLoadIdentity()
glTranslated(_trans[0], _trans[1], _trans[2])
glMultMatrixd(self.modelview_matrix_)
self.modelview_matrix_ = glGetDoublev(GL_MODELVIEW_MATRIX)
self.translate_vector_[0] = self.modelview_matrix_[3][0]
self.translate_vector_[1] = self.modelview_matrix_[3][1]
self.translate_vector_[2] = self.modelview_matrix_[3][2]
def rotate(self, _axis, _angle):
#self.modelview_matrix_ = glGetDoublev(GL_MODELVIEW_MATRIX)
t = [self.modelview_matrix_[0][0] * self.center_[0] +
self.modelview_matrix_[1][0] * self.center_[1] +
self.modelview_matrix_[2][0] * self.center_[2] +
self.modelview_matrix_[3][0],
self.modelview_matrix_[0][1] * self.center_[0] +
self.modelview_matrix_[1][1] * self.center_[1] +
self.modelview_matrix_[2][1] * self.center_[2] +
self.modelview_matrix_[3][1],
self.modelview_matrix_[0][2] * self.center_[0] +
self.modelview_matrix_[1][2] * self.center_[1] +
self.modelview_matrix_[2][2] * self.center_[2] +
self.modelview_matrix_[3][2]]
self.makeCurrent()
glLoadIdentity()
glTranslatef(t[0], t[1], t[2])
glRotated(_angle, _axis[0], _axis[1], _axis[2])
glTranslatef(-t[0], -t[1], -t[2])
glMultMatrixd(self.modelview_matrix_)
self.modelview_matrix_ = glGetDoublev(GL_MODELVIEW_MATRIX)
def keyPressEvent(self, ev):
if (ev.key() == QtCore.Qt.Key_Left):
self.translate([-0.25, 0.0, 0.0])
self.updateGL()
elif (ev.key() == QtCore.Qt.Key_Right):
self.translate([0.25, 0.0, 0.0])
self.updateGL()
elif (ev.key() == QtCore.Qt.Key_A):
self.rotate([1.0, 0.0, 0.0],2.0)
self.updateGL()
elif (ev.key() == QtCore.Qt.Key_Q):
self.rotate([1.0, 0.0, 0.0],-2.0)
self.updateGL()
I use PyQt4 and PyGLWidget
your code looks all right to me. I think the problem is caused by how you update the rotation and color. OpenGL is a state machine, you feed it with all kinds of info (vertex pos, color, etc) and things get rendered, so MAKE SURE updateVis and translate/rotate are called on EVERY FRAME.
maybe your rotation/translation depends on Qt's signal/event mechanism, and it's not called every frame?
OpenGL is not a scene graph, in which you update rotation or translation. Those glRotate, glTranslate calls, they are meant to be called from the drawing function NOT in a event handler. Update some variables in the event handler, issue a redraw and then in the drawing code call glTranslate/glRotate according to the values in the variable.