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

script/admissibleComPositionsFromEffector.py

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

script/constants_and_tools.py

-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);

script/generateROMs.py

+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)

script/relativeFootPositionQuasiFlat.py

@@ -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