# 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]))
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 ]])