# 24 DEGREES BASE POSITION
gbase_position = array([[-2.14387087e+00, -3.22000000e+00, -9.97857391e-01],
                        [-1.98593217e+00, -3.22000000e+00, -6.64834872e-01],
                        [-1.82799346e+00, -3.22000000e+00, -3.31812352e-01],
                        [-1.67005475e+00, -3.22000000e+00, 1.21016736e-03],
                        [-1.51211605e+00, -3.22000000e+00, 3.34232687e-01],
                        [-1.35417734e+00, -3.22000000e+00, 6.67255206e-01],
                        [-1.19623863e+00, -3.22000000e+00, 1.00027773e+00]])

# ROBOT POSITION BASE
base = gbase_position[0]
pN = base
pN[1] += y
normal = [-1, 0, 0]
pN = numpy.concatenate((pN, normal))
Tn = coating.poseDummy(pN, 0)
robot.SetTransform(Tn)

#CHANGE ORIENTATION
Ti = []
for body in env.GetBodies():
    Ti.append(body.GetTransform())

alpha = 1.0 * BladePosition * pi / 180
T = numpy.array([[1, 0, 0, 0], [0, cos(alpha), -sin(alpha), 0],
                 [0, sin(alpha), cos(alpha), 0], [0, 0, 0, 1]])

for ibody in range(0, len(env.GetBodies())):
    if ibody != 5:
        env.GetBodies()[ibody].SetTransform(dot(T, Ti[ibody]))
# 24 DEGREES BASE POSITION
gbase_position=array([ [ -2.14387087e+00,  -3.22000000e+00,  -9.97857391e-01],
                       [ -1.98593217e+00,  -3.22000000e+00,  -6.64834872e-01],
                       [ -1.82799346e+00,  -3.22000000e+00,  -3.31812352e-01],
                       [ -1.67005475e+00,  -3.22000000e+00,   1.21016736e-03],
                       [ -1.51211605e+00,  -3.22000000e+00,   3.34232687e-01],
                       [ -1.35417734e+00,  -3.22000000e+00,   6.67255206e-01],
                       [ -1.19623863e+00,  -3.22000000e+00,   1.00027773e+00]])

# ROBOT POSITION BASE
base = gbase_position[0]
pN = base
pN[1]+=y
normal = [-1,0,0]
pN = numpy.concatenate((pN,normal))
Tn = coating.poseDummy(pN,0)
robot.SetTransform(Tn)

#CHANGE ORIENTATION 
Ti = []
for body in env.GetBodies():
    Ti.append(body.GetTransform())

alpha = 1.0*BladePosition*pi/180
T = numpy.array([[1,0,0,0],[0,cos(alpha),-sin(alpha),0],
                 [0,sin(alpha),cos(alpha),0],[0,0,0,1]])

for ibody in range(0,len(env.GetBodies())):
    if ibody!=5:
        env.GetBodies()[ibody].SetTransform(dot(T,Ti[ibody]))
Beispiel #3
0
joints = robot.GetJoints()

# Get poinst for coating
coatedarray = numpy.load('coatedpoints.npz')
coatedarray  = coatedarray['array']

coatedsolarray = numpy.load('ikcoatedpoints.npz')
coatedsolarray  = coatedsolarray['array']

# Robot initial position
pN = numpy.array([ -1, -3.3, 0 ])
normal = [-1,0,0]
pN = numpy.concatenate((pN,normal))
distances = [0,0.1,-0.1,-0.3]
distance = distances[0]
Tn = coating.poseDummy(pN, distance)
robot.SetTransform(Tn)
#RobotPositions = [0,0.1,-0.1,-0.3]
minVelocity = 1.0*40/60 #m/s

# ikmodel
ikmodel = databases.inversekinematics.InverseKinematicsModel(robot=robot,iktype=IkParameterization.Type.Transform6D)
if not ikmodel.load():
    ikmodel.autogenerate()

# CAMERA SETTINGS
Tcamera = numpy.array([[ 0.05777387, -0.06852652,  0.99597505, -4.08520365],
       [-0.32092178, -0.94596499, -0.04646983, -1.95519543],
       [ 0.94534194, -0.31694535, -0.07664371, -0.661735  ],
       [ 0        ,  0        ,  0        ,  1        ]])