diff --git a/lecture_03/assignment_02/andrea_settimi/assignment_02.py b/lecture_03/assignment_02/andrea_settimi/assignment_02.py new file mode 100644 index 0000000..bf34b37 --- /dev/null +++ b/lecture_03/assignment_02/andrea_settimi/assignment_02.py @@ -0,0 +1,64 @@ +"""Assignment 02: Build your own robot model +""" +from compas.artists import Artist +from compas.datastructures import Mesh +from compas.geometry import Circle +from compas.geometry import Cylinder +from compas.geometry import Frame +from compas.geometry import Plane +from compas.geometry import Translation +from compas.robots import Configuration +from compas.robots import Joint +from compas.robots import RobotModel +import time + + +# create cylinder in yz plane +radius, length = 0.3, 5 +cylinder = Cylinder(Circle(Plane([0, 0, 0], [1, 0, 0]), radius), length) +cylinder.transform(Translation.from_vector([length / 2.0, 0, 0])) +mesh = Mesh.from_shape(cylinder) + +# create robot model +model = RobotModel("robot", links=[], joints=[]) + +# add links +n_links = 100 + +link0 = model.add_link("link0") +for l in range(1, n_links-1): + model.add_link(name=("link" + str(l)), visual_mesh=mesh, visual_color=(l/n_links, 0, l/n_links)) +linkN = model.add_link(("link" + str(n_links))) + +print(len(model.links)) + +# test add joints +axis = (0, 0, 1) + +links = model.links + +origin = Frame((0, 0, 0), (1, 0, 0), (0, 1, 0)) +model.add_joint("joint0", Joint.CONTINUOUS, links[0], links[1], origin, axis) + +origin = Frame((length, 0, 0), (1, 0, 0), (0, 1, 0)) + +for j in range(2, n_links): + joint_name = "joint1" + str(j) + joint_type = Joint.CONTINUOUS + joint_parent = links[j] + joint_child = links[j-1] + + model.add_joint(joint_name, Joint.CONTINUOUS, joint_child, joint_parent, origin, axis) + +# Create a configuration object matching the number of joints in your model +config = model.zero_configuration() + +# Create a double spiral path with a new configuration and show animation +for i in range(n_links-1): + config.joint_values[i] = (i/(n_links-1))*2*3.14 + artist = Artist(model) + artist.update(config) + artist.clear() + artist.draw_visual() + artist.redraw() + time.sleep(0.001)