Commit 4ad2e1ee authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

[Scripts] add scripts to generate data from rbprm-robot-data

parent b7e65f9e
#Importing helper class for RBPRM
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.gepetto import Viewer
import quaternion as quat
packageName = "talos_description"
meshPackageName = "talos_description"
rootJointType = "freeflyer"
##
# Information to retrieve urdf and srdf files.
urdfName = "talos"
urdfSuffix = "_full_v2"
srdfSuffix = ""
# This time we load the full body model of HyQ
fullBody = FullBody ()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
fullBody.setJointBounds ("base_joint_xyz", [-20,20, -20, 20, -20, 20])
rootName = 'base_joint_xyz'
nbSamples = 100000
q_0 = [ 0,0,0,1,0,0,0, # root_joint
0.0, 0.0, -0.411354, 0.859395, -0.448041, -0.001708, # leg_left
0.0, 0.0, -0.411354, 0.859395, -0.448041, -0.001708, # leg_right
0, 0.006761, # torso
0.25847, 0.173046, -0.0002, -0.525366, 0, 0, 0.1, # arm_left
0, 0, 0, 0, 0, 0, 0, # gripper_left
-0.25847, -0.173046, 0.0002, -0.525366, 0, 0, 0.1, # arm_right
0, 0, 0, 0, 0, 0, 0, # gripper_right
0, 0 # head
]
"""
r.addLandmark(r.sceneName,1)
r.addLandmark('talos/gripper_left_inner_single_link',0.3)
r.addLandmark('talos/gripper_right_inner_single_link',0.3)
r.addLandmark('talos/left_sole_link',0.3)
r.addLandmark('talos/right_sole_link',0.3)
"""
rLegId = 'rleg'
rLeg = 'leg_right_1_joint'
rfoot = 'leg_right_sole_fix_joint'
rLegOffset = [0,0,0.01]
rLegNormal = [0,0,1]
rLegx = 0.1; rLegy = 0.06
fullBody.addLimb(rLegId,rLeg,rfoot,rLegOffset,rLegNormal, rLegx, rLegy, nbSamples, "EFORT", 0.01)
lLegId = 'lleg'
lLeg = 'leg_left_1_joint'
lfoot = 'leg_left_sole_fix_joint'
lLegOffset = [0,0,0.01]
lLegNormal = [0,0,1]
lLegx = 0.1; lLegy = 0.06
fullBody.addLimb(lLegId,lLeg,lfoot,lLegOffset,rLegNormal, lLegx, lLegy, nbSamples, "EFORT", 0.01)
rarmId = 'rarm'
rarm = 'arm_right_1_joint'
rHand = 'gripper_right_inner_single_joint'
rArmOffset = [0,0,0.1]
rArmNormal = [0,0,1]
rArmx = 0.02; rArmy = 0.02
fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, nbSamples, "EFORT", 0.01)
larmId = 'larm'
larm = 'arm_left_1_joint'
lHand = 'gripper_left_inner_single_joint'
lArmOffset = [0,0,-0.1]
lArmNormal = [0,0,1]
lArmx = 0.02; lArmy = 0.02
fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, nbSamples, "EFORT", 0.01)
zeroConf = [0,0,0, 1, 0, 0, 0]
q_0[0:7] = zeroConf
fullBody.setCurrentConfig (q_0)
effectors = [rfoot, lfoot, lHand, rHand]
limbIds = [rLegId, lLegId, larmId, rarmId ]
import numpy as np
#~ effectorName = rfoot
#~ limbId = rLegId
#~ q = fullBody.getSample(limbId, 1)
#~ fullBody.setCurrentConfig(q) #setConfiguration matching sample
#~ qEffector = fullBody.getJointPosition(effectorName)
#~ q0 = quat.Quaternion(qEffector[3:7])
#~ rot = q0.toRotationMatrix() #compute rotation matrix world -> local
#~ p = qEffector[0:3] #(0,0,0) coordinate expressed in effector fram
#~ rm=np.zeros((4,4))
#~ for i in range(0,3):
#~ for j in range(0,3):
#~ rm[i,j] = rot[i,j]
#~ for i in range(0,3):
#~ rm[i,3] = qEffector[i]
#~ rm[3,3] = 1
#~ invrm = np.linalg.inv(rm)
#~ p = invrm.dot([0,0,0,1])
points = [[],[],[],[]]
def printComPosition(nbConfigs):
num_invalid = 0
for i in range(0,nbConfigs):
q = fullBody.shootRandomConfig()
q[0:7] = zeroConf
fullBody.setCurrentConfig(q) #setConfiguration matching sample
com = fullBody.getCenterOfMass()
for x in range(0,3):
q[x] = -com[x]
fullBody.setCurrentConfig(q)
#~ print ("final com" + str(com))
#~ print ("final com" + str(fullBody.getCenterOfMass()))
if(fullBody.isConfigValid(q)[0]):
for j in range(0,len(effectors)):
effectorName = effectors[j]
limbId = limbIds[j]
qEffector = fullBody.getJointPosition(effectorName)
q0 = quat.Quaternion(qEffector[3:7])
rot = q0.toRotationMatrix() #compute rotation matrix world -> local
p = qEffector[0:3] #(0,0,0) coordinate expressed in effector fram
rm=np.zeros((4,4))
for k in range(0,3):
for l in range(0,3):
rm[k,l] = rot[k,l]
for m in range(0,3):
rm[m,3] = qEffector[m]
rm[3,3] = 1
invrm = np.linalg.inv(rm)
p = invrm.dot([0,0,0,1])
points[j].append(p)
#~ print (points[j])
else:
num_invalid +=1
for j in range(0,len(limbIds)):
f1=open('./'+str(limbIds[j])+'_com.erom', 'w+')
for p in points[j]:
f1.write(str(p[0]) + "," + str(p[1]) + "," + str(p[2]) + "\n")
f1.close()
print "%invalid ", (float)(num_invalid) / (float)(nbConfigs) * 100, "%"
#~ printRootPosition(rLegId, rfoot, nbSamples)
#~ printRootPosition(lLegId, lfoot, nbSamples)
#~ printRootPosition(rarmId, rHand, nbSamples)
#~ printRootPosition(larmId, lHand, nbSamples)
printComPosition(100000)
import numpy as np
# from hpp.corbaserver.rbprm.hrp2 import Robot as rob
# from hpp.corbaserver.rbprm.tools.obj_to_constraints import load_obj, as_inequalities, rotate_inequalities
# from hpp_centroidal_dynamics import *
# from hpp_spline import *e
from numpy import array, hstack, identity, matrix, ones, vstack, zeros
from scipy.spatial import ConvexHull
# import eigenpy
import cdd
# from hpp_bezier_com_traj import *
# from qp import solve_lp
Id = matrix([[1., 0., 0.], [0., 1., 0.], [0., 0., 1.]])
z = array([0., 0., 1.])
zero3 = zeros(3)
def generators(A, b, Aeq=None, beq=None):
m = np.hstack([b, -A])
matcdd = cdd.Matrix(m)
matcdd.rep_type = cdd.RepType.INEQUALITY
if Aeq is not None:
meq = np.hstack([beq, -Aeq])
matcdd.extend(meq.tolist(), True)
H = cdd.Polyhedron(matcdd)
g = H.get_generators()
return [array(g[el][1:]) for el in range(g.row_size)], H
def filter(pts):
hull = ConvexHull(pts, qhull_options='Q12')
return [pts[i] for i in hull.vertices.tolist()]
def ineq(pts, canonicalize=False):
apts = array(pts)
m = np.hstack([ones((apts.shape[0], 1)), apts])
matcdd = cdd.Matrix(m)
matcdd.rep_type = cdd.RepType.GENERATOR
H = cdd.Polyhedron(matcdd)
bmA = H.get_inequalities()
if canonicalize:
bmA.canonicalize()
Ares = zeros((bmA.row_size, bmA.col_size - 1))
bres = zeros(bmA.row_size)
for i in range(bmA.row_size):
bmAl = array(bmA[i])
Ares[i, :] = -bmAl[1:]
bres[i] = bmAl[0]
return Ares, bres
def ineqQHull(hull):
A = hull.equations[:, :-1]
b = -hull.equations[:, -1]
return A, b
def canon(A, b):
m = np.hstack([b, -A])
matcdd = cdd.Matrix(m)
matcdd.rep_type = 1
H = cdd.Polyhedron(matcdd)
bmA = H.get_inequalities()
# bmA.canonicalize()
Ares = zeros((bmA.row_size, bmA.col_size - 1))
bres = zeros((bmA.row_size, 1))
for i in range(bmA.row_size):
# print("line ", array(bmA[i]))
# print("A ", A[i][:])
# print("b ", b[i])
bmAl = array(bmA[i])
Ares[i, :] = -bmAl[1:]
bres[i] = bmAl[0]
# print("Ares ",Ares[i,:])
# print("bres ",bres[i])
return Ares, bres
def genPolytope(A, b):
pts, H = generators(A, b)
apts = array(pts)
if len(apts) > 0:
hull = ConvexHull(apts)
return hull, pts, apts, H
return None, None, None, None
def convex_hull_ineq(pts):
return None
"""
# TODO: what is cData ?
m = cData.contactPhase_.getMass()
# get 6D polytope
(H, h) = ineqFromCdata(cData)
#project to the space where aceleration is 0
D = zeros((6, 3))
D[3:, :] = m * gX
d = zeros((6, ))
d[:3] = -m * g
A = H.dot(D)
b = h.reshape((-1, )) - H.dot(d)
#add kinematic polytope
(K, k) = (cData.Kin_[0], cData.Kin_[1].reshape(-1, ))
resA = vstack([A, K])
resb = concatenate([b, k]).reshape((-1, 1))
#DEBUG
allpts = generators(resA, resb)[0]
error = False
for pt in allpts:
print("pt ", pt)
assert (resA.dot(pt.reshape((-1, 1))) - resb).max() < 0.001, "antecedent point not in End polytope" + str(
(resA.dot(pt.reshape((-1, 1))) - resb).max())
if (H.dot(w(m, pt).reshape((-1, 1))) - h).max() > 0.001:
error = True
print("antecedent point not in End polytope" + str((H.dot(w(m, pt).reshape((-1, 1))) - h).max()))
assert not error, str(len(allpts))
return (resA, resb)
# return (A, b)
# return (vstack([A, K]), None)
"""
def default_transform_from_pos_normal(pos, normal):
# print("pos ", pos
# print("normal ", normal)
f = array([0., 0., 1.])
t = array(normal)
v = np.cross(f, t)
c = np.dot(f, t)
if c > 0.99:
rot = identity(3)
else:
# u = v / norm(v)
h = (1. - c) / (1. - c**2)
vx, vy, vz = v
rot = array([[c + h * vx**2, h * vx * vy - vz, h * vx * vz + vy],
[h * vx * vy + vz, c + h * vy**2, h * vy * vz - vx],
[h * vx * vz - vy, h * vy * vz + vx, c + h * vz**2]])
return vstack([hstack([rot, pos.reshape((-1, 1))]), [0., 0., 0., 1.]])
def continuous(h, initpts):
dic = {}
pts = []
for i, pt in enumerate(h.vertices.tolist()):
pts += [initpts[pt]]
dic[pt] = i
faces = []
for f in h.simplices:
faces += [[dic[idx] + 1 for idx in f]]
return pts, faces
def hull_to_obj(h, pts, name):
pts, faces = continuous(h, pts)
f = open(name, "w")
# first write points
for pt in pts:
# print("??")
f.write('v ' + str(pt[0]) + ' ' + str(pt[1]) + ' ' + str(pt[2]) + ' \n')
f.write('g foo\n')
for pt in faces:
# print("???")
f.write('f ' + str(pt[0]) + ' ' + str(pt[1]) + ' ' + str(pt[2]) + ' \n')
f.write('g \n')
f.close()
# function vertface2obj(v,f,name)
# % VERTFACE2OBJ Save a set of vertice coordinates and faces as a Wavefront/Alias Obj file
# % VERTFACE2OBJ(v,f,fname)
# % v is a Nx3 matrix of vertex coordinates.
# % f is a Mx3 matrix of vertex indices.
# % fname is the filename to save the obj file.
# fid = fopen(name,'w');
# for i=1:size(v,1)
# fprintf(fid,'v %f %f %f\n',v(i,1),v(i,2),v(i,3));
# end
# fprintf(fid,'g foo\n');
# for i=1:size(f,1);
# fprintf(fid,'f %d %d %d\n',f(i,1),f(i,2),f(i,3));
# end
# fprintf(fid,'g\n');
# fclose(fid);
from hpp.corbaserver.rbprm.rbprmbuilder import Builder
from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
from hpp.gepetto import Viewer
import quaternion as quat
packageName = "talos_description"
meshPackageName = "talos_description"
rootJointType = "freeflyer"
##
# Information to retrieve urdf and srdf files.
urdfName = "talos"
urdfSuffix = "_full_v2"
srdfSuffix = ""
fullBody = FullBody ()
fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
nbSamples = 100000
ps = ProblemSolver( fullBody )
r = Viewer (ps)
rootName = 'base_joint_xyz'
q_0 = [ 0,0,0,1,0,0,0, # root_joint
0.0, 0.0, -0.411354, 0.859395, -0.448041, -0.001708, # leg_left
0.0, 0.0, -0.411354, 0.859395, -0.448041, -0.001708, # leg_right
0, 0.006761, # torso
0.25847, 0.173046, -0.0002, -0.525366, 0, 0, 0.1, # arm_left
0, 0, 0, 0, 0, 0, 0, # gripper_left
-0.25847, -0.173046, 0.0002, -0.525366, 0, 0, 0.1, # arm_right
0, 0, 0, 0, 0, 0, 0, # gripper_right
0, 0 # head
]
r(q_0)
r.addLandmark(r.sceneName,1)
r.addLandmark('talos/gripper_left_inner_single_link',0.3)
r.addLandmark('talos/gripper_right_inner_single_link',0.3)
r.addLandmark('talos/left_sole_link',0.3)
r.addLandmark('talos/right_sole_link',0.3)
rLegId = 'rleg'
rLeg = 'leg_right_1_joint'
rfoot = 'leg_right_sole_fix_joint'
rLegOffset = [0,0,0.01]
rLegNormal = [0,0,1]
rLegx = 0.06; rLegy = 0.1
fullBody.addLimb(rLegId,rLeg,rfoot,rLegOffset,rLegNormal, rLegx, rLegy, nbSamples, "EFORT", 0.01)
lLegId = 'lleg'
lLeg = 'leg_left_1_joint'
lfoot = 'leg_left_sole_fix_joint'
lLegOffset = [0,0,0.01]
lLegNormal = [0,0,1]
lLegx = 0.06; lLegy = 0.1
fullBody.addLimb(lLegId,lLeg,lfoot,lLegOffset,rLegNormal, lLegx, lLegy, nbSamples, "EFORT", 0.01)
rarmId = 'rarm'
rarm = 'arm_right_1_joint'
rHand = 'gripper_right_inner_single_joint'
rArmOffset = [0,0,0.1]
rArmNormal = [0,0,1]
rArmx = 0.02; rArmy = 0.02
fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, nbSamples, "EFORT", 0.01)
larmId = 'larm'
larm = 'arm_left_1_joint'
lHand = 'gripper_left_inner_single_joint'
lArmOffset = [0,0,-0.1]
lArmNormal = [0,0,1]
lArmx = 0.02; lArmy = 0.02
fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, nbSamples, "EFORT", 0.01)
def printEffPosition(limbId, nbSamples):
limit = nbSamples-1;
f1=open('./data/talos/roms/'+limbId+'.erom', 'w+')
for i in range(0,limit):
q = fullBody.getSamplePosition(limbId,i)
f1.write(str(q[0]) + "," + str(q[1]) + "," + str(q[2]) + "\n")
f1.close()
printEffPosition(rarmId, nbSamples)
printEffPosition(rLegId, nbSamples)
printEffPosition(larmId, nbSamples)
printEffPosition(lLegId, nbSamples)
......@@ -12,7 +12,7 @@ from scipy.spatial import ConvexHull
from talos_rbprm.talos import Robot
from .constants_and_tools import hull_to_obj
from hpp.corbaserver.rbprm.tools.constants_and_tools import hull_to_obj
NUM_SAMPLES = 18000
IT_DISPLAY_PROGRESS = NUM_SAMPLES / 10
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment