Commit 6f0bf141 by Pierre Fernbach

### [tools] add first version of script to convert affordances surfaces to gazebo .world

`FIXME : really not generic ! only work for rectangles with normal along z`
parent a83158bb
 import tools.getSurfaceExtremumPoints as ep import numpy as np from pinocchio import Quaternion from pinocchio.utils import matrixToRpy NAME_ID = 0 NAME = "box" WIDTH = 0.02 Z_AXIS = np.matrix([0,0,1]).T """ 0.15 0.25 0.145 0 0 0 true 1.0 1.0 0.0 0.0 1.0 0.0 1.0 0.3 0.5 0.03 0.3 0.5 0.03 """ def computePoseFromSurface(surface): points = surface #normal = surface[1] normal = [0,0,1] center = np.zeros(3) for pt in points: center += np.array(pt) center /= len(points) center -= np.array(normal)*(WIDTH/2.) # rotation in rpy : q_rot = Quaternion() n_m = np.matrix(normal).T q_rot.setFromTwoVectors(Z_AXIS,n_m) rpy = matrixToRpy(q_rot.matrix()) pose = center.tolist() + rpy.T.tolist()[0] return pose # ASSUME RECTANGLE ALIGNED WITH X,Y AXIS !! # FIXME def computeSizeFromSurface(surface): size = [0,0,0] size[2] = WIDTH points = surface for i in range(len(points)-1): pt0 = points[i] pt1 = points[i+1] #print "pt0 = ",pt0 #print "pt1 = ",pt1 sX = (abs(pt0[0] - pt1[0])) if sX > 0.01: size[0] = sX sY = (abs(pt0[1] - pt1[1])) if sY > 0.01: size[1] = sY return size def genStringForSurface(surface): pose = computePoseFromSurface(surface) size = computeSizeFromSurface(surface) global NAME_ID res = "" res += "\n" res += "\t"+str(pose[0])+" "+str(pose[1])+" "+str(pose[2])+" "+str(pose[3])+" "+str(pose[4])+" "+str(pose[5])+"\n" res += "\ttrue\n" res += "\t\n" res += "\t\n" res += "\t\t1.0\n" res += "\t\t\n" res += "\t\t\t1.0\n" res += "\t\t\t0.0\n" res += "\t\t\t0.0\n" res += "\t\t\t1.0\n" res += "\t\t\t0.0\n" res += "\t\t\t1.0\n" res += "\t\t\n" res += "\t\n" res += "\t\t\n" res += "\t\t\n" res += "\t\t\t\n" res += "\t\t\t\t"+str(size[0])+" "+str(size[1])+" "+str(size[2])+"\n" res += "\t\t\t\n" res += "\t\t\n" res += "\t\n" res += "\t\n" res += "\t\t\n" res += "\t\t\t\n" res += "\t\t\t\t"+str(size[0])+" "+str(size[1])+" "+str(size[2])+"\n" res += "\t\t\t\n" res += "\t\t\n" res += "\t\n" res += "\t\n" res += "\n" NAME_ID += 1 return res def getGazeboString(afftool): surfaces = ep.contactSurfaces(afftool) res = "" for surface in surfaces: res += genStringForSurface(surface) res += "\n\n" return res
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!