API tools faq
paste
Advertisement
Guest User

AAAAAAAAAAAAA

a guest
Nov 22nd, 2019
118
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Python 20.91 KB | None | 0 0
raw download clone embed print report
  1. import matplotlib.pyplot as plt
  2. import numpy as np
  3. import time
  4. from scipy.optimize import fsolve
  5.  
  6. class Link():
  7.     def __init__(self, M, L):
  8.         self.M = M
  9.         self.L = L
  10.  
  11. class Foot():
  12.     def __init__(self, M, height, foot_length=0.1, isLeft=True):
  13.         self.M = M
  14.         self.foot_length = foot_length
  15.         self.height = height
  16.  
  17.         self.angle = 0
  18.         self.coord = (0,0)
  19.         self.isLeft = isLeft
  20.         self._tip = self.coord
  21.  
  22.     @property
  23.     def tip(self):
  24.         if self.isLeft:
  25.             self._tip = (self.coord[0]+self.foot_length*np.cos(self.angle),
  26.                         self.coord[1]-self.foot_length*np.sin(self.angle))
  27.         else:
  28.             self._tip = (self.coord[0]-self.foot_length*np.cos(self.angle),
  29.                         self.coord[1]+self.foot_length*np.sin(self.angle))
  30.         # self._tip = self.coord
  31.         return self._tip
  32.     @tip.setter
  33.     def tip(self, val):
  34.         self._tip = tuple(val)
  35.  
  36. class Actuator():
  37.     def __init__(self, link1, link2):
  38.         self.angle = 0       # rad
  39.         self.link1 = link1
  40.         self.link2 = link2
  41.         self.coord = (0,0)
  42.         self.speed = 0      # rad/s
  43.  
  44.         self.time_seriesx = []
  45.         self.time_seriesy = []
  46.  
  47.     def update(self, dt):
  48.         self.angle += self.speed*dt
  49.  
  50. class Robot():
  51.     def __init__(self, world=None, can_topple=False, animate=False):
  52.         self.left_planted = True
  53.         self.right_planted = False
  54.         self.build()
  55.        
  56.         if self.left_planted:
  57.             self.l_L.coord = (0, 0)
  58.             self.l_L.angle = 0
  59.         elif self.right_planted:
  60.             self.r_L.coord = (0, 0)
  61.             self.r_L.angle = 0
  62.         else:
  63.             raise ValueError("Need initial conditions")
  64.    
  65.         if world is not None:
  66.             self.world = world
  67.         if can_topple:
  68.             self.can_topple = True
  69.         if animate:
  70.             self.prepare_plot_for_animation()
  71.  
  72.     @property
  73.     def is_stable(self):
  74.         if not hasattr(self, 'can_topple'):
  75.             return True
  76.         if self.left_planted:
  77.             _off = self.l_L.coord[0]
  78.             sum_com = 0*self.l_L.M
  79.             sum_com += self.l_mid.M*(self.servo2.coord[0]-_off)/2
  80.             sum_com += self.r_mid.M*(self.servo2.coord[0]+(self.servo3.coord[0]-self.servo2.coord[0])/2-_off)
  81.             sum_com += self.r_L.M*(self.servo3.coord[0]+(self.r_L.coord[0]-self.servo3.coord[0])/2-_off)
  82.             sum_com /= self.l_L.M + self.l_mid.M + self.r_mid.M + self.r_L.M
  83.             if sum_com > self.l_L.foot_length or sum_com < 0:
  84.                 return False
  85.             else:
  86.                 return True
  87.         elif self.right_planted:
  88.             _off = self.r_L.coord[0]
  89.             sum_com = 0*self.r_L.M
  90.             sum_com += self.r_mid.M*(self.servo2.coord[0]-_off)/2
  91.             sum_com += self.l_mid.M*(self.servo2.coord[0]-(self.servo1.coord[0]-self.servo2.coord[0])/2-_off)
  92.             sum_com += self.l_L.M*(self.servo1.coord[0]-(self.l_L.coord[0]-self.servo1.coord[0])/2-_off)
  93.             sum_com /= self.l_L.M + self.l_mid.M + self.r_mid.M + self.r_L.M
  94.             if sum_com < -self.l_L.foot_length or sum_com > 0:
  95.                 return False
  96.             else:
  97.                 return True
  98.  
  99.     def build(self):
  100.         self.l_L = Foot(0.1, 0.1, isLeft=True)
  101.         self.l_mid = Link(0.1, 0.1)
  102.         self.r_mid = Link(0.1, 0.1)
  103.         self.r_L = Foot(0.1, 0.1, isLeft=False)
  104.  
  105.         self.servo1 = Actuator(self.l_L,self.l_mid)
  106.         self.servo2 = Actuator(self.l_mid,self.r_mid)
  107.         self.servo3 = Actuator(self.r_mid, self.r_L)
  108.    
  109.     def goto(self, angs):
  110.         self.servo1.angle = np.deg2rad(angs[0])
  111.         self.servo2.angle = np.deg2rad(angs[1])
  112.         self.servo3.angle = np.deg2rad(angs[2])
  113.         self.forwards_kin()
  114.  
  115.     def forwards_kin(self):
  116.         if not self.is_stable:
  117.             raise Warning("Robot is unstable")
  118.  
  119.         if self.left_planted and not self.right_planted:
  120.             # start building from left foot corner
  121.             _ang = self.l_L.angle
  122.             self.servo1.coord = (self.l_L.coord[0]+self.l_L.height*np.sin(_ang),
  123.                                     self.l_L.coord[1]+self.l_L.height*np.cos(_ang))
  124.             _ang += self.servo1.angle
  125.             self.servo2.coord = (self.servo1.coord[0]+self.l_mid.L*np.sin(_ang),
  126.                                     self.servo1.coord[1]+self.l_mid.L*np.cos(_ang))
  127.             _ang += self.servo2.angle
  128.             self.servo3.coord = (self.servo2.coord[0]+self.r_mid.L*np.sin(_ang),
  129.                                     self.servo2.coord[1]+self.r_mid.L*np.cos(_ang))
  130.             _ang += self.servo3.angle
  131.             self.r_L.coord = (self.servo3.coord[0]+self.r_L.height*np.sin(_ang),
  132.                                     self.servo3.coord[1]+self.r_L.height*np.cos(_ang))
  133.             self.r_L.angle = _ang-np.pi
  134.            
  135.             if hasattr(self, 'world'):
  136.                 def ison(point, obst):
  137.                     if point[0] < obst.top[1][0] and point[0] > obst.top[0][0]:
  138.                         return True
  139.                     else:
  140.                         return False
  141.                 for obstacle in self.world.obstacles:
  142.                     if ison(self.r_L.coord, obstacle) or ison(self.r_L.tip, obstacle):
  143.                         if self.r_L.coord[1] < obstacle.top[0][1]:
  144.                             self.right_planted = True
  145.                             self.left_planted = False
  146.                             if self.r_L.coord[1] < obstacle.top[0][1]:
  147.                                 self.translate(0, obstacle.top[0][1]-self.r_L.coord[1])
  148.  
  149.             hit_ground = (self.r_L.coord[1] < -0) and abs(self.r_L.angle) < np.deg2rad(10)
  150.             if hit_ground:
  151.                 self.right_planted = True
  152.                 self.left_planted = False
  153.                 lowest_point = min(self.r_L.coord[1],self.r_L.coord[1])
  154.                 if lowest_point < -0:
  155.                     self.translate(0, -lowest_point)
  156.  
  157.             elif self.r_L.coord[1] < -0 or self.r_L.tip[1] < -0:
  158.                 raise Exception("Collision with ground at angle > 10 degrees")
  159.  
  160.         elif self.right_planted and not self.left_planted:
  161.             # start building from right foot corner
  162.             _ang = self.r_L.angle
  163.             self.servo3.coord = (self.r_L.coord[0]+self.r_L.height*np.sin(_ang),
  164.                                     self.r_L.coord[1]+self.r_L.height*np.cos(_ang))
  165.             _ang -= self.servo3.angle
  166.             self.servo2.coord = (self.servo3.coord[0]+self.r_mid.L*np.sin(_ang),
  167.                                     self.servo3.coord[1]+self.r_mid.L*np.cos(_ang))
  168.             _ang -= self.servo2.angle
  169.             self.servo1.coord = (self.servo2.coord[0]+self.r_mid.L*np.sin(_ang),
  170.                                     self.servo2.coord[1]+self.r_mid.L*np.cos(_ang))
  171.             _ang -= self.servo1.angle
  172.             self.l_L.coord = (self.servo1.coord[0]+self.l_L.height*np.sin(_ang),
  173.                                     self.servo1.coord[1]+self.l_L.height*np.cos(_ang))
  174.             self.l_L.angle = _ang+np.pi
  175.  
  176.             if hasattr(self, 'world'):
  177.                 def ison(point, obst):
  178.                     if point[0] < obst.top[1][0] and point[0] > obst.top[0][0]:
  179.                         return True
  180.                     else:
  181.                         return False
  182.                 for obstacle in self.world.obstacles:
  183.                     if ison(self.l_L.coord, obstacle) or ison(self.l_L.tip, obstacle):
  184.                         if self.l_L.coord[1] < obstacle.top[0][1]:
  185.                             self.left_planted = True
  186.                             self.right_planted = False
  187.                             if self.l_L.coord[1] < obstacle.top[0][1]:
  188.                                 self.translate(0, obstacle.top[0][1]-self.l_L.coord[1])
  189.  
  190.             hit_ground = (self.l_L.coord[1] < -0) and abs(self.l_L.angle) < np.deg2rad(10)
  191.             if hit_ground:
  192.                 self.left_planted = True
  193.                 self.right_planted = False
  194.                 lowest_point = min(self.l_L.coord[1],self.r_L.coord[1])
  195.                 if lowest_point < -0:
  196.                     self.translate(0, -lowest_point)
  197.  
  198.             elif self.l_L.coord[1] < -0 or self.l_L.tip[1] < -0:
  199.                 raise Exception("Collision with ground at angle > 10 degrees")
  200.  
  201.         # self.servo1.time_seriesx.append(self.servo1.coord[0])
  202.         # self.servo1.time_seriesy.append(self.servo1.coord[1])
  203.         # self.servo3.time_seriesx.append(self.servo3.coord[0])
  204.         # self.servo3.time_seriesy.append(self.servo3.coord[1])
  205.  
  206.     def translate(self, x, y):
  207.         objects = [self.l_L, self.r_L, self.servo1, self.servo2, self.servo3]
  208.         for i in objects:
  209.             tmp = list(i.coord)
  210.             tmp[0] += x
  211.             tmp[1] += y
  212.             i.coord = tuple(tmp)
  213.         tmp = list(self.l_L.tip)
  214.         tmp[0] += x
  215.         tmp[1] += y
  216.         self.l_L.tip = tmp
  217.         tmp = list(self.r_L.tip)
  218.         tmp[0] += x
  219.         tmp[1] += y
  220.         self.r_L.tip = tmp
  221.  
  222.     def set_servo_speeds(self, speeds, deg=True):
  223.         if deg:
  224.             self.servo1.speed = np.deg2rad(speeds[0])
  225.             self.servo2.speed = np.deg2rad(speeds[1])
  226.             self.servo3.speed = np.deg2rad(speeds[2])
  227.         else:
  228.             self.servo1.speed = speeds[0]
  229.             self.servo2.speed = speeds[1]
  230.             self.servo3.speed = speeds[2]
  231.  
  232.     def get_angles(self, deg=True):
  233.         if deg:
  234.             return [np.rad2deg(self.servo1.angle),
  235.                     np.rad2deg(self.servo2.angle),
  236.                     np.rad2deg(self.servo3.angle)]
  237.         else:
  238.             return [self.servo1.angle,
  239.                     self.servo2.angle,
  240.                     self.servo3.angle]
  241.  
  242.     def prepare_plot_for_animation(self):
  243.         plt.ion()
  244.         self.l_L.plot = plt.plot(0, 0, 'r', linewidth=3)[0]  # create plot handle
  245.         self.l_mid.plot = plt.plot(0, 0, 'b', linewidth=3)[0]  # create plot handle
  246.         self.r_mid.plot = plt.plot(0, 0, 'm', linewidth=3)[0]  # create plot handle
  247.         self.r_L.plot = plt.plot(0, 0, 'y', linewidth=3)[0]  # create plot handle
  248.         plt.axhspan(-0.1,0)
  249.  
  250.     def plot(self):
  251.         if hasattr(self.l_L, 'plot'):
  252.             # animation
  253.             self.l_L.plot.set_xdata([self.l_L.tip[0], self.l_L.coord[0], self.servo1.coord[0]])
  254.             self.l_L.plot.set_ydata([self.l_L.tip[1], self.l_L.coord[1], self.servo1.coord[1]])
  255.             self.l_mid.plot.set_xdata([self.servo1.coord[0], self.servo2.coord[0]])
  256.             self.l_mid.plot.set_ydata([self.servo1.coord[1], self.servo2.coord[1]])
  257.             self.r_mid.plot.set_xdata([self.servo2.coord[0], self.servo3.coord[0]])
  258.             self.r_mid.plot.set_ydata([self.servo2.coord[1], self.servo3.coord[1]])
  259.             self.r_L.plot.set_xdata([self.servo3.coord[0], self.r_L.coord[0], self.r_L.tip[0]])
  260.             self.r_L.plot.set_ydata([self.servo3.coord[1], self.r_L.coord[1], self.r_L.tip[1]])
  261.             plt.axis('equal')
  262.             ax = plt.gca()
  263.             ax.relim()
  264.             ax.autoscale_view()
  265.             plt.draw()
  266.         else:
  267.             #left foot
  268.             plt.plot([self.l_L.tip[0], self.l_L.coord[0], self.servo1.coord[0]],
  269.                 [self.l_L.tip[1], self.l_L.coord[1], self.servo1.coord[1]], c='r')
  270.             #mid left
  271.             plt.plot([self.servo1.coord[0], self.servo2.coord[0]],
  272.                 [self.servo1.coord[1], self.servo2.coord[1]], c='b')
  273.             #mid right
  274.             plt.plot([self.servo2.coord[0], self.servo3.coord[0]],
  275.                 [self.servo2.coord[1], self.servo3.coord[1]], c='m')
  276.             #right foot
  277.             plt.plot([self.servo3.coord[0], self.r_L.coord[0], self.r_L.tip[0]],
  278.                 [self.servo3.coord[1], self.r_L.coord[1], self.r_L.tip[1]], c='y')
  279.             plt.axis('equal')
  280.             plt.show()
  281.  
  282.     def time_step(self, dt):
  283.         self.servo1.update(dt)
  284.         self.servo2.update(dt)
  285.         self.servo3.update(dt)
  286.         self.forwards_kin()
  287.         if hasattr(self.l_L, 'plot'):
  288.             self.plot()
  289.             plt.pause(dt/2)
  290.  
  291. class Obstacle():
  292.     def __init__(self, height, dist, width=0.15):
  293.         self.top = [(dist, height), (dist+width, height)]
  294.  
  295.         from matplotlib.patches import Rectangle
  296.         rect = Rectangle((dist,0), width, height, color='k')
  297.         plt.gca().add_patch(rect)
  298.  
  299. class World():
  300.     def __init__(self):
  301.         self.g = 9.81
  302.         self.obstacles = []
  303.    
  304.     def add_object(self, obj):
  305.         self.obstacles.append(obj)
  306.  
  307. class Planner():
  308.     def __init__(self, robot, dt=0.01):
  309.         self.r = robot
  310.         self.next_obstacle = self.get_next_obstacle()
  311.         self.start_recording()
  312.         self.dt = dt
  313.    
  314.     ######## POSITION APPROACH ##########
  315.     def move_elbow_to_pos(self, pos):
  316.         def _equn(vec):
  317.             a,b = vec
  318.             return (self.r.l_mid.L*np.sin(a) + self.r.r_mid.L*np.sin(a+b) - pos[0],
  319.                     self.r.l_mid.L*np.cos(a) + self.r.r_mid.L*np.cos(a+b)  - pos[1])
  320.         theta1,theta2 = fsolve(_equn, (np.deg2rad(60), np.deg2rad(30)))
  321.         out = [theta1, theta2, np.pi-(theta1+theta2)]
  322.         return list(np.rad2deg(out))
  323.  
  324.     ######### VELOCITY APPROACH #########
  325.     # not accurate!!!!
  326.     def move_feet_rel_parallel(self, v):
  327.         angles = self.r.get_angles(deg=False)
  328.  
  329.         def _equn(vec):
  330.             a,b = vec
  331.             return (a*self.r.l_mid.L*np.cos(angles[0]) - b*self.r.r_mid.L*np.sin(angles[0]+angles[1]-np.pi/2) - v[0],
  332.                     -a*self.r.l_mid.L*np.sin(angles[0]) - b*self.r.r_mid.L*np.cos(angles[0]+angles[1]-np.pi/2)  - v[1])
  333.        
  334.         w1,w2 = fsolve(_equn, (0., 0.), factor=5, maxfev=1000)
  335.         out = [w1, w2, -w1-w2] ##### ??? wtf
  336.         return out
  337.  
  338.     def vel_ctrl(self, times, vels):
  339.         if type(times) == int or type(times) == float:
  340.             times = np.diff(np.arange(0,times, self.dt))
  341.         elif type(times) == list:
  342.             times = np.diff(times)
  343.  
  344.         if type(vels) == int or type(vels) == float:
  345.             for i in times:
  346.                 omegas = self.move_feet_rel_parallel(vels)
  347.                 self.r.set_servo_speeds(omegas, deg=False)
  348.                 self.r.time_step(i)
  349.         elif type(vels) == list:
  350.             for i, vel in zip(times,vels):
  351.                 omegas = self.move_feet_rel_parallel(vel)
  352.                 self.r.set_servo_speeds(omegas, deg=False)
  353.                 self.r.time_step(i)
  354.  
  355.     def pos_ctrl(self, times, posns):
  356.         if type(times) == int or type(times) == float:
  357.             times = np.diff(np.arange(0,times, self.dt))
  358.         elif type(times) == list:
  359.             times = np.diff(times)
  360.  
  361.         for i, posn in zip(times, posns):
  362.             angs = self.move_elbow_to_pos(posn)
  363.             self.r.goto(angs)
  364.             self.r.time_step(i)
  365.  
  366.             self.joint_trajs.append(angs)
  367.    
  368.     def take_step(self, length, T=1, model='circle', height=False):
  369.         if height is False:
  370.             height = length
  371.         max_reach = self.r.l_mid.L + self.r.r_mid.L
  372.         min_reach = min(self.r.l_L.foot_length, self.r.r_L.foot_length)
  373.         if abs(length) > max_reach - min_reach:
  374.             raise ValueError("Max step size is {0:.3f}m".format(max_reach-min_reach))
  375.        
  376.         if self.r.left_planted and self.r.right_planted:
  377.             self.r.right_planted = False
  378.  
  379.         resting = (min_reach+max_reach)/2
  380.         t = np.arange(0, T+self.dt, step=self.dt)
  381.         if model == 'circle':
  382.             w = 2*np.pi/T
  383.             if length < 0:
  384.                 w *= -1            
  385.             x = resting - length*np.cos(w*t)/2
  386.             y = 0 + height*np.sin(w*t)/2
  387.         elif model == 'rect':
  388.             vel = (2*height+2*length)/T
  389.             ts = np.array_split(t,8)    # TODO: Split this into non-equal sections
  390.             x1 = resting - length*np.ones_like(ts[0])/2
  391.             x2 = resting - length/2 + (ts[1]-ts[1][0])*vel
  392.             x3 = resting + (ts[2]-ts[2][0])*vel
  393.             x4 = resting + length*np.ones_like(ts[3])/2
  394.             x5 = resting + length*np.ones_like(ts[4])/2
  395.             x6 = resting + length/2 - (ts[5]-ts[5][0])*vel
  396.             x7 = resting - (ts[6]-ts[6][0])*vel
  397.             x8 = resting - length*np.ones_like(ts[7])/2
  398.  
  399.             y1 = ts[0]*vel
  400.             y2 = height*np.ones_like(ts[1])/2
  401.             y3 = height*np.ones_like(ts[2])/2
  402.             y4 = height/2 - (ts[3]-ts[3][0])*vel
  403.             y5 = -(ts[4]-ts[4][0])*vel
  404.             y6 = -height*np.ones_like(ts[5])/2
  405.             y7 = -height*np.ones_like(ts[6])/2
  406.             y8 = -height/2 + (ts[7]-ts[7][0])*vel
  407.  
  408.             x = np.concatenate((x1,x2,x3,x4,x5,x6,x7,x8), axis=-1)
  409.             y = np.concatenate((y1,y2,y3,y4,y5,y6,y7,y8), axis=-1)
  410.         posn = np.vstack((x, y)).T        
  411.         self.pos_ctrl(list(t), list([list(i) for i in posn]))
  412.  
  413.     def circle_walk(self, N=10, normal_stride_length=0.09):
  414.         for _ in range(N):
  415.             dist = self.get_next_obstacle()
  416.             if dist == np.inf:
  417.                 self.take_step(normal_stride_length, height=normal_stride_length/2)
  418.                 continue
  419.             N_normal_steps = dist // normal_stride_length
  420.             if N_normal_steps > 0:
  421.                 self.take_step(normal_stride_length, height=normal_stride_length/2)
  422.             elif dist % normal_stride_length < 0.006:   # step up or down
  423.                 self.take_step(normal_stride_length, height=normal_stride_length*1.5)
  424.             else:   # get close
  425.                 self.take_step(dist % normal_stride_length - 0.005)
  426.  
  427.     def rect_walk(self, N=10, normal_stride_length=0.09):
  428.         for _ in range(N):
  429.             dist = self.get_next_obstacle()
  430.             if dist == np.inf:
  431.                 self.take_step(normal_stride_length, model='rect')
  432.                 continue
  433.             N_normal_steps = dist // normal_stride_length
  434.             if N_normal_steps > 0:
  435.                 self.take_step(normal_stride_length, model='rect')
  436.             elif dist % normal_stride_length < 0.001:
  437.                 self.take_step(normal_stride_length, model='rect')
  438.             else:
  439.                 self.take_step(dist % normal_stride_length - 0.0005, model='rect')
  440.  
  441.     def get_next_obstacle(self):
  442.         prev = np.inf
  443.         for obs in self.r.world.obstacles:
  444.             dist_front = obs.top[0][0] - self.r.r_L.coord[0]
  445.             dist_back = obs.top[1][0] - self.r.r_L.coord[0]
  446.             if dist_front < 0:
  447.                 if dist_back < 0:
  448.                     continue    # past the object
  449.                 else:
  450.                     prev = dist_back    # on the object
  451.             else:
  452.                 if dist_front < prev:
  453.                     prev = dist_front
  454.         return prev
  455.  
  456.     def start_recording(self):
  457.         self.joint_trajs = []
  458.  
  459.     def plot_joint_trajs(self):
  460.         plt.ioff()
  461.         plt.figure()
  462.         plt.subplot(111)
  463.         trajs = np.asarray(self.joint_trajs)
  464.         t = np.arange(0, trajs.shape[0]*self.dt, self.dt)
  465.         plt.plot(t, trajs[:,0], label='Joint 1')
  466.         plt.plot(t, trajs[:,1], label='Joint 2')
  467.         plt.plot(t, trajs[:,2], label='Joint 3')
  468.         plt.ylabel('Joint angle /degrees')
  469.         plt.xlabel('Time /s')
  470.         plt.legend()
  471.         plt.show()
  472.         plt.ion()
  473.  
  474.     def plot_joint_vels(self):
  475.         plt.ioff()
  476.         plt.figure()
  477.         plt.subplot(111)
  478.         trajs = np.asarray(self.joint_trajs)
  479.         t = np.arange(0, trajs.shape[0]*self.dt, self.dt)[:-1]
  480.         plt.plot(t, np.diff(trajs[:,0]), label='Joint 1')
  481.         plt.plot(t, np.diff(trajs[:,1]), label='Joint 2')
  482.         plt.plot(t, np.diff(trajs[:,2]), label='Joint 3')
  483.         plt.ylabel('Joint speed / degrees/s')
  484.         plt.xlabel('Time /s')
  485.         plt.legend()
  486.         plt.show()
  487.         plt.ion()
  488.  
  489.     def plot_joint_accels(self):
  490.         plt.ioff()
  491.         plt.figure()
  492.         plt.subplot(111)
  493.         trajs = np.asarray(self.joint_trajs)
  494.         t = np.arange(0, trajs.shape[0]*self.dt, self.dt)[1:-1]
  495.         plt.plot(t, np.diff(np.diff(trajs[:,0])), label='Joint 1')
  496.         plt.plot(t, np.diff(np.diff(trajs[:,1])), label='Joint 2')
  497.         plt.plot(t, np.diff(np.diff(trajs[:,2])), label='Joint 3')
  498.         plt.ylabel('Joint acceleration / degrees/s^2')
  499.         plt.xlabel('Time /s')
  500.         plt.legend()
  501.         plt.show()
  502.         plt.ion()
  503.  
  504. world = World()
  505. # world.add_object(Obstacle(0.03, 0.3))
  506. # world.add_object(Obstacle(0.06, 0.45))
  507.  
  508. robot = Robot(world=world, can_topple=False, animate=False)
  509. robot.goto([60,60,60])
  510.  
  511. planner = Planner(robot)
  512. planner.circle_walk(6)
  513.  
  514. import pyfirmata
  515.  
  516. board = pyfirmata.Arduino('/dev/ttyACM0')
  517. s1 = board.get_pin('d:9:s')
  518. s2 = board.get_pin('d:10:s')
  519. s3 = board.get_pin('d:11:s')
  520. print("starting")
  521.  
  522. off = [10,40,80]
  523. time.sleep(5)
  524. # for [a1,a2,a3] in planner.joint_trajs:
  525. #     s1.write(a1+off[0])
  526. #     s2.write(a2+off[1])
  527. #     s3.write(a3-off[2])
  528. #     time.sleep(0.02)
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!