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;
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 __init__(self, width=0.2, height=0.1, depth=0.3, step=0.04, angle_step=_n.pi/6): _engine.Object_model.__init__(self) self.step=step self.angle_step=angle_step self.x_length=width self.y_length=height self.z_length=depth #create pose matrices xy_top_pose=_n.identity(4) xy_bottom_pose=_n.identity(4) xz_right_pose=_n.identity(4) xz_left_pose=_n.identity(4) yz_front_pose=_n.identity(4) yz_back_pose=_n.identity(4) #orientation xy_bottom_pose[:3,:3]=_engine.rotx(_n.pi) xz_right_pose[:3,:3]=_engine.rotx(-_n.pi/2) xz_left_pose[:3,:3]=_engine.rotx(_n.pi/2) yz_front_pose[:3,:3]=_engine.roty(_n.pi/2) yz_back_pose[:3,:3]=_engine.roty(-_n.pi/2) #position xy_top_pose[:3,3]=_n.array([0,0,self.z_length/2]) xy_bottom_pose[:3,3]=_n.array([0,0,-self.z_length/2]) xz_right_pose[:3,3]=_n.array([0,self.y_length/2,0]) xz_left_pose[:3,3]=_n.array([0,-self.y_length/2,0]) yz_front_pose[:3,3]=_n.array([self.x_length/2,0,0]) yz_back_pose[:3,3]=_n.array([-self.x_length/2,0,0]) #visualization xy_top_face=Framed_face(self.x_length, self.y_length, self.step, self.angle_step) xy_bottom_face=Framed_face(self.x_length, self.y_length, self.step, self.angle_step) xz_right_face=Framed_face(self.x_length, self.z_length, self.step, self.angle_step) xz_left_face=Framed_face(self.x_length, self.z_length, self.step, self.angle_step) yz_front_face=Framed_face(self.z_length, self.y_length, self.step, self.angle_step) yz_back_face=Framed_face(self.z_length, self.y_length, self.step, self.angle_step) xy_top_face.set_trans_rot_matrix(xy_top_pose) xy_bottom_face.set_trans_rot_matrix(xy_bottom_pose) xz_right_face.set_trans_rot_matrix(xz_right_pose) xz_left_face.set_trans_rot_matrix(xz_left_pose) yz_front_face.set_trans_rot_matrix(yz_front_pose) yz_back_face.set_trans_rot_matrix(yz_back_pose) self.add_object_model(xy_top_face) self.add_object_model(xy_bottom_face) self.add_object_model(xz_right_face) self.add_object_model(xz_left_face) self.add_object_model(yz_front_face) self.add_object_model(yz_back_face)