def update_transformations(self):
next_link_trans_rot=self.trans_rot_matrix
for segment,angle in zip(self.segments,self.angles):
rot_frame=_n.identity(4)
if segment.joint_type=="RotX":
rot_frame[:3,:3]=_engine.rotx(angle)
if segment.joint_type=="RotY":
rot_frame[:3,:3]=_engine.roty(angle)
if segment.joint_type=="RotZ":
rot_frame[:3,:3]=_engine.rotz(angle)
segment.trans_rot_matrix=_n.dot(next_link_trans_rot,rot_frame)
next_link_trans_rot=_n.dot(segment.trans_rot_matrix,segment.link_trans_rot)
self.last_trans_rot=next_link_trans_rot
def generate_frames(self):
x_sample_cant=int(self.width/self.step)
y_sample_cant=int(self.height/self.step)
theta_sample_cant=int(2*_n.pi/self.angular_step)
x=self.x_start
y=self.y_start
theta=0
for i in xrange(x_sample_cant+1):
y=self.y_start
theta=0
for j in xrange(y_sample_cant+1):
theta=0
for k in xrange(theta_sample_cant+1):
pose_matrix=_n.identity(4)
pose_matrix[:3,:3]=_engine.rotz(theta)
pose_matrix[:3,3]=_n.array([x,y,0])
new_frame=Frame()
new_frame.scale=[0.01,0.01,0.01]
new_frame.set_trans_rot_matrix(pose_matrix)
self.add_object_model(new_frame)
theta+=self.angular_step
y+=self.step
x+=self.step
def generate_frames(self):
#this will generate the angles
#the following are the angles (like in polar coordinates)
phi=0 #from 0 to pi/2
theta=0 #from 0 to pi
psi=0 # from 0 to 2pi but half resolution
phi_sample_cant=int((self.end-self.start)/self.step)
theta_sample_cant=int(_n.pi/self.step)
psi_sample_cant=int(_n.pi/self.step)
pose_matrix=_n.identity(4)
rel_pos_ini=_n.array([0.0,0.0,1.0]) #relative pose
self.list_of_poses=[]
for i in xrange(phi_sample_cant+1):
theta=0
psi=0
for j in xrange(theta_sample_cant+1):
psi=0
for k in xrange(psi_sample_cant):
#this will iterate over the angles, now generate the poses.
#generate orientation part
rot_matrix=_n.dot(_n.dot(_engine.rotx(phi-_n.pi/2), _engine.roty(theta-_n.pi/2)), _engine.rotz(psi))
rel_pos=_n.dot(rot_matrix, rel_pos_ini)
pos=self.position-self.radius*rel_pos
pose_matrix=_n.identity(4)
pose_matrix[:3,:3]=rot_matrix
pose_matrix[:3,3]=pos
new_frame=Frame()
new_frame.scale=[.01,.01,.01]
new_frame.set_trans_rot_matrix(pose_matrix)
self.add_object_model(new_frame)
#self.list_of_frames.append(new_frame)
psi+=self.step;
theta+=self.step;
phi+=self.step;
