def loadRobot(M0, name):
'''
This function load a UR5 robot n a new model, move the basis to placement <M0>
and add the corresponding visuals in gepetto viewer with name prefix given by string <name>.
It returns the robot wrapper (model,data).
'''
robot = loadUR()
robot.model.jointPlacements[1] = M0 * robot.model.jointPlacements[1]
robot.visual_model.geometryObjects[0].placement = M0 * robot.visual_model.geometryObjects[0].placement
robot.visual_data.oMg[0] = M0 * robot.visual_data.oMg[0]
robot.initViewer(loadModel=True, sceneName="world/" + name)
return robot
class UR5Test(RobotTestCase):
RobotTestCase.ROBOT = example_robot_data.loadUR()
RobotTestCase.NQ = 6
RobotTestCase.NV = 6
class UR5LimitedTest(RobotTestCase):
RobotTestCase.ROBOT = example_robot_data.loadUR(limited=True)
RobotTestCase.NQ = 6
RobotTestCase.NV = 6
class UR5GripperTest(RobotTestCase):
RobotTestCase.ROBOT = example_robot_data.loadUR(gripper=True)
RobotTestCase.NQ = 6
RobotTestCase.NV = 6
from scipy.integrate import ode, odeint
from sklearn import linear_model
from sklearn.linear_model import LinearRegression
from sklearn.preprocessing import PolynomialFeatures
import matlab.engine
import time
import pandas as pd
import pinocchio
from pinocchio.utils import *
import example_robot_data
# dynamics, can be replaced by anything else
d = 12
m = 6
robot = example_robot_data.loadUR()
q0 = np.array([0., -0.2 * math.pi, -0.6 * math.pi, 0, 0, 0])
v = pinocchio.utils.zero(robot.model.nv)
a = pinocchio.utils.zero(robot.model.nv)
u0 = pinocchio.rnea(robot.model, robot.data, q0, v,
a) # recursive Newton-Euler
x0 = np.zeros(12)
x0[:6] = q0 # u0 has been defined above
def f(x, u):
q = x[:6]
dq = x[6:]
a = pinocchio.aba(robot.model, robot.data, q, dq, u)
f = np.zeros(12)
f[:6] = dq.copy()
