def tearDown(self): # delete the temp folders and files created rob1_path = filemgr.get_folder_path(self.rob1) rob2_path = filemgr.get_folder_path(self.rob2) if os.path.exists(rob1_path): shutil.rmtree(rob1_path) if os.path.exists(rob2_path): shutil.rmtree(rob2_path)
def test_get_folder_path(self): rob1_clean = filemgr.get_clean_name(self.rob1) rob2_clean = filemgr.get_clean_name(self.rob2) # scenario 1 self.assertEqual(filemgr.get_folder_path(self.rob1), os.path.join(self.base_path, rob1_clean)) # scenario 2 self.assertEqual(filemgr.get_folder_path(self.rob2), os.path.join(self.base_path, rob2_clean))
def test_get_folder_path(self): rob1_clean = filemgr.get_clean_name(self.rob1) rob2_clean = filemgr.get_clean_name(self.rob2) # scenario 1 self.assertEqual( filemgr.get_folder_path(self.rob1), os.path.join(self.base_path, rob1_clean) ) # scenario 2 self.assertEqual( filemgr.get_folder_path(self.rob2), os.path.join(self.base_path, rob2_clean) )
def set_directory(self, path=None): if path is None or not os.path.isdir(path): directory = filemgr.get_folder_path(self.name) else: directory = path self.directory = directory return directory
def OnNew(self, event): dialog = ui_definition.DialogDefinition( ui_labels.MAIN_WIN['prog_name'], self.robo.name, self.robo.nl, self.robo.nj, self.robo.structure, self.robo.is_floating, self.robo.is_mobile ) if dialog.ShowModal() == wx.ID_OK: result = dialog.get_values() new_robo = Robot( name=result['name'], NL=result['num_links'], NJ=result['num_joints'], NF=result['num_frames'], structure=result['structure'], is_floating=result['is_floating'], is_mobile=result['is_mobile'] ) new_robo.set_defaults(base=True) if result['keep_geo']: nf = min(self.robo.NF, new_robo.NF) new_robo.ant[:nf] = self.robo.ant[:nf] new_robo.sigma[:nf] = self.robo.sigma[:nf] new_robo.mu[:nf] = self.robo.mu[:nf] new_robo.gamma[:nf] = self.robo.gamma[:nf] new_robo.alpha[:nf] = self.robo.alpha[:nf] new_robo.theta[:nf] = self.robo.theta[:nf] new_robo.b[:nf] = self.robo.b[:nf] new_robo.d[:nf] = self.robo.d[:nf] new_robo.r[:nf] = self.robo.r[:nf] if result['keep_dyn']: nl = min(self.robo.NL, new_robo.NL) new_robo.Nex[:nl] = self.robo.Nex[:nl] new_robo.Fex[:nl] = self.robo.Fex[:nl] new_robo.FS[:nl] = self.robo.FS[:nl] new_robo.IA[:nl] = self.robo.IA[:nl] new_robo.FV[:nl] = self.robo.FV[:nl] new_robo.MS[:nl] = self.robo.MS[:nl] new_robo.M[:nl] = self.robo.M[:nl] new_robo.J[:nl] = self.robo.J[:nl] if result['keep_joint']: nj = min(self.robo.NJ, new_robo.NJ) new_robo.eta[:nj] = self.robo.eta[:nj] new_robo.k[:nj] = self.robo.k[:nj] new_robo.qdot[:nj] = self.robo.qdot[:nj] new_robo.qddot[:nj] = self.robo.qddot[:nj] new_robo.GAM[:nj] = self.robo.GAM[:nj] if result['keep_base']: new_robo.Z = self.robo.Z new_robo.w0 = self.robo.w0 new_robo.wdot0 = self.robo.wdot0 new_robo.v0 = self.robo.v0 new_robo.vdot0 = self.robo.vdot0 new_robo.G = self.robo.G new_robo.set_defaults(joint=True) self.robo = new_robo self.robo.directory = filemgr.get_folder_path(self.robo.name) self.feed_data() dialog.Destroy()
def OnNew(self, event): dialog = ui_definition.DialogDefinition( ui_labels.MAIN_WIN['prog_name'], self.robo.name, self.robo.nl, self.robo.nj, self.robo.structure, self.robo.is_mobile ) if dialog.ShowModal() == wx.ID_OK: result = dialog.get_values() new_robo = Robot(*result['init_pars']) if result['keep_geo']: nf = min(self.robo.NF, new_robo.NF) new_robo.ant[:nf] = self.robo.ant[:nf] new_robo.sigma[:nf] = self.robo.sigma[:nf] new_robo.mu[:nf] = self.robo.mu[:nf] new_robo.gamma[:nf] = self.robo.gamma[:nf] new_robo.alpha[:nf] = self.robo.alpha[:nf] new_robo.theta[:nf] = self.robo.theta[:nf] new_robo.b[:nf] = self.robo.b[:nf] new_robo.d[:nf] = self.robo.d[:nf] new_robo.r[:nf] = self.robo.r[:nf] if result['keep_dyn']: nl = min(self.robo.NL, new_robo.NL) new_robo.Nex[:nl] = self.robo.Nex[:nl] new_robo.Fex[:nl] = self.robo.Fex[:nl] new_robo.FS[:nl] = self.robo.FS[:nl] new_robo.IA[:nl] = self.robo.IA[:nl] new_robo.FV[:nl] = self.robo.FV[:nl] new_robo.MS[:nl] = self.robo.MS[:nl] new_robo.M[:nl] = self.robo.M[:nl] new_robo.J[:nl] = self.robo.J[:nl] if result['keep_base']: new_robo.Z = self.robo.Z new_robo.w0 = self.robo.w0 new_robo.wdot0 = self.robo.wdot0 new_robo.v0 = self.robo.v0 new_robo.vdot0 = self.robo.vdot0 new_robo.G = self.robo.G self.robo = new_robo self.robo.directory = filemgr.get_folder_path(self.robo.name) self.feed_data() dialog.Destroy()
def OnNew(self, event): dialog = ui_definition.DialogDefinition( ui_labels.MAIN_WIN['prog_name'], self.robo.name, self.robo.nl, self.robo.nj, self.robo.structure, self.robo.is_mobile) if dialog.ShowModal() == wx.ID_OK: result = dialog.get_values() new_robo = Robot(*result['init_pars']) if result['keep_geo']: nf = min(self.robo.NF, new_robo.NF) new_robo.ant[:nf] = self.robo.ant[:nf] new_robo.sigma[:nf] = self.robo.sigma[:nf] new_robo.mu[:nf] = self.robo.mu[:nf] new_robo.gamma[:nf] = self.robo.gamma[:nf] new_robo.alpha[:nf] = self.robo.alpha[:nf] new_robo.theta[:nf] = self.robo.theta[:nf] new_robo.b[:nf] = self.robo.b[:nf] new_robo.d[:nf] = self.robo.d[:nf] new_robo.r[:nf] = self.robo.r[:nf] if result['keep_dyn']: nl = min(self.robo.NL, new_robo.NL) new_robo.Nex[:nl] = self.robo.Nex[:nl] new_robo.Fex[:nl] = self.robo.Fex[:nl] new_robo.FS[:nl] = self.robo.FS[:nl] new_robo.IA[:nl] = self.robo.IA[:nl] new_robo.FV[:nl] = self.robo.FV[:nl] new_robo.MS[:nl] = self.robo.MS[:nl] new_robo.M[:nl] = self.robo.M[:nl] new_robo.J[:nl] = self.robo.J[:nl] if result['keep_base']: new_robo.Z = self.robo.Z new_robo.w0 = self.robo.w0 new_robo.wdot0 = self.robo.wdot0 new_robo.v0 = self.robo.v0 new_robo.vdot0 = self.robo.vdot0 new_robo.G = self.robo.G self.robo = new_robo self.robo.directory = filemgr.get_folder_path(self.robo.name) self.feed_data() dialog.Destroy()
def __init__(self, name, NL=0, NJ=0, NF=0, is_mobile=False, structure=TREE): # member variables: self.name = name """ name of the robot: string""" self.directory = filemgr.get_folder_path(name) """ directory name""" self.is_mobile = is_mobile """ whethere the base frame is floating: bool""" self.nl = NL """ number of links: int""" self.nj = NJ """ number of joints: int""" self.nf = NF """ number of frames: int""" self.structure = structure """ type of robot's structure""" self.sigma = [0 for i in xrange(NF + 1)] """ joint type: list of int""" self.ant = range(-1, self.NF - 1) """ index of antecedent joint: list of int""" self.mu = [0 for i in xrange(NF + 1)] """motorization, if 1, then the joint im motorized""" self.theta = [0] + [var('th%s' % (i+1)) for i in xrange(NF)] """ geometrical parameter: list of var""" self.r = [0 for i in xrange(NF + 1)] """ geometrical parameter: list of var""" self.alpha = [0 for i in xrange(NF + 1)] """ geometrical parameter: list of var""" self.d = [0 for i in xrange(NF + 1)] """ geometrical parameter: list of var""" self.gamma = [0 for i in xrange(NF + 1)] """ geometrical parameter: list of var""" self.b = [0 for i in xrange(NF + 1)] """ geometrical parameter: list of var""" self.Z = eye(4) """ transformation from reference frame to zero frame""" num = range(self.NL) numj = range(self.NJ) self.w0 = zeros(3, 1) """ base angular velocity: 3x1 matrix""" self.wdot0 = zeros(3, 1) """ base angular acceleration: 3x1 matrix""" self.v0 = zeros(3, 1) """ base linear velocity: 3x1 matrix""" self.vdot0 = zeros(3, 1) """ base linear acceleration: 3x1 matrix""" self.qdot = [var('QP{0}'.format(i)) for i in numj] """ joint speed: list of var""" self.qddot = [var('QDP{0}'.format(i)) for i in numj] """ joint acceleration: list of var""" self.Nex = [zeros(3, 1) for i in num] """ external moment of link: list of 3x1 matrix""" self.Nex[-1] = Matrix(var('CX{0}, CY{0}, CZ{0}'.format(self.NL - 1))) self.Fex = [zeros(3, 1) for i in num] """ external force of link: list of 3x1 matrix""" self.Fex[-1] = Matrix(var('FX{0}, FY{0}, FZ{0}'.format(self.NL - 1))) self.FS = [var('FS{0}'.format(i)) for i in num] """ dry friction coefficient: list of ver""" self.IA = [var('IA{0}'.format(i)) for i in num] """ joint actuator inertia: list of var""" self.FV = [var('FV{0}'.format(i)) for i in num] """ viscous friction coefficient: list of var""" self.MS = [Matrix(var('MX{0}, MY{0}, MZ{0}'.format(i))) for i in num] """ first momentum of link: list of 3x1 matrix""" self.M = [var('M{0}'.format(i)) for i in num] """ mass of link: list of var""" self.GAM = [var('GAM{0}'.format(i)) for i in numj] """ joint torques: list of var""" J_str = 'XX{0},XY{0},XZ{0},XY{0},YY{0},YZ{0},XZ{0},YZ{0},ZZ{0}' self.J = [Matrix(3, 3, var(J_str.format(i))) for i in num] """ inertia tensor of link: list of 3x3 matrix""" self.G = Matrix([0, 0, var('G3')]) """ gravity vector: 3x1 matrix"""
def __init__( self, name, links=0, joints=0, frames=0, is_floating=True, structure=tools.TREE, is_mobile=False, is_symbolic=True ): """ Constructor period. Usage: """ """Name of the robot.""" self.name = name """Folder to store the files related to the robot.""" self.directory = filemgr.get_folder_path(name) """Total number of links in the robot.""" self.num_links = links """Total number of joints in the robot.""" self.num_joints = joints """Total number of frames in the robot.""" self.num_frames = frames """To indicate if the base is floating or not.""" self.is_floating = is_floating """Type of the robot structure - simple, tree, closed-loop""" self.structure = structure """To indicate if the robot is a wheeled mobile robot""" self.is_mobile = is_mobile """To indicate if computation should be symbolic or numeric""" self.is_symbolic = is_symbolic # properties dependent on number of links """ List to hold the dynamic parameters. The indices of the list start with 0 and it corresponds to parameters of link 0 (virtual link of the base). """ self.dyns = [DynParams(j) for j in self.link_nums] # properties dependent on number of joints """ To indicate if a joint is rigid or flexible. 0 for rigid and 1 for flexible. The indices of the list start with 0 and corresponds to a virtual joint of the base. This joint is usually rigid. """ self.etas = [0 for j in self.joint_nums] """Joint stiffness usually indicated by k.""" self.stiffness = [0 for j in self.joint_nums] """Joint velocities.""" self.qdots = [var('QP{0}'.format(j)) for j in self.joint_nums] """Joint accelerations.""" self.qddots = [var('QDP{0}'.format(j)) for j in self.joint_nums] """Joint torques.""" self.torques = [var('GAM{0}'.format(j)) for j in self.joint_nums] # properties dependent on number of frames """ List to hold the geometric parameters. NOTE: This might be moved to a separate function. The indices of the list start with 0 and the first object corresponds to parameters of frame 0 (base) wrt its antecedent (some arbitary reference frame). """ self.geos = [GeoParams(j) for j in self.frame_nums] # properties independent of number of links, joints and frames """Gravity vector a 3x1 Matrix.""" self.gravity = Matrix([0, 0, var('G3')]) # the values of properties below would be modified during # the computation of dynamic models. """Base velocity 6x1 column vector - a Screw.""" self.base_vel = Screw() """Base acceleration 6x1 column vector - a Screw.""" self.base_accel = Screw() """Transformation matrix of base wrt a reference frame at time 0.""" self.base_tmat = eye(4) # call init methods self._init_maps()
def __init__(self, name, NL=0, NJ=0, NF=0, is_mobile=False, structure=TREE): # member variables: self.name = name """ name of the robot: string""" self.directory = filemgr.get_folder_path(name) """ directory name""" self.is_mobile = is_mobile """ whethere the base frame is floating: bool""" self.nl = NL """ number of links: int""" self.nj = NJ """ number of joints: int""" self.nf = NF """ number of frames: int""" self.structure = structure """ type of robot's structure""" self.sigma = [0 for i in xrange(NF + 1)] """ joint type: list of int""" self.ant = range(-1, self.NF - 1) """ index of antecedent joint: list of int""" self.mu = [0 for i in xrange(NF + 1)] """motorization, if 1, then the joint im motorized""" self.theta = [0] + [var('th%s' % (i + 1)) for i in xrange(NF)] """ geometrical parameter: list of var""" self.r = [0 for i in xrange(NF + 1)] """ geometrical parameter: list of var""" self.alpha = [0 for i in xrange(NF + 1)] """ geometrical parameter: list of var""" self.d = [0 for i in xrange(NF + 1)] """ geometrical parameter: list of var""" self.gamma = [0 for i in xrange(NF + 1)] """ geometrical parameter: list of var""" self.b = [0 for i in xrange(NF + 1)] """ geometrical parameter: list of var""" self.Z = eye(4) """ transformation from reference frame to zero frame""" num = range(self.NL) numj = range(self.NJ) self.w0 = zeros(3, 1) """ base angular velocity: 3x1 matrix""" self.wdot0 = zeros(3, 1) """ base angular acceleration: 3x1 matrix""" self.v0 = zeros(3, 1) """ base linear velocity: 3x1 matrix""" self.vdot0 = zeros(3, 1) """ base linear acceleration: 3x1 matrix""" self.qdot = [var('QP{0}'.format(i)) for i in numj] """ joint speed: list of var""" self.qddot = [var('QDP{0}'.format(i)) for i in numj] """ joint acceleration: list of var""" self.Nex = [zeros(3, 1) for i in num] """ external moment of link: list of 3x1 matrix""" self.Nex[-1] = Matrix(var('CX{0}, CY{0}, CZ{0}'.format(self.NL - 1))) self.Fex = [zeros(3, 1) for i in num] """ external force of link: list of 3x1 matrix""" self.Fex[-1] = Matrix(var('FX{0}, FY{0}, FZ{0}'.format(self.NL - 1))) self.FS = [var('FS{0}'.format(i)) for i in num] """ dry friction coefficient: list of ver""" self.IA = [var('IA{0}'.format(i)) for i in num] """ joint actuator inertia: list of var""" self.FV = [var('FV{0}'.format(i)) for i in num] """ viscous friction coefficient: list of var""" self.MS = [Matrix(var('MX{0}, MY{0}, MZ{0}'.format(i))) for i in num] """ first momentum of link: list of 3x1 matrix""" self.M = [var('M{0}'.format(i)) for i in num] """ mass of link: list of var""" self.GAM = [var('GAM{0}'.format(i)) for i in numj] """ joint torques: list of var""" J_str = 'XX{0},XY{0},XZ{0},XY{0},YY{0},YZ{0},XZ{0},YZ{0},ZZ{0}' self.J = [Matrix(3, 3, var(J_str.format(i))) for i in num] """ inertia tensor of link: list of 3x3 matrix""" self.G = Matrix([0, 0, var('G3')]) """ gravity vector: 3x1 matrix"""