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 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 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"""
