def test_speeds(self):
print 'Speeds and accelerations'
kinematics.velocities(self.robo)
kinematics.accelerations(self.robo)
kinematics.jdot_qdot(self.robo)
print 'Kinematic constraint equations'
kinematics.kinematic_constraints(samplerobots.sr400())
def test_robo_misc(self):
print "######## test_robo_misc ##########"
self.robo = samplerobots.sr400()
q = list(var('th1:10'))
self.assertEqual(self.robo.q_vec, q)
self.assertEqual(self.robo.chain(6), [6, 5, 4, 3, 2, 1])
self.assertEqual(self.robo.chain(6, 3), [6, 5, 4])
self.assertEqual(self.robo.loop_chain(8, 9), [8, 9])
self.assertEqual(self.robo.loop_chain(0, 6), [0, 1, 2, 3, 4, 5, 6])
self.assertEqual(self.robo.loop_chain(6, 0), [6, 5, 4, 3, 2, 1, 0])
self.assertEqual(self.robo.loop_chain(9, 10), [9, 8, 7, 1, 2, 3, 10])
self.assertEqual(self.robo.loop_terminals, [(9, 10)])
l1 = self.robo.get_geom_head()
l2 = self.robo.get_dynam_head()
l3 = self.robo.get_ext_dynam_head()
for name in l1[1:] + l2[1:] + l3[1:]:
for i in xrange(self.robo.NL):
if name in tools.INT_KEYS:
self.assertEqual(self.robo.put_val(i, name, i), tools.OK)
else:
v = var(name + str(i))
self.assertEqual(self.robo.put_val(i, name, v), tools.OK)
for name in l3[1:]+l2[1:]+l1[1:]:
for i in xrange(self.robo.NL):
if name in tools.INT_KEYS:
self.assertEqual(self.robo.get_val(i, name), i)
else:
v = var(name + str(i))
self.assertEqual(self.robo.get_val(i, name), v)
def test_loop(self):
print "######## test_loop ##########"
self.robo = samplerobots.sr400()
invgeom.loop_solve(self.robo, self.symo)
l_solver = self.symo.gen_func('IGM_gen', self.robo.q_vec,
self.robo.q_active)
T = geometry.dgm(self.robo, self.symo, 9, 10,
fast_form=True, trig_subs=True)
t_loop = self.symo.gen_func('DGM_generated1', T, self.robo.q_vec)
for x in xrange(10):
arg = random.normal(size=6)
solution = l_solver(arg)
for q in solution:
self.assertLess(amax(matrix(t_loop(q))-eye(4)), 1e-12)
def test_loop(self):
print "######## test_loop ##########"
self.robo = samplerobots.sr400()
invgeom.loop_solve(self.robo, self.symo)
self.symo.gen_func_string('IGM_gen', self.robo.q_vec,
self.robo.q_active, syntax = 'matlab')
l_solver = self.symo.gen_func('IGM_gen', self.robo.q_vec,
self.robo.q_active)
T = geometry.dgm(self.robo, self.symo, 9, 10,
fast_form=True, trig_subs=True)
t_loop = self.symo.gen_func('DGM_generated1', T, self.robo.q_vec)
for x in xrange(10):
arg = random.normal(size=6)
solution = l_solver(arg)
for q in solution:
self.assertLess(amax(matrix(t_loop(q))-eye(4)), 1e-12)
def test_dgm_sr400(self):
print "######## test_dgm_sr400 ##########"
self.robo = samplerobots.sr400()
T = geometry.dgm(self.robo, self.symo, 0, 6,
fast_form=True, trig_subs=True)
f06 = self.symo.gen_func('DGM_generated1', T, self.robo.q_vec)
T = geometry.dgm(self.robo, self.symo, 6, 0,
fast_form=True, trig_subs=True)
f60 = self.symo.gen_func('DGM_generated2', T, self.robo.q_vec)
for x in xrange(10):
arg = random.normal(size=9)
M = matrix(f06(arg))*matrix(f60(arg))-eye(4)
self.assertLess(amax(M), 1e-12)
t06 = matrix([[1, 0, 0, 3], [0, -1, 0, 0],
[0, 0, -1, -1], [0, 0, 0, 1]])
self.assertLess(amax(matrix(f06(zeros(9))) - t06), 1e-12)
def test_speeds(self):
print 'Speeds and accelerations'
kinematics.speeds_accelerations(self.robo)
print 'Kinematic constraint equations'
kinematics.kinematic_constraints(samplerobots.sr400())
def test_speeds(self):
print 'Speeds and accelerations'
kinematics.speeds_accelerations(self.robo)
print 'Kinematic constraint equations'
kinematics.kinematic_constraints(samplerobots.sr400())