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;