def scene_sphere():
def blue(face, u, v):
return (0.1, 0.1, 1.0), 0.3, 0.2, 6
l = [Light((1.0, -1.0, 1.0), (1.0, 1.0, 1.0))]
s = Sphere(blue)
return s, l
def __init__(self, link_radii=arm_radius, joint_radii=arm_radius, link_lengths=[15,15,5,5], joint_colors=arm_color,
link_colors=arm_color, base_loc=np.array([2., 0., -15]), **kwargs):
'''
Instantiate the graphics and the virtual arm for a kinematic chain
'''
num_joints = 2
self.num_joints = 2
self.link_radii = make_list(link_radii, num_joints)
self.joint_radii = make_list(joint_radii, num_joints)
self.link_lengths = make_list(link_lengths, num_joints)
self.joint_colors = make_list(joint_colors, num_joints)
self.link_colors = make_list(link_colors, num_joints)
self.curr_vecs = np.zeros([num_joints, 3]) #rows go from proximal to distal links
# set initial vecs to correct orientations (arm starts out vertical)
self.curr_vecs[0,2] = self.link_lengths[0]
self.curr_vecs[1:,0] = self.link_lengths[1:]
# Create links
self.links = []
for i in range(self.num_joints):
joint = Sphere(radius=self.joint_radii[i], color=self.joint_colors[i])
# The most distal link gets a tapered cylinder (for purely stylistic reasons)
if i < self.num_joints - 1:
link = Cylinder(radius=self.link_radii[i], height=self.link_lengths[i], color=self.link_colors[i])
else:
link = Cone(radius1=self.link_radii[-1], radius2=self.link_radii[-1]/2, height=self.link_lengths[-1], color=self.link_colors[-1])
link_i = Group((link, joint))
self.links.append(link_i)
link_offsets = [0] + self.link_lengths[:-1]
self.link_groups = [None]*self.num_joints
for i in range(self.num_joints)[::-1]:
if i == self.num_joints-1:
self.link_groups[i] = self.links[i]
else:
self.link_groups[i] = Group([self.links[i], self.link_groups[i+1]])
self.link_groups[i].translate(0, 0, link_offsets[i])
# Call the parent constructor
super(RobotArmGen3D, self).__init__([self.link_groups[0]], **kwargs)
# Instantiate the kinematic chain object
if self.num_joints == 2:
self.kin_chain = robot_arms.PlanarXZKinematicChain(link_lengths)
self.kin_chain.joint_limits = [(-pi,pi), (-pi,0)]
else:
self.kin_chain = robot_arms.PlanarXZKinematicChain(link_lengths)
# TODO the code below is (obviously) specific to a 4-joint chain
self.kin_chain.joint_limits = [(-pi,pi), (-pi,0), (-pi/2,pi/2), (-pi/2, 10*pi/180)]
self.base_loc = base_loc
self.translate(*self.base_loc, reset=True)
def extract_predicted_parameters_as_json(fetched, k):
sphere = Sphere(fetched['sphere_center'][k],
np.sqrt(fetched['sphere_radius_squared'][k]))
return {
'type': 'sphere',
'center_x': sphere.center[0],
'center_y': sphere.center[1],
'center_z': sphere.center[2],
'radius': sphere.radius,
}
def scene_intersect():
def blue(face, u, v):
return (0.1, 0.1, 1.0), 0.3, 0.2, 6
def red(face, u, v):
return (1.0, 0.1, 0.1), 0.3, 0.2, 6
l = [Light((1.0, -1.0, 1.0), (1.0, 1.0, 1.0))]
s1 = Sphere(blue)
s1.translate(-0.6, 0.0, 0.0)
s2 = Sphere(red)
s2.translate(0.6, 0.0, 0.0)
i = Intersect(s1, s2)
return i, l
def scene_union():
def blue(face, u, v):
return (0.1, 0.1, 1.0), 0.3, 0.2, 6
def red(face, u, v):
return (1.0, 0.1, 0.1), 0.3, 0.2, 6
l = [Light((1.0, -1.0, 1.0), (1.0, 1.0, 1.0))]
s1 = Sphere(blue)
s1.translate(-0.6, 0.0, 0.0)
s2 = Sphere(red)
s2.translate(0.6, 0.0, 0.0)
u = Union(s1, s2)
return u, l
def test_create_sphere(self):
sphere = Sphere()
self.assertIsInstance(sphere.vertices, np.ndarray,
"Vertices is not an instance of numpy.ndarray")
self.assertIsInstance(sphere.indices, np.ndarray,
"Indices is not an instance of numpy.ndarray")
self.assertIs(sphere.vertices.dtype.type, np.float32,
"Numpy array's elements are not numpy.float32")
self.assertIs(sphere.indices.dtype.type, np.uint32,
"Numpy array's elements are not numpy.uint32")
self.assertGreater(len(sphere.vertices), 0,
"Sphere does not have any vertices")
self.assertGreater(len(sphere.indices), 0,
"Sphere does not have any indices for vertices")
def __init__(self, link_radii=(.2, .2), ball_radii=(.5,.5),lengths=(20, 20), ball_colors = ((1,1,1,1),(1,1,1,1)),\
link_colors = ((1,1,1,1), (1,1,1,1)), base_loc=np.array([2., 0., -10.]), **kwargs):
self.link_radii = link_radii
self.ball_radii = ball_radii
self.lengths = lengths
self.endpt_cursor = Sphere(radius=ball_radii[1], color=(1, 0, 1, 1)) #ball_colors[1])
self.forearm = Group([
Cylinder(radius=link_radii[1], height=lengths[1], color=link_colors[1]),
self.endpt_cursor.translate(0, 0, lengths[1])]).translate(0,0,lengths[0])
self.upperarm = Group([
Cylinder(radius=link_radii[0], height=lengths[0],color=link_colors[0]),
Sphere(radius=ball_radii[0],color=ball_colors[0]).translate(0, 0, lengths[0]),
self.forearm])
self.system = TwoJoint(self.upperarm, self.forearm, lengths = (self.lengths))
super(RobotArm, self).__init__([self.upperarm], **kwargs)
self.num_links = len(link_radii)
self.num_joints = 3 # abstract joints. this system is fully characterized by the endpoint position since the elbow angle is determined by IK
self.base_loc = base_loc
self.translate(*self.base_loc, reset=True)
def scene_texsphere():
def pattern(face, u, v):
up = int(u * 24) % 2
vp = int(v * 12) % 2
r, g, b = 0.0, 0.1, 0.0
if up == 0:
r = u
if vp == 0:
b = v
return (r, g, b), 0.3, 0.2, 6
l = [Light((1.0, -1.0, 1.0), (1.0, 1.0, 1.0))]
s = Sphere(pattern)
return s, l
def scene_difference2():
def blue(face, u, v):
return (0.1, 0.1, 1.0), 0.3, 0.2, 6
def red(face, u, v):
return (1.0, 0.1, 0.1), 0.3, 0.2, 6
l = [Light((1.0, -1.0, 1.0), (1.0, 1.0, 1.0))]
c1 = Sphere(blue)
c2 = Cylinder(red)
c2.translate(0.0, -0.5, 0.0)
c2.scale(0.4, 2.0, 0.4)
d = Difference(c1, c2)
d.rotatex(85.0)
d.rotatey(20.0)
d.rotatez(40.0)
return d, l
class RobotArm(Plant, Group):
def __init__(self, link_radii=(.2, .2), ball_radii=(.5,.5),lengths=(20, 20), ball_colors = ((1,1,1,1),(1,1,1,1)),\
link_colors = ((1,1,1,1), (1,1,1,1)), base_loc=np.array([2., 0., -10.]), **kwargs):
self.link_radii = link_radii
self.ball_radii = ball_radii
self.lengths = lengths
self.endpt_cursor = Sphere(radius=ball_radii[1], color=(1, 0, 1, 1)) #ball_colors[1])
self.forearm = Group([
Cylinder(radius=link_radii[1], height=lengths[1], color=link_colors[1]),
self.endpt_cursor.translate(0, 0, lengths[1])]).translate(0,0,lengths[0])
self.upperarm = Group([
Cylinder(radius=link_radii[0], height=lengths[0],color=link_colors[0]),
Sphere(radius=ball_radii[0],color=ball_colors[0]).translate(0, 0, lengths[0]),
self.forearm])
self.system = TwoJoint(self.upperarm, self.forearm, lengths = (self.lengths))
super(RobotArm, self).__init__([self.upperarm], **kwargs)
self.num_links = len(link_radii)
self.num_joints = 3 # abstract joints. this system is fully characterized by the endpoint position since the elbow angle is determined by IK
self.base_loc = base_loc
self.translate(*self.base_loc, reset=True)
def get_endpoint_pos(self):
# print 'curr_vecs', self.system.curr_vecs
# print
return np.sum(self.system.curr_vecs, axis=0) + self.base_loc
def set_endpoint_pos(self, pos, **kwargs):
self.system.set_endpoint_3D(pos - self.base_loc)
def get_intrinsic_coordinates(self):
return self.get_endpoint_pos()
def set_intrinsic_coordinates(self, pos):
self.set_endpoint_pos(pos)
def create_primitive_from_dict(d):
assert d['type'] == 'sphere'
location = np.array(
[d['location_x'], d['location_y'], d['location_z']], dtype=float)
radius = float(d['radius'])
return Sphere(center=location, radius=radius)