text: from matplotlib import pyplot as plt
  from numpy import cos as cos
  from numpy import sin as sin
  import numpy as np
  from numpy import diff
  import matplotlib.pyplot as plt
  from mujoco_py import (functions)
  import ImpFuncs as IF
  
  
  import globals
  L1=globals.L1
  L2=globals.L2
  L3=globals.L3
  SimStep=globals.SimStep
  waitingTime=globals.waitingTime
  sim=globals.sim
  viewer=globals.viewer
  
  #___# General navigation functions:
  #full path navigation function
  def Navigate(K,M,B,P,D,wantedPath, wantedTimes, waitingTime,method,methodCntrl,methodParams):
      #initialazing:
      dofs=np.shape(P[0])
      elbow=0 #can be 0/1 - choose the path of the inverse kinematics of the manipulator.
      q = invkin(wantedPath[0][0], wantedPath[0][1],wantedPath[0][2],elbow)
      sim.data.qpos[0] = q[0]
      sim.data.qpos[1] = q[1]
      sim.data.qpos[2] = q[2]
  
      #Impedance initital condition (for IMP only):
      xm=np.array([wantedPath[0][0],wantedPath[0][1],wantedPath[0][2]]).reshape(-1,1)
      xmDot=np.array([0,0,0]).reshape(-1,1) #specific for Robot at rest
  
      #Generalazied data storage vectors:
      accumulatedForce = []
      accumulatedControl = []
      accumulatedTravel = []
      accumulatedPlannedPath = []
      accumulatedJoints = []
      accumulatedJointsVel = []
      waitSim2(waitingTime)
      forcebool=0 #boolean variable that can be used to check if the endeffector is being touched.
  
      #General navigation loop:
      for i in range(len(wantedPath) - 1):
          nSteps = np.int32(wantedTimes[i] / SimStep)
          path = linearPathPlan([wantedPath[i][0], wantedPath[i][1],wantedPath[i][2]], [wantedPath[i + 1][0], wantedPath[i + 1][1], wantedPath[i + 1][2]], wantedTimes[i])
          [q_vec, q_dot_vec] = inversePathplan(path, wantedTimes[i], method,elbow,i)
  
          x_vec=np.array([path[0,:],path[2,:],path[4,:]])
          xDot_vec = np.array([path[1, :], path[3, :],path[5,:]])
  
          if methodCntrl == "PD": #PD navigation between 2 pts
              [travel, sensorData, controlData, jointData, jointVelData] = straightLineNav(q_vec, q_dot_vec, nSteps, P, D)
          if methodCntrl =="imp": #Imp navigation between 2 pts
              [travel, sensorData, controlData, jointData, jointVelData,xm] = IF.straightLineNavimp(x_vec,xDot_vec, nSteps,P,D,K,M,B,xm,xmDot,methodParams,dofs,elbow,q_vec, q_dot_vec,forcebool)
  
          #append:
          accumulatedJoints.append(jointData)
          accumulatedJointsVel.append(jointVelData)
          accumulatedForce.append(sensorData)
          accumulatedControl.append(controlData)
          accumulatedTravel.append(travel)
          accumulatedPlannedPath.append(np.transpose(path))  # path needed to be transposed in order to work
  
  
      # #finish path with step command to final target - can be applied ##
      # tmplength=len(wantedPath)-1
      # tmq=invkin(wantedPath[tmplength][0], wantedPath[tmplength][1],wantedPath[tmplength][2],elbow)
      # tmpTime=4
      # [q_vec,q_dot_vec]=stepPath(tmq[0],tmq[1],tmq[2],tmpTime)
      # nSteps = np.int32(tmpTime / SimStep)
      # [x_vec, xDot_vec] = stepPath(xm[0], xm[1],xm[2], tmpTime)
      #
      # x_vec=np.transpose(x_vec)
      # xDot_vec=np.transpose(xDot_vec)
      #
      # if methodCntrl == "PD":  # PD navigation between 2 pts
      #     [travel, sensorData, controlData, jointData, jointVelData] = straightLineNav(q_vec, q_dot_vec, nSteps, P, D)
      # if methodCntrl == "imp":  # Imp navigation between 2 pts
      #     [travel, sensorData, controlData, jointData, jointVelData,xm] = IF.straightLineNavimp(x_vec,xDot_vec, nSteps,P,D,K,M,B,xm,xmDot,methodParams,dofs,elbow,q_vec,q_dot_vec,forcebool)
      #
      # # append:
      # accumulatedJoints.append(jointData)
      # accumulatedJointsVel.append(jointVelData)
      # accumulatedForce.append(sensorData)
      # accumulatedControl.append(controlData)
      # accumulatedTravel.append(travel)
      # accumulatedPlannedPath.append(np.transpose(path))  # path needed to be transposed in order to work
  
      # Total outputs arrange:
      totalJoint    = outputArrange(accumulatedJoints)
      totalJointVel = outputArrange(accumulatedJointsVel)
      totalForce    = outputArrange(accumulatedForce)
      totalControl  = outputArrange(accumulatedControl)
      totalTravel   = outputArrange(accumulatedTravel)
      totalPath     = outputArrange(accumulatedPlannedPath)
  
      #saving csv files with all the navigation data.
      from numpy import savetxt
      savetxt('totalTravel.csv',totalTravel,delimiter=',')
      savetxt('totalPath.csv',totalPath,delimiter=',')
      savetxt('totalControl.csv', totalControl, delimiter=',')
      savetxt('totalForce.csv', totalForce, delimiter=',')
      savetxt('totalJointVel.csv', totalJointVel, delimiter=',')
      savetxt('totalJoint.csv', totalJoint, delimiter=',')
  
      #plotting: #plot all graphs.
      #jointPlotter(totalJoint)
      #jointVelPlotter(totalJointVel)
      #forcePlotter(totalForce)
      #controlPlotter(totalControl)
      #pathPlotter(totalTravel, totalPath)
      #plt.show()
  
      sim.reset()
  
  #____#kinematics:
  
  # inverse kinematics function. input  px,py,phi,elbow - output: q1,q2,q3
  def invkin(px, py,phi,elbow):
          delta=0
          change_elbow=0
          x3 = px - L3 * cos(phi)
          y3 = py - L3 * sin(phi)
          c2 = (x3 ** 2 + y3 ** 2 - L1 ** 2 - L2 ** 2) / (2 * L1 * L2+delta)
          if c2>1:
              c2=1
          if elbow ==1:
              s2 = (1 - c2 ** 2) ** 0.5
          if elbow == 0:
              s2 = -(1 - c2 ** 2) ** 0.5
          theta2 = np.arctan2(s2, c2)
          k1 = L1 + L2 * c2
          k2 = L2 * s2
          c1 = (k1 * x3 + k2 * y3) / (k1 ** 2 + k2 ** 2 + delta)
          s1 = (-k2 * x3 + k1 * y3) / (k1 ** 2 + k2 ** 2 + delta)
          theta1 = np.arctan2(s1, c1)
          if np.abs(theta2)<0:
              change_elbow==1
          theta3 = phi - theta1 - theta2
          if change_elbow==1:
              if elbow ==1:
                  elbow=0
              if elbow ==0:
                  elbow=1
          q = [theta1, theta2, theta3, elbow]
          return q
  # forward kinematics function. input  q1,q2,q3 output: x1,x2,phi
  def forwardkin(q1,q2,q3):
      global L1
      global L2
      x=L3*(cos(q1+q2+q3))+ L2*(cos(q1)*cos(q2)-sin(q1)*sin(q2))+L1*cos(q1)
      y=L3*(sin(q1+q2+q3))+L2*(cos(q1)*sin(q2)+sin(q1)*cos(q2))+L1*sin(q1)
      phi=q1+q2+q3
      return x,y,phi
  
  
  # planar path planning function. Input: current position of the end effector,
  # target position of the robot, time of navigation, number of points in the path
  # output: navigation vector in cartesian domain.
  def linearPathPlan(curr_pos, target_pos, T):
      global SimStep
      xPath = []
      yPath = []
      phiPath = []
      x_dot_Path = []
      y_dot_Path = []
      phi_dot_Path = []
      t_list = np.linspace(0, T, num=np.int32(T / SimStep))
      # Const definitions:
      kx = (target_pos[0] - curr_pos[0]) / T ** 3
      kphi =  (target_pos[2] - curr_pos[2]) / T ** 3
      a5 = 6 / T ** 2
      a4 = -(15 / T)
      a3 = 10
      delta = 0.0001
  
      # Specific case when path is vertical:
  
  
      if target_pos[0] == curr_pos[0]:
          ky = (target_pos[1] - curr_pos[1]) / T ** 3
          for t in t_list:
              x = target_pos[0]
              y = ky * (a5 * (t ** 5) + a4 * (t ** 4) + a3 * (t ** 3)) + curr_pos[1]
              phi = kphi * (a5 * (t ** 5) + a4 * (t ** 4) + a3 * (t ** 3)) + curr_pos[2]
  
              x_dot = 0
              y_dot = ky * (a5 * 5 * (t ** 4) + a4 * 4 * (t ** 3) + 3 * a3 * (t ** 2))
              phi_dot = kphi * (a5 * 5 * (t ** 4) + a4 * 4 * (t ** 3) + 3 * a3 * (t ** 2))
  
              xPath.append(x)
              yPath.append(y)
              phiPath.append(phi)
  
              x_dot_Path.append(x_dot)
              y_dot_Path.append(y_dot)
              phi_dot_Path.append(phi_dot)
  
          path = np.array([xPath, x_dot_Path, yPath, y_dot_Path, phiPath, phi_dot_Path])
          return path
  
  
      # General path:
  
      ky = ((target_pos[1] - curr_pos[1]) / (delta + target_pos[0] - curr_pos[0]))
      ky0 = (target_pos[0] * curr_pos[1] - target_pos[1] * curr_pos[0]) / (delta + target_pos[0] - curr_pos[0])
          # General case:
      for t in t_list:
              x = kx * (a5 * (t ** 5) + a4 * (t ** 4) + a3 * (t ** 3)) + curr_pos[0]
              y = ky * x + ky0
              phi = kphi * (a5 * (t ** 5) + a4 * (t ** 4) + a3 * (t ** 3)) + curr_pos[2]
              x_dot = kx * (a5 * 5 * (t ** 4) + a4 * 4 * (t ** 3) + 3 * a3 * (t ** 2))
              y_dot = ky * x_dot
              phi_dot = kphi * (a5 * 5 * (t ** 4) + a4 * 4 * (t ** 3) + 3 * a3 * (t ** 2))
              xPath.append(x)
              yPath.append(y)
              phiPath.append(phi)
              x_dot_Path.append(x_dot)
              y_dot_Path.append(y_dot)
              phi_dot_Path.append(phi_dot)
  
      path = np.array([xPath, x_dot_Path, yPath, y_dot_Path, phiPath, phi_dot_Path])
      return path
  
  # function that convert path vector to path vector in the joints domain
  def inversePathplan(path, Time,method,elbow,ii):
      xPath = path[0, :]
      x_dot_Path = path[1, :]
      yPath = path[2, :]
      y_dot_Path = path[3, :]
      phiPath = path[4, :]
      phi_dot_Path = path[5, :]
  
      # plt.scatter(xPath, yPath) # used for debug.
      q_vec = []
      t_list = np.linspace(0, Time, np.int32(Time / SimStep))
      for i in range(len(t_list)):
          q_vec.append(invkin(xPath[i], yPath[i],phiPath[i],elbow))
      q_vec = np.array(q_vec)
  
      if method=='numerical': #numeric
           q_dot_vec = np.array([diff(q_vec[:, 0]) / SimStep, diff(q_vec[:, 1]) / SimStep,diff(q_vec[:, 2]) / SimStep])
           b = [0, 0, 0]
           q_dot_vec = q_dot_vec.transpose()
           q_dot_vec = np.append(q_dot_vec, b).reshape(q_dot_vec.shape[0] + 1, q_dot_vec.shape[1])
  
      return [q_vec, q_dot_vec]
  
  # one point navigating function with PD control.
  def pdNav2Target(q_target, q_dot_target,P,D):
      #output parameters:
      effector_pos = []
      sensorData = []
      control_effort = []
      angle=[]
      angleVel=[]
      # Control:
      error = q_target - sim.data.actuator_length
      q_dot = sim.data.actuator_velocity - q_dot_target
      #torques output:
      action = np.matmul(P, error) - np.matmul(D, q_dot) +[sim.data.qfrc_bias[0], sim.data.qfrc_bias[1], sim.data.qfrc_bias[2]] # sim.data.qfrc_bias is Gravity bias/robot dependant
  
      #Force collection:
      forces,torques =IF.getForces(3,'gripper')
  
      # collecting sensors data: particular to 3Dof Robot
      sensorData.append([forces[0],forces[1],forces[2],torques[0],torques[1],torques[2]])
      effector_pos.append([np.array(sim.data.site_xpos[0])[0], np.array(sim.data.site_xpos[0])[1], np.array(sim.data.site_xpos[0])[2]])
      control_effort.append([action[0],action[1],action[2]])
      angle.append([sim.data.actuator_length[0],sim.data.actuator_length[1],sim.data.actuator_length[2]])
      angleVel.append([sim.data.actuator_velocity[0],sim.data.actuator_velocity[1],sim.data.actuator_velocity[2]])
  
      return effector_pos, sensorData, control_effort, action,angle,angleVel  # collecting position of endeffector
  # total navigation function - > gets all navigation points and looping for one
  # point navigation function.
  def straightLineNav(qvec, q_dot_vec, nPts,P,D):
      jointDataTotal = []
      jointVelDataTotal = []
      effector_pos_total = []
      sensorDataTotal = []
      control_effort_total = []
  
      for i in range(nPts):
          effector_pos, sensorData, control_effort, action, angle, angleVel = pdNav2Target([qvec[i, 0], qvec[i, 1], qvec[i, 2]], [q_dot_vec[i, 0], q_dot_vec[i, 1],q_dot_vec[i, 2]],P,D)
  
          #Output appending:
          jointDataTotal.append(angle)
          jointVelDataTotal.append(angleVel)
          effector_pos_total.append(effector_pos)
          sensorDataTotal.append(sensorData)
          control_effort_total.append(control_effort)
  
          # Action and rendering:
          sim.data.ctrl[:] = action
          sim.step()
          sim.forward()
          viewer.render()
      #Arrange vectors:
      full_joints_data = outputArrange(jointDataTotal)
      full_joints_vel_data = outputArrange(jointVelDataTotal)
      full_sensor_data = outputArrange(sensorDataTotal)
      full_effector_data = outputArrange(effector_pos_total)
      full_control_data = outputArrange(control_effort_total)
  
      return full_effector_data, full_sensor_data, full_control_data ,full_joints_data , full_joints_vel_data
  
  
  #__#General purpose functions:
  #function that arranges the output vector and a form that is easy to use.
  #input un arranged vector -> output arranged vector
  def outputArrange(vec):
      sizeF = 0
      for i in range(len(vec)):
          for j in range(len(vec[i])):
              sizeF = sizeF + 1
  
      vec_width=len(vec[0][0])
      full_vec = np.zeros([sizeF, vec_width])
      sizeF = 0
      for i in range(len(vec)):
          for j in range(len(vec[i])):
              full_vec[sizeF, :] = np.array(vec[i][j])
              sizeF = sizeF + 1
      return full_vec
  #step input signal to the robot joints space
  def stepPath(q1,q2,q3,T):
      global SimStep
      nSteps = np.int32(T / SimStep)
      path=np.zeros([nSteps,np.size(sim.data.actuator_length)])
      path_qdot=np.zeros([nSteps,np.size(sim.data.actuator_length)])
      for i in range(nSteps):
          path[i,:]=[q1,q2,q3]
          path_qdot[i,:]=[0,0,0]
      return path,path_qdot
  #step input signal to the robot cartesian space
  def stepPathCartesian(x,y,T):
      global SimStep
      nSteps = np.int32(T / SimStep)
      path=np.zeros([nSteps,np.size(sim.data.actuator_length)])
      path_qdot=np.zeros([nSteps,np.size(sim.data.actuator_length)])
      for i in range(nSteps):
          path[i,:]=[x,y]
          path_qdot[i,:]=[0,0]
      return path,path_qdot
  #get forces and torques that acting about simulation site with name sitename
  
  
  #___# all the rest:
  #function that output q dot by knowitng xdot vec and q vec.
  #specific to RR planar robot
  def get_q_dot_from_jack(q,x_dot,y_dot):
      global waitingTime
      global L1
      global L2
      J=np.zeros([2,2])
      delta=0.1
      J[0,0]=-L2*(cos(q[0])*sin(q[1]) + cos(q[1])*sin(q[0])) -L1*sin(q[0])
      J[1,0]= L2*(cos(q[0])*cos(q[1]) - sin(q[0])*sin(q[1])) +L1*cos(q[0])
      J[0,1]= -L2*(cos(q[0])*sin(q[1]) + cos(q[1])*sin(q[0]))
      J[1,1] = L2 * (cos(q[0]) * cos(q[1]) - sin(q[0]) * sin(q[1]))
      x_vec=np.array([x_dot,y_dot]).reshape(-1,1)
      try: #try singular point
          J_inv=np.linalg.inv(J)
          qdot_vec = np.matmul(np.linalg.inv(J), x_vec)
      except Exception:
          print('robot went through a singular point')
      try: #custom solution for first step at qvec=0,0
          if sim.data.time<waitingTime*1.1:
              qdot_vec=np.transpose([0,0])
      finally:
          return (np.transpose(qdot_vec))
      return (np.transpose(qdot_vec))
  #function that output qdotaim by knowing xdot vec and q vec
  #specific for RRR planar robot
  def get_q_dotaim(q,qd,x_dotaim):
      global waitingTime
      global L1
      global L2
      q=np.array(q)
      qd=np.array(qd)
      Jd=np.zeros([2,2])
      Jd[0,0] = -L2*(cos(q[0])*cos(q[1])*qd[0] + cos(q[0])*cos(q[1])*qd[1] - sin(q[0])*sin(q[1])*qd[0] - sin(q[0])*sin(q[1])*qd[1]) - L1*cos(q[0])*qd[0]
      Jd[1,0] = -L2*(cos(q[0])*sin(q[1])*qd[1] + cos(q[1])*sin(q[0])*qd[0] + cos(q[0])*sin(q[1])*qd[1] + cos(q[1])*sin(q[0])*qd[1]) - L1*sin(q[0])*qd[0]
      Jd[0,1] = -L2*(cos(q[0])*cos(q[1])*qd[0] + cos(q[0])*cos(q[1])*qd[1] - sin(q[0])*sin(q[1])*qd[0] - sin(q[0])*sin(q[1])*qd[1])
      Jd[1,1] = -L2*(cos(q[0])*sin(q[1])*qd[0] + cos(q[1])*sin(q[0])*qd[0] + cos(q[0])*sin(q[1])*qd[1] + cos(q[1])*sin(q[0])*qd[1])
  
      J=np.zeros([2,2])
      J[0,0]=-L2*(cos(q[0])*sin(q[1]) + cos(q[1])*sin(q[0])) -L1*sin(q[0])
      J[1,0]= L2*(cos(q[0])*cos(q[1]) - sin(q[0])*sin(q[1])) +L1*cos(q[0])
      J[0,1]= -L2*(cos(q[0])*sin(q[1]) + cos(q[1])*sin(q[0]))
      J[1,1] = L2 * (cos(q[0]) * cos(q[1]) - sin(q[0]) * sin(q[1]))
  
      try: #try singular point
          J_inv=np.linalg.inv(J)
          qdotaim_vec = np.matmul(np.linalg.inv(J),x_dotaim-np.matmul(Jd,qd))
      except Exception:
          print('robot went through a singular point')
      try: #custom solution for first step at qvec=0,0
          if sim.data.time<waitingTime*1.1:
              qdotaim_vec=([0,0])
      finally:
          return (np.array(qdotaim_vec))
      return (np.array(qdotaim_vec))
  
  #___#Plotting functions
  def pathPlotter(travel,path):
      fig = plt.figure(0)
      plt.plot(travel[:, 0], travel[:, 1], c='r')
      plt.plot(travel[0, 0], travel[0, 1], c='r', marker='X')
      plt.axis('equal')
      plt.grid()
      plt.xlabel("X axis")
      plt.ylabel("Y axis")
      plt.scatter(path[1:, 0], path[1:, 2], c='b')
      plt.scatter(path[0, 0], path[0, 2], c='b', marker='X')
  def forcePlotter(force):
      fig = plt.figure(1)
      plt.plot(force[:, 0], 'green')
      plt.xlabel("Sim step")
      plt.ylabel("x force")
      plt.grid()
      plt.draw()
  
      fig = plt.figure(2)
      plt.plot(force[:, 1], 'blue')
      plt.grid()
      plt.xlabel("Sim step")
      plt.ylabel("y force")
      plt.draw()
  
      '''''''''
      fig = plt.figure(3)
      plt.plot(force[:, 2], 'red')
      plt.grid()
      plt.xlabel("Sim step")
      plt.ylabel("z force")
      plt.draw()
  
      
      fig = plt.figure(4)
      plt.plot(force[:, 3], 'green')
      plt.grid()
      plt.xlabel("Sim step")
      plt.ylabel("torque 1")
      plt.draw()
  
      fig = plt.figure(5)
      plt.plot(force[:, 4], 'blue')
      plt.grid()
      plt.xlabel("Sim step")
      plt.ylabel("torque 2")
      plt.draw()  
      '''''''''
  
      fig = plt.figure(3)
      plt.plot(force[:, 5], 'red')
      plt.grid()
      plt.xlabel("Sim step")
      plt.ylabel("torque 3")
      plt.draw()
  
  def controlPlotter(control):
      fig = plt.figure(4)
      plt.plot(control[:, 0], 'red')
      plt.grid()
      plt.xlabel("Sim step")
      plt.ylabel("q1 control effort")
      plt.draw()
  
      fig = plt.figure(5)
      plt.plot(control[:, 1], 'blue')
      plt.grid()
      plt.xlabel("Sim step")
      plt.ylabel("q2 control effort")
      plt.draw()
  def jointPlotter(joint):
      fig = plt.figure(6)
      plt.plot(joint[:, 0], 'red')
      plt.grid()
      plt.xlabel("Sim step")
      plt.ylabel("q1 [rad]]")
      plt.draw()
  
      fig = plt.figure(7)
      plt.plot(joint[:, 1], 'blue')
      plt.grid()
      plt.xlabel("Sim step")
      plt.ylabel("q2 [rad]")
      plt.draw()
  def jointVelPlotter(jointVel):
      fig = plt.figure(8)
      plt.plot(jointVel[:, 0], 'red')
      plt.grid()
      plt.xlabel("Sim step")
      plt.ylabel("q1 dot [rad/sec]]")
      plt.draw()
  
      fig = plt.figure(9)
      plt.plot(jointVel[:, 1], 'blue')
      plt.grid()
      plt.xlabel("Sim step")
      plt.ylabel("q2 dot [rad/sec]")
      plt.draw()
  
  #__#Waiting functions:
  #waits with documentation and  action
  def waitSim(T):
      global SimStep
      nSteps = np.int32(T / SimStep)
      q_vec=np.zeros([nSteps,np.size(sim.data.actuator_length)])
      q_dot_vec = np.zeros([nSteps, np.size(sim.data.actuator_length)])
      for i in range(nSteps):
          q_vec[i,:]=[sim.data.qpos[0],sim.data.qpos[1]]
          q_dot_vec[i,:]=sim.data.actuator_velocity
      [travel, sensorData, controlData] = straightLineNav(q_vec,q_dot_vec, nSteps)
      return travel, sensorData, controlData
  #waits without documentationand without any action
  def waitSim2(T):
      nSteps = np.int32(T / SimStep)
      for i in range(nSteps):
           sim.step()
           sim.forward()
           #viewer.render()
  
  #get desired impedance params:
  def getDesiredParamsAnalytic(xm,xmDot,xmDotaim):
      qd = invkin(xm[0],xm[1],xm[2])
      qdDot=get_q_dot_from_jack(qd, xmDot[0], xmDot[1])
      qdDot=np.array(qdDot).reshape(-1,1)
      qdDotaim=get_q_dotaim(qd,qdDot,xmDotaim)
      return qd, qdDot, qdDotaim