def __init__(self,
name,
mc,
step_pin,
dir_pin,
invert_dir=False,
log=None):
Axis.__init__(self, name, log=log)
#self.mc = mc
self.usbio = mc.usbio
if self.usbio.serial is None:
raise Exception("USBIO missing serial")
self.step_pin = step_pin
self.dir_pin = dir_pin
self.invert_dir = invert_dir
# Set a known output state
# Unknown state to force set
self.is_forward = None
self.forward()
self.usbio.set_gpio(self.step_pin, True)
self.step_delay_s = None
#self.step_delay_s = 0.001
#self.step_delay_s = 5
# Total number of steps
self.net = 0
self._stop = threading.Event()
self._estop = threading.Event()
class Player(pygame.sprite.Sprite):
def __init__(self):
pygame.sprite.Sprite.__init__(self)
self.image, self.rect = Utility.load_image_file('player.png')
grect = Utility.screen.get_rect()
self.rect.x = (grect.width - self.rect.width) / 2
self.rect.y = (grect.height - self.rect.height)
self.axis = Axis()
self.speed = 5
self.speed_slow = 2
def update(self, event, time):
self.axis.update(event)
v = self.axis.get_vector()
if v[0] != 0 and v[1] != 0:
v = (v[0] * 0.707, v[1] * 0.707)
"""Move player character """
if self.axis.get_shift():
print('shift')
self.rect.move_ip((v[0] * self.speed_slow, v[1] * self.speed_slow))
else:
self.rect.move_ip((v[0] * self.speed, v[1] * self.speed))
#self.rect.move_ip(1,1)
grect = Utility.screen.get_rect()
self.rect.x = max(0, min(self.rect.x, grect.width - self.rect.width))
self.rect.y = max(0, min(self.rect.y, grect.height - self.rect.height))
def __init__(self, name, mc, step_pin, dir_pin, invert_dir=False, log=None):
Axis.__init__(self, name, log=log)
# self.mc = mc
self.usbio = mc.usbio
if self.usbio.serial is None:
raise Exception("USBIO missing serial")
self.step_pin = step_pin
self.dir_pin = dir_pin
self.invert_dir = invert_dir
# Set a known output state
# Unknown state to force set
self.is_forward = None
self.forward()
self.usbio.set_gpio(self.step_pin, True)
self.step_delay_s = None
# self.step_delay_s = 0.001
# self.step_delay_s = 5
# Total number of steps
self.net = 0
self._stop = threading.Event()
self._estop = threading.Event()
def jog(self, forwards=True):
Axis.jog(self, forwards)
if forwards:
self.current_steps = self.actual_high_lim
self.current_pos = self.actual_high_lim
else:
self.current_steps = self.actual_low_lim
self.current_pos = self.actual_low_lim
def send(self, command, *parameters):
Axis.send(self, command, *parameters)
if command is TELL_SWITCHES:
return self._mock_switches()
if command is MOTOR_TYPE:
return 2.5
if command is CONFIGURE_ENCODER:
return 14
def CreateCenter( self, vector, colour ):
# Create the axis, add the geometry and collision
axis = Axis( self.name, vector, colour, default=True )
axis.AddGeometry( Box( 0.1, 0.1, 0.1, origin=Point3(0.05, 0.05, 0.05) ), sizeStyle=NONE )
axis.AddCollisionSolid( CollisionSphere( 0, 0.1 ), sizeStyle=NONE )
axis.reparentTo( self )
return axis
def __init__(self):
pygame.sprite.Sprite.__init__(self)
self.image, self.rect = Utility.load_image_file('player.png')
grect = Utility.screen.get_rect()
self.rect.x = (grect.width - self.rect.width) / 2
self.rect.y = (grect.height - self.rect.height)
self.axis = Axis()
self.speed = 5
self.speed_slow = 2
def __init__(self):
''' connect to midi device and set up ros
'''
self.sp = SmartPAD('SmartPAD')
self.axis = Axis()
self.axis.init()
self.web = pages.Pages()
rospy.loginfo("Using input device %s" % self.sp.device)
os.environ['PORT'] = '8081'
def CreateSquare( self, vector, colour ):
# Create the geometry and collision
self.square = Square( 0.2, 0.2, Vec3(0, 1, 0), origin=Point3(0.1, 0.1, 0) )
self.square.setBillboardPointEye()
collSphere = CollisionSphere( 0, 0.125 )
# Create the axis, add the geometry and collision
axis = Axis( self.name, CAMERA_VECTOR, colour, planar=True, default=True )
axis.AddGeometry( self.square, sizeStyle=NONE )
axis.AddCollisionSolid( collSphere, sizeStyle=NONE )
axis.reparentTo( self )
return axis
def __init__(self, name, indexer, config=None):
Axis.__init__(self, name, steps_per_um=config['cnc']['steps_per_um'])
self.indexer = indexer
if self.indexer.serial is None:
raise Exception("Indexer missing serial")
self.movement_notify = lambda: None
# Ensure stopped
self.indexer.step(self.name, 0)
self._stop = threading.Event()
self._estop = threading.Event()
def CreateArrow( self, vector, colour ):
# Create the geometry and collision
line = Line( (0, 0, 0), vector )
cone = Cone( 0.05, 0.25, axis=vector, origin=vector * 0.25 )
collTube = CollisionTube( (0,0,0), Point3( vector ) * 0.95, 0.05 )
# Create the axis, add the geometry and collision
axis = Axis( self.name, vector, colour )
axis.AddGeometry( line, sizeStyle=SCALE )
axis.AddGeometry( cone, vector, colour )
axis.AddCollisionSolid( collTube, sizeStyle=TRANSLATE_POINT_B )
axis.reparentTo( self )
return axis
def __init__(self, name, indexer, config=None):
Axis.__init__(self, name, steps_per_um=config['cnc']['steps_per_um'])
self.indexer = indexer
if self.indexer.serial is None:
raise Exception("Indexer missing serial")
self.movement_notify = lambda: None
# Ensure stopped
self.indexer.step(self.name, 0)
self._stop = threading.Event()
self._estop = threading.Event()
def CreateCamCircle( self, colour, radius ):
# Create the geometry and collision
circle = self.CreateCircle( colour, radius )
collPoly = CollisionPolygon( Point3(-1.2, 0, -1.2), Point3(-1.25, 0, 1.25), Point3(1.25, 0, 1.25), Point3(1.25, 0, -1.25) )
# Create the axis, add the geometry and collision
self.camAxis = Axis( self.name, CAMERA_VECTOR, colour, planar=True, default=True )
self.camAxis.AddGeometry( circle, sizeStyle=SCALE )
self.camAxis.AddCollisionSolid( collPoly, sizeStyle=SCALE )
self.camAxis.reparentTo( self )
return self.camAxis
def print_axis_and_parabola(original,
threshold,
stepRho=1,
stepTheta=np.pi / 180):
"""
Prints axis and parabola in the original image
"""
# original image
image = cv2.cvtColor(original.copy(), cv2.COLOR_GRAY2RGB)
# axis
axis = Axis(original, threshold)
original_axis = axis.get_axis()
rotate_angle, axis_90degree = axis.get_axis_in_90degree()
axis.draw_axis(image)
axis.draw_axis_origin(image)
# parabola
parabola = Parabola(original, rotate_angle, original_axis,
axis.axis_origin)
a, b, c = parabola.get_parabola()
parabola.draw_parabola(image)
return image
def CreateBox( self, vector, colour ):
# Create the geometry and collision
line = Line( (0, 0, 0), vector )
box = Box( 0.1, 0.1, 0.1, origin=Point3(0.05, 0.05, 0.05) + vector * 0.05 )
collSphere = CollisionSphere( Point3( vector * -0.05 ), 0.1 )
# Create the axis, add the geometry and collision
axis = Axis( self.name, vector, colour )
axis.AddGeometry( line, colour=GREY, highlight=False, sizeStyle=SCALE )
axis.AddGeometry( box, vector, colour )
axis.AddCollisionSolid( collSphere, vector )
axis.reparentTo( self )
return axis
def __determine_y_axes(self):
lowest_primary_y_value = None
highest_primary_y_value = None
lowest_secondary_y_value = None
highest_secondary_y_value = None
for data_set in self.data_sets:
lowest_y_value = data_set.get_lowest_value()
highest_y_value = data_set.get_highest_value()
if self.additional_axis_meta_data.get_axis_meta_data_item(
data_set.name).axis_type == AxisType.PRIMARY:
if lowest_primary_y_value is None:
lowest_primary_y_value = lowest_y_value
else:
if lowest_y_value < lowest_primary_y_value:
lowest_primary_y_value = lowest_y_value
if highest_primary_y_value is None:
highest_primary_y_value = highest_y_value
else:
if highest_y_value > highest_primary_y_value:
highest_primary_y_value = highest_y_value
elif self.additional_axis_meta_data.get_axis_meta_data_item(
data_set.name).axis_type == AxisType.SECONDARY:
if lowest_secondary_y_value is None:
lowest_secondary_y_value = lowest_y_value
else:
if lowest_y_value < lowest_secondary_y_value:
lowest_secondary_y_value = lowest_y_value
if highest_secondary_y_value is None:
highest_secondary_y_value = highest_y_value
else:
if highest_y_value > highest_secondary_y_value:
highest_secondary_y_value = highest_y_value
y_data_type, y_low, y_high = self.__evaluate_axis_data_by_type(
lowest_primary_y_value, highest_primary_y_value)
self.y_primary_axis = Axis(y_low, y_high, y_data_type)
y_data_type, y_low, y_high = self.__evaluate_axis_data_by_type(
lowest_secondary_y_value, highest_secondary_y_value)
self.y_secondary_axis = Axis(y_low, y_high, y_data_type)
def CreateArrow(self, vec, colour):
# Create the geometry and collision
vec.normalize()
line = pm.NodePath(Line((0, 0, 0), vec))
cone = pm.NodePath(Cone(0.05, 0.25, axis=vec, origin=vec * 0.125))
collTube = pm.CollisionTube((0, 0, 0), pm.Point3(vec) * 0.95, 0.05)
# Create the axis, add the geometry and collision
axis = Axis(self.name, vec, colour)
axis.AddGeometry(line, sizeStyle=SCALE)
axis.AddGeometry(cone, vec, colour)
axis.AddCollisionSolid(collTube, sizeStyle=TRANSLATE_POINT_B)
axis.reparentTo(self)
return axis
def __init__(self, address):
super().__init__(address)
self.about = About(self)
self.access = Access(self)
self.adjustment = Adjustment(self)
self.axis = Axis(self)
self.displacement = Displacement(self)
self.ecu = Ecu(self)
self.functions = Functions(self)
self.manual = Manual(self)
self.network = Network(self)
self.nlc = Nlc(self)
self.pilotlaser = Pilotlaser(self)
self.realtime = Realtime(self)
self.system = System(self)
self.system_service = System_service(self)
self.update = Update(self)
try:
self.streaming = Streaming(self)
except NameError as e:
if "Streaming" in str(e):
print("Warning: Streaming is not supported on your platform")
else:
raise e
def demo1():
theta = Axis('th', unit=u'deg')
theta2 = Axis('tth', unit=u'deg')
theta.position = 5.
theta.speed = 10.
theta2.position = 24
theta2.speed = 5
move(theta2, 30, theta, 40)
def CreateRing( self, vector, colour, lookAt ):
# Create the billboard effect
bbe = BillboardEffect.make( vector, False, True, 0, lookAt, (0, 0, 0) )
# Create an arc
arc = Arc( numSegs=32, degrees=180, axis=Vec3(0, 0, 1) )
arc.setH( 180 )
arc.setEffect( bbe )
# Create the axis from the arc
axis = Axis( self.name, vector, colour )
axis.AddGeometry( arc, sizeStyle=SCALE )
axis.AddCollisionSolid( self.collSphere, sizeStyle=SCALE )
axis.reparentTo( self )
return axis
def __init__(self, labels, screen_width, screen_height):
self.screen_width = screen_width / 2
self.screen_height = screen_height
self.screen = pygame.Surface((self.screen_width, self.screen_height))
self.xliftoff = 50
self.yliftoff = 50
self.bar_width = 17
self.labels = labels
self.axes = Axis(self.screen, self.screen_width, self.screen_height,
self.xliftoff, self.yliftoff, self.bar_width)
self.movement = 0
self.profile = 0
self.power = 0
self.charts = 0
pub.subscribe(self.movement_handler, "gamepad.movement")
pub.subscribe(self.profile_handler, "gamepad.profile")
pub.subscribe(self.power_handler, "Thruster.Power")
def __determine_x_axis(self):
lowest_x_value, highest_x_value = self.data_sets.get_lowest_and_highest_key(
)
x_data_type, x_low, x_high = self.__evaluate_axis_data_by_type(
lowest_x_value, highest_x_value)
# x_low = lowest_x_value
# x_high = highest_x_value
# #self.x_axis = Axis(x_low, x_high, x_data_type, self.x_axis_format)
self.x_axis = Axis(lowest_x_value, highest_x_value, x_data_type,
self.x_axis_format)
def CreateCamCircle( self, colour, radius ):
# Create the geometry and collision
circle = self.CreateCircle( colour, radius )
collPoly = CollisionPolygon( Point3(-1.2, 0, -1.2), Point3(-1.25, 0, 1.25), Point3(1.25, 0, 1.25), Point3(1.25, 0, -1.25) )
# Create the axis, add the geometry and collision
self.camAxis = Axis( self.name, CAMERA_VECTOR, colour, planar=True, default=True )
self.camAxis.AddGeometry( circle, sizeStyle=SCALE )
self.camAxis.AddCollisionSolid( collPoly, sizeStyle=SCALE )
self.camAxis.reparentTo( self )
return self.camAxis
def CreateRing( self, vector, colour, lookAt ):
# Create the billboard effect
bbe = BillboardEffect.make( vector, False, True, 0, lookAt, (0, 0, 0) )
# Create an arc
arc = Arc( numSegs=32, degrees=180, axis=Vec3(0, 0, 1) )
#line = Line(start=Vec3(0,0,0), end=Vec3(0, 0, 1))
arc.setH( 180 )
arc.setEffect( bbe )
# Create the axis from the arc
axis = Axis( self.name, vector, colour )
axis.AddGeometry( arc, sizeStyle=SCALE , vector=str(vector))
#axis.AddGeometry( line, sizeStyle=SCALE )
axis.AddCollisionSolid( self.collSphere, sizeStyle=SCALE, vector=str(vector))
axis.reparentTo( self )
#==
line = Line( (0, 0, 0), vector )
cone = Cone( 0.05, 0.25, axis=vector, origin=vector * 0.25 )
axis.AddGeometry( line, sizeStyle=SCALE )
axis.AddGeometry( cone, vector, colour )
#==
return axis
def addAxis(self, key, **kwprops):
"""
Add a new axis to the plot, with its name given by key. If key already exists,
do nothing.
Return the axis, regardless of whether it already exists or was just created.
"""
if key not in self._axes.keys():
self._axes[key] = Axis(self._figure.canvas(), self, **kwprops)
self.addChild(self._axes[key])
return self._axes[key]
def __init__(self,
parent,
index,
onoff='on',
hidden='false',
type='XY',
stacked='false',
bar_hgap=0.00,
**kwargs):
GraceObject.__init__(self, parent, locals())
self.legend = Legend(self)
self.frame = Frame(self)
self.xaxis = Axis(self, 'x')
self.yaxis = Axis(self, 'y')
self.altxaxis = Axis(self, 'x', 'alt', 'off')
self.altyaxis = Axis(self, 'y', 'alt', 'off')
self.title = Title(self)
self.subtitle = Subtitle(self)
self.view = View(self)
self.world = World(self)
self.datasets = []
self._datasetIndex = INDEX_ORIGIN
self.drawing_objects = []
class Smartie(object):
''' Class to connect to and publish the Korg NanoKontrol midi device
as a ros joystick
'''
def __init__(self):
''' connect to midi device and set up ros
'''
self.sp = SmartPAD('SmartPAD')
self.axis = Axis()
self.axis.init()
self.web = pages.Pages()
rospy.loginfo("Using input device %s" % self.sp.device)
os.environ['PORT'] = '8081'
def finish(self):
self.sp.finish()
self.axis.fini()
self.web.finish()
#del self.sp
#del self.axis
def run(self):
try:
thread.start_new_thread(self.web.run, ())
while not rospy.is_shutdown():
done, data = self.sp.readin()
if done:
rospy.signal_shutdown("Smartie Done")
self.axis.process_keys(data)
finally:
rospy.loginfo("Smartie Died")
self.finish()
def test_sig(vhdl_output_path=None):
axis_y = Axis(32)
axis_x = Axis(32)
y_pow, y_fac, y_exp = [Signal(intbv(0)[32:]) for i in range(3)]
clk = Signal(bool(0))
clk_gen_1 = clk_gen(clk, period=10)
reset = ResetSignal(0, active=1, async=False)
# power_1 = power(clk, reset, y_pow, x, prec, end, start)
# factorial_1 = factorial(clk, reset, y_fac, prec, end, start)
# exponential_1 = exponential(clk, reset, y_exp, x, prec, end, start)
sigmoid_1 = sigmoid(clk, reset, axis_y, axis_x, 4, fraction=16)
@instance
def reset_gen():
reset.next = 0
yield delay(4000)
yield clk.negedge
reset.next = 1
@instance
def stimulus():
axis_x.tdata.next = 0
yield delay(1000)
print("sig z", axis_x.tdata, "o prec 4 =", axis_y.tdata)
axis_x.tdata.next = 2
yield delay(1500)
print("sig z", axis_x.tdata, "o prec 4 =", axis_y.tdata)
axis_x.tdata.next = 8
yield delay(1500)
print("sig z", axis_x.tdata, "o prec 4 =", axis_y.tdata)
raise StopSimulation()
if vhdl_output_path is not None:
sigmoid_1.convert(hdl='VHDL', path=vhdl_output_path)
return instances()
def CreateCenter(self, vector, colour):
# Create the axis, add the geometry and collision
axis = Axis(self.name, vector, colour, default=True)
axis.AddGeometry(NodePath(Box(0.1, 0.1, 0.1)), sizeStyle=NONE)
axis.AddCollisionSolid(CollisionSphere(0, 0.1), sizeStyle=NONE)
axis.reparentTo(self)
return axis
def __init__(self):
self.axis = Axis(verbose=0)
self.axis.init()
self.poser = ArmPos()
self.csvf = open('kin.csv', 'wb')
self.csv = csv.writer(self.csvf)
#complicated reset just for fun with Python comprehensions
self.angles = [0.2, 1.4, 0.2, 1.5]
self.rtinvert = [ 1.0, 1.0, 1.0, 1.0]
self.lfinvert = [ 1.0,-1.0, 1.0,-1.0]
self.lch = ['la0', 'la1', 'la2', 'la3']
self.rch = ['ra0', 'ra1', 'ra2', 'ra3']
data = [ Datum(j[0],j[1]*j[2]) for j in zip(self.rch,self.angles,self.rtinvert)]
self.axis.process_keys(data)
rospy.sleep(0.5)
data = [ Datum(j[0],j[1]*j[2]) for j in zip(self.lch,self.angles,self.lfinvert)]
self.axis.process_keys(data)
self.pose = [0.2, 1.4, 0.2, 1.5, 0.2,-1.4, 0.2,-1.5]
self.tfBuffer = tf2_ros.Buffer()
self.tfListener = tf2_ros.TransformListener(self.tfBuffer)
self.poser.subscribe(Kinemate.leftandright)
def __init__(self, parent, index,
onoff='on',
hidden='false',
type='XY',
stacked = 'false',
bar_hgap=0.00,
**kwargs
):
GraceObject.__init__(self, parent, locals())
self.legend = Legend(self)
self.frame = Frame(self)
self.xaxis = Axis(self, 'x')
self.yaxis = Axis(self, 'y')
self.altxaxis = Axis(self, 'x', 'alt', 'off')
self.altyaxis = Axis(self, 'y', 'alt', 'off')
self.title = Title(self)
self.subtitle = Subtitle(self)
self.view = View(self)
self.world = World(self)
self.datasets = []
self._datasetIndex = INDEX_ORIGIN
self.drawing_objects = []
def __init__(self, name, puzzle_string):
self.name = name
# string of 81 digits representing the original puzzle
self.puzzle_string = puzzle_string
# string of 81 digits representing valid, unique solution
self.solution = ""
# list of strings representing valid but not necessarily unique solutions
self.valid_completion_list = []
# string representing the end of an attempt to solve an invalid puzzle
self.final_string = ""
# dictionary of the 81 boxes in the puzzle
self.box_map = {}
# dictionary of the 27 row/column/square axes
self.axis_map = {}
self.solved = False
# All valid puzzles have at least 17 clues initially
self.too_few_clues = False
self.no_solution = False
self.multiple_solution = False
self.error_description = ""
# List of functions applied to solve puzzle and whether progress made
self.method_log = []
# Difficulty level
self.difficulty = ""
# initialize axis map
for i in range(0, 27):
self.axis_map[i] = Axis(i)
# initialize box map
for i in range(0, 81):
self.box_map[i] = Box(i)
# put the known box values into the box map
for i in range(0, 81):
value = int(puzzle_string[i])
if value != 0:
self.box_map[i].given = True
# make sure value can legally be put in this box
if value in self.box_map[i].tally:
self.update_new_known(i, value)
else:
self.no_solution = True
self.error_description = f'Unable to initialize: ' \
f'{value} cannot be placed in row {self.box_map[i].row +1}, ' \
f'column {self.box_map[i].col + 1}'
break
class Plot:
def __init__(self, labels, screen_width, screen_height):
self.screen_width = screen_width / 2
self.screen_height = screen_height
self.screen = pygame.Surface((self.screen_width, self.screen_height))
self.xliftoff = 50
self.yliftoff = 50
self.bar_width = 17
self.labels = labels
self.axes = Axis(self.screen, self.screen_width, self.screen_height,
self.xliftoff, self.yliftoff, self.bar_width)
self.movement = 0
self.profile = 0
self.power = 0
self.charts = 0
pub.subscribe(self.movement_handler, "gamepad.movement")
pub.subscribe(self.profile_handler, "gamepad.profile")
pub.subscribe(self.power_handler, "Thruster.Power")
def update(self, order):
self.update_charts()
values = self.charts[order]
directions = [
Bar(i, self.screen, self.screen_width, self.screen_height,
self.xliftoff, self.yliftoff, self.bar_width) for i in range(6)
]
self.screen.fill((200, 200, 255))
self.screen = self.axes.draw()
for i in range(6):
self.screen = directions[i].draw(self.screen, values[i],
self.labels[i])
return self.screen
def movement_handler(self, message):
self.movement = message["gamepad_message"]
def profile_handler(self, message):
self.profile = message
def power_handler(self, message):
self.power = message["Thruster_message"]
def update_charts(self):
self.charts = [self.movement, self.power[0]]
def CreateBox(self, vector, colour):
# Create the geometry and collision
line = NodePath(Line((0, 0, 0), vector))
box = NodePath(Box(0.1, 0.1, 0.1, vector * 0.05))
collSphere = CollisionSphere(Point3(vector * -0.05), 0.1)
# Create the axis, add the geometry and collision
axis = Axis(self.name, vector, colour)
axis.AddGeometry(line, colour=GREY, highlight=False, sizeStyle=SCALE)
axis.AddGeometry(box, vector, colour)
axis.AddCollisionSolid(collSphere, vector)
axis.reparentTo(self)
return axis
def CreateArrow( self, vec, colour ):
# Create the geometry and collision
vec.normalize()
line = pm.NodePath( Line( (0, 0, 0), vec ) )
cone = pm.NodePath( Cone( 0.05, 0.25, axis=vec, origin=vec * 0.125 ) )
collTube = pm.CollisionTube( (0,0,0), pm.Point3( vec ) * 0.95, 0.05 )
# Create the axis, add the geometry and collision
axis = Axis( self.name, vec, colour )
axis.AddGeometry( line, sizeStyle=SCALE )
axis.AddGeometry( cone, vec, colour )
axis.AddCollisionSolid( collTube, sizeStyle=TRANSLATE_POINT_B )
axis.reparentTo( self )
return axis
def CreateSquare( self, vec, colour ):
# Create the geometry and collision
self.square = pm.NodePath( Square( 0.2, 0.2, pm.Vec3(0, 1, 0) ) )
self.square.setBillboardPointEye()
collSphere = pm.CollisionSphere( 0, 0.125 )
# Create the axis, add the geometry and collision
axis = Axis( self.name, CAMERA_VECTOR, colour, planar=True, default=True )
axis.AddGeometry( self.square, sizeStyle=NONE )
axis.AddCollisionSolid( collSphere, sizeStyle=NONE )
axis.reparentTo( self )
return axis
def CreateRing( self, vector, colour, rot ):
# Create an arc
arc = Arc( numSegs=32, degrees=180, axis=Vec3(0, 0, 1) )
arc.setH( 180 )
# Create the axis from the arc
axis = Axis( self.name, vector, colour )
axis.AddGeometry( arc, sizeStyle=SCALE )
axis.AddCollisionSolid( self.collSphere, sizeStyle=SCALE )
axis.reparentTo( self )
# Create the billboard effect and apply it to the arc. We need an
# extra NodePath to help the billboard effect so it orients properly.
hlpr = NodePath( 'helper' )
hlpr.setHpr( rot )
hlpr.reparentTo( self )
arc.reparentTo( hlpr )
bbe = BillboardEffect.make( Vec3(0, 0, 1), False, True, 0,
self.camera, (0, 0, 0) )
arc.setEffect( bbe )
return axis
from cothread import catools as ca
from axis import Axis
from coordsys import CoordSys
from controller import make_controller
from trajectory import Trajectory
# number of decimals to use in verifying positions
# cant be very high on a clipper since it seems to
# set 'in position' a little early
DECIMALS = 2
brick_pv_root = bpr = 'BRICK1:'
brick_axes = {
'm1': Axis(bpr, 'PMAC_BRICK_TEST:MOTOR1', 1, 0),
'm2': Axis(bpr, 'PMAC_BRICK_TEST:MOTOR2', 2, 0),
'm3': Axis(bpr, 'PMAC_BRICK_TEST:MOTOR3', 3, 0),
'm4': Axis(bpr, 'PMAC_BRICK_TEST:MOTOR4', 4, 0),
'm5': Axis(bpr, 'PMAC_BRICK_TEST:MOTOR5', 5, 0),
'm6': Axis(bpr, 'PMAC_BRICK_TEST:MOTOR6', 6, 0),
'm7': Axis(bpr, 'PMAC_BRICK_TEST:MOTOR7', 7, 0),
'm8': Axis(bpr, 'PMAC_BRICK_TEST:MOTOR8', 8, 0),
'A2': Axis(bpr, 'BRICK1CS2:A', 1, 2),
'B2': Axis(bpr, 'BRICK1CS2:B', 2, 2),
'A3': Axis(bpr, 'BRICK1CS3:A', 1, 3),
'B3': Axis(bpr, 'BRICK1CS3:B', 2, 3),
'X3': Axis(bpr, 'BRICK1CS3:X', 7, 3),
'Y3': Axis(bpr, 'BRICK1CS3:Y', 8, 3)
}
brick_groups = {
class Rotation( Base ):
def __init__( self, *args, **kwargs ):
Base.__init__( self, *args, **kwargs )
# Create the camera helper
self.cameraHelper = self.rootNp.attachNewNode( 'cameraHelper' )
# Create the 'ball' border
self.border = self.CreateCircle( GREY, 1 )
# Create the collision sphere - except for the camera normal, all axes
# will use this single collision object
self.collSphere = CollisionSphere( 0, 1 )
# Create x, y, z and camera normal axes
self.axes.append( self.CreateRing( Vec3(1, 0, 0), RED, self.camera ) )
self.axes.append( self.CreateRing( Vec3(0, 1, 0), GREEN, self.cameraHelper ) )
self.axes.append( self.CreateRing( Vec3(0, 0, 1), BLUE, self.camera ) )
# DEBUG
self.foobar = self.CreateCamCircle( TEAL, 1.2 )
self.axes.append( self.foobar )
def CreateRing( self, vector, colour, lookAt ):
# Create the billboard effect
bbe = BillboardEffect.make( vector, False, True, 0, lookAt, (0, 0, 0) )
# Create an arc
arc = Arc( numSegs=32, degrees=180, axis=Vec3(0, 0, 1) )
arc.setH( 180 )
arc.setEffect( bbe )
# Create the axis from the arc
axis = Axis( self.name, vector, colour )
axis.AddGeometry( arc, sizeStyle=SCALE )
axis.AddCollisionSolid( self.collSphere, sizeStyle=SCALE )
axis.reparentTo( self )
return axis
def CreateCircle( self, colour, radius ):
# Create a circle
arc = Arc( radius, numSegs=64, axis=Vec3(0, 1, 0) )
arc.setColorScale( colour )
arc.setLightOff()
arc.reparentTo( self )
# Set the billboard effect
arc.setBillboardPointEye()
return arc
def CreateCamCircle( self, colour, radius ):
# Create the geometry and collision
circle = self.CreateCircle( colour, radius )
collPoly = CollisionPolygon( Point3(-1.2, 0, -1.2), Point3(-1.25, 0, 1.25), Point3(1.25, 0, 1.25), Point3(1.25, 0, -1.25) )
# Create the axis, add the geometry and collision
self.camAxis = Axis( self.name, CAMERA_VECTOR, colour, planar=True, default=True )
self.camAxis.AddGeometry( circle, sizeStyle=SCALE )
self.camAxis.AddCollisionSolid( collPoly, sizeStyle=SCALE )
self.camAxis.reparentTo( self )
return self.camAxis
def SetSize( self, factor ):
Base.SetSize( self, factor )
# Scale up any additional geo
self.border.setScale( self.size )
def GetAxis( self, collEntry ):
axis = Base.GetAxis( self, collEntry )
# Return None if the axis is None
if axis is None:
return None
if axis.vector != CAMERA_VECTOR:
# Return the axis from the specified normal within a tolerance of
# degrees
normal = collEntry.getSurfaceNormal( self )
normal.normalize()
for axis in self.axes:
if math.fabs( normal.angleDeg( axis.vector ) - 90 ) < ( 2.5 / self.size ):
return axis
else:
# Get the collision point on the poly, return the axis if the
# mouse is within tolerance of the circle
point = collEntry.getSurfacePoint( collEntry.getIntoNodePath() )
length = Vec3( point / 1.25 ).length()
if length > 0.9 and length < 1:
return axis
def Update( self, task ):
Base.Update( self, task )
# Update the position of the camera helper based on the position of
# the camera
finalMat = self.camera.getMat( self ) * Mat4().scaleMat( 1, 1, -1 )
self.cameraHelper.setMat( self, finalMat )
# DEBUG - make the camera normal collision plane look at the camera.
# Probably should be a better way to do this.
self.camAxis.collNodePaths[0].lookAt( self.camera )
return task.cont
def Transform( self ):
startVec = self.startVec
axis = self.GetSelectedAxis()
if axis is not None and axis.vector == CAMERA_VECTOR:
endVec = self.getRelativeVector( self.rootNp, self.GetAxisPoint( axis ) - self.getPos() )
cross = startVec.cross( endVec )
direction = self.getRelativeVector( self.camera, Vec3(0, -1, 0) ).dot( cross )
sign = math.copysign( 1, direction )
# Get the rotation axis
rotAxis = self.getRelativeVector( self.camera, Vec3(0, -1, 0) ) * sign
else:
if self.collEntry.getIntoNode() == self.initCollEntry.getIntoNode():
endVec = self.collEntry.getSurfaceNormal( self )
else:
endVec = self.getRelativeVector( self.rootNp, self.GetAxisPoint( self.foobar ) - self.getPos() )
# If an axis is selected then constrain the vectors by projecting
# them onto a plane whose normal is the axis vector
if axis is not None:
plane = Plane( axis.vector, Point3( 0 ) )
startVec = Vec3( plane.project( Point3( startVec ) ) )
endVec = Vec3( plane.project( Point3( endVec ) ) )
# Get the rotation axis
rotAxis = endVec.cross( startVec ) * -1
# Return if the rotation vector is not valid, ie it does not have any
# length
if not rotAxis.length():
return
# Normalize all vectors
startVec.normalize()
endVec.normalize()
rotAxis.normalize()
# Get the amount of degrees to rotate
degs = startVec.angleDeg( endVec )
# Transform the gizmo if in local rotation mode
newRotMat = Mat4().rotateMat( degs, rotAxis )
if self.local:
self.setMat( newRotMat * self.getMat() )
# Transform all attached node paths
for i, np in enumerate( self.attachedNps ):
# Split the transform into scale, rotation and translation
# matrices
transMat, rotMat, scaleMat = commonUtils.GetTrsMatrices( np.getTransform() )
# Perform transforms in local or world space
if self.local:
np.setMat( scaleMat * newRotMat * rotMat * transMat )
else:
self.initNpXforms[i].getQuat().extractToMatrix( rotMat )
np.setMat( scaleMat * rotMat * newRotMat * transMat )
def OnNodeMouse1Down( self, planar, collEntry ):
Base.OnNodeMouse1Down( self, planar, collEntry )
# Store the initial collision entry
self.initCollEntry = collEntry
# If the selected axis is the camera vector then use a point on the
# plane whose normal is the camera vector as the starting vector,
# otherwise use the surface normal from the collision with the sphere
axis = self.GetSelectedAxis()
if axis is not None and axis.vector == CAMERA_VECTOR:
self.startVec = self.getRelativeVector( self.rootNp, self.initMousePoint - self.getPos() )
else:
self.startVec = self.initCollEntry.getSurfaceNormal( self )
def OnMouse2Down( self ):
Base.OnMouse2Down( self )
axis = self.GetSelectedAxis()
if ( hasattr( self, 'collEntry' ) and hasattr( self, 'initCollEntry' ) and
self.collEntry.getIntoNode() != self.initCollEntry.getIntoNode() ):
self.startVec = self.getRelativeVector( self.rootNp, self.GetAxisPoint( self.foobar ) - self.getPos() )
else:
self.startVec = self.getRelativeVector( self.rootNp, self.initMousePoint - self.getPos() )
def OnNodeMouseOver( self, collEntry ):
Base.OnNodeMouseOver( self, collEntry )
# Store the collision entry
self.collEntry = collEntry
bri=bai.brad[ri]
for rj in range(len(baj.brad)):
if li==lj and ri<rj: continue # skip symmetric part of the basis
brj=baj.brad[rj]
addHmatSmat(Hmat,Smat,bai,baj,bri,brj)
if self.xsym!=0: # exchange symmetric terms
addHmatSmat(Hmat,Smat,bai,baj.x12(),bri,brj.x12(),self.xsym)
return Hmat,Smat
lmax=8
gaunt=GauntCoeffTable(lmax)
gaunt.test()
charg1=2
charg2=2
ax=Axis('r',20,0.,5.,order=2)
print 'axis length',ax.len()
bas=BasTwo(0,ax,0,0,2)
#print bas
set_printoptions(precision=4,suppress=False,linewidth=132)
Hmat,Smat=bas.hamiltonian()
plt.matshow(Hmat)
plt.show()
print 'solving eigenproblem...'
val=la.eigvals(Hmat,Smat)
ls = argsort(val.real)
print 'eig',val[ls[:20]].real
def inverse_kinematics(x,y,z,alpha,beta,gamma,**frame_file):
angles = {}
frames = []
pos = [x,y,z]
filename = frame_file.pop('input', 'serial_manipulator_6.json')
with open(filename) as input:
data = json.load(input)
dof = data.pop('dof',6) #6 dof by default
frames_prototype = data['frames']
if not frames_prototype:
raise Exception('No frames in file')
convention = data.pop('convention','xyz')
for prototype in frames_prototype:
frames.append(Frame(prototype['displacement'],prototype['previous_relation']))
T = Axis.invert_zyz(alpha,beta,gamma)
T[0].append(x)
T[1].append(y)
T[2].append(z)
T.append([0,0,0,1])
result = Tree(0)
#compute teta1
result.add_child('teta1_a',atan2( -pos[0],pos[1] ))
result.add_child('teta1_b',atan2( pos[0],-pos[1] ))
#compute teta2,teta3
#rotate plane around z to zy for each teta1
for branch, teta1 in result.childs.items():
ang = teta1.value
p = [pos]
#rotate plane to zy
np = Utils.multiply_matrix(p,[[cos(ang),-sin(ang),0],
[sin(ang), cos(ang), 0],
[0, 0, 1]])
new_p = [np[0][0],np[0][1],np[0][2] - 99]
#we got a plane
p_mag = Utils.magnitude(new_p)
#intersect circunference with radius c centered in new_p
#with one centered in [0,99] (due to the displacement from frame 1 to frame 2 )
#and with radius of 120
c = 155
p2 = Utils.intersect_circunference(new_p[1],new_p[2],c,0,0,120)
teta2_a = atan2(-p2[0][0],p2[0][1])
teta2_b = atan2(-p2[1][0],p2[1][1])
teta1.add_child('teta2_a',teta2_a)
teta1.add_child('teta2_b',teta2_b)
i = 0
for branch2, teta2 in teta1.childs.items():
p2_mag = Utils.magnitude(p2[i])
cosalpha = -(p_mag**2 - c**2 - p2_mag**2) / (2*c*p2_mag)
alpha = acos(cosalpha)
if i == 0:
teta3 = alpha - pi/2
else:
teta3 = alpha + pi/2 -2 *pi
t3_branch = teta2.add_child('teta3',teta3)
i+=1
frames[0].rotate_joint_z(teta1.value)
frames[1].rotate_joint_z(teta2.value)
frames[2].rotate_joint_z(teta3)
T03 = [[1,0,0,0],[0,1,0,0],[0,0,1,0],[0,0,0,1]]
for frame in frames[0:3]:
T03 = Utils.multiply_matrix(T03,frame.position_m)
T30 = Utils.inv_4x4_matrix(T03)
T36 = Utils.multiply_matrix(T30,T)
costeta5 = T36[1][2]
#two possible teta5 values
teta5 = [0,0]
teta5[0] = acos(costeta5)
teta5[1]= -teta5[0]
enum = ['a','b']
j = 0
for t5 in teta5:
if round(sin(t5),4) == 0:
t3_branch.add_child('error','Configuration singularity sin(teta5) = 0')
continue
else:
sinTeta6 = round(- T36[1][1] / sin(t5),4)
cosTeta6 = round(T36[1][0] / sin(t5),4)
cosTeta4 = round(- T36[0][2] / sin(t5),4)
sinTeta4 = round(- T36[2][2] / sin(t5),4)
teta4 = acos(cosTeta4)
if sinTeta4 < 0:
teta4 = - teta4
teta6 = acos(cosTeta6)
if sinTeta6 < 0 :
teta6 = - teta6
t4_branch = t3_branch.add_child('teta4' + enum[j],teta4)
t5_branch = t4_branch.add_child('teta5',t5)
t5_branch.add_child('teta6',teta6)
j+=1
return result
class Kinemate(object):
''' Class to connect to and publish the Korg NanoKontrol midi device
as a ros joystick
'''
#ROS joint names
leftandright = ['leftShoulderPitch', 'leftShoulderRoll', 'leftShoulderYaw',
'leftElbowPitch', 'rightShoulderPitch', 'rightShoulderRoll',
'rightShoulderYaw', 'rightElbowPitch' ]
#Axis joint names
lalr = ['la0','la1','la2','la3','ra0','ra1','ra2','ra3']
def __init__(self):
self.axis = Axis(verbose=0)
self.axis.init()
self.poser = ArmPos()
self.csvf = open('kin.csv', 'wb')
self.csv = csv.writer(self.csvf)
#complicated reset just for fun with Python comprehensions
self.angles = [0.2, 1.4, 0.2, 1.5]
self.rtinvert = [ 1.0, 1.0, 1.0, 1.0]
self.lfinvert = [ 1.0,-1.0, 1.0,-1.0]
self.lch = ['la0', 'la1', 'la2', 'la3']
self.rch = ['ra0', 'ra1', 'ra2', 'ra3']
data = [ Datum(j[0],j[1]*j[2]) for j in zip(self.rch,self.angles,self.rtinvert)]
self.axis.process_keys(data)
rospy.sleep(0.5)
data = [ Datum(j[0],j[1]*j[2]) for j in zip(self.lch,self.angles,self.lfinvert)]
self.axis.process_keys(data)
self.pose = [0.2, 1.4, 0.2, 1.5, 0.2,-1.4, 0.2,-1.5]
self.tfBuffer = tf2_ros.Buffer()
self.tfListener = tf2_ros.TransformListener(self.tfBuffer)
self.poser.subscribe(Kinemate.leftandright)
def finish(self):
self.axis.fini()
self.csvf.close()
def setPose(self,joint,val):
self.pose[Kinemate.lalr.index(joint)] = val
self.axis.process_keys([Datum(joint,val)])
rospy.sleep(0.05)
pass
def delta(self,Euc):
lpalmtf = self.tfBuffer.lookup_transform('torso', 'leftPalm', rospy.Time())
lp = lpalmtf.transform.translation
rpalmtf = self.tfBuffer.lookup_transform('torso', 'rightPalm', rospy.Time())
rp = rpalmtf.transform.translation
pos = [lp.x,lp.y,lp.z,rp.x,rp.y,rp.z]
return [ pos[i]-Euc[i] for i in range(len(Euc)) ]
def derivative(self):
palm = self.delta([0,0,0,0,0,0])
pose = self.pose
for j in range(4):
ljoint = Kinemate.lalr[j]
lcurval = pose[j]
self.axis.process_keys([Datum(ljoint,lcurval+0.1)])
rospy.sleep(0.05)
rjoint = Kinemate.lalr[j+4]
rcurval = pose[j+4]
self.axis.process_keys([Datum(rjoint,rcurval+0.1)])
print "Derivative %d:" % j
self.savePose(start=palm)
self.axis.process_keys([Datum(ljoint,lcurval)])
rospy.sleep(0.05)
self.axis.process_keys([Datum(rjoint,rcurval)])
def savePose(self,start=[0,0,0,0,0,0]):
rospy.sleep(1.0)
palm = self.delta(start)
requested = [ "%.3f" % v for v in self.pose]
actual = [ "%.3f" % v for v in self.poser.val()]
xyz = [ "%.3f" % v for v in palm]
print "Req: %s" % requested
print "Act: %s" % actual
#print "(%.3f,%.3f,%.3f)" % (palm.x,palm.y,palm.z)
print "Result: %s" % xyz
self.writePose(rospy.Time.now().to_sec(),self.pose,self.poser.val(),palm)
def writePose(self,ts,req,act,xyz):
row = [ts]
row.extend(req)
row.extend(act)
row.extend(xyz)
ans = [ round(v,4) for v in row]
self.csv.writerow(ans)
print ans
def run(self):
rate = rospy.Rate(10) # 10hz
time.sleep(1)
rtShoulderPitch = [-0.2,-0.7]
rtShoulderRoll = [ 1.4, 0.9]
rtShoulderYaw = [ 0.2, 0.0]
rtElbowPitch = [ 1.5, 1.0]
try:
while not rospy.is_shutdown():
for j0 in rtShoulderPitch:
self.setPose('ra0',j0)
self.setPose('la0',j0)
for j1 in rtShoulderRoll:
self.setPose('ra1',j1)
self.setPose('la1',-j1)
for j2 in rtShoulderYaw:
self.setPose('ra2',j2)
self.setPose('la2',-0.5-j2)
for j3 in rtElbowPitch:
self.setPose('ra3',j3)
self.setPose('la3',-j3)
print " "
print "===Test Position==="
self.savePose()
self.derivative()
rospy.signal_shutdown("Kinemate Done")
finally:
rospy.loginfo("Kinemate Died")
self.finish()
def __init__(self, name = 'dummy', log=None):
Axis.__init__(self, name, log=log)
self.net = 0
class Graph(GraceObject):
_staticType = 'Graph'
def __init__(self, parent, index,
onoff='on',
hidden='false',
type='XY',
stacked = 'false',
bar_hgap=0.00,
**kwargs
):
GraceObject.__init__(self, parent, locals())
self.legend = Legend(self)
self.frame = Frame(self)
self.xaxis = Axis(self, 'x')
self.yaxis = Axis(self, 'y')
self.altxaxis = Axis(self, 'x', 'alt', 'off')
self.altyaxis = Axis(self, 'y', 'alt', 'off')
self.title = Title(self)
self.subtitle = Subtitle(self)
self.view = View(self)
self.world = World(self)
self.datasets = []
self._datasetIndex = INDEX_ORIGIN
self.drawing_objects = []
def __setattr__(self, key, value):
# check Graph specific attributes
if key == 'type':
self._check_type(str, key, value)
elif key == 'stacked':
self._check_type(str, key, value)
self._check_membership(key, value, ('true', 'false'))
elif key == 'bar_hgap':
self._check_type((float, int), key, value)
GraceObject.__setattr__(self, key, value)
def __str__(self):
graphString = \
"""@g%(index)s %(onoff)s
@g%(index)s hidden %(hidden)s
@g%(index)s type %(type)s
@g%(index)s stacked %(stacked)s
@g%(index)s bar hgap %(bar_hgap)s
@with g%(index)s
%(world)s
%(view)s
%(title)s
%(subtitle)s
%(legend)s
%(frame)s
%(xaxis)s
%(yaxis)s
%(altxaxis)s
%(altyaxis)s""" % self
datasetString = '\n'.join(str(dataset) for dataset in self.datasets)
doString = '\n'.join(str(do) for do in self.drawing_objects)
return '\n'.join((doString, graphString, datasetString))
def add_dataset(self, data, cls=DataSet, *args, **kwargs):
# make sure that cls is a subclass of Dataset
if not issubclass(cls, DataSet):
message = '%s is not a subclass of DataSet' % cls.__name__
raise TypeError(message)
# make an instance of the dataset class and add to list
dataset = cls(parent=self, data=data, index=self._datasetIndex,
*args, **kwargs)
self.datasets.append(dataset)
# increment counter and return the dataset instance
self._datasetIndex += 1
return dataset
def add_drawing_object(self, cls, *args, **kwargs):
# make sure that cls is a subclass of DrawingObject
if not issubclass(cls, DrawingObject):
message = '%s is not a subclass of DrawingObject' % cls.__name__
raise TypeError(message)
# here, the class argument is mandatory, because there are many built
# in types of drawing objects
drawingObject = cls(self, *args, **kwargs)
self.drawing_objects.append(drawingObject)
# return the instance of the drawing object
return drawingObject
def remove_extraworld_drawing_objects(self):
"""Remove drawing objects that are outside of the world
coordinates. Get it, 'extraworld' is like
'extra-terrestrial'.
"""
new_dos = []
old_dos = []
xmin,ymin,xmax,ymax = self.get_world()
for do in self.drawing_objects:
if do.loctype=="view":
new_dos.append(do)
else:
do_xmin,do_ymin,do_xmax,do_ymax = do.limits()
if (xmin<=do_xmin and do_xmax<=xmax and
ymin<=do_ymin and do_ymax<=ymax):
new_dos.append(do)
else:
old_dos.append(do)
self.drawing_objects = new_dos
for do in old_dos:
del do
def alldata(self):
result = []
for dataset in self.datasets:
result.extend(dataset.data)
return result
def move_dataset_to_front(self, dataset):
"""Move data set to the front. This emulates the functionality of the
xmgrace GUI.
"""
_dataset = self.datasets.pop(dataset.index-INDEX_ORIGIN)
_index = _dataset.index
assert _dataset==dataset, "Not the same dataset"
self.datasets.append(dataset)
for index,dataset in enumerate(self.datasets):
dataset.index = index + INDEX_ORIGIN
def move_dataset_to_back(self, dataset):
"""Move data set to the back. This emulates the functionality of the
xmgrace GUI.
"""
_dataset = self.datasets.pop(dataset.index-INDEX_ORIGIN)
assert _dataset==dataset, "Not the same dataset"
self.datasets.insert(0, dataset)
for index,dataset in enumerate(self.datasets):
dataset.index = index + INDEX_ORIGIN
def move_dataset_forward(self, dataset):
"""Move data set forward by one dataset. This emulates the
functionality of the xmgrace GUI.
"""
_dataset = self.datasets.pop(dataset.index-INDEX_ORIGIN)
_index = _dataset.index
assert _dataset==dataset, "Not the same dataset"
self.datasets.insert(_index-INDEX_ORIGIN+1, dataset)
for index,dataset in enumerate(self.datasets):
dataset.index = index + INDEX_ORIGIN
def move_dataset_backward(self, dataset):
"""Move data set backward by one dataset. This emulates the
functionality of the xmgrace GUI.
"""
_dataset = self.datasets.pop(dataset.index-INDEX_ORIGIN)
_index = _dataset.index
assert _dataset==dataset, "Not the same dataset"
_newListIndex = max(0, _index-INDEX_ORIGIN-1)
self.datasets.insert(_newListIndex, dataset)
for index,dataset in enumerate(self.datasets):
dataset.index = index + INDEX_ORIGIN
def set_dataset_order(self, datasets):
"""Specify the order of the data sets.
"""
# make sure that the lengths of order and datasets are the same
assert len(datasets)==len(self.datasets),\
"'datasets' not same length as Graph.datasets."
# use the move_dataset_to_back
for dataset in datasets:
self.move_dataset_to_back(dataset)
def logy(self): self.yaxis.set_log()
def logx(self): self.xaxis.set_log()
def logxy(self):
self.xaxis.set_log()
self.yaxis.set_log()
def liny(self): self.yaxis.set_lin()
def linx(self): self.xaxis.set_lin()
def linxy(self):
self.xaxis.set_lin()
self.yaxis.set_lin()
def data_smallest_positive(self,only_visible=True):
all = []
for dataset in self.datasets:
result = dataset.smallest_positive(only_visible=only_visible)
all.append(result)
if all:
xmins, ymins = zip(*all)
xmins = [i for i in xmins if not i is None and i > 0]
ymins = [i for i in ymins if not i is None and i > 0]
return min(xmins), min(ymins)
else:
return None, None
def drawing_object_smallest_positive(self):
all = []
for drawing_object in self.drawing_objects:
if drawing_object.loctype=="world":
result = drawing_object.smallest_positive()
all.append(result)
if all:
xmins, ymins = zip(*all)
xmins = [i for i in xmins if not i is None and i > 0]
ymins = [i for i in ymins if not i is None and i > 0]
return min(xmins), min(ymins)
else:
return None, None
def smallest_positive(self,only_visible=True):
data_sp = self.data_smallest_positive(only_visible=only_visible)
drawing_object_sp = self.drawing_object_smallest_positive()
if None not in drawing_object_sp and None not in data_sp:
xmins, ymins = zip(data_sp,drawing_object_sp)
return min(xmins), min(ymins)
elif None not in drawing_object_sp:
return drawing_object_sp
elif None not in data_sp:
return data_sp
else:
message="""
In Graph.smallest_positive()...
There are no datasets or drawing_objects on which to determine the
smallest positive number.
"""
raise TypeError, message
def data_limits(self,only_visible=True):
all = []
for dataset in self.datasets:
result = dataset.limits(only_visible=only_visible)
if not None in result:
all.append(result)
if len(all):
xmins, ymins, xmaxs, ymaxs = zip(*all)
return min(xmins), min(ymins), max(xmaxs), max(ymaxs)
else:
return None, None, None, None
def drawing_object_limits(self):
all = []
for drawing_object in self.drawing_objects:
result = drawing_object.limits()
if drawing_object.loctype=="world" and not None in result:
all.append(result)
if len(all):
xmins, ymins, xmaxs, ymaxs = zip(*all)
return min(xmins), min(ymins), max(xmaxs), max(ymaxs)
else:
return None, None, None, None
def limits(self,only_visible=True):
data_limits = self.data_limits(only_visible=only_visible)
drawing_object_limits = self.drawing_object_limits()
if None not in drawing_object_limits and None not in data_limits:
xmins, ymins, xmaxs, ymaxs = zip(data_limits,drawing_object_limits)
return min(xmins), min(ymins), max(xmaxs), max(ymaxs)
elif None not in drawing_object_limits:
return drawing_object_limits
elif None not in data_limits:
return data_limits
else:
message="""
In Graph.limits()...
There are no datasets or drawing_objects on which to determine the limits.
"""
raise TypeError, message
def set_world_to_limits(self, epsilon=1e-12):
xmin, ymin, xmax, ymax = self.limits()
self.world.xmin = xmin - epsilon
self.world.xmax = xmax + epsilon
self.world.ymin = ymin - epsilon
self.world.ymax = ymax + epsilon
def autoscale_old(self):
self.set_world_to_limits()
def set_view(self, xmin, ymin, xmax, ymax):
self.view.xmin = xmin
self.view.xmax = xmax
self.view.ymin = ymin
self.view.ymax = ymax
def get_view(self):
return (self.view.xmin,
self.view.ymin,
self.view.xmax,
self.view.ymax)
def set_labels(self, xLabel, yLabel):
self.xaxis.label.text = xLabel
self.yaxis.label.text = yLabel
def get_dataset(self,num):
if num >= len(self.datasets):
return None
else:
return self.datasets[num]
def calculate_ticks(self, iMin, iMax,
lowTarget=3, highTarget=7, scale=LINEAR_SCALE):
# variables
n1s = [1,2,10,20]
n4s = [5,50]
ns = n1s + n4s
# trick functionality for log axes
if scale == LINEAR_SCALE:
domain = iMin, iMax
if scale == LOGARITHMIC_SCALE:
domain = math.log10(iMin), math.log10(iMax)
# determine appropriate scale for ticks
iRange = domain[1] - domain[0]
if iRange > 0:
_scale = int(math.floor(math.log10(iRange) - 1.0))
else:
_scale = 0
# find number of major ticks
for n in ns:
nTry = int(math.floor(iRange / (n * 10**_scale)))
if nTry >= lowTarget and nTry <= highTarget:
break
major = 10**_scale*n
if scale == LOGARITHMIC_SCALE:
major = 10**math.ceil(major)
# determine the number of minor ticks
if n in n1s:
n_minor_ticks=1
elif n in n4s:
n_minor_ticks=4
if scale==LOGARITHMIC_SCALE:
n_minor_ticks = 8
return major, n_minor_ticks
def set_world(self, xMin, yMin, xMax, yMax):
self.world.xmin = xMin
self.world.xmax = xMax
self.world.ymin = yMin
self.world.ymax = yMax
def get_world(self):
return (self.world.xmin,
self.world.ymin,
self.world.xmax,
self.world.ymax)
def calculate_sizes(self, printWidth):
charSize = 2.3 * printWidth ** -0.4
lineWidth = 3.45 * printWidth ** -0.5
offset = 0.023 * printWidth ** -0.5
return charSize, lineWidth, offset
def format_for_print(self, printWidth):
charSize, lineWidth, offset = self.calculate_sizes(printWidth)
self.set_linewidths(lineWidth)
self.set_suffix(charSize * 1.1, 'size', True)
self.set_suffix(charSize, '_size', True)
self.set_suffix(charSize * 0.67, 'minor_size', True)
megaset = []
for dataset in self.datasets:
megaset.extend(dataset.data)
try:
mul = 1.75 * len(megaset)**-.35
mul = 1 * len(megaset)**-.35
except ZeroDivisionError:
pass
for dataset in self.datasets:
dataset.set_suffix(charSize * mul, 'size', True)
self.xaxis.ticklabel.offset_loc = 'spec'
self.yaxis.ticklabel.offset_loc = 'spec'
self.xaxis.ticklabel.offset_tup = (0, offset)
self.yaxis.ticklabel.offset_tup = (0, offset)
def autoformat(self, printWidth=6.5):
self.autoscale()
self.format_for_print(printWidth)
def autoscalex(self, pad=0, only_visible=True):
xMin, yMin, xMax, yMax = self.limits(only_visible=only_visible)
if self.xaxis.scale==LINEAR_SCALE:
xMajor, nxMinor = self.calculate_ticks(xMin, xMax)
xMinor = xMajor / float(nxMinor + 1)
if nxMinor == 1:
self.world.xmax = math.ceil(xMax / float(xMinor)) * xMinor + \
pad*xMinor
self.world.xmin = math.floor(xMin / float(xMinor)) * xMinor - \
pad*xMinor
else:
self.world.xmax = math.ceil(xMax / float(xMajor)) * xMajor + \
pad*xMajor
self.world.xmin = math.floor(xMin / float(xMajor)) * xMajor - \
pad*xMajor
elif self.xaxis.scale==LOGARITHMIC_SCALE:
# if x has a zero value, autoscale with second smallest
if xMin <= 0:
xMin, dummy = self.smallest_positive(only_visible=only_visible)
xMajor, nxMinor = self.calculate_ticks(xMin, xMax,
scale=LOGARITHMIC_SCALE)
self.world.xmax = 10**(math.ceil(math.log10(xMax) /
float(math.log10(xMajor)))
* math.log10(xMajor) + pad*math.log10(xMajor))
self.world.xmin = 10**(math.floor(math.log10(xMin) /
float(math.log10(xMajor)))
* math.log10(xMajor) - pad*math.log10(xMajor))
else:
message = "'%s' is an unknown x-axis scale"%self.xaxis.scale
raise TypeError(message)
self.xaxis.tick.major = xMajor
self.xaxis.tick.minor_ticks = nxMinor
self.xaxis.auto_precision()
def autoscaley(self, pad=0, only_visible=True):
xMin, yMin, xMax, yMax = self.limits(only_visible=only_visible)
if self.yaxis.scale==LINEAR_SCALE:
yMajor, nyMinor = self.calculate_ticks(yMin, yMax)
yMinor = yMajor / float(nyMinor + 1)
if nyMinor == 1:
self.world.ymax = math.ceil(yMax / float(yMinor)) * yMinor + \
pad*yMinor
self.world.ymin = math.floor(yMin / float(yMinor)) * yMinor - \
pad*yMinor
else:
self.world.ymax = math.ceil(yMax / float(yMajor)) * yMajor + \
pad*yMajor
self.world.ymin = math.floor(yMin / float(yMajor)) * yMajor - \
pad*yMajor
elif self.yaxis.scale==LOGARITHMIC_SCALE:
# if y has a zero value, autoscale with second smallest
if yMin <= 0:
dummy, yMin = self.smallest_positive(only_visible=only_visible)
yMajor, nyMinor = self.calculate_ticks(yMin, yMax,
scale=LOGARITHMIC_SCALE)
self.world.ymax = 10**(math.ceil(math.log10(yMax) /
float(math.log10(yMajor)))
* math.log10(yMajor))
self.world.ymin = 10**(math.floor(math.log10(yMin) /
float(math.log10(yMajor)))
* math.log10(yMajor))
else:
message = "'%s' is an unknown y-axis scale"%self.yaxis.scale
raise TypeError(message)
self.yaxis.tick.major = yMajor
self.yaxis.tick.minor_ticks = nyMinor
self.yaxis.auto_precision()
def autoscale(self, padx=0, pady=0, only_visible=True):
self.autoscalex(pad=padx,only_visible=only_visible)
self.autoscaley(pad=pady,only_visible=only_visible)
def autotickx(self):
"""Automatically generate x-axis ticks based on world coords.
"""
xmin, ymin, xmax, ymax = self.get_world()
scale = self.xaxis.scale
major, n_minor_ticks = self.calculate_ticks(xmin,xmax,scale=scale)
self.xaxis.tick.major = major
self.xaxis.tick.minor_ticks = n_minor_ticks
self.xaxis.auto_precision()
def autoticky(self):
"""Automatically generate y-axis ticks based on world coords.
"""
xmin, ymin, xmax, ymax = self.get_world()
scale = self.yaxis.scale
major, n_minor_ticks = self.calculate_ticks(ymin,ymax,scale=scale)
self.yaxis.tick.major = major
self.yaxis.tick.minor_ticks = n_minor_ticks
self.yaxis.auto_precision()
def autotick(self):
"""Automatically generate ticks based on world coords.
"""
self.autotickx()
self.autoticky()
def set_different_colors(self, skip=1, attr='name', exclude=('White',),
colorsList=[]):
"""Set datasets in graph to different colors. This function behaves
similarly to the similar function in the xmgrace GUI. Can
alternatively specify a list of colors for ordering the colors.
"""
# make sure attr argument is the right type
if attr not in ('name', 'index'):
message = "attr must be 'name' or 'index' (got %s)" % attr
raise ValueError(message)
# put all color indexes or color names that should be excluded from
# the list in a dictionary, and make color names case insensitive
lookup = {}
for item in exclude:
if isinstance(item, str):
item = item.upper()
lookup[item] = None
# put colors into a list
if not colorsList:
colorsList = []
for color in self.root.colors:
if not (color.name.upper() in lookup or color.index in lookup):
colorsList.append(getattr(color, attr))
# set datasets to different colors
nColors = len(colorsList)
for index, dataset in enumerate(self.datasets):
color = colorsList[(skip *index) % nColors]
dataset.set_suffix(color, 'color')
def set_different_symbols(self, skip=1, attr="index", exclude=(0,),
symbolsList=[]):
"""Set datasets in graph to different symbols. This function behaves
similarly to the similar function in the xmgrace GUI. Can
alternatively specify a list of symbols for ordering the symbol
shapes.
"""
# make sure attr argument is the right type
if attr not in ('name', 'index'):
message = "attr must be 'name' or 'index' (got %s)" % attr
raise ValueError(message)
# put all symbol indexes that should be excluded from the list
# in a dictionary
lookup = {}
for item in exclude:
if isinstance(item, str):
item = SYMBOLS[item]
lookup[item] = None
# put symbols into a list
indices = INDEX2SYMBOLS.keys()
indices.sort()
if not symbolsList:
symbolsList = []
for index in indices:
if index not in lookup:
symbolsList.append(index)
# set datasets to different symbols
nSymbols = len(symbolsList)
for index, dataset in enumerate(self.datasets):
shape = symbolsList[(skip *index) % nSymbols]
dataset.symbol.set_suffix(shape, "shape")
def set_different_linestyles(self, skip=1, attr="index", exclude=(0,),
linestylesList=[]):
"""Set datasets in graph to different linestyles. This function
behaves similarly to the similar function in the xmgrace
GUI. Can alternatively specify a list of linestyles for ordering the
line styles.
"""
# make sure attr argument is the right type
if attr not in ('name', 'index'):
message = "attr must be 'name' or 'index' (got %s)" % attr
raise ValueError(message)
# put all linestyle indexes that should be excluded from the list
# in a dictionary
lookup = {}
for item in exclude:
if isinstance(item, str):
item = LINESTYLES[item]
lookup[item] = None
# put line styles into a list
indices = INDEX2LINESTYLES.keys()
indices.sort()
if not linestylesList:
linestylesList = []
for index in indices:
if index not in lookup:
linestylesList.append(index)
# set datasets to different line styles
nLinestyles = len(linestylesList)
for index, dataset in enumerate(self.datasets):
linestyle = linestylesList[(skip *index) % nLinestyles]
dataset.line.set_suffix(linestyle, "linestyle")
def set_different_linewidths(self, skip=0.5, start=0.5,linewidthsList=[]):
"""Set datasets in graph to different linewidths. This function
behaves similarly to the similar function in the xmgrace
GUI. Can alternatively specify a list of line widths.
"""
# determine linewidths
if not linewidthsList:
linewidthsList = [skip*index+start
for index in range(len(self.datasets))]
# set datasets to different line widths
for index, dataset in enumerate(self.datasets):
dataset.line.set_suffix(linewidthsList[index], "linewidth")
def half_open(self):
"""Make frame half open
"""
self.frame.type = 1
self.xaxis.bar.linestyle = 0
self.xaxis.tick.place = "normal"
self.yaxis.bar.linestyle = 0
self.yaxis.tick.place = "normal"
def __init__(self, name='dummy', log=None):
Axis.__init__(self, name, log=log)
self.net = 0
q[1] = [np.pi/2.]
q[2] = [-np.pi/4.]
q[3] = [-np.pi/2.]
q[4] = [0.5]
mbc.q = q
rbd.forwardKinematics(mb, mbc)
X_s = sva.PTransformd(sva.RotY(-np.pi/2.), e.Vector3d(0.1, 0., 0.))
mbv = MultiBodyViz(mbg, mb, endEffectorDict={'b4':(X_s, 0.1, (0., 1., 0.))})
# test MultiBodyViz
from tvtk.tools import ivtk
viewer = ivtk.viewer()
mbv.addActors(viewer.scene)
mbv.display(mb, mbc)
# test axis
from axis import Axis
a1 = Axis(text='test', length=0.2)
a1.addActors(viewer.scene)
a1.X = sva.PTransformd(sva.RotX(np.pi/2.), e.Vector3d.UnitX())
# test vector6d
from vector6d import ForceVecViz, MotionVecViz
M = sva.MotionVecd(e.Vector3d(0.2, 0.1, 0.), e.Vector3d(0.1, 0., 0.2))
F = sva.ForceVecd(e.Vector3d(-0.2, -0.1, 0.), e.Vector3d(-0.1, 0., -0.2))
MV = MotionVecViz(M, a1.X)
FV = ForceVecViz(F, sva.PTransformd(sva.RotX(np.pi/2.), e.Vector3d.UnitX()*1.4))
MV.addActors(viewer.scene)
FV.addActors(viewer.scene)
