def printTree(self, padding='', last=False, maxdepth=3, maxcount=50):
"""Print tree contained in file.
Parameters
----------
padding : string
Arguements passed in command line.
last : boolean
Last node in sequence
maxdepth : number
Limit dept
maxcount : number
Limit count
"""
if maxdepth is not None and maxdepth < 0:
print padding[:-1] + ' `' + bold(blue('...'))
return
if last:
print padding[:-1] + ' `' + bold(
blue(self._keyfnc(self.group) + '/'))
padding = padding[:-1] + ' '
else:
print padding + '`' + bold(blue(self._keyfnc(self.group) + '/'))
count = len(self)
large = False
if maxcount is not None and count > maxcount:
count = maxcount + 1
large = True
if self is None or not count:
print padding, ' `' + boldblack('[empty]')
else:
for key, val in self.iteritems():
count -= 1
if count == 0 and large:
#print padding + ' |-'+(cyan('(...)'))
print padding + ' ' + '`...'
break
self._printNode(key,
val,
padding=padding + ' |',
last=not count,
maxdepth=maxdepth - 1,
maxcount=maxcount)
def print_board(self):
display_data = [cell + "\n" + blue(index) for index,
cell in enumerate(self.board)]
data = [display_data[0:3], display_data[3:6], display_data[6:]]
table = SingleTable(data)
table.inner_row_border = True
print(table.table)
print()
def printTree(self, padding= '', last=False,maxdepth=3,maxcount=50):
"""Print tree contained in file.
Parameters
----------
padding : string
Arguements passed in command line.
last : boolean
Last node in sequence
maxdepth : number
Limit dept
maxcount : number
Limit count
"""
if maxdepth is not None and maxdepth<0:
print padding[:-1] + ' `'+bold(blue('...'))
return
if last:
print padding[:-1] + ' `' + bold(blue(self._keyfnc(self.group) + '/' ))
padding = padding[:-1]+' '
else:
print padding + '`' + bold(blue(self._keyfnc(self.group) + '/') )
count = len(self)
large = False
if maxcount is not None and count>maxcount:
count=maxcount+1
large = True
if self is None or not count:
print padding, ' `'+boldblack('[empty]')
else:
for key, val in self.iteritems():
count -= 1
if count==0 and large:
#print padding + ' |-'+(cyan('(...)'))
print padding + ' '+'`...'
break
self._printNode(key,val,padding=padding+' |',last=not count,maxdepth=maxdepth-1,maxcount=maxcount)
def count(self):
''' This scenario lets the drone count all the objects on a predefined trajectory
'''
print highlight_magenta(
' COUNTING STARTED ')
print magenta('fly to start point!')
counting_list = [[50], [51], [52], [53], [54], [55], [56], [57], [58],
[59], [60], [61], [62], [63], [64], [65], [66], [67],
[68], [69], [70], [71], [72], [73], [74], [75], [76],
[77], [78], [79]]
red_c = 0
blue_c = 0
green_c = 0
counted_list = []
while [11] not in self.ids:
rospy.sleep(0.5)
print magenta('starting to count')
while [12] not in self.ids:
for i in self.ids:
if i in counting_list:
counting_list.remove(i)
counted_list.append(i)
print magenta(' >>> found one <<< ')
rospy.sleep(0.5)
for i in counted_list:
if 50 <= i[0] <= 59:
red_c += 1
elif 60 <= i[0] <= 69:
blue_c += 1
elif 70 <= i[0] <= 79:
green_c += 1
print highlight_magenta(
' REPORT ')
print red(' red:' + str(red_c))
print blue(' blue:' + str(blue_c))
print green(' green:' + str(green_c))
print highlight_magenta(
' COUNTING COMPLETED ')
self.task_str = ""
def calibrate(self, *_):
print blue('---- Calibration started ---- \n')
pose_t_in_v = self.get_pose_vive(self.tracked_objects[0])
self.tf_t_in_v = self.pose_to_tf(pose_t_in_v, "tracker")
self.broadc.sendTransform(self.tf_t_in_v)
# self.stbroadc.sendTransform(self.tf_d_in_t)
# Dirty fix to make sure every transform is broadcasted & received.
rospy.sleep(2.)
# Calibrate: fix current drone pose as pose of world frame.
self.tf_w_in_v = self.get_transform("init_drone", "vive")
self.tf_w_in_v.child_frame_id = "world"
self.stbroadc.sendTransform(self.tf_w_in_v)
print blue('tf_w_in_v \n', self.tf_w_in_v)
print blue('---- Calibrated ---- \n')
def testSimpleMatch(self): line = '+tag' expected = str(underline(blue(line))) self.assertEqual(expected, self.dball.colorize(line))
downhill=False,
stairs=False,
seed_value=args.seed,
on_rack=False,
gait='trot')
steps = 0
t_r = 0
if (args.RandomTest):
env.Set_Randomization(default=False)
else:
env.incline_deg = args.WedgeIncline
env.incline_ori = math.radians(args.WedgeOrientation)
state = env.reset()
print(bold(blue("\nTest Parameters:\n")), green('\nWedge Inclination:'),
red(env.incline_deg), green('\nWedge Orientation:'),
red(math.degrees(args.WedgeOrientation)),
green('\nCoeff. of friction:'), red(env.friction),
green('\nMotor saturation torque:'), red(env.clips))
for i_step in range(args.EpisodeLength):
print('Roll:', math.degrees(env.support_plane_estimated_roll),
'Pitch:', math.degrees(env.support_plane_estimated_pitch))
action = policy.dot(state)
# action = [1.0,1.0,1.0,1.0,
# 0.0,0.0,0.0,0.0,
# -1.0,-1.0,-1.0,-1.0,
# -1.0,-1.0,-1.0,-1.0,
# 0.0,0.0,0.0,0.0 ]
# action = [0.5,0.5,0.5,0.5,
if (args.RandomTest):
env.Set_Randomization(default=False)
else:
env.incline_deg = args.WedgeIncline
env.incline_ori = math.radians(args.WedgeOrientation)
env.SetFootFriction(args.FrictionCoeff)
# env.SetLinkMass(0,args.FrontMass)
# env.SetLinkMass(11,args.BackMass)
env.clips = args.MotorStrength
env.pertub_steps = 300
env.y_f = args.PerturbForce
state = env.reset()
print(
bold(blue("\nTest Parameters:\n")),
green('\nWedge Inclination:'),
red(env.incline_deg),
green('\nWedge Orientation:'),
red(math.degrees(env.incline_ori)),
green('\nCoeff. of friction:'),
red(env.friction),
# green('\nMass of the front half of the body:'),red(env.FrontMass),
# green('\nMass of the rear half of the body:'),red(env.BackMass),
green('\nMotor saturation torque:'),
red(env.clips))
# Simulation starts
simstep = 1000
plot_data = []
t_r = 0
def _one_colorize(self, line, pattern, match): return line.replace(match, str(underline(blue(match))))
def blue(self):
return ansi(c.blue(self.txt))