├── 2Dexamples ├── cdf.py ├── data2D.pt ├── fig1.png ├── gp.png ├── mlp.py ├── model.pth ├── model.py ├── nn_cdf.py ├── nn_cdf_casadi.py ├── primitives2D_torch.py ├── robot2D_torch.py └── robot_plot2D.py ├── LICENSE ├── frankaemika ├── data_generator.py ├── mlp.py ├── model_dict.pt ├── mp_ik.py ├── nn_cdf.py ├── panda_urdf │ ├── LICENSE.txt │ ├── meshes │ │ ├── collision │ │ │ ├── finger.obj │ │ │ ├── hand.obj │ │ │ ├── link0.obj │ │ │ ├── link1.obj │ │ │ ├── link2.obj │ │ │ ├── link3.obj │ │ │ ├── link4.obj │ │ │ ├── link5.obj │ │ │ ├── link6.mtl │ │ │ ├── link6.obj │ │ │ └── link7.obj │ │ └── visual │ │ │ ├── colors.png │ │ │ ├── finger.mtl │ │ │ ├── finger.obj │ │ │ ├── hand.mtl │ │ │ ├── hand.obj │ │ │ ├── link1.mtl │ │ │ ├── link1.obj │ │ │ ├── link2.mtl │ │ │ ├── link2.obj │ │ │ ├── link3.mtl │ │ │ ├── link3.obj │ │ │ ├── link4.mtl │ │ │ ├── link4.obj │ │ │ ├── link5.mtl │ │ │ ├── link5.obj │ │ │ ├── link6.mtl │ │ │ └── link6.obj │ └── panda.urdf ├── pybullet_panda_sim.py ├── qp_mp.py ├── throw_wik.py └── wik_eval.py ├── readme.md └── requirements.txt /2Dexamples/cdf.py: -------------------------------------------------------------------------------- 1 | # ----------------------------------------------------------------------------- 2 | # SPDX-License-Identifier: MIT 3 | # This file is part of the CDF project. 4 | # Copyright (c) 2024 Idiap Research Institute 5 | # Contributor: Yiming Li 6 | # ----------------------------------------------------------------------------- 7 | 8 | # 2D example for configuration space distance field 9 | import numpy as np 10 | import os 11 | import sys 12 | import torch 13 | import math 14 | import matplotlib.pyplot as plt 15 | from robot2D_torch import Robot2D 16 | from primitives2D_torch import Circle 17 | from torchmin import minimize 18 | import time 19 | import math 20 | import robot_plot2D 21 | import copy 22 | import matplotlib.gridspec as gridspec 23 | 24 | PI = math.pi 25 | CUR_PATH = os.path.dirname(os.path.realpath(__file__)) 26 | 27 | class CDF2D: 28 | def __init__(self,device) -> None: 29 | self.device = device 30 | self.nbData = 50 31 | self.nbDiscretization = 50 32 | self.Q_grid = self.create_grid_torch(self.nbData).to(device) 33 | self.link_length = torch.tensor([[2,2]]).float().to(device) 34 | self.num_joints = self.link_length.size(1) 35 | self.q_max = torch.tensor([PI,PI]).to(device) 36 | self.q_min = torch.tensor([-PI,-PI]).to(device) 37 | 38 | # data generation 39 | self.task_space = [[-3.0,-3.0],[3.0,3.0]] 40 | self.batchsize = 40000 # batch size of q 41 | self.epsilon = 1e-3 # distance threshold to filter data 42 | 43 | # robot 44 | self.robot = Robot2D(num_joints=self.num_joints ,init_states = self.Q_grid,link_length=self.link_length,device = device) 45 | 46 | # c space distance field 47 | if not os.path.exists(os.path.join(CUR_PATH,'data2D.pt')): 48 | self.generate_data() 49 | self.q_grid_template = torch.load(os.path.join(CUR_PATH,'data2D.pt')) 50 | 51 | def create_grid(self,nb_data): 52 | t = np.linspace(-math.pi,math.pi, nb_data) 53 | self.q0,self.q1 = np.meshgrid(t,t) 54 | return self.q0,self.q1 55 | 56 | def create_grid_torch(self,nb_data): 57 | q0,q1 = self.create_grid(nb_data) 58 | q0_torch = torch.from_numpy(q0).float() 59 | q1_torch = torch.from_numpy(q1).float() 60 | Q_sets = torch.cat([q0_torch.unsqueeze(-1),q1_torch.unsqueeze(-1)],dim=-1).view(-1,2) 61 | return Q_sets 62 | 63 | def inference_sdf(self,q,obj_lists,return_grad = False): 64 | # using predefined object 65 | kpts = self.robot.surface_points_sampler(q) 66 | B,N = kpts.size(0),kpts.size(1) 67 | dist = torch.cat([obj.signed_distance(kpts.reshape(-1,2)).reshape(B,N,-1) for obj in obj_lists],dim=-1) 68 | 69 | # using closest point from robot surface 70 | sdf = torch.min(dist,dim=-1)[0] 71 | sdf = sdf.min(dim=-1)[0] 72 | if return_grad: 73 | grad = torch.autograd.grad(sdf,q,torch.ones_like(sdf))[0] 74 | return sdf,grad 75 | return sdf 76 | 77 | def find_q(self,obj_lists,batchsize = None): 78 | # find q that makes d(x,q) = 0. x is the obstacle surface 79 | # using L-BFGS method 80 | if not batchsize: 81 | batchsize = self.batchsize 82 | 83 | def cost_function(q): 84 | # find q that d(x,q) = 0 85 | # q : B,2 86 | 87 | d = self.inference_sdf(q,obj_lists) 88 | cost = torch.sum(d**2) 89 | return cost 90 | 91 | t0 = time.time() 92 | # optimizer for data generation 93 | q = torch.rand(batchsize,2).to(self.device)*(self.q_max-self.q_min)+self.q_min 94 | q0 =copy.deepcopy(q) 95 | res = minimize( 96 | cost_function, 97 | q, 98 | method='l-bfgs', 99 | options=dict(line_search='strong-wolfe'), 100 | max_iter=50, 101 | disp=0 102 | ) 103 | 104 | d = self.inference_sdf(q,obj_lists).squeeze() 105 | 106 | mask = torch.abs(d) < 0.05 107 | q_valid,d = res.x[mask],d[mask] 108 | boundary_mask = ((q_valid > self.q_min) & (q_valid < self.q_max)).all(dim=1) 109 | final_q = q_valid[boundary_mask] 110 | q0 = q0[mask][boundary_mask] 111 | # print('number of q_valid: \t{} \t time cost:{}'.format(len(final_q),time.time()-t0)) 112 | return q0,final_q,res.x 113 | 114 | def generate_data(self,nbDiscretization=50): 115 | x = torch.linspace(self.task_space[0][0],self.task_space[1][0],self.nbDiscretization).to(self.device) 116 | y = torch.linspace(self.task_space[0][1],self.task_space[1][1],self.nbDiscretization).to(self.device) 117 | xx,yy = torch.meshgrid(x,y) 118 | xx,yy = xx.reshape(-1,1),yy.reshape(-1,1) 119 | p = torch.cat([xx,yy],dim=-1).to(self.device) 120 | 121 | data = {} 122 | for i,_p in enumerate(p): 123 | grids = [Circle(center=_p,radius=0.001,device=device)] 124 | q = self.find_q(grids)[1] 125 | data[i] = { 126 | 'p':_p, 127 | 'q':q 128 | } 129 | print('i: {} \t number of q: {}'.format(i,len(q[1]))) 130 | np.save(os.path.join(CUR_PATH,'data2D.npy'),data) 131 | data = np.load(os.path.join(CUR_PATH,'data2D.npy'),allow_pickle=True).item() 132 | max_q_per_x = 200 133 | tensor_data = torch.inf*torch.ones(self.nbData,self.nbData,max_q_per_x,2).to(self.device) 134 | for idx in data.keys(): 135 | p = data[idx]['p'] 136 | q = data[idx]['q'] 137 | i = idx/50 138 | j = idx%50 139 | if len(q) > max_q_per_x: 140 | q = q[:max_q_per_x] 141 | tensor_data[int(i),int(j),:len(q),:] = q 142 | torch.save(tensor_data,os.path.join(CUR_PATH,'data2D.pt')) 143 | return tensor_data 144 | 145 | def calculate_cdf(self,q,obj_lists,method='online_computation',return_grad = False): 146 | # x : (Nx,2) 147 | # q : (Np,2) 148 | # return d : (Np) distance between q and x in C space. d = min_{q*}{L2(q-q*)}. sdf(x,q*)=0 149 | Np = q.shape[0] 150 | if method == None: 151 | method = 'online_computation' 152 | if method == 'offline_grid': 153 | if not hasattr(self,'q_list_template'): 154 | obj_points = torch.cat([obj.sample_surface(200) for obj in obj_lists]) 155 | grid = self.x_to_grid(obj_points) 156 | q_list_template = (self.q_grid_template[grid[:,0],grid[:,1],:,:]).reshape(-1,2) 157 | self.q_list_template = q_list_template[q_list_template[:,0] != torch.inf] 158 | dist = torch.norm(q.unsqueeze(1) - self.q_list_template.to(self.device).unsqueeze(0),dim=-1) 159 | if method == 'online_computation': 160 | if not hasattr(self,'q_0_level_set'): 161 | self.q_0_level_set = self.find_q(obj_lists)[1] 162 | dist = torch.norm(q.unsqueeze(1) - self.q_0_level_set.unsqueeze(0),dim=-1) 163 | 164 | d = torch.min(dist,dim=-1)[0] 165 | # compute sign of d, based on the sdf 166 | # exit() 167 | d_ts = self.inference_sdf(q,obj_lists) 168 | 169 | mask = (d_ts < 0) 170 | d[mask] = -d[mask] 171 | if return_grad: 172 | grad = torch.autograd.grad(d,q,torch.ones_like(d))[0] 173 | return d,grad 174 | return d 175 | 176 | def x_to_grid(self,p): 177 | # p: (N,2) 178 | # return grid index (N,2) 179 | x_workspace = torch.tensor([self.task_space[0][0],self.task_space[1][0]]).to(self.device) 180 | y_workspace = torch.tensor([self.task_space[0][1],self.task_space[1][1]]).to(self.device) 181 | 182 | x_grid = (p[:,0]-x_workspace[0])/(x_workspace[1]-x_workspace[0])*self.nbDiscretization 183 | y_grid = (p[:,1]-y_workspace[0])/(y_workspace[1]-y_workspace[0])*self.nbDiscretization 184 | 185 | x_grid.clamp_(0,self.nbDiscretization-1) 186 | y_grid.clamp_(0,self.nbDiscretization-1) 187 | return torch.stack([x_grid,y_grid],dim=-1).long() 188 | 189 | def projection(self,q,d,grad): 190 | # q : (N,2) 191 | # d : (N) 192 | # grad : (N,2) 193 | # return q_proj : (N,2) 194 | q_proj = q - grad*d.unsqueeze(-1) 195 | return q_proj 196 | 197 | def plot_projection(self,ax): 198 | q = torch.rand(1000,2).to(self.device)*2*math.pi-math.pi 199 | q0 = copy.deepcopy(q) 200 | d,grad = self.inference_c_space_sdf_using_data(q,sign=False) 201 | q = self.projection(q,d,grad) 202 | q,grad= q.detach(),grad.detach() # release memory 203 | ax.set_title(f'{iter} iteractions', size=25) # Add a title to your plot 204 | ax.plot(q[:,0].detach().cpu().numpy(),q[:,1].detach().cpu().numpy(),'.',color='lightgreen') 205 | return q0,q 206 | 207 | def plot_sdf(self,obj_lists, ax): 208 | ax.set_aspect('equal', 'box') # Make sure the pixels are square 209 | ax.set_title('Configuration space', size=30) # Add a title to your plot 210 | ax.set_xlabel('q1', size=20) 211 | ax.set_ylabel('q2', size=20) 212 | axis_limits = (-PI, PI) # Set the limits for both axes to be the same 213 | ax.set_xlim(axis_limits) 214 | ax.set_ylim(axis_limits) 215 | ax.tick_params(axis='both', labelsize=20) 216 | 217 | sdf = self.inference_sdf(self.Q_grid,obj_lists) 218 | sdf = sdf.detach().cpu().numpy() 219 | 220 | ax.contour(self.q0, self.q1, sdf.reshape(self.nbData, self.nbData), levels=[0], linewidths=6, colors='black', alpha=1.0) 221 | ct = ax.contourf(self.q0, self.q1, sdf.reshape(self.nbData, self.nbData), levels=6, linewidths=1, cmap='coolwarm') 222 | ax.clabel(ct, inline=False, fontsize=15, colors='black', fmt='%.1f') 223 | 224 | # fig = plt.gcf() # Get the current figure 225 | # fig.colorbar(ct, ax=ax) # Add a colorbar to your plot 226 | 227 | def shooting(self,q0,obj_lists,dt = 1e-2,timestep = 500,method = 'SDF'): 228 | q = q0 229 | q.requires_grad = True 230 | q_list = [] 231 | for t in range(timestep): 232 | if method == 'SDF': 233 | d,g = self.inference_sdf(q,obj_lists, return_grad=True) 234 | if method == 'CDField': 235 | d,g = self.calculate_cdf(q,obj_lists,return_grad=True) 236 | g = torch.nn.functional.normalize(g,dim=-1) 237 | g_orth = torch.stack([g[:,1],-g[:,0]],dim=-1) 238 | # if g_orth[:,1] < 0: 239 | # g_orth = -g_orth 240 | q = q + dt*g_orth 241 | q_list.append(q.detach().cpu().numpy()) 242 | return np.array(q_list).transpose(1,0,2) 243 | 244 | def shooting_proj(self,q0,obj_lists,dt = 1e-2,timestep = 500,method = 'SDF'): 245 | q = q0 246 | q.requires_grad = True 247 | q_list = [] 248 | if method == 'SDF': 249 | for t in range(timestep): 250 | q_list.append(q.detach().cpu().numpy()) 251 | d,g = self.inference_sdf(q,obj_lists,return_grad=True) 252 | # if method == 'CDField': 253 | # d,g = self.calculate_cdf(q,obj_lists,return_grad=True) 254 | g = torch.nn.functional.normalize(g,dim=-1) 255 | # if g_orth[:,1] < 0: 256 | # g_orth = -g_orth 257 | q = q - dt*g*d.unsqueeze(-1) 258 | # q_list.append(q.detach().cpu().numpy()) 259 | if method == 'CDField': 260 | d,g = self.calculate_cdf(q,obj_lists,return_grad=True) 261 | q = q - g*d.unsqueeze(-1) 262 | q_list = np.linspace(q0.detach().cpu().numpy(),q.detach().cpu().numpy(),timestep) 263 | return np.array(q_list).transpose(1,0,2) 264 | 265 | def plot_cdf(self,ax,obj_lists,method='online_computation'): 266 | d = self.calculate_cdf(self.Q_grid,obj_lists,method).detach().cpu().numpy() 267 | ax.set_aspect('equal', 'box') # Make sure the pixels are square 268 | ax.set_title('Configuration space', size=30) # Add a title to your plot 269 | ax.set_xlabel('q1', size=20) 270 | ax.set_ylabel('q2', size=20) 271 | axis_limits = (-PI, PI) # Set the limits for both axes to be the same 272 | ax.set_xlim(axis_limits) 273 | ax.set_ylim(axis_limits) 274 | ax.tick_params(axis='both', labelsize=20) 275 | 276 | ax.contour(self.q0, self.q1, d.reshape(self.nbData, self.nbData), levels=[0], linewidths=6, colors='black', alpha=1.0) 277 | ct = ax.contourf(self.q0, self.q1, d.reshape(self.nbData, self.nbData), levels=8, linewidths=1, cmap='coolwarm') 278 | ax.clabel(ct, inline=False, fontsize=15, colors='black', fmt='%.1f') 279 | 280 | 281 | def plot_objects(self,ax,obj_lists): 282 | for obj in obj_lists: 283 | # plt.gca().add_patch(obj.create_patch()) 284 | ax.add_patch(obj.create_patch()) 285 | return ax 286 | 287 | def compare_with_lbfgs(self): 288 | q0,q,_ = self.find_q() 289 | return q0,q 290 | 291 | def plot_fig1(obj_lists): 292 | color_list = ['magenta','orange', 'cyan', 'green', 'purple', 'navy'] 293 | fig1, ax1 = plt.subplots(figsize=(10,8)) # Create the first plot 294 | fig2, ax2 = plt.subplots(figsize=(10, 8)) # Create the third plot 295 | 296 | fig = plt.figure(figsize=(25,20)) # Create a figure 297 | gs = gridspec.GridSpec(2, 6) # Create a gridspec 298 | xlim=(-4.0,4.0) 299 | ylim=(-4.0,4.0) 300 | axs = [] 301 | for i in range(12): 302 | ax = fig.add_subplot(gs[i // 6, i % 6]) # Add a subplot 303 | ax.axis('off') # Turn off the axis 304 | cdf.plot_objects(ax,obj_lists) 305 | ax.set_aspect('equal', 'box') # Make sure the pixels are square 306 | ax.set_xlim(xlim) # Set the x limits 307 | ax.set_ylim(ylim) # Set the y limits 308 | axs.append(ax) 309 | 310 | # Plot sdf on the first subplot 311 | import matplotlib.cm as cm 312 | cdf.plot_sdf(ax=ax1,obj_lists=obj_lists) 313 | # plot shooting 314 | shooting_q0 = torch.tensor([[-0.2,0.0],[1.2,1.0],[-1.0,0.5]]).to(device) 315 | projection_q0 = torch.tensor([[0.0,-1.5],[2.0,2.0],[-1.5,-2.0]]).to(device) 316 | shooting_tangent = cdf.shooting(shooting_q0,obj_lists,method='SDF') 317 | shooting_gradient = cdf.shooting_proj(projection_q0,obj_lists,method='SDF') 318 | shooting_q_sdf = np.concatenate([shooting_gradient,shooting_tangent],axis=0) 319 | for c,shoot in enumerate(shooting_q_sdf): 320 | ax1.plot(shoot[:,0],shoot[:,1],'r--',color = color_list[c],linewidth=3) 321 | ax1.plot(shoot[0,0],shoot[0,1],'*',color = color_list[c],markersize=10) 322 | # plot robot 323 | robot_plot2D.plot_2d_manipulators(joint_angles_batch=shoot[0:500:20],ax = axs[c*2],color = color_list[c],show_start_end=False,show_eef_traj=True) 324 | cdf.plot_cdf(ax2,obj_lists) 325 | 326 | shooting_tangent = cdf.shooting(shooting_q0,obj_lists,method='CDField') 327 | shooting_gradient = cdf.shooting_proj(projection_q0,obj_lists,method='CDField') 328 | shooting_q_cdf = np.concatenate([shooting_gradient,shooting_tangent],axis=0) 329 | for c,shoot in enumerate(shooting_q_cdf): 330 | ax2.plot(shoot[:,0],shoot[:,1],'r--',color = color_list[c],linewidth=3) 331 | ax2.plot(shoot[0,0],shoot[0,1],'*',color = color_list[c],markersize=10) 332 | if c<3: 333 | # gradient projection 334 | robot_plot2D.plot_2d_manipulators(joint_angles_batch=shoot,ax = axs[c*2+1],color = color_list[c],show_start_end=True,show_eef_traj=True) 335 | else: 336 | # geodesic shooting 337 | robot_plot2D.plot_2d_manipulators(joint_angles_batch=shoot[0:500:20],ax = axs[c*2+1],color = color_list[c],show_start_end=False,show_eef_traj=True) 338 | plt.show() 339 | 340 | def plot_projection(obj_lists): 341 | fig1,(ax1,ax2,ax3) = plt.subplots(1,3,figsize=(24,8)) 342 | 343 | # plot cdf 344 | cdf.plot_cdf(ax1,obj_lists) 345 | cdf.plot_cdf(ax2,obj_lists) 346 | q_random = torch.rand(1000,2,requires_grad=True).to(device)*2*math.pi-math.pi 347 | 348 | # plot CDF 349 | d,grad = cdf.calculate_cdf(q_random,obj_lists,return_grad=True) 350 | q_proj = cdf.projection(q_random,d,grad) 351 | ax1.plot(q_random[:,0].detach().cpu().numpy(),q_random[:,1].detach().cpu().numpy(),'.',color='lightgreen') 352 | ax2.plot(q_proj[:,0].detach().cpu().numpy(),q_proj[:,1].detach().cpu().numpy(),'.',color='lightgreen') 353 | ax1.set_title('Initial Samples', size=25) # Add a title to your plot 354 | ax2.set_title('CDF + Gradient Projection', size=25) # Add a title to your plot 355 | 356 | # plot SDF for comparison 357 | cdf.plot_sdf(obj_lists,ax3) 358 | q0,q,q_all = cdf.find_q(obj_lists,batchsize=1000) 359 | ax3.set_title(f'SDF+Optimization', size=25) # Add a title to your plot 360 | ax3.plot(q_all[:,0].detach().cpu().numpy(),q_all[:,1].detach().cpu().numpy(),'.',color='lightgreen') 361 | 362 | # plot robot manipulator in task space for CDF 363 | fig, ax = plt.subplots(figsize=(8, 8)) # Create a figure 364 | q_proj_np = q_proj.detach().cpu().numpy() 365 | for i, _q in enumerate(q_proj_np): 366 | robot_plot2D.plotArm( 367 | ax=ax, 368 | a=_q, 369 | d=cdf.link_length[0].cpu().numpy(), 370 | p=np.array([0.0, 0.0]), # base position 371 | sz=0.05, 372 | label="via", 373 | alpha=0.05, 374 | zorder=2, 375 | xlim=None, 376 | ylim=None, 377 | robot_base=True, # to visualize the base 378 | color='lightgreen' # Set the color of the robot 379 | ) 380 | cdf.plot_objects(ax,obj_lists) 381 | 382 | plt.show() 383 | 384 | if __name__ == "__main__": 385 | device = torch.device("cuda:0" if torch.cuda.is_available() else "cpu") 386 | # device = torch.device("cpu") 387 | cdf = CDF2D(device) 388 | 389 | scene_1 = [Circle(center=torch.tensor([2.5,2.5]),radius=0.5,device=device)] 390 | scene_2 = [Circle(center=torch.tensor([2.3,-2.3]),radius=0.3,device=device), 391 | Circle(center=torch.tensor([0.0,2.45]),radius=0.3,device=device), 392 | ] 393 | 394 | # # plot the figure in the paper 395 | plot_fig1(scene_1) 396 | 397 | # # plot gradient projection 398 | # plot_projection(scene_2) -------------------------------------------------------------------------------- /2Dexamples/data2D.pt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yimingli1998/cdf/e475591f2ccf2a61251e34088931deda6d47b283/2Dexamples/data2D.pt -------------------------------------------------------------------------------- /2Dexamples/fig1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yimingli1998/cdf/e475591f2ccf2a61251e34088931deda6d47b283/2Dexamples/fig1.png -------------------------------------------------------------------------------- /2Dexamples/gp.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yimingli1998/cdf/e475591f2ccf2a61251e34088931deda6d47b283/2Dexamples/gp.png -------------------------------------------------------------------------------- /2Dexamples/mlp.py: -------------------------------------------------------------------------------- 1 | # ----------------------------------------------------------------------------- 2 | # SPDX-License-Identifier: MIT 3 | # This file is part of the CDF project. 4 | # Copyright (c) 2024 Idiap Research Institute 5 | # Contributor: Yimming Li 6 | # ----------------------------------------------------------------------------- 7 | 8 | 9 | import torch 10 | from torch import nn 11 | from torch.nn import Sequential as Seq, Linear as Lin, ReLU, ReLU6, ELU, Dropout, BatchNorm1d as BN, LayerNorm as LN, Tanh 12 | import numpy as np 13 | 14 | def xavier(param): 15 | """ initialize weights with xavier. 16 | Args: 17 | param (network params): params to initialize. 18 | """ 19 | nn.init.xavier_uniform(param) 20 | 21 | def he_init(param): 22 | """initialize weights with he. 23 | Args: 24 | param (network params): params to initialize. 25 | """ 26 | nn.init.kaiming_uniform_(param,nonlinearity='relu') 27 | nn.init.normal(param) 28 | 29 | def weights_init(m): 30 | """Function to initialize weights of a nn. 31 | Args: 32 | m (network params): pass in model.parameters() 33 | """ 34 | fn = he_init 35 | if isinstance(m, nn.Conv2d): 36 | fn(m.weight.data) 37 | m.bias.data.zero_() 38 | elif isinstance(m, nn.Conv3d): 39 | fn(m.weight.data) 40 | m.bias.data.zero_() 41 | elif isinstance(m, nn.Linear): 42 | fn(m.weight.data) 43 | if(m.bias is not None): 44 | m.bias.data.zero_() 45 | 46 | def MLP(channels, act_fn=ReLU, islast = False): 47 | """Automatic generation of mlp given some 48 | Args: 49 | channels (int): number of channels in input 50 | dropout_ratio (float, optional): dropout used after every layer. Defaults to 0.0. 51 | batch_norm (bool, optional): batch norm after every layer. Defaults to False. 52 | act_fn ([type], optional): activation function after every layer. Defaults to ReLU. 53 | layer_norm (bool, optional): layer norm after every layer. Defaults to False. 54 | nerf (bool, optional): use positional encoding (x->[sin(x),cos(x)]). Defaults to True. 55 | Returns: 56 | nn sequential layers 57 | """ 58 | if not islast: 59 | layers = [Seq(Lin(channels[i - 1], channels[i]), act_fn()) 60 | for i in range(1, len(channels))] 61 | else: 62 | layers = [Seq(Lin(channels[i - 1], channels[i]), act_fn()) 63 | for i in range(1, len(channels)-1)] 64 | layers.append(Seq(Lin(channels[-2], channels[-1]))) 65 | 66 | layers = Seq(*layers) 67 | 68 | return layers 69 | 70 | 71 | class MLPRegression(nn.Module): 72 | def __init__(self, input_dims=10, output_dims=1, mlp_layers=[128, 128, 128, 128, 128],skips=[2], act_fn=ReLU, nerf=True): 73 | """Create an instance of mlp nn model 74 | Args: 75 | input_dims (int): number of channels 76 | output_dims (int): output channel size 77 | mlp_layers (list, optional): perceptrons in each layer. Defaults to [256, 128, 128]. 78 | dropout_ratio (float, optional): dropout after every layer. Defaults to 0.0. 79 | batch_norm (bool, optional): batch norm after every layer. Defaults to False. 80 | scale_mlp_units (float, optional): Quick way to scale up and down the number of perceptrons, as this gets multiplied with values in mlp_layers. Defaults to 1.0. 81 | act_fn ([type], optional): activation function after every layer. Defaults to ELU. 82 | layer_norm (bool, optional): layer norm after every layer. Defaults to False. 83 | nerf (bool, optional): use positional encoding (x->[sin(x),cos(x)]). Defaults to False. 84 | """ 85 | super(MLPRegression, self).__init__() 86 | 87 | mlp_arr = [] 88 | if (nerf): 89 | input_dims = 3*input_dims 90 | if len(skips)>0: 91 | mlp_arr.append(mlp_layers[0:skips[0]]) 92 | mlp_arr[0][-1]-=input_dims 93 | for s in range(1,len(skips)): 94 | mlp_arr.append(mlp_layers[skips[s-1]:skips[s]]) 95 | mlp_arr[-1][-1]-=input_dims 96 | mlp_arr.append(mlp_layers[skips[-1]:]) 97 | else: 98 | mlp_arr.append(mlp_layers) 99 | 100 | mlp_arr[-1].append(output_dims) 101 | 102 | mlp_arr[0].insert(0,input_dims) 103 | self.layers = nn.ModuleList() 104 | for arr in mlp_arr[0:-1]: 105 | self.layers.append(MLP(arr,act_fn=act_fn, islast=False)) 106 | self.layers.append(MLP(mlp_arr[-1],act_fn=act_fn, islast=True)) 107 | 108 | self.nerf = nerf 109 | 110 | def forward(self, x): 111 | """forward pass on network.""" 112 | if(self.nerf): 113 | x_nerf = torch.cat((x, torch.sin(x), torch.cos(x)), dim=-1) 114 | else: 115 | x_nerf = x 116 | y = self.layers[0](x_nerf) 117 | for layer in self.layers[1:]: 118 | y = layer(torch.cat((y, x_nerf), dim=1)) 119 | return y 120 | 121 | def reset_parameters(self): 122 | """Use this function to initialize weights. Doesn't help much for mlp. 123 | """ 124 | self.apply(weights_init) -------------------------------------------------------------------------------- /2Dexamples/model.pth: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yimingli1998/cdf/e475591f2ccf2a61251e34088931deda6d47b283/2Dexamples/model.pth -------------------------------------------------------------------------------- /2Dexamples/model.py: -------------------------------------------------------------------------------- 1 | # ----------------------------------------------------------------------------- 2 | # SPDX-License-Identifier: MIT 3 | # This file is part of the CDF project. 4 | # Copyright (c) 2024 Idiap Research Institute 5 | # Contributor: Yiming Li 6 | # ----------------------------------------------------------------------------- 7 | 8 | import torch 9 | import torch.nn as nn 10 | 11 | 12 | class NN(nn.Module): 13 | def __init__(self, input_dim, hidden_dim, output_dim, activate): 14 | super(NN, self).__init__() 15 | self.layer_in = nn.Linear(input_dim, hidden_dim[0]) 16 | self.layer_hidden = nn.ModuleList( 17 | [nn.Linear(hidden_dim[i], hidden_dim[i + 1]) for i in range(len(hidden_dim) - 1)]) 18 | self.layer_out = nn.Linear(hidden_dim[-1], output_dim) 19 | self.activate = activate 20 | 21 | def forward(self, x): 22 | # x = (x - torch.tensor([-torch.pi, -torch.pi, -torch.pi, -torch.pi], dtype=torch.float32, device='cuda')) \ 23 | # / torch.tensor([2*torch.pi, 2*torch.pi, 2*torch.pi, 2*torch.pi], dtype=torch.float32, device='cuda') 24 | x = self.activate(self.layer_in(x)) 25 | for i in range(len(self.layer_hidden)): 26 | x = self.activate(self.layer_hidden[i](x)) 27 | x = self.layer_out(x) 28 | x = torch.sigmoid(x) 29 | 30 | return x 31 | 32 | def init_weights(m): 33 | if isinstance(m, nn.Linear): 34 | nn.init.xavier_normal_(m.weight) 35 | if m.bias is not None: 36 | nn.init.zeros_(m.bias) 37 | 38 | 39 | -------------------------------------------------------------------------------- /2Dexamples/nn_cdf.py: -------------------------------------------------------------------------------- 1 | # ----------------------------------------------------------------------------- 2 | # SPDX-License-Identifier: MIT 3 | # This file is part of the CDF project. 4 | # Copyright (c) 2024 Idiap Research Institute 5 | # Contributor: Yimming Li 6 | # ----------------------------------------------------------------------------- 7 | 8 | import torch 9 | from mlp import MLPRegression 10 | import os 11 | from cdf import CDF2D 12 | from tqdm import tqdm 13 | 14 | 15 | class Train_CDF: 16 | def __init__(self,device) -> None: 17 | self.device = device 18 | 19 | self.cdf = CDF2D(device) 20 | 21 | self.q_template = self.cdf.q_grid_template.view(-1,200,2) 22 | 23 | x = torch.linspace(self.cdf.task_space[0][0],self.cdf.task_space[1][0],self.cdf.nbData).to(self.device) 24 | y = torch.linspace(self.cdf.task_space[0][1],self.cdf.task_space[1][1],self.cdf.nbData).to(self.device) 25 | xx,yy = torch.meshgrid(x,y) 26 | xx,yy = xx.reshape(-1,1),yy.reshape(-1,1) 27 | self.p = torch.cat([xx,yy],dim=-1).to(self.device) 28 | 29 | 30 | def matching_csdf(self,q): 31 | # q: [batchsize,2] 32 | # return d:[len(x),len(q)] 33 | dist = torch.norm(q.unsqueeze(1).expand(-1,200,-1) - self.q_template.unsqueeze(1),dim=-1) 34 | d,idx = torch.min(dist,dim=-1) 35 | q_template = torch.gather(self.q_template,1,idx.unsqueeze(-1).expand(-1,-1,2)) 36 | return d,q_template 37 | 38 | 39 | def train(self,input_dim, hidden_dim, output_dim, activate, batch_size, learning_rate, weight_decay, save_path, device, 40 | epochs): 41 | # model 42 | net = MLPRegression(input_dims=input_dim, 43 | output_dims=output_dim, 44 | mlp_layers=hidden_dim, 45 | skips=[], 46 | act_fn=activate, 47 | nerf=True).to(device) 48 | # net.apply(model.init_weights) 49 | optimizer = torch.optim.Adam(net.parameters(), lr=learning_rate, 50 | weight_decay=weight_decay) 51 | if not os.path.exists(save_path): 52 | os.makedirs(save_path) 53 | 54 | batch_p = self.p.unsqueeze(1).expand(-1,batch_size,-1).reshape(-1,2) 55 | max_loss = float("inf") 56 | net.train() 57 | for i in tqdm(range(epochs)): 58 | q = torch.rand(batch_size,2,requires_grad=True).to(self.device)*2*torch.pi-torch.pi 59 | batch_q = q.unsqueeze(0).expand(len(self.p),-1,-1).reshape(-1,2) 60 | 61 | d,q_temp = self.matching_csdf(q) 62 | q_temp = q_temp.reshape(-1,2) 63 | mask = d.reshape(-1) 5 | # Contributor: Yimming Li 6 | # ----------------------------------------------------------------------------- 7 | 8 | import torch 9 | import os 10 | from cdf import CDF2D 11 | import nn_cdf 12 | import casadi as cs 13 | import l4casadi as l4c 14 | 15 | if __name__ == "__main__": 16 | device = torch.device("cuda" if torch.cuda.is_available() else "cpu") 17 | train_cdf = nn_cdf.Train_CDF(device) 18 | 19 | net = torch.load(os.path.join('./2Dexamples', 'model.pth')) 20 | net.eval() 21 | 22 | 23 | l4c_model = l4c.L4CasADi(net, device='cuda') # device='cuda' for GPU 24 | x_sym = cs.MX.sym('x', 1, 4) 25 | y_sym = l4c_model(x_sym) 26 | f = cs.Function('y', [x_sym], [y_sym]) 27 | df = cs.Function('dy', [x_sym], [cs.jacobian(y_sym, x_sym)]) 28 | ddf = cs.Function('ddy', [x_sym], [cs.hessian(y_sym, x_sym)[0]]) 29 | 30 | x = cs.DM([[0.], [2.],[0.], [2.]]) 31 | print(l4c_model(x)) 32 | print(f(x)) 33 | print(df(x)) 34 | print(ddf(x)) -------------------------------------------------------------------------------- /2Dexamples/primitives2D_torch.py: -------------------------------------------------------------------------------- 1 | # ----------------------------------------------------------------------------- 2 | # SPDX-License-Identifier: MIT 3 | # This file is part of the CDF project. 4 | # Copyright (c) 2024 Idiap Research Institute 5 | # Contributor: Yimming Li 6 | # ----------------------------------------------------------------------------- 7 | 8 | 9 | import copy 10 | import math 11 | import numpy as np 12 | import matplotlib.pyplot as plt 13 | import matplotlib.patches as patches 14 | import torch 15 | 16 | class Circle: 17 | def __init__(self,center,radius,device='cpu'): 18 | self.center = center.to(device) 19 | self.radius = radius 20 | self.device = device 21 | 22 | def signed_distance(self,p): 23 | # p: N x 2 24 | N = p.size(0) 25 | return (torch.norm(p-self.center.unsqueeze(0).expand(N,-1),dim=1) - self.radius).unsqueeze(-1) 26 | 27 | def normal(self,p): 28 | d = self.signed_distance(p) 29 | grad = torch.autograd.grad(d.sum(), p, create_graph=True, retain_graph=True)[0] 30 | n = torch.nn.functional.normalize(grad,dim = -1) 31 | return n 32 | 33 | def sample_surface(self,N): 34 | theta = torch.rand(N,1).to(self.device) * 2 * math.pi 35 | x = torch.cat([torch.cos(theta).to(self.device),torch.sin(theta).to(self.device)],dim=-1) 36 | return x * self.radius + self.center.unsqueeze(0).expand(N,-1) 37 | 38 | def create_patch(self,color='black'): 39 | center = self.center.cpu().numpy() 40 | radius = self.radius 41 | circle = patches.Circle(center, radius, linewidth=3, edgecolor=color, facecolor='None') 42 | return circle 43 | 44 | class Box: 45 | def __init__(self,center,w,h,device='cpu'): 46 | self.center = center 47 | self.w = w 48 | self.h = h 49 | self.device = device 50 | 51 | def signed_distance(self,p): 52 | N = p.size(0) 53 | p = p - self.center.unsqueeze(0).expand(N,-1) 54 | d = torch.abs(p) - torch.tensor([self.w/2.,self.h/2.]).to(self.device).unsqueeze(0).expand(N,-1) 55 | dist = torch.norm(torch.nn.functional.relu(d),dim=1) + torch.min(torch.cat([torch.max(d,dim=1)[0].unsqueeze(1),torch.zeros(N,1).to(self.device)],dim=1),dim=1)[0] 56 | 57 | return dist.unsqueeze(-1) 58 | 59 | def normal(self,p): 60 | d = self.signed_distance(p) 61 | grad = torch.autograd.grad(d.sum(), p, create_graph=True, retain_graph=True)[0] 62 | n = torch.nn.functional.normalize(grad,dim = -1) 63 | return n 64 | 65 | def create_patch(self,color='r'): 66 | center = self.center.cpu().numpy() 67 | w = self.w 68 | h = self.h 69 | rect = patches.Rectangle(center-[w/2.0, h/2.0] , w, h, linewidth=1, edgecolor=color, facecolor='none') 70 | return rect 71 | 72 | class Triangle: 73 | def __init__(self,p0,p1,p2): 74 | self.p0 = torch.tensor(p0, dtype=torch.float) 75 | self.p1 = torch.tensor(p1, dtype=torch.float) 76 | self.p2 = torch.tensor(p2, dtype=torch.float) 77 | 78 | def signed_distance(self,p): 79 | e0 = self.p1 - self.p0 80 | e1 = self.p2 - self.p1 81 | e2 = self.p0 - self.p2 82 | v0 = p - self.p0 83 | v1 = p - self.p1 84 | v2 = p - self.p2 85 | pq0 = v0 - e0 * torch.clamp(torch.dot(v0,e0)/torch.dot(e0,e0), 0, 1) 86 | pq1 = v1 - e1 * torch.clamp(torch.dot(v1,e1)/torch.dot(e1,e1), 0, 1) 87 | pq2 = v2 - e2 * torch.clamp(torch.dot(v2,e2)/torch.dot(e2,e2), 0, 1) 88 | s = torch.sign((e0[0]*e2[1] - e0[1]*e2[0])) 89 | 90 | d = torch.min(torch.min(torch.cat([torch.dot(pq0,pq0).unsqueeze(0), s*(v0[0]*e0[1]-v0[1]*e0[0]).unsqueeze(0)], dim = 0), 91 | torch.cat([torch.dot(pq1,pq1).unsqueeze(0), s*(v1[0]*e1[1]-v1[1]*e1[0]).unsqueeze(0)], dim = 0)), 92 | torch.cat([torch.dot(pq2,pq2).unsqueeze(0), s*(v2[0]*e2[1]-v2[1]*e2[0]).unsqueeze(0)], dim = 0)) 93 | return -torch.sqrt(d[0])*torch.sign(d[1]) 94 | 95 | def normal(self,p): 96 | d = self.signed_distance(p) 97 | grad = torch.autograd.grad(d, p, create_graph=True, retain_graph=True)[0] 98 | n = torch.nn.functional.normalize(grad,dim = -1) 99 | return n 100 | 101 | def create_patch(self,color='g'): 102 | p0 = self.p0.numpy() 103 | p1 = self.p1.numpy() 104 | p2 = self.p2.numpy() 105 | vertices = [p0,p1,p2] 106 | triangle = patches.Polygon(vertices, linewidth=1, edgecolor=color, facecolor=color) 107 | return triangle 108 | 109 | class Ellipse: 110 | def __init__(self,center,ab): 111 | self.center = torch.tensor(center) 112 | self.ab = torch.tensor(ab) 113 | 114 | def signed_distance(self,p): 115 | e = 1e-8 116 | p = p-self.center 117 | p = torch.abs(p) 118 | if p[0] > p[1]: 119 | n_p = torch.flip(p,[0]) 120 | n_ab = torch.flip(self.ab,[0]) 121 | else: 122 | n_p = p 123 | n_ab = self.ab 124 | l = n_ab[1]*n_ab[1] - n_ab[0]*n_ab[0] 125 | m = n_ab[0]*n_p[0]/(l+e) 126 | n = n_ab[1]*n_p[1]/(l+e) 127 | m2 = m*m 128 | n2 = n*n 129 | c = (m2+n2-1.0)/3.0 130 | c3 = c*c*c 131 | q = c3 + m2*n2*2.0 132 | d = c3 + m2*n2 133 | g = m + m*n2 134 | co = None 135 | if d < 0.0: 136 | h = torch.acos(q/(c3-e))/3.0 137 | s = torch.cos(h) 138 | t = torch.sin(h)*(3.0**0.5) 139 | rx = torch.sqrt( -c*(s + t + 2.0) + m2 ) 140 | ry = torch.sqrt( -c*(s - t + 2.0) + m2 ) 141 | co = (ry+torch.sign(l)*rx+torch.abs(g)/(rx*ry+e)- m)/2.0 142 | else: 143 | h = 2.0*m*n*math.sqrt(d) 144 | s = torch.sign(q+h)*torch.pow(torch.abs(q+h), 1.0/3.0) 145 | u = torch.sign(q-h)*torch.pow(torch.abs(q-h), 1.0/3.0) 146 | rx = -s - u - c*4.0 + 2.0*m2 147 | ry = (s - u)*math.sqrt(3.0) 148 | rm = torch.sqrt( rx*rx + ry*ry ) 149 | co = (ry/(torch.sqrt(rm-rx)+e)+2.0*g/(rm+e)-m)/2.0 150 | r = n_ab * torch.tensor([co, torch.sqrt(1.0-co*co)]) 151 | return torch.sqrt(torch.sum((r-n_p)**2)) * torch.sign(n_p[1]-r[1]) 152 | 153 | def normal(self,p): 154 | d = self.signed_distance(p) 155 | grad = torch.autograd.grad(d.sum(), p, create_graph=True, retain_graph=True)[0] 156 | n = torch.nn.functional.normalize(grad,dim = -1) 157 | return n 158 | 159 | def create_patch(self,color='g'): 160 | center = self.center.numpy() 161 | ab = self.ab.numpy() 162 | ellipse = patches.Ellipse(center, ab[0]*2, ab[1]*2, angle=0, linewidth=1, edgecolor=color, facecolor=color) 163 | return ellipse 164 | 165 | 166 | # Operators 167 | class Union: 168 | def __init__(self, a, *bs, k=None): 169 | self.a = a 170 | self.bs = bs 171 | self.k = k 172 | def signed_distance(self,p): 173 | def f(p): 174 | d1 = self.a.signed_distance(p) 175 | for i,b in enumerate(self.bs): 176 | d2 = b.signed_distance(p) 177 | K = self.k[i] 178 | if K is None: 179 | d1 = torch.min(d1, d2) 180 | else: 181 | h = torch.clamp(0.5 + 0.5 * (d2 - d1) /K, 0, 1) 182 | m = d2 + (d1 - d2) * h 183 | d1 = m - K * h * (1 - h) 184 | return d1 185 | return f(p) 186 | 187 | def normal(self,p): 188 | d = self.signed_distance(p) 189 | grad = torch.autograd.grad(d.sum(), p, create_graph=True, retain_graph=True)[0] 190 | n = torch.nn.functional.normalize(grad,dim = -1) 191 | return n 192 | 193 | def create_patch(self): 194 | pass 195 | 196 | class Difference: 197 | def __init__(self, a, *bs, k=None): 198 | self.a = a 199 | self.bs = bs 200 | self.k = k 201 | 202 | def signed_distance(self,p): 203 | def f(p): 204 | d1 = self.a.signed_distance(p) 205 | for i,b in enumerate(self.bs): 206 | d2 = b.signed_distance(p) 207 | K = self.k[i] 208 | if K is None: 209 | d1 = torch.max(d1, -d2) 210 | else: 211 | h = torch.clamp(0.5 - 0.5 * (d2 + d1) / K, 0, 1) 212 | m = d1 + (-d2 - d1) * h 213 | d1 = m + K * h * (1 - h) 214 | return d1 215 | return f(p) 216 | 217 | def create_patch(self): 218 | pass 219 | 220 | class Intersection: 221 | def __init__(self, a, *bs, k=None): 222 | self.a = a 223 | self.bs = bs 224 | self.k = k 225 | 226 | def signed_distance(self,p): 227 | def f(p): 228 | d1 = self.a.signed_distance(p) 229 | for i,b in enumerate(self.bs): 230 | d2 = b.signed_distance(p) 231 | K = self.k[i] 232 | if K is None: 233 | d1 = torch.max(d1, d2) 234 | else: 235 | h = torch.clamp(0.5 - 0.5 * (d2 - d1) / K, 0, 1) 236 | m = d2 + (d1 - d2) * h 237 | d1 = m + K * h * (1 - h) 238 | return d1 239 | return f(p) 240 | 241 | def create_patch(self): 242 | pass 243 | 244 | class Blend: 245 | def __init__(self, a, *bs, k=[0.5]): 246 | self.a = a 247 | self.bs = bs 248 | self.k = k 249 | 250 | def signed_distance(self,p): 251 | def f(p): 252 | d1 = self.a.signed_distance(p) 253 | for i,b in enumerate(self.bs): 254 | d2 = b.signed_distance(p) 255 | K = self.k[i] 256 | d1 = K * d2 + (1 - K) * d1 257 | return d1 258 | return f(p) 259 | 260 | def create_patch(self): 261 | pass 262 | 263 | 264 | class Negate: 265 | def __init__(self, shape): 266 | self.shape = shape 267 | 268 | def signed_distance(self,p): 269 | def f(p): 270 | return -self.shape.signed_distance(p) 271 | return f(p) 272 | 273 | class Dilate: 274 | def __init__(self, shape,r): 275 | self.shape = shape 276 | self.r = r 277 | 278 | def signed_distance(self,p): 279 | def f(p): 280 | return self.shape.signed_distance(p) - self.r 281 | return f(p) 282 | 283 | class Erode: 284 | def __init__(self, shape,r): 285 | self.shape = shape 286 | self.r = r 287 | 288 | def signed_distance(self,p): 289 | def f(p): 290 | return self.shape.signed_distance(p) + self.r 291 | return f(p) 292 | 293 | class Shell: 294 | def __init__(self, shape,thickness): 295 | self.shape = shape 296 | self.thickness = thickness 297 | 298 | def signed_distance(self,p): 299 | def f(p): 300 | return torch.abs(self.shape.signed_distance(p)) - self.thickness / 2 301 | return f(p) 302 | 303 | if __name__ == "__main__": 304 | 305 | # obj = Circle(center=[0,0],radius=[1]) 306 | # p = torch.ones(3,2) 307 | # p[2] +=1. 308 | # p.requires_grad = True 309 | # print(p.shape) 310 | # print(obj.signed_distance(p).shape) 311 | # print(obj.normal(p).shape) 312 | 313 | box = Box(center=[0.6,0.5],w=0.1,h=0.2) 314 | print(box.signed_distance(torch.tensor([0.6,0.6]))) 315 | # box = Erode(box,0.05) 316 | # circle = Circle(center=[0.36,0.3],radius=0.2) 317 | # operator = Union(box,circle,k=[0.05]) 318 | 319 | delta = 0.01 320 | x = np.arange(0.5, 1, delta) 321 | y = np.arange(0.5, 1, delta) 322 | X, Y = np.meshgrid(x, y) 323 | m, n = X.shape 324 | 325 | X_tensor = torch.from_numpy(X) 326 | Y_tensor = torch.from_numpy(Y) 327 | P = torch.cat([X_tensor.unsqueeze(-1),Y_tensor.unsqueeze(-1)],dim=-1).view(-1,2) 328 | D = box.signed_distance(P).view(m,n).numpy() 329 | print(D) 330 | fig, ax = plt.subplots() 331 | CS = ax.contour(X, Y, D,levels = [0]) 332 | ax.clabel(CS, inline=False, fontsize=10) 333 | ax.set_title('Simplest default with labels') 334 | plt.show() -------------------------------------------------------------------------------- /2Dexamples/robot2D_torch.py: -------------------------------------------------------------------------------- 1 | # ----------------------------------------------------------------------------- 2 | # SPDX-License-Identifier: MIT 3 | # This file is part of the CDF project. 4 | # Copyright (c) 2024 Idiap Research Institute 5 | # Contributor: Yimming Li 6 | # ----------------------------------------------------------------------------- 7 | 8 | 9 | import copy 10 | import math 11 | import numpy as np 12 | import matplotlib.pyplot as plt 13 | import torch 14 | 15 | class Robot2D: 16 | def __init__(self, 17 | num_joints=3, 18 | init_states=torch.tensor([[2*np.pi/3,-np.pi/3,-np.pi/3]]), 19 | link_length=torch.tensor([[2,2,1]]).float(), 20 | base_frame='default', 21 | device='cpu', 22 | ): 23 | self.device = device 24 | self.num_joints = num_joints 25 | self.init_states = init_states.to(self.device) 26 | self.link_length = link_length.to(self.device) 27 | self.B = init_states.size(0) 28 | if base_frame == 'default': 29 | self.base_frame = torch.zeros((self.B,2)).to(self.device) 30 | else: 31 | self.base_frame = base_frame.to(self.device) 32 | self.eef = self.forward_kinematics_eef(self.init_states) 33 | self.f_rob = self.forward_kinematics_all_joints(self.init_states) 34 | self.J = self.Jacobian(self.init_states) 35 | 36 | # Forward kinematics for end-effector (in robot coordinate system) 37 | def forward_kinematics_eef(self,x): 38 | self.B = x.size(0) 39 | L = torch.tril(torch.ones([self.num_joints, self.num_joints])).expand(self.B,-1,-1).float().to(self.device) 40 | x = x.unsqueeze(2) 41 | link_length = self.link_length.unsqueeze(1) 42 | f = torch.stack([ 43 | torch.matmul(link_length, torch.cos(torch.matmul(L,x))), 44 | torch.matmul(link_length, torch.sin(torch.matmul(L,x))) 45 | ], dim=0).transpose(0,1).squeeze() 46 | if self.B ==1: 47 | f = f.unsqueeze(0) 48 | return f + torch.zeros((self.B,2)).to(self.device) 49 | 50 | # Forward kinematics for all joints (in robot coordinate system) 51 | def forward_kinematics_all_joints(self,x): 52 | self.B = x.size(0) 53 | L = torch.tril(torch.ones([self.num_joints,self.num_joints])).expand(self.B,-1,-1).float().to(self.device) 54 | x = x.unsqueeze(2) 55 | diag_length = torch.diag(self.link_length.squeeze()).unsqueeze(0) 56 | f = torch.stack([ 57 | torch.matmul(L,torch.matmul(diag_length, torch.cos(torch.matmul(L,x)))), 58 | torch.matmul(L,torch.matmul(diag_length, torch.sin(torch.matmul(L,x)))) 59 | ], dim=0).transpose(0,1).squeeze() 60 | if self.B ==1: 61 | f = f.unsqueeze(0) 62 | f = torch.cat([torch.zeros([self.B,2,1]).to(self.device), f], dim=-1) 63 | return f + torch.zeros((self.B,2)).to(self.device).unsqueeze(2).expand(-1, -1, self.num_joints + 1) 64 | 65 | # Forward kinematics for whole body (a is a parameter to control the position. a in [0,1]) 66 | def forward_kinematics_any_point(self,x,a): 67 | ls = torch.sum(self.link_length)*a 68 | F = torch.zeros(self.B,2).to(self.device) 69 | f_rob = self.forward_kinematics_all_joints(x) 70 | for i,l in enumerate(ls): 71 | for j in range(self.num_joints): 72 | temp = self.link_length[0][j] 73 | if l > temp: 74 | l = l - temp 75 | else: 76 | f = f_rob[i,:,j] + (f_rob[i,:,j+1] - f_rob[i,:,j])*(l/temp) 77 | F[i] = f 78 | break 79 | return F 80 | 81 | # Jacobian with analytical computation (for single time step) 82 | def Jacobian(self,x): 83 | self.B = x.size(0) 84 | L = torch.tril(torch.ones([self.num_joints,self.num_joints])).expand(self.B,-1,-1).float().to(self.device) 85 | x = x.unsqueeze(2) 86 | diag_length = torch.diag(self.link_length.squeeze()).unsqueeze(0) 87 | J = torch.stack([ 88 | torch.matmul(torch.matmul(-torch.sin(torch.matmul(L,x)).transpose(1,2), diag_length), L), 89 | torch.matmul(torch.matmul(torch.cos(torch.matmul(L,x)).transpose(1,2),diag_length), L), 90 | torch.ones(self.B,1,self.num_joints).to(self.device) 91 | ],dim=1).squeeze() 92 | if self.B ==1: 93 | J = J.unsqueeze(0) 94 | return J 95 | 96 | def surface_points_sampler(self,x,n=100): 97 | self.B = x.size(0) 98 | f_rob = self.forward_kinematics_all_joints(x) # B,2,N 99 | N = f_rob.size(2) 100 | t = torch.linspace(0,1,n).unsqueeze(0).expand(self.B,-1).to(self.device) 101 | kpts_list = [] 102 | for i in range (N-1): 103 | # print(f_rob[:,:,i+1]-f_rob[:,:,i]) 104 | kpts = torch.einsum('ij,ik->ijk',f_rob[:,:,i+1]-f_rob[:,:,i],t) + f_rob[:,:,i].unsqueeze(-1).expand(-1,-1,n) 105 | kpts_list.append(kpts) 106 | kpts = torch.cat(kpts_list,dim=-1).transpose(1,2) 107 | return kpts 108 | 109 | def distance(self,x,p): 110 | B = x.size(0) 111 | kpts = self.surface_points_sampler(x,n=200) 112 | Ns = kpts.size(1) 113 | p = p.unsqueeze(0).unsqueeze(2).expand(B,-1,Ns,-1) 114 | kpts = kpts.unsqueeze(1).expand(-1,p.size(1),-1,-1) 115 | dist = torch.norm(kpts-p,dim=-1).min(dim=-1)[0] 116 | return dist 117 | 118 | 119 | if __name__ == "__main__": 120 | 121 | x = torch.tensor([[-1.6,-0.75]]) # Initial robot pose 122 | # x[1] = x[1] + np.pi 123 | rbt = Robot2D(num_joints=2,init_states = x,link_length=torch.tensor([[2,2]]).float()) 124 | # a = torch.rand(5) 125 | # a.requires_grad =True 126 | # print(x.shape) 127 | # print(a.shape) 128 | # f = rbt.forward_kinematics_any_point(x,a) 129 | 130 | kpts = rbt.surface_points_sampler(x).numpy() 131 | # plt.plot(rbt.f_rob[0,0,:], rbt.f_rob[0,1,:], color=str(0), linewidth=1) # Plot robot 132 | plt.scatter(kpts[0,0,:], kpts[0,1,:], color=str(0), linewidth=1) # Plot robot 133 | plt.axis("equal") 134 | plt.show() 135 | 136 | -------------------------------------------------------------------------------- /2Dexamples/robot_plot2D.py: -------------------------------------------------------------------------------- 1 | # ----------------------------------------------------------------------------- 2 | # SPDX-License-Identifier: MIT 3 | # This file is part of the CDF project. 4 | # Copyright (c) 2024 Idiap Research Institute 5 | # Contributor: Yimming Li 6 | # ----------------------------------------------------------------------------- 7 | 8 | 9 | """ 10 | Fancy visualization for a planar manipulator 11 | 12 | Copyright (c) 2023 Idiap Research Institute 13 | Written by Boyang Ti 14 | 15 | This file is part of RCFS 16 | License: MIT 17 | """ 18 | import numpy as np 19 | import numpy.matlib 20 | import matplotlib.pyplot as plt 21 | import matplotlib.path as mpath 22 | from matplotlib.patches import PathPatch 23 | from matplotlib.lines import Line2D 24 | import matplotlib.cm as cm 25 | 26 | def plotArmLink(ax, a, d, p, sz, facecol, edgecol, **kwargs): 27 | nbSegm = 30 28 | 29 | Path = mpath.Path 30 | 31 | # calculate the link border 32 | xTmp = np.zeros((2, nbSegm)) 33 | p = p + np.array([0, 0]).reshape(2, -1) 34 | t1 = np.linspace(0, -np.pi, int(nbSegm / 2)) 35 | t2 = np.linspace(np.pi, 0, int(nbSegm / 2)) 36 | xTmp[0, :] = np.hstack((sz * np.sin(t1), d + sz * np.sin(t2))) 37 | xTmp[1, :] = np.hstack((sz * np.cos(t1), sz * np.cos(t2))) 38 | # xTmp[2, :] = np.zeros((1, nbSegm)) 39 | R = np.array([[np.cos(a), -np.sin(a)], [np.sin(a), np.cos(a)]]) 40 | x = R @ xTmp + np.matlib.repmat(p, 1, nbSegm) 41 | p2 = R @ np.array([d, 0]).reshape(2, -1) + p 42 | 43 | # add the link patch 44 | codes = Path.LINETO * np.ones(np.size(x[0:2, :], 1), dtype=Path.code_type) 45 | codes[0] = Path.MOVETO 46 | path = Path(x[0:2, :].T, codes) 47 | patch = PathPatch(path, facecolor=facecol, edgecolor=edgecol, **kwargs) 48 | ax.add_patch(patch) 49 | 50 | # add the initial point 51 | msh = ( 52 | np.vstack( 53 | ( 54 | np.sin(np.linspace(0, 2 * np.pi, nbSegm)), 55 | np.cos(np.linspace(0, 2 * np.pi, nbSegm)), 56 | ) 57 | ) 58 | * sz 59 | * 0.4 60 | ) 61 | 62 | codes = Path.LINETO * np.ones(np.size(msh[0:2, :], 1), dtype=Path.code_type) 63 | codes[0] = Path.MOVETO 64 | path = Path((msh[0:2, :] + p).T, codes) 65 | patch = PathPatch(path, facecolor=facecol, edgecolor=edgecol, **kwargs) 66 | ax.add_patch(patch) 67 | # add the end point 68 | path = Path((msh[0:2, :] + p2).T, codes) 69 | patch = PathPatch(path, facecolor=facecol, edgecolor=edgecol, **kwargs) 70 | ax.add_patch(patch) 71 | 72 | return p2 73 | 74 | 75 | def plotArmBasis(ax, p1, sz, facecol, edgecol, **kwargs): 76 | Path = mpath.Path 77 | 78 | nbSegm = 30 79 | sz = sz * 1.2 80 | 81 | xTmp1 = np.zeros((2, nbSegm)) 82 | t1 = np.linspace(0, np.pi, nbSegm - 2) 83 | xTmp1[0, :] = np.hstack([sz * 1.5, sz * 1.5 * np.cos(t1), -sz * 1.5]) 84 | xTmp1[1, :] = np.hstack([-sz * 1.2, sz * 1.5 * np.sin(t1), -sz * 1.2]) 85 | x1 = xTmp1 + np.matlib.repmat(p1, 1, nbSegm) 86 | # add the link patch 87 | codes = Path.LINETO * np.ones(np.size(x1, 1), dtype=Path.code_type) 88 | codes[0] = Path.MOVETO 89 | path = Path(x1.T, codes) 90 | patch = PathPatch(path, facecolor=facecol, edgecolor=edgecol, **kwargs) 91 | ax.add_patch(patch) 92 | 93 | nb_line = 4 94 | mult = 1.2 95 | xTmp2 = np.zeros((2, nb_line)) # 2D only 96 | xTmp2[0, :] = np.linspace(-sz * mult, sz * mult, nb_line) 97 | xTmp2[1, :] = [-sz * mult] * nb_line 98 | 99 | x2 = xTmp2 + np.tile((p1.flatten() + np.array([0.0, 0.0]))[:, None], (1, nb_line)) 100 | x3 = xTmp2 + np.tile( 101 | (p1.flatten() + np.array([-0.2, -0.8]) * sz)[:, None], (1, nb_line) 102 | ) 103 | 104 | for i in range(nb_line): 105 | tmp = np.zeros((2, 2)) # N*2 106 | tmp[0] = [x2[0, i], x2[1, i]] 107 | tmp[1] = [x3[0, i], x3[1, i]] 108 | patch = Line2D(tmp[:, 0], tmp[:, 1], color=[0, 0, 0, 1], lw=2, zorder=1) 109 | ax.add_line(patch) 110 | 111 | 112 | def plotArm( 113 | ax, 114 | a, 115 | d, 116 | p, 117 | sz=0.1, 118 | facecol=None, 119 | edgecol=None, 120 | xlim=None, 121 | ylim=None, 122 | robot_base=False, 123 | **kwargs 124 | ): 125 | if edgecol is None: 126 | edgecol = [0.99, 0.99, 0.99] 127 | if facecol is None: 128 | facecol = [0.5, 0.5, 0.5] 129 | p = np.reshape(p, (-1, 1)) 130 | if robot_base: 131 | plotArmBasis(ax, p, sz, facecol, edgecol, **kwargs) 132 | for i in range(len(a)): 133 | p = plotArmLink( 134 | ax=ax, 135 | a=np.sum(a[0 : i + 1]), 136 | d=d[i], 137 | p=p + np.array([0.0, 0.0]).reshape(2, -1), 138 | sz=sz, 139 | facecol=facecol, 140 | edgecol=edgecol, 141 | **kwargs 142 | ) 143 | if xlim is not None: 144 | ax.set_xlim(xlim) 145 | if ylim is not None: 146 | ax.set_ylim(ylim) 147 | if xlim is None and ylim is None: 148 | ax.autoscale(True) 149 | 150 | 151 | def plotArm_Tool( 152 | ax, 153 | a, 154 | d, 155 | p, 156 | sz=0.1, 157 | facecol=None, 158 | edgecol=None, 159 | xlim=None, 160 | ylim=None, 161 | robot_base=False, 162 | **kwargs 163 | ): 164 | if edgecol is None: 165 | edgecol = [0.99, 0.99, 0.99] 166 | if facecol is None: 167 | facecol = [0.5, 0.5, 0.5] 168 | p = np.reshape(p, (-1, 1)) 169 | if robot_base: 170 | plotArmBasis(ax, p, sz, facecol, edgecol, **kwargs) 171 | for i in range(len(a)): 172 | if i == len(a) - 1: 173 | p = plotArmLink( 174 | ax=ax, 175 | a=np.sum(a[0 : i + 1]), 176 | d=d[i], 177 | p=p + np.array([0.0, 0.0]).reshape(2, -1), 178 | sz=sz, 179 | facecol=facecol, 180 | edgecol=edgecol, 181 | alpha=0.4, 182 | ) 183 | else: 184 | p = plotArmLink( 185 | ax=ax, 186 | a=np.sum(a[0 : i + 1]), 187 | d=d[i], 188 | p=p + np.array([0.0, 0.0]).reshape(2, -1), 189 | sz=sz, 190 | facecol=facecol, 191 | edgecol=edgecol, 192 | **kwargs 193 | ) 194 | if xlim is not None: 195 | ax.set_xlim(xlim) 196 | if ylim is not None: 197 | ax.set_ylim(ylim) 198 | if xlim is None and ylim is None: 199 | ax.autoscale(True) 200 | 201 | 202 | def plot_planar_axis(ax, p): 203 | length = 0.2 204 | num = np.size(p, 0) 205 | for i in range(num): 206 | x_1 = np.array([p[i, 0], p[i, 0] + length * np.cos(p[i, 2])]) 207 | y_1 = np.array([p[i, 1], p[i, 1] + length * np.sin(p[i, 2])]) 208 | (ln1,) = ax.plot(x_1, y_1, lw=2, solid_capstyle="round", color="r", zorder=1) 209 | ln1.set_solid_capstyle("round") 210 | 211 | x_2 = np.array([p[i, 0], p[i, 0] + length * np.cos(p[i, 2] + np.pi / 2)]) 212 | y_2 = np.array([p[i, 1], p[i, 1] + length * np.sin(p[i, 2] + np.pi / 2)]) 213 | (ln2,) = ax.plot(x_2, y_2, lw=2, solid_capstyle="round", color="b", zorder=1) 214 | ln2.set_solid_capstyle("round") 215 | 216 | 217 | def plot_2d_manipulators(link1_length=2, 218 | link2_length=2, 219 | joint_angles_batch=None, 220 | ax=None, 221 | color = 'green', 222 | alpha = 1.0, 223 | show_start_end = False, 224 | show_eef_traj = False): 225 | # Check if joint_angles_batch is None or has incorrect shape 226 | if joint_angles_batch is None or joint_angles_batch.shape[1] != 2: 227 | raise ValueError("joint_angles_batch must be provided with shape (N, 2)") 228 | 229 | if ax is None: 230 | fig, ax = plt.subplots() 231 | 232 | # Number of sets of joint angles 233 | num_sets = joint_angles_batch.shape[0] 234 | 235 | 236 | # Create a figure 237 | cmap = cm.get_cmap('Reds', num_sets) # You can choose other colormaps like 'Greens', 'Reds', etc. 238 | cmap2 = cm.get_cmap('Reds', num_sets) # You can choose other colormaps like 'Greens', 'Reds', etc. 239 | # the color will 240 | traj_list = [] 241 | for i in range(num_sets): 242 | if i ==0 or i==num_sets-1: 243 | alpha = 1.0 244 | line_width = 3 245 | else: 246 | alpha = 0.4 247 | line_width = 2 248 | # Extract joint angles for the current set 249 | theta1, theta2 = joint_angles_batch[i] 250 | 251 | # Calculate the position of the first joint 252 | joint1_x = link1_length * np.cos(theta1) 253 | joint1_y = link1_length * np.sin(theta1) 254 | 255 | # Calculate the position of the end effector (tip of the second link) 256 | end_effector_x = joint1_x + link2_length * np.cos(theta1 + theta2) 257 | end_effector_y = joint1_y + link2_length * np.sin(theta1 + theta2) 258 | 259 | # Stack the base, joint, and end effector positions 260 | positions = np.vstack([[0, 0], [joint1_x, joint1_y], [end_effector_x, end_effector_y]]) # shape: (3, 2) 261 | traj_list.append(positions) 262 | # Plotting 263 | if show_start_end: 264 | if i>0 and i 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /frankaemika/data_generator.py: -------------------------------------------------------------------------------- 1 | # ----------------------------------------------------------------------------- 2 | # SPDX-License-Identifier: MIT 3 | # This file is part of the CDF project. 4 | # Copyright (c) 2024 Idiap Research Institute 5 | # Contributor: Yimming Li 6 | # ----------------------------------------------------------------------------- 7 | 8 | 9 | import torch 10 | import os 11 | CUR_DIR = os.path.dirname(os.path.realpath(__file__)) 12 | import numpy as np 13 | import sys 14 | sys.path.append(os.path.join(CUR_DIR,'../../RDF')) 15 | from panda_layer.panda_layer import PandaLayer 16 | from bf_sdf import BPSDF 17 | from torchmin import minimize 18 | import time 19 | import math 20 | import copy 21 | 22 | PI = math.pi 23 | 24 | class DataGenerator(): 25 | def __init__(self,device): 26 | # panda model 27 | self.panda = PandaLayer(device) 28 | self.bp_sdf = BPSDF(8,-1.0,1.0,self.panda,device) 29 | self.model = torch.load(os.path.join(CUR_DIR,'../../RDF/models/BP_8.pt')) 30 | self.q_max = self.panda.theta_max 31 | self.q_min = self.panda.theta_min 32 | # device 33 | self.device = device 34 | 35 | # data generation 36 | self.workspace = [[-0.5,-0.5,0.0],[0.5,0.5,1.0]] 37 | self.n_disrete = 20 # total number of x: n_discrete**3 38 | self.batchsize = 20000 # batch size of q 39 | # self.pose = torch.eye(4).unsqueeze(0).to(self.device).expand(self.batchsize,4,4).float() 40 | self.epsilon = 1e-3 # distance threshold to filter data 41 | 42 | def compute_sdf(self,x,q,return_index = False): 43 | # x : (Nx,3) 44 | # q : (Nq,7) 45 | # return_index : if True, return the index of link that is closest to x 46 | # return d : (Nq) 47 | # return idx : (Nq) optional 48 | 49 | pose = torch.eye(4).unsqueeze(0).to(self.device).expand(len(q),4,4).float() 50 | if not return_index: 51 | d,_ = self.bp_sdf.get_whole_body_sdf_batch(x,pose, q,self.model,use_derivative =False) 52 | d = d.min(dim=1)[0] 53 | return d 54 | else: 55 | d,_,idx = self.bp_sdf.get_whole_body_sdf_batch(x,pose, q,self.model,use_derivative =False,return_index = True) 56 | d,pts_idx = d.min(dim=1) 57 | idx = idx[torch.arange(len(idx)),pts_idx] 58 | return d,idx 59 | 60 | def given_x_find_q(self,x,q = None, batchsize = None,return_mask = False,epsilon = 1e-3): 61 | # x : (N,3) 62 | # scale x to workspace 63 | if not batchsize: 64 | batchsize = self.batchsize 65 | 66 | def cost_function(q): 67 | # find q that d(x,q) = 0 68 | # q : B,2 69 | # x : N,3 70 | 71 | d = self.compute_sdf(x,q) 72 | cost = torch.sum(d**2) 73 | return cost 74 | 75 | t0 = time.time() 76 | # optimizer for data generation 77 | if q is None: 78 | q = torch.rand(batchsize,7).to(self.device)*(self.q_max-self.q_min)+self.q_min 79 | q0 = copy.deepcopy(q) 80 | res = minimize( 81 | cost_function, 82 | q, 83 | method='l-bfgs', 84 | options=dict(line_search='strong-wolfe'), 85 | max_iter=50, 86 | disp=0 87 | ) 88 | 89 | d,idx = self.compute_sdf(x,res.x,return_index=True) 90 | d,idx = d.squeeze(),idx.squeeze() 91 | 92 | mask = torch.abs(d) < epsilon 93 | # q_valid,d,idx = res.x[mask],d[mask],idx[mask] 94 | boundary_mask = ((res.x > self.q_min) & (res.x < self.q_max)).all(dim=1) 95 | final_mask = mask & boundary_mask 96 | final_q,idx = res.x[final_mask],idx[final_mask] 97 | # q0 = q0[mask][boundary_mask] 98 | 99 | print('number of q_valid: \t{} \t time cost:{}'.format(len(final_q),time.time()-t0)) 100 | if return_mask: 101 | return final_mask,final_q,idx 102 | else: 103 | return final_q,idx 104 | 105 | def distance_q(self,x,q): 106 | # x : (Nx,3) 107 | # q : (Np,7) 108 | # return d : (Np) distance between q and x in C space. d = min_{q*}{L2(q-q*)}. sdf(x,q*)=0 109 | 110 | # compute d 111 | Np = q.shape[0] 112 | q_template,link_idx = self.given_x_find_q(x) 113 | print(q_template.shape) 114 | 115 | if link_idx.min() == 0: 116 | return torch.zeros(Np).to(self.device) 117 | else: 118 | link_idx[link_idx==7] = 6 119 | link_idx[link_idx==8] = 7 120 | d = torch.inf*torch.ones(Np,7).to(self.device) 121 | for i in range(link_idx.min(),link_idx.max()+1): 122 | mask = (link_idx==i) 123 | d_norm = torch.norm(q[:,:i].unsqueeze(1)- q_template[mask][:,:i].unsqueeze(0),dim=-1) 124 | d[:,i-1] = torch.min(d_norm,dim=-1)[0] 125 | d = torch.min(d,dim=-1)[0] 126 | 127 | # compute sign of d 128 | d_ts = self.compute_sdf(x,q) 129 | mask = (d_ts < 0) 130 | d[mask] = -d[mask] 131 | return d 132 | 133 | def projection(self,x,q): 134 | q.requires_grad = True 135 | d = self.distance_q(x,q) 136 | grad = torch.autograd.grad(d,q,torch.ones_like(d),create_graph=True)[0] 137 | q_new = q - grad*d.unsqueeze(-1) 138 | return q_new 139 | 140 | def generate_offline_data(self,save_path = CUR_DIR): 141 | 142 | x = torch.linspace(self.workspace[0][0],self.workspace[1][0],self.n_disrete).to(self.device) 143 | y = torch.linspace(self.workspace[0][1],self.workspace[1][1],self.n_disrete).to(self.device) 144 | z = torch.linspace(self.workspace[0][2],self.workspace[1][2],self.n_disrete).to(self.device) 145 | x,y,z = torch.meshgrid(x,y,z) 146 | pts = torch.stack([x,y,z],dim=-1).reshape(-1,3) 147 | data = {} 148 | for i,p in enumerate(pts): 149 | q,idx = self.given_x_find_q(p.unsqueeze(0)) 150 | data[i] ={ 151 | 'x': p.detach().cpu().numpy(), 152 | 'q': q.detach().cpu().numpy(), 153 | 'idx': idx.detach().cpu().numpy(), 154 | } 155 | print(f'point {i} finished, number of q: {len(q)}') 156 | # np.save(os.path.join(save_path,'data.npy'),data) 157 | 158 | def analysis_data(x): 159 | # Compute the squared Euclidean distance between each row 160 | diff = x.unsqueeze(1) - x.unsqueeze(0) 161 | diff = diff.pow(2).sum(-1) 162 | 163 | # Set the diagonal elements to a large value to exclude self-distance 164 | diag_indices = torch.arange(x.shape[0]) 165 | diff[diag_indices, diag_indices] = float('inf') 166 | 167 | # Compute the Euclidean distance by taking the square root 168 | diff = diff.sqrt() 169 | min_dist = torch.min(diff,dim=1)[0] 170 | print(f'distance\tmax:{min_dist.max()}\tmin:{min_dist.min()}\taverage:{min_dist.mean()}') 171 | 172 | 173 | 174 | if __name__ == "__main__": 175 | device = torch.device("cuda:0" if torch.cuda.is_available() else "cpu") 176 | 177 | gen = DataGenerator(device) 178 | # x = torch.tensor([[0.5,0.5,0.5]]).to(device) 179 | # gen.single_point_generation(x) 180 | gen.generate_offline_data() -------------------------------------------------------------------------------- /frankaemika/mlp.py: -------------------------------------------------------------------------------- 1 | # ----------------------------------------------------------------------------- 2 | # SPDX-License-Identifier: MIT 3 | # This file is part of the CDF project. 4 | # Copyright (c) 2024 Idiap Research Institute 5 | # Contributor: Yimming Li 6 | # ----------------------------------------------------------------------------- 7 | 8 | 9 | import torch 10 | from torch import nn 11 | from torch.nn import Sequential as Seq, Linear as Lin, ReLU, ReLU6, ELU, Dropout, BatchNorm1d as BN, LayerNorm as LN, Tanh 12 | import numpy as np 13 | 14 | def xavier(param): 15 | """ initialize weights with xavier. 16 | Args: 17 | param (network params): params to initialize. 18 | """ 19 | nn.init.xavier_uniform(param) 20 | 21 | def he_init(param): 22 | """initialize weights with he. 23 | Args: 24 | param (network params): params to initialize. 25 | """ 26 | nn.init.kaiming_uniform_(param,nonlinearity='relu') 27 | nn.init.normal(param) 28 | 29 | def weights_init(m): 30 | """Function to initialize weights of a nn. 31 | Args: 32 | m (network params): pass in model.parameters() 33 | """ 34 | fn = he_init 35 | if isinstance(m, nn.Conv2d): 36 | fn(m.weight.data) 37 | m.bias.data.zero_() 38 | elif isinstance(m, nn.Conv3d): 39 | fn(m.weight.data) 40 | m.bias.data.zero_() 41 | elif isinstance(m, nn.Linear): 42 | fn(m.weight.data) 43 | if(m.bias is not None): 44 | m.bias.data.zero_() 45 | 46 | def MLP(channels, act_fn=ReLU, islast = False): 47 | """Automatic generation of mlp given some 48 | Args: 49 | channels (int): number of channels in input 50 | dropout_ratio (float, optional): dropout used after every layer. Defaults to 0.0. 51 | batch_norm (bool, optional): batch norm after every layer. Defaults to False. 52 | act_fn ([type], optional): activation function after every layer. Defaults to ReLU. 53 | layer_norm (bool, optional): layer norm after every layer. Defaults to False. 54 | nerf (bool, optional): use positional encoding (x->[sin(x),cos(x)]). Defaults to True. 55 | Returns: 56 | nn sequential layers 57 | """ 58 | if not islast: 59 | layers = [Seq(Lin(channels[i - 1], channels[i]), act_fn()) 60 | for i in range(1, len(channels))] 61 | else: 62 | layers = [Seq(Lin(channels[i - 1], channels[i]), act_fn()) 63 | for i in range(1, len(channels)-1)] 64 | layers.append(Seq(Lin(channels[-2], channels[-1]))) 65 | 66 | layers = Seq(*layers) 67 | 68 | return layers 69 | 70 | 71 | class MLPRegression(nn.Module): 72 | def __init__(self, input_dims=10, output_dims=1, mlp_layers=[128, 128, 128, 128, 128],skips=[2], act_fn=ReLU, nerf=True): 73 | """Create an instance of mlp nn model 74 | Args: 75 | input_dims (int): number of channels 76 | output_dims (int): output channel size 77 | mlp_layers (list, optional): perceptrons in each layer. Defaults to [256, 128, 128]. 78 | dropout_ratio (float, optional): dropout after every layer. Defaults to 0.0. 79 | batch_norm (bool, optional): batch norm after every layer. Defaults to False. 80 | scale_mlp_units (float, optional): Quick way to scale up and down the number of perceptrons, as this gets multiplied with values in mlp_layers. Defaults to 1.0. 81 | act_fn ([type], optional): activation function after every layer. Defaults to ELU. 82 | layer_norm (bool, optional): layer norm after every layer. Defaults to False. 83 | nerf (bool, optional): use positional encoding (x->[sin(x),cos(x)]). Defaults to False. 84 | """ 85 | super(MLPRegression, self).__init__() 86 | 87 | mlp_arr = [] 88 | if (nerf): 89 | input_dims = 3*input_dims 90 | if len(skips)>0: 91 | mlp_arr.append(mlp_layers[0:skips[0]]) 92 | mlp_arr[0][-1]-=input_dims 93 | for s in range(1,len(skips)): 94 | mlp_arr.append(mlp_layers[skips[s-1]:skips[s]]) 95 | mlp_arr[-1][-1]-=input_dims 96 | mlp_arr.append(mlp_layers[skips[-1]:]) 97 | else: 98 | mlp_arr.append(mlp_layers) 99 | 100 | mlp_arr[-1].append(output_dims) 101 | 102 | mlp_arr[0].insert(0,input_dims) 103 | self.layers = nn.ModuleList() 104 | for arr in mlp_arr[0:-1]: 105 | self.layers.append(MLP(arr,act_fn=act_fn, islast=False)) 106 | self.layers.append(MLP(mlp_arr[-1],act_fn=act_fn, islast=True)) 107 | 108 | self.nerf = nerf 109 | 110 | def forward(self, x): 111 | """forward pass on network.""" 112 | if(self.nerf): 113 | x_nerf = torch.cat((x, torch.sin(x), torch.cos(x)), dim=-1) 114 | else: 115 | x_nerf = x 116 | y = self.layers[0](x_nerf) 117 | for layer in self.layers[1:]: 118 | y = layer(torch.cat((y, x_nerf), dim=1)) 119 | return y 120 | 121 | def reset_parameters(self): 122 | """Use this function to initialize weights. Doesn't help much for mlp. 123 | """ 124 | self.apply(weights_init) -------------------------------------------------------------------------------- /frankaemika/model_dict.pt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yimingli1998/cdf/e475591f2ccf2a61251e34088931deda6d47b283/frankaemika/model_dict.pt -------------------------------------------------------------------------------- /frankaemika/mp_ik.py: -------------------------------------------------------------------------------- 1 | # ----------------------------------------------------------------------------- 2 | # SPDX-License-Identifier: MIT 3 | # This file is part of the CDF project. 4 | # Copyright (c) 2024 Idiap Research Institute 5 | # Contributor: Yimming Li 6 | # ----------------------------------------------------------------------------- 7 | 8 | 9 | import pybullet as p 10 | import pybullet_data as pd 11 | import numpy as np 12 | import sys 13 | import time 14 | from pybullet_panda_sim import PandaSim, SphereManager 15 | import torch 16 | import os 17 | CUR_PATH = os.path.dirname(os.path.realpath(__file__)) 18 | from mlp import MLPRegression 19 | from nn_cdf import CDF 20 | 21 | def main_loop(): 22 | # device = torch.device("cuda" if torch.cuda.is_available() else "cpu") 23 | device = torch.device("cpu") 24 | cdf = CDF(device) 25 | 26 | # cdf.np_csdf(torch.rand(100,3).to(device) - torch.tensor([[0.5,0.5,0.0]]).to(device),torch.rand(10,7).to(device),visualize=False) 27 | 28 | # trainer.train_nn(epoches=20000) 29 | model = MLPRegression(input_dims=10, output_dims=1, mlp_layers=[1024, 512, 256, 128, 128],skips=[], act_fn=torch.nn.ReLU, nerf=True) 30 | model.load_state_dict(torch.load(os.path.join(CUR_PATH,'model_dict.pt'))[49900]) 31 | model.to(device) 32 | 33 | 34 | # p.connect(p.GUI, options='--background_color_red=0.5 --background_color_green=0.5' + 35 | # ' --background_color_blue=0.5 --width=1600 --height=1000') 36 | p.connect(p.GUI, options='--background_color_red=1 --background_color_green=1' + 37 | ' --background_color_blue=1 --width=1000 --height=1000') 38 | 39 | p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0) 40 | p.configureDebugVisualizer(lightPosition=[5, 5, 5]) 41 | p.setPhysicsEngineParameter(maxNumCmdPer1ms=1000) 42 | #p.resetDebugVisualizerCamera(cameraDistance=1.5, cameraYaw=110, cameraPitch=-10, cameraTargetPosition=[0, 0, 0.5]) 43 | # p.resetDebugVisualizerCamera(cameraDistance=1.5, cameraYaw=90, cameraPitch=0, cameraTargetPosition=[0, 0, 0.5]) 44 | #p.resetDebugVisualizerCamera(cameraDistance=1.5, cameraYaw=110, cameraPitch=-25, cameraTargetPosition=[0, 0, 0.5]) 45 | p.resetDebugVisualizerCamera(cameraDistance=1.5, cameraYaw=145, cameraPitch=-10, cameraTargetPosition=[0, 0, 0.6]) 46 | 47 | p.setAdditionalSearchPath(pd.getDataPath()) 48 | timeStep = 0.01 49 | p.setTimeStep(timeStep) 50 | p.setGravity(0, 0, -9.81) 51 | p.setRealTimeSimulation(1) 52 | ## spawn franka robot 53 | base_pos = [0, 0, 0] 54 | base_rot = p.getQuaternionFromEuler([0, 0, 0]) 55 | panda = PandaSim(p, base_pos, base_rot) 56 | q0 = panda.get_joint_positions() 57 | sphere_manager = SphereManager(p) 58 | # sphere_center = [0.3, 0.4, 0.5] 59 | # sphere_manager.create_sphere(sphere_center, 0.05, [0.8500, 0.3250, 0.0980, 1]) 60 | q_init = torch.tensor([q0],requires_grad=True).to(device).float() 61 | while True: 62 | t0 = time.time() 63 | sphere_center = np.random.rand(3)*0.5 64 | # sphere_center[2] += 0.5 65 | # print(f'sphere_center: {sphere_center}') 66 | x = torch.tensor([sphere_center],requires_grad=True).to(device).float() 67 | # print(f'x: {x.shape}, q: {q.shape}') 68 | q = q_init 69 | d,grad = cdf.inference_d_wrt_q(x,q,model) 70 | # print(f'd: {d}, grad: {grad}') 71 | q_proj = cdf.projection(q,d,grad) 72 | q_init = q_proj 73 | print(f'x: {x}') 74 | print(f'q_proj: {q_proj}') 75 | panda.set_joint_positions(q_proj[0].data.cpu().numpy()) 76 | # q0[0] = (q0[0]+0.1)%np.pi 77 | p.addUserDebugText(f"FPS:{int(1/(time.time()-t0))}", [-0.6,0.0,0.8], textColorRGB=(1, 0, 0), textSize=2.0) 78 | sphere_manager.create_sphere(sphere_center, 0.05, [0.8500, 0.3250, 0.0980, 1]) 79 | time.sleep(1.0) 80 | p.removeAllUserDebugItems() 81 | sphere_manager.delete_spheres() 82 | 83 | if __name__ == '__main__': 84 | main_loop() -------------------------------------------------------------------------------- /frankaemika/nn_cdf.py: -------------------------------------------------------------------------------- 1 | # ----------------------------------------------------------------------------- 2 | # SPDX-License-Identifier: MIT 3 | # This file is part of the CDF project. 4 | # Copyright (c) 2024 Idiap Research Institute 5 | # Contributor: Yimming Li 6 | # ----------------------------------------------------------------------------- 7 | 8 | 9 | # 7D panda robot 10 | import numpy as np 11 | import os 12 | import sys 13 | import torch 14 | import math 15 | import time 16 | CUR_PATH = os.path.dirname(os.path.realpath(__file__)) 17 | from mlp import MLPRegression 18 | sys.path.append(os.path.join(CUR_PATH,'../../RDF')) 19 | from panda_layer.panda_layer import PandaLayer 20 | import bf_sdf 21 | 22 | PI = math.pi 23 | # torch.manual_seed(10) 24 | np.random.seed(10) 25 | # torch.autograd.set_detect_anomaly(True) 26 | 27 | class CDF: 28 | def __init__(self,device) -> None: 29 | # device 30 | self.device = device 31 | 32 | # # uncomment these lines to process the generated data and train your own CDF 33 | # self.raw_data = np.load(os.path.join(CUR_PATH,'data.npy'),allow_pickle=True).item() 34 | # self.process_data(self.raw_data) 35 | # self.data_path = os.path.join(CUR_PATH,'data.pt') 36 | # self.data = self.load_data(self.data_path) 37 | # self.len_data = len(self.data['k']) 38 | 39 | self.batch_x = 10 40 | self.batch_q = 100 41 | self.max_q_per_link = 100 42 | 43 | # panda robot 44 | self.panda = PandaLayer(device) 45 | 46 | def process_data(self,data): 47 | import pytorch3d.ops 48 | keys = list(data.keys()) # Create a copy of the keys 49 | processed_data = {} 50 | for k in keys: 51 | if len(data[k]['q']) == 0: 52 | data.pop(k) 53 | continue 54 | q = torch.from_numpy(data[k]['q']).float().to(self.device) 55 | q_idx = torch.from_numpy(data[k]['idx']).float().to(self.device) 56 | q_idx[q_idx==7] = 6 57 | q_idx[q_idx==8] = 7 58 | q_lib = torch.inf*torch.ones(self.max_q_per_link,7,7).to(self.device) 59 | for i in range(1,8): 60 | mask = (q_idx==i) 61 | if len(q[mask])>self.max_q_per_link: 62 | fps_q = pytorch3d.ops.sample_farthest_points(q[mask].unsqueeze(0),K=self.max_q_per_link)[0] 63 | q_lib[:,:,i-1] = fps_q.squeeze() 64 | # print(q_lib[:,:,i]) 65 | elif len(q[mask])>0: 66 | q_lib[:len(q[mask]),:,i-1] = q[mask] 67 | 68 | processed_data[k] = { 69 | 'x':torch.from_numpy(data[k]['x']).float().to(self.device), 70 | 'q':q_lib, 71 | } 72 | final_data = { 73 | 'x': torch.cat([processed_data[k]['x'].unsqueeze(0) for k in processed_data.keys()],dim=0), 74 | 'q': torch.cat([processed_data[k]['q'].unsqueeze(0) for k in processed_data.keys()],dim=0), 75 | 'k':torch.tensor([k for k in processed_data.keys()]).to(self.device) 76 | } 77 | 78 | torch.save(final_data,os.path.join(CUR_PATH,'data.pt')) 79 | return data 80 | 81 | def load_data(self,path): 82 | data = torch.load(path) 83 | return data 84 | 85 | def select_data(self): 86 | # x_batch:(batch_x,3) 87 | # q_batch:(batch_q,7) 88 | # d:(batch_x,batch_q) 89 | 90 | x = self.data['x'] 91 | q = self.data['q'] 92 | 93 | idx = torch.randint(0,len(x),(self.batch_x,)) 94 | # idx = torch.tensor([4000]) 95 | x_batch,q_lib = x[idx],q[idx] 96 | # print(x_batch) 97 | q_batch = self.sample_q() 98 | d,grad = self.decode_distance(x_batch,q_batch,q_lib) 99 | return x_batch,q_batch,d,grad 100 | 101 | def decode_distance(self,q_batch,q_lib): 102 | # batch_q:(batch_q,7) 103 | # q_lib:(batch_x,self.max_q_per_link,7,7) 104 | 105 | batch_x = q_lib.shape[0] 106 | batch_q = q_batch.shape[0] 107 | d_tensor = torch.ones(batch_x,batch_q,7).to(self.device)*torch.inf 108 | grad_tensor = torch.zeros(batch_x,batch_q,7,7).to(self.device) 109 | for i in range(7): 110 | q_lib_temp = q_lib[:,:,:i+1,i].reshape(batch_x*self.max_q_per_link,-1).unsqueeze(0).expand(batch_q,-1,-1) 111 | q_batch_temp = q_batch[:,:i+1].unsqueeze(1).expand(-1,batch_x*self.max_q_per_link,-1) 112 | d_norm = torch.norm((q_batch_temp - q_lib_temp),dim=-1).reshape(batch_q,batch_x,self.max_q_per_link) 113 | 114 | d_norm_min,d_norm_min_idx = d_norm.min(dim=-1) 115 | grad = torch.autograd.grad(d_norm_min.reshape(-1),q_batch_temp,torch.ones_like(d_norm_min.reshape(-1)),retain_graph=True)[0] 116 | grad_min_q = grad.reshape(batch_q,batch_x,self.max_q_per_link,-1).gather(2,d_norm_min_idx.unsqueeze(-1).unsqueeze(-1).expand(-1,-1,-1,i+1))[:,:,0,:] 117 | grad_tensor[:,:,:i+1,i] = grad_min_q.transpose(0,1) 118 | d_tensor[:,:,i] = d_norm_min.transpose(0,1) 119 | 120 | d,d_min_idx = d_tensor.min(dim=-1) 121 | grad_final = grad_tensor.gather(3,d_min_idx.unsqueeze(-1).unsqueeze(-1).expand(-1,-1,7,7))[:,:,:,0] 122 | return d, grad_final 123 | 124 | def sample_q(self,batch_q = None): 125 | if batch_q is None: 126 | batch_q = self.batch_q 127 | q_sampled = self.panda.theta_min + torch.rand(batch_q,7).to(self.device)*(self.panda.theta_max-self.panda.theta_min) 128 | q_sampled.requires_grad = True 129 | return q_sampled 130 | 131 | def projection(self,q,d,grad): 132 | q_new = q - grad*d.unsqueeze(-1) 133 | return q_new 134 | 135 | def train_nn(self,epoches=500): 136 | 137 | # model 138 | # input: [x,q] (B,3+7) 139 | 140 | model = MLPRegression(input_dims=10, output_dims=1, mlp_layers=[1024, 512, 256, 128, 128],skips=[], act_fn=torch.nn.ReLU, nerf=True) 141 | model.to(device) 142 | optimizer = torch.optim.Adam(model.parameters(), lr=0.001) 143 | scheduler = torch.optim.lr_scheduler.ReduceLROnPlateau(optimizer, mode='min', factor=0.5, patience=5000, 144 | threshold=0.01, threshold_mode='rel', 145 | cooldown=0, min_lr=0, eps=1e-04, verbose=True) 146 | scaler = torch.cuda.amp.GradScaler(enabled=True) 147 | COSLOSS = torch.nn.CosineSimilarity(dim=1, eps=1e-6) 148 | model_dict = {} 149 | for iter in range(epoches): 150 | model.train() 151 | with torch.cuda.amp.autocast(): 152 | x_batch,q_batch,d,gt_grad = self.select_data() 153 | 154 | x_inputs = x_batch.unsqueeze(1).expand(-1,self.batch_q,-1).reshape(-1,3) 155 | q_inputs = q_batch.unsqueeze(0).expand(self.batch_x,-1,-1).reshape(-1,7) 156 | 157 | inputs = torch.cat([x_inputs,q_inputs],dim=-1) 158 | outputs = d.reshape(-1,1) 159 | gt_grad = gt_grad.reshape(-1,7) 160 | 161 | weights = torch.ones_like(outputs).to(device) 162 | # weights = (1/outputs).clamp(0,1) 163 | 164 | d_pred = model.forward(inputs) 165 | d_grad_pred = torch.autograd.grad(d_pred, q_inputs, torch.ones_like(d_pred), retain_graph=True,create_graph=True)[0] 166 | 167 | # Compute the Eikonal loss 168 | eikonal_loss = torch.abs(d_grad_pred.norm(2, dim=-1) - 1).mean() 169 | 170 | # Compute the tension loss 171 | dd_grad_pred = torch.autograd.grad(d_grad_pred, q_inputs, torch.ones_like(d_pred), retain_graph=True,create_graph=True)[0] 172 | 173 | # gradient loss 174 | gradient_loss = (1 - COSLOSS(d_grad_pred, gt_grad)).mean() 175 | # tension loss 176 | tension_loss = dd_grad_pred.square().sum(dim=-1).mean() 177 | # Compute the MSE loss 178 | d_loss = ((d_pred-outputs)**2*weights).mean() 179 | 180 | # Combine the two losses with appropriate weights 181 | w0 = 5.0 182 | w1 = 0.01 183 | w2 = 0.01 184 | w3 = 0.1 185 | loss = w0 * d_loss + w1 * eikonal_loss + w2 * tension_loss + w3 * gradient_loss 186 | 187 | # # Print the losses for monitoring 188 | 189 | scaler.scale(loss).backward() 190 | scaler.step(optimizer) 191 | scaler.update() 192 | optimizer.zero_grad() 193 | scheduler.step(loss) 194 | if iter % 10 == 0: 195 | print(f"Epoch:{iter}\tMSE Loss: {d_loss.item():.3f}\tEikonal Loss: {eikonal_loss.item():.3f}\tTension Loss: {tension_loss.item():.3f}\tGradient Loss: {gradient_loss.item():.3f}\tTotal loss:{loss.item():.3f}\tTime: {time.strftime('%H:%M:%S', time.gmtime())}") 196 | model_dict[iter] = model.state_dict() 197 | torch.save(model_dict, os.path.join(CUR_PATH,'model_dict.pt')) 198 | return model 199 | 200 | def inference(self,x,q,model): 201 | model.eval() 202 | x,q = x.to(self.device),q.to(self.device) 203 | # q.requires_grad = True 204 | x_cat = x.unsqueeze(1).expand(-1,len(q),-1).reshape(-1,3) 205 | q_cat = q.unsqueeze(0).expand(len(x),-1,-1).reshape(-1,7) 206 | inputs = torch.cat([x_cat,q_cat],dim=-1) 207 | cdf_pred = model.forward(inputs) 208 | return cdf_pred 209 | 210 | def inference_d_wrt_q(self,x,q,model,return_grad = True): 211 | cdf_pred = self.inference(x,q,model) 212 | d = cdf_pred.reshape(len(x),len(q)).min(dim=0)[0] 213 | if return_grad: 214 | grad = torch.autograd.grad(d,q,torch.ones_like(d),retain_graph=True,create_graph=True)[0] 215 | # dgrad = torch.autograd.grad(grad,q,torch.ones_like(grad),retain_graph=True,create_graph=True)[0] 216 | return d,grad 217 | else: 218 | return d 219 | 220 | def eval_nn(self,model,num_iter = 3): 221 | eval_time = False 222 | eval_acc = True 223 | if eval_time: 224 | x = torch.rand(100,3).to(device)-torch.tensor([[0.5,0.5,0]]).to(device) 225 | q = self.sample_q(batch_q=100) 226 | time_cost_list = [] 227 | for i in range(100): 228 | t0 = time.time() 229 | d = self.inference_d_wrt_q(x,q,model,return_grad = False) 230 | t1 = time.time() 231 | grad = torch.autograd.grad(d,q,torch.ones_like(d),retain_graph=True,create_graph=True)[0] 232 | q_proj = self.projection(q,d,grad) 233 | t2 = time.time() 234 | if i >0: 235 | time_cost_list.append([t1-t0,t2-t1]) 236 | mean_time_cost = np.mean(time_cost_list,axis=0) 237 | print(f'inference time cost:{mean_time_cost[0]}\t projection time cost: {mean_time_cost[1]}') 238 | 239 | if eval_acc: 240 | # bp_sdf model 241 | bp_sdf = bf_sdf.BPSDF(8,-1.0,1.0,self.panda,device) 242 | bp_sdf_model = torch.load(os.path.join(CUR_PATH,'../../RDF/models/BP_8.pt')) 243 | 244 | res = [] 245 | for i in range (1000): 246 | x = torch.rand(1,3).to(device)-torch.tensor([[0.5,0.5,0]]).to(device) 247 | q = self.sample_q(batch_q=1000) 248 | for _ in range (num_iter): 249 | d,grad = self.inference_d_wrt_q(x,q,model,return_grad = True) 250 | q = self.projection(q,d,grad) 251 | q,grad = q.detach(),grad.detach() # release memory 252 | pose = torch.eye(4).unsqueeze(0).expand(len(q),-1,-1).to(self.device).float() 253 | sdf,_ = bp_sdf.get_whole_body_sdf_batch(x, pose, q, bp_sdf_model,use_derivative=False) 254 | 255 | error = sdf.reshape(-1).abs() 256 | MAE = error.mean() 257 | RMSE = torch.sqrt(torch.mean(error**2)) 258 | SR = (error<0.03).sum().item()/len(error) 259 | res.append([MAE.item(),RMSE.item(),SR]) 260 | print(f'iter {i} finished, MAE:{MAE}\tRMSE:{RMSE}\tSR:{SR}') 261 | res = np.array(res) 262 | print(f'MAE:{res[:,0].mean()}\tRMSE:{res[:,1].mean()}\tSR:{res[:,2].mean()}') 263 | print(f'MAE:{res[:,0].std()}\tRMSE:{res[:,1].std()}\tSR:{res[:,2].std()}') 264 | 265 | def eval_nn_noise(self,model,num_iter = 3): 266 | bp_sdf = bf_sdf.BPSDF(8,-1.0,1.0,self.panda,device) 267 | bp_sdf_model = torch.load(os.path.join(CUR_PATH,'../../RDF/models/BP_8.pt')) 268 | 269 | res = [] 270 | for i in range (1000): 271 | x = torch.rand(1,3).to(device)-torch.tensor([[0.5,0.5,0]]).to(device) 272 | noise = torch.normal(0,0.03,(1,3)).to(device) 273 | x_noise = x + noise 274 | q = self.sample_q(batch_q=1000) 275 | for _ in range (num_iter): 276 | d,grad = self.inference_d_wrt_q(x_noise,q,model,return_grad = True) 277 | q = self.projection(q,d,grad) 278 | q,grad = q.detach(),grad.detach() # release memory 279 | pose = torch.eye(4).unsqueeze(0).expand(len(q),-1,-1).to(self.device).float() 280 | sdf,_ = bp_sdf.get_whole_body_sdf_batch(x, pose, q, bp_sdf_model,use_derivative=False) 281 | 282 | error = sdf.reshape(-1).abs() 283 | MAE = error.mean() 284 | RMSE = torch.sqrt(torch.mean(error**2)) 285 | SR = (error<0.03).sum().item()/len(error) 286 | res.append([MAE.item(),RMSE.item(),SR]) 287 | print(f'iter {i} finished, MAE:{MAE}\tRMSE:{RMSE}\tSR:{SR}') 288 | res = np.array(res) 289 | print(f'MAE:{res[:,0].mean()}\tRMSE:{res[:,1].mean()}\tSR:{res[:,2].mean()}') 290 | print(f'MAE:{res[:,0].std()}\tRMSE:{res[:,1].std()}\tSR:{res[:,2].std()}') 291 | 292 | 293 | if __name__ == "__main__": 294 | 295 | device = torch.device("cuda" if torch.cuda.is_available() else "cpu") 296 | # device = torch.device("cpu") 297 | cdf = CDF(device) 298 | 299 | # trainer.train_nn(epoches=20000) 300 | 301 | model = MLPRegression(input_dims=10, output_dims=1, mlp_layers=[1024, 512, 256, 128, 128],skips=[], act_fn=torch.nn.ReLU, nerf=True) 302 | # model.load_state_dict(torch.load(os.path.join(CUR_PATH,'model_dict.pt'))[19900]) 303 | model.load_state_dict(torch.load(os.path.join(CUR_PATH,'model_dict.pt'))[49900]) 304 | model.to(device) 305 | cdf.eval_nn(model) -------------------------------------------------------------------------------- /frankaemika/panda_urdf/LICENSE.txt: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS -------------------------------------------------------------------------------- /frankaemika/panda_urdf/meshes/collision/finger.obj: -------------------------------------------------------------------------------- 1 | # File produced by Open Asset Import Library (http://www.assimp.sf.net) 2 | # (assimp v3.0.1262) 3 | 4 | mtllib finger.stl.obj.mtl 5 | 6 | # 96 vertex positions 7 | v 0.01036 0.0264034 0.000154629 8 | v 0.0104486 0.0025833 0.000146801 9 | v -0.0103872 0.00253418 0.000131696 10 | v -0.0103872 0.00253418 0.000131696 11 | v -0.0104795 0.0161016 0.0189882 12 | v -0.0104013 0.0263094 0.00016651 13 | v -0.0104013 0.0263094 0.00016651 14 | v -0.0104795 0.0161016 0.0189882 15 | v -0.0103889 0.0252203 0.0191876 16 | v 0.0104486 0.0025833 0.000146801 17 | v -0.0087304 -2.35252e-05 0.0361648 18 | v -0.0103872 0.00253418 0.000131696 19 | v 0.0104005 0.0252534 0.0190366 20 | v -0.0103889 0.0252203 0.0191876 21 | v 0.00583983 0.0142743 0.0538035 22 | v -0.0103872 0.00253418 0.000131696 23 | v -0.0104013 0.0263094 0.00016651 24 | v 0.01036 0.0264034 0.000154629 25 | v 0.00861608 0.0139887 0.0513279 26 | v 0.0104948 0.0151026 0.0184356 27 | v 0.0104005 0.0252534 0.0190366 28 | v 0.0104948 0.0151026 0.0184356 29 | v 0.00869277 -0.000132643 0.0501662 30 | v 0.0104486 0.0025833 0.000146801 31 | v 0.00861608 0.0139887 0.0513279 32 | v 0.00869277 -0.000132643 0.0501662 33 | v 0.0104948 0.0151026 0.0184356 34 | v -0.00862294 -5.68019e-05 0.0509528 35 | v -0.0103872 0.00253418 0.000131696 36 | v -0.0087304 -2.35252e-05 0.0361648 37 | v 0.0104486 0.0025833 0.000146801 38 | v 0.00869277 -0.000132643 0.0501662 39 | v -0.0087304 -2.35252e-05 0.0361648 40 | v 0.00869277 -0.000132643 0.0501662 41 | v -0.00548142 -9.11208e-05 0.0537247 42 | v -0.0087304 -2.35252e-05 0.0361648 43 | v 0.0104005 0.0252534 0.0190366 44 | v 0.0104948 0.0151026 0.0184356 45 | v 0.0104486 0.0025833 0.000146801 46 | v 0.0104005 0.0252534 0.0190366 47 | v 0.0104486 0.0025833 0.000146801 48 | v 0.01036 0.0264034 0.000154629 49 | v 0.0104005 0.0252534 0.0190366 50 | v 0.01036 0.0264034 0.000154629 51 | v -0.0103889 0.0252203 0.0191876 52 | v -0.0103889 0.0252203 0.0191876 53 | v -0.00527792 0.0142931 0.053849 54 | v 0.00583983 0.0142743 0.0538035 55 | v -0.00777838 0.0142182 0.0523659 56 | v -0.00527792 0.0142931 0.053849 57 | v -0.0103889 0.0252203 0.0191876 58 | v -0.00862294 -5.68019e-05 0.0509528 59 | v -0.00548142 -9.11208e-05 0.0537247 60 | v -0.00777838 0.0142182 0.0523659 61 | v -0.0103872 0.00253418 0.000131696 62 | v -0.00862294 -5.68019e-05 0.0509528 63 | v -0.0104795 0.0161016 0.0189882 64 | v 0.0104005 0.0252534 0.0190366 65 | v 0.00583983 0.0142743 0.0538035 66 | v 0.00861608 0.0139887 0.0513279 67 | v 0.01036 0.0264034 0.000154629 68 | v -0.0104013 0.0263094 0.00016651 69 | v -0.0103889 0.0252203 0.0191876 70 | v -0.0104795 0.0161016 0.0189882 71 | v -0.00884117 0.0139176 0.0505894 72 | v -0.0103889 0.0252203 0.0191876 73 | v -0.00884117 0.0139176 0.0505894 74 | v -0.00777838 0.0142182 0.0523659 75 | v -0.0103889 0.0252203 0.0191876 76 | v 0.00583983 0.0142743 0.0538035 77 | v -0.00548142 -9.11208e-05 0.0537247 78 | v 0.00613802 -2.06026e-05 0.0535776 79 | v -0.00527792 0.0142931 0.053849 80 | v -0.00548142 -9.11208e-05 0.0537247 81 | v 0.00583983 0.0142743 0.0538035 82 | v 0.00869277 -0.000132643 0.0501662 83 | v 0.00613802 -2.06026e-05 0.0535776 84 | v -0.00548142 -9.11208e-05 0.0537247 85 | v 0.00613802 -2.06026e-05 0.0535776 86 | v 0.00869277 -0.000132643 0.0501662 87 | v 0.00861608 0.0139887 0.0513279 88 | v -0.00548142 -9.11208e-05 0.0537247 89 | v -0.00862294 -5.68019e-05 0.0509528 90 | v -0.0087304 -2.35252e-05 0.0361648 91 | v -0.00777838 0.0142182 0.0523659 92 | v -0.00548142 -9.11208e-05 0.0537247 93 | v -0.00527792 0.0142931 0.053849 94 | v -0.00862294 -5.68019e-05 0.0509528 95 | v -0.00884117 0.0139176 0.0505894 96 | v -0.0104795 0.0161016 0.0189882 97 | v 0.00613802 -2.06026e-05 0.0535776 98 | v 0.00861608 0.0139887 0.0513279 99 | v 0.00583983 0.0142743 0.0538035 100 | v -0.00777838 0.0142182 0.0523659 101 | v -0.00884117 0.0139176 0.0505894 102 | v -0.00862294 -5.68019e-05 0.0509528 103 | 104 | # 0 UV coordinates 105 | 106 | # 96 vertex normals 107 | vn 0.000724164 0.000331308 -1 108 | vn 0.000724164 0.000331308 -1 109 | vn 0.000724164 0.000331308 -1 110 | vn -0.99999 -0.000585782 -0.00447174 111 | vn -0.99999 -0.000585782 -0.00447174 112 | vn -0.99999 -0.000585782 -0.00447174 113 | vn -0.99995 0.00990875 0.00122006 114 | vn -0.99995 0.00990875 0.00122006 115 | vn -0.99995 0.00990875 0.00122006 116 | vn 0.00240304 -0.997479 -0.0709137 117 | vn 0.00240304 -0.997479 -0.0709137 118 | vn 0.00240304 -0.997479 -0.0709137 119 | vn 0.000668274 0.953556 0.301214 120 | vn 0.000668274 0.953556 0.301214 121 | vn 0.000668274 0.953556 0.301214 122 | vn -0.0005789 0.00146393 -0.999999 123 | vn -0.0005789 0.00146393 -0.999999 124 | vn -0.0005789 0.00146393 -0.999999 125 | vn 0.998344 0.00589157 0.0572229 126 | vn 0.998344 0.00589157 0.0572229 127 | vn 0.998344 0.00589157 0.0572229 128 | vn 0.998185 -0.050838 0.0322792 129 | vn 0.998185 -0.050838 0.0322792 130 | vn 0.998185 -0.050838 0.0322792 131 | vn 0.998371 0.000729252 0.0570496 132 | vn 0.998371 0.000729252 0.0570496 133 | vn 0.998371 0.000729252 0.0570496 134 | vn -0.871287 -0.490747 0.00522707 135 | vn -0.871287 -0.490747 0.00522707 136 | vn -0.871287 -0.490747 0.00522707 137 | vn 0.0362712 -0.99794 -0.0529128 138 | vn 0.0362712 -0.99794 -0.0529128 139 | vn 0.0362712 -0.99794 -0.0529128 140 | vn -0.00372285 -0.999988 -0.00316058 141 | vn -0.00372285 -0.999988 -0.00316058 142 | vn -0.00372285 -0.999988 -0.00316058 143 | vn 0.999909 0.00984192 -0.00926313 144 | vn 0.999909 0.00984192 -0.00926313 145 | vn 0.999909 0.00984192 -0.00926313 146 | vn 0.999991 0.00372285 -0.00191858 147 | vn 0.999991 0.00372285 -0.00191858 148 | vn 0.999991 0.00372285 -0.00191858 149 | vn -0.0011495 0.99815 0.0607926 150 | vn -0.0011495 0.99815 0.0607926 151 | vn -0.0011495 0.99815 0.0607926 152 | vn 0.00284562 0.953846 0.300284 153 | vn 0.00284562 0.953846 0.300284 154 | vn 0.00284562 0.953846 0.300284 155 | vn -0.218924 0.920873 0.32259 156 | vn -0.218924 0.920873 0.32259 157 | vn -0.218924 0.920873 0.32259 158 | vn -0.661425 -0.0350314 0.749193 159 | vn -0.661425 -0.0350314 0.749193 160 | vn -0.661425 -0.0350314 0.749193 161 | vn -0.998169 -0.0513123 0.0320353 162 | vn -0.998169 -0.0513123 0.0320353 163 | vn -0.998169 -0.0513123 0.0320353 164 | vn 0.377717 0.867563 0.323518 165 | vn 0.377717 0.867563 0.323518 166 | vn 0.377717 0.867563 0.323518 167 | vn -0.00448618 0.998355 0.0571685 168 | vn -0.00448618 0.998355 0.0571685 169 | vn -0.00448618 0.998355 0.0571685 170 | vn -0.998589 0.0087761 0.0523757 171 | vn -0.998589 0.0087761 0.0523757 172 | vn -0.998589 0.0087761 0.0523757 173 | vn -0.665951 0.690851 0.281485 174 | vn -0.665951 0.690851 0.281485 175 | vn -0.665951 0.690851 0.281485 176 | vn 0.0127505 -0.0155288 0.999798 177 | vn 0.0127505 -0.0155288 0.999798 178 | vn 0.0127505 -0.0155288 0.999798 179 | vn 0.00408502 -0.00870048 0.999954 180 | vn 0.00408502 -0.00870048 0.999954 181 | vn 0.00408502 -0.00870048 0.999954 182 | vn 0.006542 -0.999267 0.0377181 183 | vn 0.006542 -0.999267 0.0377181 184 | vn 0.006542 -0.999267 0.0377181 185 | vn 0.798906 -0.0450007 0.59977 186 | vn 0.798906 -0.0450007 0.59977 187 | vn 0.798906 -0.0450007 0.59977 188 | vn -0.00899609 -0.999957 -0.00218479 189 | vn -0.00899609 -0.999957 -0.00218479 190 | vn -0.00899609 -0.999957 -0.00218479 191 | vn -0.510144 -0.000216662 0.860089 192 | vn -0.510144 -0.000216662 0.860089 193 | vn -0.510144 -0.000216662 0.860089 194 | vn -0.998608 -0.0142745 0.0507836 195 | vn -0.998608 -0.0142745 0.0507836 196 | vn -0.998608 -0.0142745 0.0507836 197 | vn 0.665648 0.00209596 0.746263 198 | vn 0.665648 0.00209596 0.746263 199 | vn 0.665648 0.00209596 0.746263 200 | vn -0.858159 -4.90786e-05 0.513384 201 | vn -0.858159 -4.90786e-05 0.513384 202 | vn -0.858159 -4.90786e-05 0.513384 203 | 204 | # Mesh '' with 32 faces 205 | g 206 | usemtl DefaultMaterial 207 | f 1//1 2//2 3//3 208 | f 4//4 5//5 6//6 209 | f 7//7 8//8 9//9 210 | f 10//10 11//11 12//12 211 | f 13//13 14//14 15//15 212 | f 16//16 17//17 18//18 213 | f 19//19 20//20 21//21 214 | f 22//22 23//23 24//24 215 | f 25//25 26//26 27//27 216 | f 28//28 29//29 30//30 217 | f 31//31 32//32 33//33 218 | f 34//34 35//35 36//36 219 | f 37//37 38//38 39//39 220 | f 40//40 41//41 42//42 221 | f 43//43 44//44 45//45 222 | f 46//46 47//47 48//48 223 | f 49//49 50//50 51//51 224 | f 52//52 53//53 54//54 225 | f 55//55 56//56 57//57 226 | f 58//58 59//59 60//60 227 | f 61//61 62//62 63//63 228 | f 64//64 65//65 66//66 229 | f 67//67 68//68 69//69 230 | f 70//70 71//71 72//72 231 | f 73//73 74//74 75//75 232 | f 76//76 77//77 78//78 233 | f 79//79 80//80 81//81 234 | f 82//82 83//83 84//84 235 | f 85//85 86//86 87//87 236 | f 88//88 89//89 90//90 237 | f 91//91 92//92 93//93 238 | f 94//94 95//95 96//96 239 | 240 | -------------------------------------------------------------------------------- /frankaemika/panda_urdf/meshes/collision/hand.obj: -------------------------------------------------------------------------------- 1 | # File produced by Open Asset Import Library (http://www.assimp.sf.net) 2 | # (assimp v3.1.187496374) 3 | 4 | mtllib hand.obj.mtl 5 | 6 | # 102 vertex positions 7 | v 0.0167349 0.0901289 0.0644319 8 | v 0.0171343 -0.0892712 0.0636144 9 | v 0.0315898 -0.000558518 0.00621423 10 | v -0.0170537 -0.0955248 0.0221212 11 | v -0.0175104 -0.0979076 0.0516662 12 | v -0.0315975 -0.0019865 0.00115293 13 | v -0.025981 0.0757697 -0.00508223 14 | v -0.0316359 -0.000578733 0.00597307 15 | v 0.0170332 -0.0968023 0.030322 16 | v 0.0316158 -0.00186086 0.00117971 17 | v 0.0175851 -0.0974071 0.0519846 18 | v 0.0185559 0.0946019 0.0566173 19 | v -0.0168042 -0.0919061 0.00629548 20 | v -0.0168122 -0.0875329 -0.00120645 21 | v 0.0162301 -0.0995375 0.0515841 22 | v -4.10844e-05 -0.10399 0.00569088 23 | v -1.72733e-05 -0.103743 -0.0091993 24 | v -0.00361677 -0.101158 -0.0126103 25 | v -0.00342956 -0.102598 -0.0089643 26 | v -0.0252704 0.0820651 0.00519704 27 | v -0.0280109 0.0417843 -0.0063964 28 | v -0.000112569 0.0928085 0.0659622 29 | v -0.0125667 0.0937125 0.0651685 30 | v -0.014422 0.0912765 0.0656897 31 | v -0.0172547 -0.0907003 0.063082 32 | v -0.0154717 -0.0907063 0.0651901 33 | v -0.0167672 0.0902522 0.0643322 34 | v 0.0169555 -0.0916029 0.00556283 35 | v 0.0176072 -0.0938476 0.0584485 36 | v 0.0125057 -0.100188 0.0568941 37 | v 0.0155509 0.1002 0.0546726 38 | v 0.0192688 0.0984936 0.0449417 39 | v 0.0162184 0.100285 0.0458698 40 | v 0.00191744 -0.0455199 -0.0256594 41 | v 0.0280151 0.0420423 -0.0063496 42 | v 0.00381034 -0.0829662 -0.0244493 43 | v 0.0206709 0.0936918 0.0433083 44 | v 0.0254479 0.0811805 0.0035006 45 | v -0.00363682 -0.0835859 -0.0246063 46 | v -0.0165327 -0.0822363 -0.00511811 47 | v -0.0140171 -0.100091 0.0560269 48 | v -0.0160131 -0.0996286 0.0493014 49 | v -0.00321509 -0.087766 -0.0238844 50 | v -0.0129879 -0.0924958 0.0657119 51 | v -0.000505639 -0.0929453 0.0659531 52 | v 0.0135396 0.0926721 0.0656879 53 | v 0.0134248 -0.091927 0.0657313 54 | v -0.0192292 0.0972606 0.0505884 55 | v 0.0165582 -0.0851322 -0.00387274 56 | v 0.00349477 -0.102716 -0.00802934 57 | v 0.0238692 0.0904014 0.00436824 58 | v 0.0227078 0.0937356 0.01434 59 | v 0.0260343 0.0752976 -0.00503085 60 | v 0.0248167 0.0743775 -0.0071557 61 | v -0.00150015 -0.0458698 -0.0259248 62 | v -0.000833771 0.08026 -0.00732172 63 | v 0.0229591 0.0779014 -0.00739904 64 | v -0.0227611 0.0801744 -0.00690954 65 | v 3.18097e-05 -0.0994514 -0.0171612 66 | v -0.0166321 -0.098554 0.0547339 67 | v 0.00360213 -0.101195 -0.0126276 68 | v -0.00161673 0.0974711 0.0623073 69 | v 0.0157184 -0.0917796 0.0649125 70 | v -0.0184487 0.0920155 0.0583932 71 | v -0.000626994 -0.0879747 -0.0249616 72 | v 0.003076 -0.0879729 -0.0239787 73 | v 0.019802 0.0970111 0.020914 74 | v 0.00994155 0.100035 0.0583408 75 | v 0.0171295 0.0990672 0.0543052 76 | v -0.000314131 0.0946153 0.00757466 77 | v -1.59471e-05 0.0984961 0.0270818 78 | v -0.000337323 0.0865616 -0.00416328 79 | v 0.0223143 0.08825 -0.0019332 80 | v 0.022377 0.0833552 -0.00552611 81 | v -0.021325 0.0909269 0.000979802 82 | v -0.0227328 0.0854345 -0.00412033 83 | v -0.0244953 0.0858923 -0.00183273 84 | v 0.00184279 -0.0840023 -0.0255249 85 | v -0.0246502 0.076668 -0.00693213 86 | v 0.0131387 -0.0940629 0.0646832 87 | v -0.00932387 -0.0999826 0.0584892 88 | v -0.0190668 0.0976971 0.0246275 89 | v -0.00449439 0.100426 0.0581191 90 | v -0.0146231 0.100168 0.0557349 91 | v 0.0158049 -0.0994163 0.0549703 92 | v 0.0213426 0.0943331 0.00894201 93 | v -0.000173406 0.090995 0.000576784 94 | v 0.0214829 0.0917947 0.00268413 95 | v 0.0245152 0.0817026 -0.00505142 96 | v 0.0247294 0.0857401 -0.00123169 97 | v -0.0206576 0.0946679 0.00953798 98 | v 0.00119059 -0.100301 0.0586231 99 | v 0.000469602 -0.0966559 0.0630784 100 | v 0.0153969 0.0929056 0.0645563 101 | v -0.000867829 -0.0825485 -0.0258702 102 | v -0.0235558 0.0911663 0.00460792 103 | v -0.014617 -0.0934417 0.0645 104 | v -0.0230261 0.0928969 0.00987686 105 | v -0.0218221 0.0954417 0.0217564 106 | v -0.0181926 0.0995179 0.0438142 107 | v -0.0160455 0.0932093 0.0636169 108 | v -0.0169948 0.0993629 0.0541148 109 | 110 | # 0 UV coordinates 111 | 112 | # 200 vertex normals 113 | vn 0.96934 0.00103808 0.245721 114 | vn -0.986759 -0.159734 -0.0281361 115 | vn -0.997162 0.0697563 -0.0283202 116 | vn 0.986571 -0.160626 -0.0296212 117 | vn 0.99137 -0.125585 0.0376046 118 | vn 0.972565 0.0101093 0.232413 119 | vn -0.981257 -0.166939 -0.096261 120 | vn -0.991092 -0.129756 0.0299978 121 | vn -0.984842 -0.16507 -0.0532708 122 | vn 0.303856 -0.952595 -0.0153173 123 | vn -0.745916 -0.631674 -0.211181 124 | vn -0.996795 0.0769815 0.0217643 125 | vn -0.992516 0.0633187 -0.104418 126 | vn -0.044276 0.241169 0.969473 127 | vn -0.763533 -0.00240486 0.645764 128 | vn 0.98594 -0.162885 -0.0372967 129 | vn 0.986849 -0.0650596 0.147976 130 | vn 0.991549 -0.115042 0.0599665 131 | vn 0.203398 -0.97883 0.0228453 132 | vn 0.516573 0.854926 0.0474658 133 | vn 0.587131 0.00352045 -0.809484 134 | vn 0.99247 0.0777169 0.094674 135 | vn -0.832137 -0.113798 -0.542768 136 | vn -0.990867 -0.10289 0.0871544 137 | vn -0.24667 -0.969077 0.00656585 138 | vn -0.796752 -0.375025 -0.473859 139 | vn -0.0190316 -2.76701e-05 0.999819 140 | vn -0.0193168 -7.80837e-06 0.999813 141 | vn 0.0200855 -9.11851e-05 0.999798 142 | vn 0.0159035 0.000225346 0.999874 143 | vn -0.991348 0.0773534 0.106043 144 | vn 0.846324 -0.531423 -0.0364002 145 | vn 0.838336 -0.119317 -0.531936 146 | vn 0.713391 -0.685355 -0.146159 147 | vn 0.752915 -0.0507794 -0.656156 148 | vn 0.962439 0.234445 0.136915 149 | vn 0.98673 0.0540529 0.153111 150 | vn 0.984464 0.162241 0.0671482 151 | vn 0.963684 0.258201 0.0681518 152 | vn 0.988283 0.13579 0.0696969 153 | vn 0.992401 0.0632947 -0.105521 154 | vn 0.997132 0.0735636 0.0178061 155 | vn 0.997125 0.0744552 -0.0141125 156 | vn 0.525671 0.0308018 -0.85013 157 | vn 0.0112429 0.145846 -0.989244 158 | vn -0.0191625 0.145985 -0.989101 159 | vn 0.295801 0.090268 -0.950975 160 | vn -0.753521 -0.0512984 -0.655419 161 | vn -0.557064 -0.525003 -0.643468 162 | vn -0.826525 -0.175737 -0.534765 163 | vn -0.815256 -0.315571 -0.485564 164 | vn -0.947514 -0.156066 -0.279037 165 | vn -0.93384 -0.292644 0.205677 166 | vn 0.168928 -0.868066 -0.466825 167 | vn -0.713161 -0.698683 0.0569532 168 | vn -0.304509 -0.95237 -0.0162868 169 | vn 0.0219418 0.621147 0.783387 170 | vn -0.00551132 0.61581 0.787876 171 | vn 0.36612 0.000413018 0.930568 172 | vn 0.336199 1.25396e-05 0.941791 173 | vn -0.969753 -7.34049e-05 0.244089 174 | vn -0.962126 0.000708185 0.272604 175 | vn -0.988229 0.0614231 0.14011 176 | vn -0.360462 -0.538401 -0.761703 177 | vn 0.829279 -0.171621 -0.53183 178 | vn 0.677571 -0.00184298 0.735455 179 | vn 0.496348 0.867083 -0.0424839 180 | vn 0.0755333 0.722595 0.687133 181 | vn 0.904977 0.383975 0.183246 182 | vn 0.0127058 0.980664 -0.195286 183 | vn 0.851811 0.0713165 -0.518972 184 | vn 0.0351954 0.591653 -0.805424 185 | vn -0.538898 0.643215 -0.543933 186 | vn 0.0777842 -0.00363624 -0.996964 187 | vn -0.19873 0.11334 -0.973479 188 | vn -0.793118 -0.605993 -0.0611343 189 | vn 0.283571 -0.958905 -0.00945907 190 | vn -0.178475 -0.865689 -0.467685 191 | vn -0.987868 0.153186 -0.0255232 192 | vn -0.206645 -0.00149425 0.978415 193 | vn -0.501132 0.000514844 0.865371 194 | vn 0.0467749 -0.445101 0.894258 195 | vn -0.0152497 -0.997215 0.0730019 196 | vn -0.0160242 0.980702 -0.194854 197 | vn -0.10472 0.777309 0.620342 198 | vn -0.0162484 0.999109 -0.0389505 199 | vn 0.812943 -0.46373 -0.352249 200 | vn 0.734943 -0.648754 -0.197427 201 | vn 0.355505 -0.889833 -0.28603 202 | vn 0.77975 -0.493229 0.385637 203 | vn 0.264343 -0.962054 0.0676396 204 | vn 0.542077 -0.664141 0.514848 205 | vn 0.470493 -0.711825 0.52148 206 | vn 0.584351 -0.807368 -0.0817933 207 | vn 0.826668 -0.548978 0.123462 208 | vn 0.873315 -0.427986 0.232701 209 | vn 0.0134981 0.821385 0.570215 210 | vn 0.481279 -0.00399798 -0.876558 211 | vn 0.0262961 0.976262 -0.21499 212 | vn 0.035393 0.991525 -0.125004 213 | vn -0.0252692 0.468427 -0.883141 214 | vn 0.029533 0.795427 -0.605329 215 | vn 0.0095104 0.447459 -0.894254 216 | vn 0.0293922 0.327461 -0.944407 217 | vn 0.0628955 0.135567 -0.98877 218 | vn -0.512379 0.0361849 -0.857997 219 | vn -0.82575 0.0709576 -0.559555 220 | vn 0.671124 0.490386 -0.555981 221 | vn -0.583104 0.00463288 -0.812385 222 | vn -0.0162994 0.980639 -0.195146 223 | vn -0.157039 0.79873 0.580834 224 | vn 0.973711 -0.16665 -0.155286 225 | vn 0.0757908 -0.76565 0.638776 226 | vn -0.0242742 0.999674 0.00785851 227 | vn 0.0265568 0.999088 0.0334344 228 | vn 0.763663 0.633795 -0.122975 229 | vn 0.59401 0.799769 0.086723 230 | vn 0.277533 0.743174 0.608825 231 | vn 0.344643 0.752738 0.560898 232 | vn 0.784743 0.463667 0.411327 233 | vn -0.0358679 0.684935 -0.72772 234 | vn 0.932801 0.220325 -0.285201 235 | vn 0.624229 0.728724 -0.281602 236 | vn -0.0116186 0.888023 -0.459652 237 | vn 0.0118944 0.888215 -0.459274 238 | vn 0.12674 0.000708284 -0.991936 239 | vn -0.412606 -0.00846907 -0.91087 240 | vn -0.798626 -0.520306 -0.302452 241 | vn -0.541514 -0.834316 0.103337 242 | vn -0.319295 -0.94743 -0.0206758 243 | vn -0.723656 -0.67023 -0.164661 244 | vn 0.024012 -0.608318 0.79333 245 | vn -0.0315557 -0.776033 0.629902 246 | vn 0.0205919 -0.997403 0.0690183 247 | vn -0.0311286 -0.997047 0.0701972 248 | vn -0.982775 0.184638 -0.00783846 249 | vn 0.216124 -0.540324 -0.813229 250 | vn 0.246085 -0.284389 -0.926588 251 | vn 0.556314 -0.528433 -0.641306 252 | vn 0.792539 -0.37877 -0.477928 253 | vn 0.880372 -0.312129 0.3571 254 | vn 0.325605 -0.451322 0.830837 255 | vn 0.101105 -0.786037 0.609856 256 | vn 0.53596 -0.156588 -0.829594 257 | vn 0.580736 0.650676 0.489251 258 | vn 0.491562 0.198884 0.847828 259 | vn 0.828341 0.380744 0.410957 260 | vn 0.035746 0.926351 -0.374962 261 | vn 0.656777 0.749488 -0.083134 262 | vn 0.720531 0.692906 -0.0267625 263 | vn 0.572813 0.248018 -0.781263 264 | vn 0.990951 0.132224 -0.0230681 265 | vn 0.985773 0.167665 0.011866 266 | vn -0.0153618 0.730516 -0.682723 267 | vn 0.0128537 0.730055 -0.683268 268 | vn -0.0185446 0.449169 -0.893255 269 | vn -0.619205 -0.779582 -0.0940023 270 | vn -0.320427 -0.88655 -0.333701 271 | vn -0.0989435 -0.715546 0.691523 272 | vn -0.0841099 -0.712852 0.696252 273 | vn -0.60803 -0.644894 0.463045 274 | vn -0.423761 -0.343804 0.837989 275 | vn -0.605208 0.788757 -0.107633 276 | vn -0.0174541 0.996656 -0.0798282 277 | vn -0.0193589 0.998627 -0.0486783 278 | vn -0.634294 0.640925 -0.432304 279 | vn 0.0379495 -0.163387 -0.985832 280 | vn 0.538782 0.502971 -0.675821 281 | vn 0.813644 0.191216 -0.549017 282 | vn 0.652479 0.653287 -0.384041 283 | vn 0.717815 0.58535 -0.37697 284 | vn -0.286085 -0.763532 0.578942 285 | vn -0.0374327 -0.618147 0.785171 286 | vn -0.709079 -0.37258 0.598657 287 | vn -0.878489 0.351219 0.323886 288 | vn -0.890816 0.299973 0.341268 289 | vn -0.868133 0.372523 0.327981 290 | vn -0.605163 0.772391 -0.192849 291 | vn -0.984555 0.156402 0.0786764 292 | vn -0.982298 0.176886 0.06166 293 | vn -0.981595 0.187328 0.0371542 294 | vn -0.44589 0.832394 -0.329094 295 | vn -0.363723 0.729882 0.578773 296 | vn -0.575721 0.322294 0.751446 297 | vn 0.442358 0.333164 -0.832659 298 | vn -0.70625 0.351573 -0.614497 299 | vn -0.654528 0.356931 -0.666478 300 | vn -0.838844 0.274101 -0.47033 301 | vn -0.354702 0.933343 0.0552966 302 | vn -0.561743 0.820557 -0.105505 303 | vn -0.851741 0.52214 0.0436597 304 | vn -0.759618 0.642942 0.0980105 305 | vn -0.0360333 0.916716 -0.39791 306 | vn -0.560808 0.66826 0.488798 307 | vn -0.414234 0.480837 0.772791 308 | vn -0.390321 -0.168871 -0.905059 309 | vn -0.752648 -0.506645 0.420514 310 | vn -0.029743 0.995213 -0.0930902 311 | vn -0.346496 -0.193469 -0.917884 312 | vn -0.602128 0.796943 -0.0481963 313 | 314 | # Mesh '' with 200 faces 315 | g 316 | usemtl DefaultMaterial 317 | f 1//1 2//1 3//1 318 | f 4//2 5//2 6//2 319 | f 7//3 6//3 8//3 320 | f 9//4 10//4 11//4 321 | f 10//5 3//5 11//5 322 | f 12//6 1//6 3//6 323 | f 13//7 6//7 14//7 324 | f 8//8 6//8 5//8 325 | f 4//9 6//9 13//9 326 | f 15//10 16//10 17//10 327 | f 18//11 19//11 13//11 328 | f 20//12 7//12 8//12 329 | f 21//13 6//13 7//13 330 | f 22//14 23//14 24//14 331 | f 25//15 26//15 27//15 332 | f 28//16 10//16 9//16 333 | f 2//17 29//17 3//17 334 | f 29//18 11//18 3//18 335 | f 30//19 16//19 15//19 336 | f 31//20 32//20 33//20 337 | f 34//21 35//21 36//21 338 | f 37//22 3//22 38//22 339 | f 6//23 39//23 40//23 340 | f 25//24 8//24 5//24 341 | f 41//25 42//25 16//25 342 | f 18//26 14//26 43//26 343 | f 22//27 24//27 44//27 344 | f 44//28 45//28 22//28 345 | f 46//29 22//29 45//29 346 | f 45//30 47//30 46//30 347 | f 20//31 8//31 48//31 348 | f 9//32 11//32 15//32 349 | f 36//33 10//33 49//33 350 | f 50//34 28//34 9//34 351 | f 10//35 36//35 35//35 352 | f 12//36 37//36 32//36 353 | f 37//37 12//37 3//37 354 | f 37//38 38//38 51//38 355 | f 37//39 52//39 32//39 356 | f 52//40 37//40 51//40 357 | f 10//41 35//41 53//41 358 | f 10//42 53//42 38//42 359 | f 10//43 38//43 3//43 360 | f 54//44 35//44 34//44 361 | f 55//45 56//45 57//45 362 | f 56//46 55//46 58//46 363 | f 57//47 54//47 34//47 364 | f 39//48 6//48 21//48 365 | f 59//49 18//49 43//49 366 | f 40//50 39//50 43//50 367 | f 14//51 40//51 43//51 368 | f 14//52 6//52 40//52 369 | f 60//53 25//53 5//53 370 | f 59//54 61//54 17//54 371 | f 42//55 60//55 5//55 372 | f 16//56 42//56 17//56 373 | f 62//57 22//57 46//57 374 | f 22//58 62//58 23//58 375 | f 1//59 46//59 63//59 376 | f 47//60 63//60 46//60 377 | f 25//61 64//61 8//61 378 | f 64//62 25//62 27//62 379 | f 64//63 48//63 8//63 380 | f 65//64 59//64 43//64 381 | f 66//65 36//65 49//65 382 | f 63//66 2//66 1//66 383 | f 33//67 32//67 67//67 384 | f 68//68 62//68 46//68 385 | f 32//69 69//69 12//69 386 | f 70//70 71//70 67//70 387 | f 53//71 35//71 54//71 388 | f 72//72 73//72 74//72 389 | f 75//73 76//73 77//73 390 | f 78//74 55//74 34//74 391 | f 55//75 79//75 58//75 392 | f 4//76 42//76 5//76 393 | f 50//77 15//77 17//77 394 | f 59//78 17//78 18//78 395 | f 7//79 20//79 77//79 396 | f 24//80 26//80 44//80 397 | f 27//81 26//81 24//81 398 | f 45//82 80//82 47//82 399 | f 41//83 16//83 81//83 400 | f 82//84 71//84 70//84 401 | f 62//85 83//85 23//85 402 | f 33//86 84//86 83//86 403 | f 49//87 28//87 61//87 404 | f 61//88 28//88 50//88 405 | f 61//89 50//89 17//89 406 | f 63//90 85//90 29//90 407 | f 85//91 30//91 15//91 408 | f 80//92 85//92 63//92 409 | f 85//93 80//93 30//93 410 | f 9//94 15//94 50//94 411 | f 85//95 15//95 11//95 412 | f 11//96 29//96 85//96 413 | f 83//97 62//97 68//97 414 | f 34//98 36//98 78//98 415 | f 70//99 67//99 86//99 416 | f 71//100 33//100 67//100 417 | f 58//101 76//101 72//101 418 | f 73//102 87//102 88//102 419 | f 72//103 74//103 56//103 420 | f 56//104 74//104 57//104 421 | f 34//105 55//105 57//105 422 | f 21//106 79//106 55//106 423 | f 21//107 7//107 79//107 424 | f 89//108 73//108 90//108 425 | f 39//109 21//109 55//109 426 | f 91//110 82//110 70//110 427 | f 84//111 23//111 83//111 428 | f 28//112 49//112 10//112 429 | f 92//113 80//113 93//113 430 | f 31//114 33//114 68//114 431 | f 68//115 33//115 83//115 432 | f 51//116 86//116 52//116 433 | f 69//117 32//117 31//117 434 | f 46//118 94//118 68//118 435 | f 94//119 31//119 68//119 436 | f 94//120 12//120 69//120 437 | f 76//121 75//121 72//121 438 | f 90//122 53//122 89//122 439 | f 86//123 51//123 88//123 440 | f 75//124 70//124 87//124 441 | f 87//125 70//125 88//125 442 | f 95//126 55//126 78//126 443 | f 39//127 55//127 95//127 444 | f 14//128 18//128 13//128 445 | f 42//129 41//129 60//129 446 | f 19//130 17//130 42//130 447 | f 19//131 4//131 13//131 448 | f 45//132 93//132 80//132 449 | f 92//133 93//133 81//133 450 | f 16//134 30//134 92//134 451 | f 92//135 81//135 16//135 452 | f 96//136 77//136 20//136 453 | f 66//137 59//137 65//137 454 | f 65//138 78//138 66//138 455 | f 59//139 66//139 61//139 456 | f 49//140 61//140 66//140 457 | f 2//141 63//141 29//141 458 | f 80//142 63//142 47//142 459 | f 80//143 92//143 30//143 460 | f 78//144 36//144 66//144 461 | f 94//145 69//145 31//145 462 | f 94//146 46//146 1//146 463 | f 12//147 94//147 1//147 464 | f 70//148 86//148 88//148 465 | f 67//149 52//149 86//149 466 | f 52//150 67//150 32//150 467 | f 57//151 89//151 54//151 468 | f 53//152 90//152 38//152 469 | f 90//153 51//153 38//153 470 | f 75//154 87//154 72//154 471 | f 72//155 87//155 73//155 472 | f 56//156 58//156 72//156 473 | f 42//157 4//157 19//157 474 | f 19//158 18//158 17//158 475 | f 97//159 81//159 44//159 476 | f 81//160 93//160 44//160 477 | f 41//161 97//161 60//161 478 | f 26//162 97//162 44//162 479 | f 98//163 99//163 91//163 480 | f 100//164 33//164 71//164 481 | f 84//165 33//165 100//165 482 | f 96//166 75//166 77//166 483 | f 95//167 78//167 65//167 484 | f 73//168 89//168 74//168 485 | f 53//169 54//169 89//169 486 | f 73//170 88//170 51//170 487 | f 90//171 73//171 51//171 488 | f 41//172 81//172 97//172 489 | f 44//173 93//173 45//173 490 | f 25//174 97//174 26//174 491 | f 101//175 48//175 64//175 492 | f 64//176 27//176 101//176 493 | f 102//177 48//177 101//177 494 | f 96//178 98//178 91//178 495 | f 48//179 99//179 20//179 496 | f 99//180 98//180 20//180 497 | f 98//181 96//181 20//181 498 | f 96//182 91//182 75//182 499 | f 84//183 101//183 23//183 500 | f 27//184 24//184 101//184 501 | f 89//185 57//185 74//185 502 | f 77//186 76//186 79//186 503 | f 79//187 76//187 58//187 504 | f 79//188 7//188 77//188 505 | f 102//189 84//189 100//189 506 | f 91//190 99//190 82//190 507 | f 48//191 100//191 99//191 508 | f 100//192 48//192 102//192 509 | f 75//193 91//193 70//193 510 | f 84//194 102//194 101//194 511 | f 24//195 23//195 101//195 512 | f 43//196 95//196 65//196 513 | f 25//197 60//197 97//197 514 | f 82//198 100//198 71//198 515 | f 43//199 39//199 95//199 516 | f 100//200 82//200 99//200 517 | 518 | -------------------------------------------------------------------------------- /frankaemika/panda_urdf/meshes/collision/link0.obj: -------------------------------------------------------------------------------- 1 | # File produced by Open Asset Import Library (http://www.assimp.sf.net) 2 | # (assimp v3.1.187496374) 3 | 4 | mtllib link0.obj.mtl 5 | 6 | # 102 vertex positions 7 | v -0.00122674 -0.0946137 -3.24928e-05 8 | v 0.0710809 0.00844602 0.00133993 9 | v 0.071567 -0.00127396 0.0013494 10 | v -0.00051723 0.0946704 -6.42576e-06 11 | v 0.0706646 -0.0115626 0.00133336 12 | v -0.135131 -0.0658707 0.00751985 13 | v -0.136305 -0.0659457 3.26395e-07 14 | v -0.131625 -0.0687992 -5.27309e-07 15 | v 0.0543353 -0.0469489 0.00104792 16 | v 0.0619449 -0.0359785 0.00118116 17 | v 0.0673593 -0.0245271 0.00127621 18 | v 0.0526517 -0.0163561 0.14 19 | v 0.0548547 0.00447165 0.14 20 | v -0.0549666 0.00345713 0.14 21 | v 0.0391015 0.0387489 0.14 22 | v 0.00777804 -0.0910717 0.00846191 23 | v 0.0395665 -0.0384696 0.140003 24 | v -0.029337 -0.0466451 0.140001 25 | v 0.000349224 -0.0550819 0.14 26 | v 0.0686186 0.0208178 0.00129774 27 | v 0.0635871 0.0329931 0.00120988 28 | v 0.0525783 0.048771 0.00101755 29 | v 0.05859 0.0412413 0.00112268 30 | v 0.0509475 0.0207886 0.14 31 | v -0.0218528 0.0506849 0.14 32 | v -0.0317059 0.0450099 0.14 33 | v 0.0317161 0.0450507 0.14 34 | v -0.0400853 0.0378337 0.14 35 | v -0.0477037 0.0275963 0.14 36 | v 0.00485769 0.0549582 0.14 37 | v 0.0474762 -0.0279259 0.14 38 | v -0.00886536 0.0543586 0.140002 39 | v 0.0276805 -0.0476799 0.14 40 | v -0.054766 -0.0054659 0.14 41 | v -0.0528838 -0.015585 0.14 42 | v -0.122615 0.00563594 0.0611487 43 | v -0.122711 -0.00614431 0.060957 44 | v 0.0143701 -0.0532489 0.139999 45 | v -0.071675 -0.0756205 0.0575204 46 | v -0.111235 -0.0474773 0.0590551 47 | v -0.137824 -0.0624363 0.00755269 48 | v -0.133699 0.0669321 0.00741502 49 | v -0.13583 0.0663151 8.33834e-07 50 | v -0.136293 0.0646171 0.00756638 51 | v -0.131612 0.0688472 7.4968e-07 52 | v 0.0031716 0.0933497 0.00863144 53 | v 0.00408648 0.0939818 0.000170711 54 | v 0.0462474 0.0300326 0.14 55 | v 0.0548768 -0.00456585 0.14 56 | v -0.129159 0.0686836 0.00744486 57 | v -0.0528609 0.0157105 0.14 58 | v 0.0200202 0.0514462 0.139999 59 | v 0.00648893 0.0735192 0.08108 60 | v 0.00826788 -0.0918247 0.00024289 61 | v -0.0464371 -0.0299037 0.14 62 | v -0.140043 0.0607227 -1.6486e-07 63 | v 0.00769447 0.0922715 0.000234291 64 | v -0.00159938 0.0936322 0.00849181 65 | v 0.00734398 0.0914466 0.00831775 66 | v -0.148808 -0.0420547 -7.21858e-08 67 | v -0.149244 -0.0371861 0.00781712 68 | v -0.152578 -0.0248176 1.68373e-07 69 | v 0.00907648 0.0737151 0.0784549 70 | v -0.121673 -0.0147367 0.0614178 71 | v 0.00678171 -0.0735316 0.0809757 72 | v -0.130951 -0.0681401 0.00766805 73 | v -0.0189172 -0.051755 0.14 74 | v -0.00908423 -0.0542971 0.14 75 | v -0.0658985 -0.0765477 0.0589793 76 | v -0.0376956 -0.0401377 0.14 77 | v -0.143472 -0.0548617 -4.38612e-08 78 | v -0.145452 -0.0485961 0.00768247 79 | v -0.1203 -0.0235449 0.0615099 80 | v -0.118609 -0.03105 0.06134 81 | v -0.114761 -0.0419663 0.0601784 82 | v -0.154079 -0.00554168 -3.75503e-07 83 | v -0.152725 -0.0137992 0.00819143 84 | v -0.153166 -0.00203576 0.00835078 85 | v -0.142117 0.0555469 0.00762839 86 | v 0.0535544 0.0128185 0.14 87 | v -0.109983 0.0486112 0.0588796 88 | v 0.00366946 -0.0932492 0.00847566 89 | v 0.00349566 -0.0942157 0.000177738 90 | v 0.00906588 -0.073708 0.0785962 91 | v -0.117004 -0.0366378 0.0608364 92 | v -0.151587 -0.0248333 0.00804774 93 | v -0.15241 0.0258275 -1.09161e-08 94 | v -0.153841 0.0105757 0 95 | v -0.15202 0.0217804 0.00810954 96 | v -0.121149 0.0181378 0.0615703 97 | v -0.113238 0.0447149 0.0596848 98 | v -0.000866581 -0.0937369 0.00817823 99 | v -0.141842 -0.0559592 0.00761703 100 | v -0.149634 0.0355816 0.00786878 101 | v -0.14929 0.0402499 3.01802e-07 102 | v -0.152949 0.0100689 0.00821985 103 | v -0.145377 0.0508924 -1.27878e-07 104 | v -0.11571 0.0395945 0.0607111 105 | v -0.0231112 -0.0759725 0.0689683 106 | v -0.117233 0.035881 0.060957 107 | v -0.146172 0.0467529 0.00770437 108 | v -0.119414 0.0287903 0.0611397 109 | 110 | # 0 UV coordinates 111 | 112 | # 200 vertex normals 113 | vn 0.0190098 -2.30831e-05 -0.999819 114 | vn 0.0188816 6.69136e-05 -0.999822 115 | vn 0.0191344 -0.000120273 -0.999817 116 | vn -0.518479 -0.850405 0.0893954 117 | vn 0.0222843 -0.00331509 -0.999746 118 | vn 0.0205047 -0.00139704 -0.999789 119 | vn 7.25974e-07 -5.81537e-06 1 120 | vn 9.85433e-07 -5.68784e-06 1 121 | vn 0.991829 0.0497189 0.11749 122 | vn 0.692904 -0.711464 0.11706 123 | vn 4.45103e-05 3.61862e-05 1 124 | vn 0.0193277 0.000437566 -0.999813 125 | vn 0.0199677 0.0010368 -0.9998 126 | vn 0.0232843 0.0046319 -0.999718 127 | vn 0.0210964 0.0022106 -0.999775 128 | vn 0.0197075 -0.000616531 -0.999806 129 | vn 6.42989e-05 2.18738e-05 1 130 | vn 0.91755 0.380023 0.116978 131 | vn 3.35708e-05 -1.87847e-05 1 132 | vn -7.63878e-06 1.53928e-05 1 133 | vn -8.50133e-06 1.24836e-05 1 134 | vn 8.62742e-06 2.71648e-06 1 135 | vn 4.31916e-06 1.10982e-05 1 136 | vn 0.000248513 -0.00151077 0.999999 137 | vn -0.000100683 -0.000160507 1 138 | vn 0.000441559 2.26903e-05 1 139 | vn -0.759017 -0.00436697 0.651056 140 | vn -0.758153 -0.0170388 0.651854 141 | vn -4.08147e-05 -6.51499e-05 1 142 | vn 3.63574e-05 2.96612e-05 1 143 | vn -0.488489 -0.714003 0.501575 144 | vn -0.779928 -0.612675 0.127831 145 | vn -0.6563 0.74377 0.126791 146 | vn -0.51319 0.854867 0.0764032 147 | vn 0.143998 0.98555 0.0891981 148 | vn 6.6605e-05 0.00021454 1 149 | vn 0.794096 -0.595678 0.120749 150 | vn 0.815277 -0.566949 0.117862 151 | vn 0.897326 -0.425249 0.118194 152 | vn 0.906239 -0.40539 0.119958 153 | vn 0.848855 0.515505 0.117047 154 | vn 0.992823 0.00242477 0.11957 155 | vn 0.000137166 -1.78568e-05 1 156 | vn -0.256878 0.90791 0.331229 157 | vn -4.88056e-05 2.05051e-05 1 158 | vn 1.50367e-05 -7.30032e-06 1 159 | vn -3.22116e-05 6.85885e-05 1 160 | vn -0.0415945 0.953296 0.29916 161 | vn 0.0339572 -0.0169327 -0.99928 162 | vn -9.01903e-05 -3.23136e-05 1 163 | vn -0.795774 0.599474 0.0858742 164 | vn 0.423964 0.898605 0.112979 165 | vn 0.047941 0.0640793 -0.996793 166 | vn 0.0548988 0.990257 0.12797 167 | vn 0.0494285 0.963916 0.261577 168 | vn 0.0344071 0.0175091 -0.999255 169 | vn 0.691328 0.715168 0.102958 170 | vn 0.691451 0.712649 0.118438 171 | vn -0.973898 -0.213048 0.0783147 172 | vn 7.42399e-05 3.32698e-05 1 173 | vn 0.884937 0.449942 0.120157 174 | vn 0.989315 -0.0869511 0.117026 175 | vn 0.962281 -0.245852 0.116503 176 | vn -0.192564 0.977601 0.0849446 177 | vn -0.191831 0.976828 0.0949088 178 | vn -0.123193 0.952732 0.277713 179 | vn -0.441485 0.766549 0.466362 180 | vn 0.467327 0.85467 0.22615 181 | vn 0.692847 -0.713157 0.106629 182 | vn -0.756883 -0.0565346 0.651101 183 | vn 0.123333 -0.943265 0.308285 184 | vn -0.493923 -0.720807 0.48629 185 | vn -0.477017 -0.8711 0.116787 186 | vn -0.44611 -0.794944 0.411157 187 | vn -6.11448e-05 -0.000331755 1 188 | vn -1.43156e-05 -8.03482e-05 1 189 | vn -0.228986 -0.885719 0.403816 190 | vn -0.608389 -0.519659 0.599848 191 | vn -0.920452 -0.383492 0.0755124 192 | vn -0.736949 -0.180808 0.651318 193 | vn -0.611742 -0.514007 0.601306 194 | vn 0.419302 0.901141 0.110141 195 | vn -0.991437 -0.0387918 0.124694 196 | vn -0.993276 -0.0773118 0.0861707 197 | vn -0.833082 0.535892 0.137092 198 | vn 8.17296e-05 2.2162e-05 1 199 | vn 0.943552 0.308615 0.120276 200 | vn 0.981039 0.152834 0.119183 201 | vn 0.973953 0.194242 0.116982 202 | vn 0.767761 0.629431 0.119836 203 | vn 0.775121 0.620511 0.118969 204 | vn 0.975511 -0.184098 0.120356 205 | vn -0.334118 0.859344 0.38716 206 | vn -0.186878 0.944088 0.271613 207 | vn -0.461627 0.746886 0.478605 208 | vn 0.418676 0.882839 0.21285 209 | vn 0.516842 0.832916 0.197804 210 | vn 0.465813 -0.878241 0.10822 211 | vn 0.444917 -0.890581 0.0944139 212 | vn 0.0505572 -0.0737673 -0.995993 213 | vn 0.458814 -0.864421 0.205587 214 | vn 0.516064 -0.83361 0.196908 215 | vn -0.387155 -0.789577 0.476109 216 | vn -0.244693 -0.878338 0.410666 217 | vn -0.208468 -0.972704 0.101918 218 | vn -0.193464 -0.977355 0.0857284 219 | vn -0.182203 -0.978184 0.0997879 220 | vn -0.519874 -0.667682 0.532852 221 | vn -0.000155222 -6.31553e-05 1 222 | vn -4.55781e-05 -2.14887e-05 1 223 | vn -0.773654 -0.387569 0.501247 224 | vn -0.836842 -0.541097 0.0831258 225 | vn -0.938711 -0.313668 0.142944 226 | vn 0.642112 0.752523 0.146293 227 | vn -0.975003 -0.187197 0.119691 228 | vn -0.98721 -0.103388 0.121358 229 | vn -0.991212 0.0930191 0.0940533 230 | vn -0.745994 0.128207 0.653495 231 | vn -0.357321 0.923722 0.138052 232 | vn -2.44058e-05 1.82076e-05 1 233 | vn -0.562279 0.66248 0.494937 234 | vn -0.609088 0.612822 0.50345 235 | vn 0.183429 0.950262 0.251704 236 | vn 0.21534 0.929691 0.298837 237 | vn 0.229004 0.927667 0.294942 238 | vn 0.604019 -0.779541 0.165763 239 | vn 0.230026 -0.929068 0.289689 240 | vn 0.372231 -0.889715 0.264293 241 | vn -0.171199 -0.978844 0.112047 242 | vn 0.0790207 -0.991596 0.102437 243 | vn -0.774998 -0.384377 0.501629 244 | vn -0.890148 -0.437507 0.127373 245 | vn -0.67661 -0.535887 0.505 246 | vn -0.844574 -0.525041 0.105012 247 | vn -0.695542 -0.313148 0.646653 248 | vn -0.714122 -0.263551 0.648514 249 | vn -0.974069 0.210719 0.082385 250 | vn -0.866667 0.0208868 0.498449 251 | vn -0.993956 0.0146296 0.108802 252 | vn -0.994723 0.0188777 0.100843 253 | vn -0.990466 0.0795982 0.112429 254 | vn -0.875827 0.475205 0.0843008 255 | vn -0.648594 0.437886 0.622561 256 | vn -0.631999 0.470323 0.615934 257 | vn -0.542549 0.633495 0.551656 258 | vn -0.569254 0.593159 0.569309 259 | vn -0.854631 -0.127937 0.503228 260 | vn 0.187376 -0.949866 0.250288 261 | vn -0.0299751 -0.962324 0.270247 262 | vn -0.733474 -0.46004 0.500378 263 | vn -0.819413 -0.278244 0.501142 264 | vn -0.818885 -0.280329 0.500842 265 | vn -0.840131 -0.200724 0.503876 266 | vn -0.865477 -0.0010483 0.500948 267 | vn -0.861767 -0.077237 0.501391 268 | vn -0.734892 0.455474 0.502471 269 | vn -0.789187 0.35685 0.499842 270 | vn -0.899886 0.416094 0.130652 271 | vn -0.934904 0.343772 0.0881805 272 | vn -0.750321 -0.110129 0.651836 273 | vn -0.744296 -0.138475 0.653336 274 | vn -0.0486186 -0.96337 0.263731 275 | vn -0.0569915 -0.952558 0.298973 276 | vn -0.0786935 -0.945844 0.314939 277 | vn 0.0865387 -0.962264 0.257989 278 | vn 0.0988769 -0.988653 0.113084 279 | vn -0.040165 -0.954927 0.294112 280 | vn -0.679455 -0.364641 0.636692 281 | vn -0.676925 -0.535527 0.504959 282 | vn -0.860556 -0.0952615 0.500368 283 | vn -0.866101 -0.0391639 0.498333 284 | vn -0.848661 -0.170347 0.500757 285 | vn -0.976331 0.171097 0.132301 286 | vn -0.784297 0.365988 0.500931 287 | vn -0.743675 0.147478 0.652072 288 | vn -0.75636 0.0667582 0.650741 289 | vn -0.726694 0.470061 0.500957 290 | vn -0.946042 0.295169 0.133715 291 | vn -0.699993 0.303725 0.646344 292 | vn -0.690574 0.326044 0.645603 293 | vn -0.722223 0.238827 0.64912 294 | vn -0.862635 0.0731211 0.500513 295 | vn -0.860436 0.0839585 0.502594 296 | vn -0.823144 0.266065 0.501641 297 | vn -0.850508 0.155797 0.502359 298 | vn -0.850275 0.158766 0.501823 299 | vn -0.824425 0.262885 0.501214 300 | vn -0.000104827 0.000138107 -1 301 | vn -6.4345e-05 4.8788e-05 -1 302 | vn -0.000255153 -5.05889e-05 -1 303 | vn 8.56579e-06 1.35717e-06 -1 304 | vn 3.45225e-06 -5.63502e-06 -1 305 | vn 0.000304041 -4.54974e-06 -1 306 | vn 0.0004587 -0.000166978 -1 307 | vn -4.30629e-05 3.84989e-05 -1 308 | vn -0.000206776 -3.99625e-05 -1 309 | vn -7.87823e-05 -3.27836e-06 -1 310 | vn -0.000409956 0.000110369 -1 311 | vn 6.31049e-05 7.3988e-06 -1 312 | vn -0.0003035 2.77655e-05 -1 313 | 314 | # Mesh '' with 200 faces 315 | g 316 | usemtl DefaultMaterial 317 | f 1//1 2//1 3//1 318 | f 4//2 2//2 1//2 319 | f 3//3 5//3 1//3 320 | f 6//4 7//4 8//4 321 | f 9//5 1//5 10//5 322 | f 1//6 11//6 10//6 323 | f 12//7 13//7 14//7 324 | f 13//7 15//8 14//7 325 | f 2//9 13//9 3//9 326 | f 16//10 9//10 17//10 327 | f 14//11 18//11 19//11 328 | f 20//12 2//12 4//12 329 | f 21//13 20//13 4//13 330 | f 22//14 23//14 4//14 331 | f 23//15 21//15 4//15 332 | f 1//16 5//16 11//16 333 | f 13//17 24//17 15//17 334 | f 20//18 21//18 24//18 335 | f 25//19 26//19 14//19 336 | f 15//20 27//20 14//20 337 | f 28//21 29//21 14//20 338 | f 14//22 30//22 25//22 339 | f 14//22 31//23 12//23 340 | f 32//24 25//24 30//24 341 | f 33//25 17//25 14//25 342 | f 34//26 35//26 14//26 343 | f 36//27 37//27 14//27 344 | f 14//28 37//28 34//28 345 | f 38//29 33//29 14//29 346 | f 19//30 38//30 14//30 347 | f 39//31 18//31 40//31 348 | f 7//32 6//32 41//32 349 | f 42//33 43//33 44//33 350 | f 45//34 43//34 42//34 351 | f 4//35 46//35 47//35 352 | f 17//36 31//36 14//36 353 | f 9//37 31//37 17//37 354 | f 10//38 31//38 9//38 355 | f 11//39 31//39 10//39 356 | f 12//40 31//40 11//40 357 | f 21//41 23//41 48//41 358 | f 13//42 49//42 3//42 359 | f 12//43 49//43 13//43 360 | f 25//44 32//44 50//44 361 | f 14//45 29//45 51//45 362 | f 52//46 30//46 14//46 363 | f 27//47 52//47 14//47 364 | f 53//48 32//48 30//48 365 | f 9//49 54//49 1//49 366 | f 35//50 55//50 14//50 367 | f 56//51 44//51 43//51 368 | f 57//52 47//52 46//52 369 | f 4//53 47//53 57//53 370 | f 58//54 46//54 4//54 371 | f 53//55 46//55 58//55 372 | f 57//56 22//56 4//56 373 | f 57//57 59//57 22//57 374 | f 59//58 15//58 22//58 375 | f 60//59 61//59 62//59 376 | f 24//60 48//60 15//60 377 | f 24//61 21//61 48//61 378 | f 49//62 5//62 3//62 379 | f 5//63 12//63 11//63 380 | f 50//64 4//64 45//64 381 | f 4//65 50//65 58//65 382 | f 32//66 53//66 58//66 383 | f 42//67 26//67 25//67 384 | f 52//68 27//68 63//68 385 | f 9//69 16//69 54//69 386 | f 37//70 64//70 34//70 387 | f 38//71 19//71 65//71 388 | f 40//72 6//72 39//72 389 | f 8//73 66//73 6//73 390 | f 6//74 66//74 39//74 391 | f 18//75 67//75 19//75 392 | f 68//76 19//76 67//76 393 | f 67//77 69//77 68//77 394 | f 40//78 70//78 55//78 395 | f 71//79 72//79 60//79 396 | f 73//80 74//80 35//80 397 | f 55//81 75//81 40//81 398 | f 57//82 46//82 59//82 399 | f 76//83 77//83 78//83 400 | f 62//84 77//84 76//84 401 | f 79//85 44//85 56//85 402 | f 80//86 24//86 13//86 403 | f 24//87 80//87 20//87 404 | f 2//88 80//88 13//88 405 | f 20//89 80//89 2//89 406 | f 15//90 48//90 22//90 407 | f 22//91 48//91 23//91 408 | f 49//92 12//92 5//92 409 | f 25//93 50//93 42//93 410 | f 58//94 50//94 32//94 411 | f 42//95 81//95 26//95 412 | f 59//96 46//96 63//96 413 | f 59//97 63//97 27//97 414 | f 82//98 54//98 16//98 415 | f 82//99 83//99 54//99 416 | f 54//100 83//100 1//100 417 | f 16//101 84//101 82//101 418 | f 33//102 84//102 16//102 419 | f 39//103 67//103 18//103 420 | f 69//104 67//104 39//104 421 | f 8//105 39//105 66//105 422 | f 1//106 39//106 8//106 423 | f 69//107 39//107 1//107 424 | f 40//108 18//108 70//108 425 | f 70//109 18//109 14//109 426 | f 55//110 70//110 14//110 427 | f 72//111 75//111 85//111 428 | f 7//112 41//112 71//112 429 | f 60//113 72//113 61//113 430 | f 59//114 27//114 15//114 431 | f 62//115 61//115 86//115 432 | f 86//116 77//116 62//116 433 | f 87//117 88//117 89//117 434 | f 51//118 90//118 14//118 435 | f 42//119 50//119 45//119 436 | f 14//120 26//120 28//120 437 | f 42//121 44//121 81//121 438 | f 44//122 91//122 81//122 439 | f 53//123 63//123 46//123 440 | f 52//124 53//124 30//124 441 | f 63//125 53//125 52//125 442 | f 33//126 16//126 17//126 443 | f 65//127 84//127 38//127 444 | f 33//128 38//128 84//128 445 | f 69//129 1//129 92//129 446 | f 83//130 92//130 1//130 447 | f 75//131 72//131 93//131 448 | f 93//132 72//132 71//132 449 | f 6//133 40//133 75//133 450 | f 41//134 93//134 71//134 451 | f 85//135 55//135 35//135 452 | f 74//136 85//136 35//136 453 | f 87//137 94//137 95//137 454 | f 36//138 96//138 78//138 455 | f 76//139 96//139 88//139 456 | f 78//140 96//140 76//140 457 | f 88//141 96//141 89//141 458 | f 97//142 79//142 56//142 459 | f 29//143 91//143 98//143 460 | f 28//144 91//144 29//144 461 | f 26//145 81//145 28//145 462 | f 91//146 28//146 81//146 463 | f 73//147 64//147 86//147 464 | f 65//148 82//148 84//148 465 | f 92//149 65//149 99//149 466 | f 93//150 41//150 75//150 467 | f 72//151 85//151 61//151 468 | f 61//152 85//152 74//152 469 | f 61//153 74//153 73//153 470 | f 36//154 78//154 37//154 471 | f 37//155 77//155 64//155 472 | f 98//156 91//156 79//156 473 | f 79//157 100//157 98//157 474 | f 97//158 101//158 79//158 475 | f 95//159 101//159 97//159 476 | f 64//160 73//160 34//160 477 | f 73//161 35//161 34//161 478 | f 69//162 92//162 99//162 479 | f 19//163 69//163 99//163 480 | f 68//164 69//164 19//164 481 | f 92//165 82//165 65//165 482 | f 92//166 83//166 82//166 483 | f 99//167 65//167 19//167 484 | f 85//168 75//168 55//168 485 | f 41//169 6//169 75//169 486 | f 64//170 77//170 86//170 487 | f 77//171 37//171 78//171 488 | f 86//172 61//172 73//172 489 | f 87//173 89//173 94//173 490 | f 79//174 101//174 100//174 491 | f 102//175 90//175 51//175 492 | f 36//176 14//176 90//176 493 | f 91//177 44//177 79//177 494 | f 95//178 94//178 101//178 495 | f 29//179 98//179 51//179 496 | f 100//180 51//180 98//180 497 | f 102//181 51//181 100//181 498 | f 89//182 96//182 36//182 499 | f 36//183 90//183 89//183 500 | f 101//184 102//184 100//184 501 | f 94//185 89//185 102//185 502 | f 90//186 102//186 89//186 503 | f 101//187 94//187 102//187 504 | f 4//188 1//188 87//188 505 | f 4//189 87//189 45//189 506 | f 1//190 8//190 87//190 507 | f 60//191 87//191 71//191 508 | f 56//192 87//191 97//192 509 | f 76//193 87//193 62//193 510 | f 56//194 43//194 87//194 511 | f 45//195 87//195 43//195 512 | f 8//196 7//196 87//196 513 | f 62//197 87//197 60//197 514 | f 97//198 87//198 95//198 515 | f 71//199 87//199 7//199 516 | f 88//200 87//200 76//200 517 | 518 | -------------------------------------------------------------------------------- /frankaemika/panda_urdf/meshes/collision/link6.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'link2.blend' 2 | # Material Count: 17 3 | 4 | newmtl Face064_002_001_002_001.001 5 | Ns -1.960784 6 | Ka 1.000000 1.000000 1.000000 7 | Kd 1.000000 0.000000 0.000000 8 | Ks 0.003906 0.003906 0.003906 9 | Ke 0.000000 0.000000 0.000000 10 | Ni 1.000000 11 | d 1.000000 12 | illum 2 13 | 14 | newmtl Face065_002_001_002_001.001 15 | Ns -1.960784 16 | Ka 1.000000 1.000000 1.000000 17 | Kd 0.000000 1.000000 0.000000 18 | Ks 0.003906 0.003906 0.003906 19 | Ke 0.000000 0.000000 0.000000 20 | Ni 1.000000 21 | d 1.000000 22 | illum 2 23 | 24 | newmtl Face374_002_001_002_001.001 25 | Ns -1.960784 26 | Ka 1.000000 1.000000 1.000000 27 | Kd 1.000000 1.000000 1.000000 28 | Ks 0.003906 0.003906 0.003906 29 | Ke 0.000000 0.000000 0.000000 30 | Ni 1.000000 31 | d 1.000000 32 | illum 2 33 | 34 | newmtl Face539_002_001_002_001.001 35 | Ns -1.960784 36 | Ka 1.000000 1.000000 1.000000 37 | Kd 0.250980 0.250980 0.250980 38 | Ks 0.003906 0.003906 0.003906 39 | Ke 0.000000 0.000000 0.000000 40 | Ni 1.000000 41 | d 1.000000 42 | illum 2 43 | map_Kd D:\dev\pybullet_robots\data\franka_panda\meshes\visual\colors.png 44 | 45 | newmtl Part__Feature001_009_001_002_001.001 46 | Ns -1.960784 47 | Ka 1.000000 1.000000 1.000000 48 | Kd 0.250980 0.250980 0.250980 49 | Ks 0.003906 0.003906 0.003906 50 | Ke 0.000000 0.000000 0.000000 51 | Ni 1.000000 52 | d 1.000000 53 | illum 2 54 | map_Kd D:\dev\pybullet_robots\data\franka_panda\meshes\visual\colors.png 55 | 56 | newmtl Part__Feature002_006_001_002_001.001 57 | Ns -1.960784 58 | Ka 1.000000 1.000000 1.000000 59 | Kd 0.250980 0.250980 0.250980 60 | Ks 0.003906 0.003906 0.003906 61 | Ke 0.000000 0.000000 0.000000 62 | Ni 1.000000 63 | d 1.000000 64 | illum 2 65 | map_Kd D:\dev\pybullet_robots\data\franka_panda\meshes\visual\colors.png 66 | 67 | newmtl Shell002_002_001_002_001.001 68 | Ns -1.960784 69 | Ka 1.000000 1.000000 1.000000 70 | Kd 1.000000 1.000000 1.000000 71 | Ks 0.003906 0.003906 0.003906 72 | Ke 0.000000 0.000000 0.000000 73 | Ni 1.000000 74 | d 1.000000 75 | illum 2 76 | 77 | newmtl Shell003_002_001_002_001.001 78 | Ns -1.960784 79 | Ka 1.000000 1.000000 1.000000 80 | Kd 1.000000 1.000000 1.000000 81 | Ks 0.003906 0.003906 0.003906 82 | Ke 0.000000 0.000000 0.000000 83 | Ni 1.000000 84 | d 1.000000 85 | illum 2 86 | 87 | newmtl Shell004_001_001_002_001.001 88 | Ns -1.960784 89 | Ka 1.000000 1.000000 1.000000 90 | Kd 1.000000 1.000000 1.000000 91 | Ks 0.003906 0.003906 0.003906 92 | Ke 0.000000 0.000000 0.000000 93 | Ni 1.000000 94 | d 1.000000 95 | illum 2 96 | 97 | newmtl Shell005_001_001_002_001.001 98 | Ns -1.960784 99 | Ka 1.000000 1.000000 1.000000 100 | Kd 1.000000 1.000000 1.000000 101 | Ks 0.003906 0.003906 0.003906 102 | Ke 0.000000 0.000000 0.000000 103 | Ni 1.000000 104 | d 1.000000 105 | illum 2 106 | 107 | newmtl Shell006_003_002_001.001 108 | Ns -1.960784 109 | Ka 1.000000 1.000000 1.000000 110 | Kd 0.901961 0.921569 0.929412 111 | Ks 0.015625 0.015625 0.015625 112 | Ke 0.000000 0.000000 0.000000 113 | Ni 1.000000 114 | d 1.000000 115 | illum 2 116 | 117 | newmtl Shell007_002_002_001.001 118 | Ns -1.960784 119 | Ka 1.000000 1.000000 1.000000 120 | Kd 0.250000 0.250000 0.250000 121 | Ks 0.003906 0.003906 0.003906 122 | Ke 0.000000 0.000000 0.000000 123 | Ni 1.000000 124 | d 1.000000 125 | illum 2 126 | 127 | newmtl Shell011_002_002_001.001 128 | Ns -1.960784 129 | Ka 1.000000 1.000000 1.000000 130 | Kd 1.000000 1.000000 1.000000 131 | Ks 0.003906 0.003906 0.003906 132 | Ke 0.000000 0.000000 0.000000 133 | Ni 1.000000 134 | d 1.000000 135 | illum 2 136 | 137 | newmtl Shell012_002_002_001.001 138 | Ns -1.960784 139 | Ka 1.000000 1.000000 1.000000 140 | Kd 1.000000 1.000000 1.000000 141 | Ks 0.003906 0.003906 0.003906 142 | Ke 0.000000 0.000000 0.000000 143 | Ni 1.000000 144 | d 1.000000 145 | illum 2 146 | map_Kd D:\dev\pybullet_robots\data\franka_panda\meshes\visual\colors.png 147 | 148 | newmtl Shell_003_001_002_001.001 149 | Ns -1.960784 150 | Ka 1.000000 1.000000 1.000000 151 | Kd 0.250980 0.250980 0.250980 152 | Ks 0.003906 0.003906 0.003906 153 | Ke 0.000000 0.000000 0.000000 154 | Ni 1.000000 155 | d 1.000000 156 | illum 2 157 | map_Kd D:\dev\pybullet_robots\data\franka_panda\meshes\visual\colors.png 158 | 159 | newmtl Union001_001_001_002_001.001 160 | Ns -1.960784 161 | Ka 1.000000 1.000000 1.000000 162 | Kd 0.039216 0.541176 0.780392 163 | Ks 0.003906 0.003906 0.003906 164 | Ke 0.000000 0.000000 0.000000 165 | Ni 1.000000 166 | d 1.000000 167 | illum 2 168 | 169 | newmtl Union_001_001_002_001.001 170 | Ns -1.960784 171 | Ka 1.000000 1.000000 1.000000 172 | Kd 0.039216 0.541176 0.780392 173 | Ks 0.003906 0.003906 0.003906 174 | Ke 0.000000 0.000000 0.000000 175 | Ni 1.000000 176 | d 1.000000 177 | illum 2 178 | -------------------------------------------------------------------------------- /frankaemika/panda_urdf/meshes/visual/colors.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yimingli1998/cdf/e475591f2ccf2a61251e34088931deda6d47b283/frankaemika/panda_urdf/meshes/visual/colors.png -------------------------------------------------------------------------------- /frankaemika/panda_urdf/meshes/visual/finger.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'link2.blend' 2 | # Material Count: 2 3 | 4 | newmtl Part__Feature001_006.001 5 | Ns -1.960784 6 | Ka 1.000000 1.000000 1.000000 7 | Kd 0.901961 0.921569 0.929412 8 | Ks 0.250000 0.250000 0.250000 9 | Ke 0.000000 0.000000 0.000000 10 | Ni 1.000000 11 | d 1.000000 12 | illum 2 13 | map_Kd colors.png 14 | 15 | newmtl Part__Feature_007.001 16 | Ns -1.960784 17 | Ka 1.000000 1.000000 1.000000 18 | Kd 0.250980 0.250980 0.250980 19 | Ks 0.250000 0.250000 0.250000 20 | Ke 0.000000 0.000000 0.000000 21 | Ni 1.000000 22 | d 1.000000 23 | illum 2 24 | map_Kd colors.png 25 | -------------------------------------------------------------------------------- /frankaemika/panda_urdf/meshes/visual/hand.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'link2.blend' 2 | # Material Count: 5 3 | 4 | newmtl Part__Feature001_008_005.001 5 | Ns -1.960784 6 | Ka 1.000000 1.000000 1.000000 7 | Kd 0.250980 0.250980 0.250980 8 | Ks 0.007812 0.007812 0.007812 9 | Ke 0.000000 0.000000 0.000000 10 | Ni 1.000000 11 | d 1.000000 12 | illum 2 13 | map_Kd colors.png 14 | 15 | newmtl Part__Feature002_005_005.001 16 | Ns -1.960784 17 | Ka 1.000000 1.000000 1.000000 18 | Kd 0.901961 0.921569 0.929412 19 | Ks 0.015625 0.015625 0.015625 20 | Ke 0.000000 0.000000 0.000000 21 | Ni 1.000000 22 | d 1.000000 23 | illum 2 24 | 25 | newmtl Part__Feature005_001_005.001 26 | Ns -1.960784 27 | Ka 1.000000 1.000000 1.000000 28 | Kd 1.000000 1.000000 1.000000 29 | Ks 0.015625 0.015625 0.015625 30 | Ke 0.000000 0.000000 0.000000 31 | Ni 1.000000 32 | d 1.000000 33 | illum 2 34 | map_Kd colors.png 35 | 36 | newmtl Part__Feature005_001_005_001.001 37 | Ns -1.960784 38 | Ka 1.000000 1.000000 1.000000 39 | Kd 0.901961 0.921569 0.929412 40 | Ks 0.015625 0.015625 0.015625 41 | Ke 0.000000 0.000000 0.000000 42 | Ni 1.000000 43 | d 1.000000 44 | illum 2 45 | map_Kd colors.png 46 | 47 | newmtl Part__Feature_009_005.001 48 | Ns -1.960784 49 | Ka 1.000000 1.000000 1.000000 50 | Kd 0.250980 0.250980 0.250980 51 | Ks 0.015625 0.015625 0.015625 52 | Ke 0.000000 0.000000 0.000000 53 | Ni 1.000000 54 | d 1.000000 55 | illum 2 56 | map_Kd colors.png 57 | -------------------------------------------------------------------------------- /frankaemika/panda_urdf/meshes/visual/link1.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'link2.blend' 2 | # Material Count: 1 3 | 4 | newmtl Part__Feature_001 5 | Ns -1.960784 6 | Ka 1.000000 1.000000 1.000000 7 | Kd 1.000000 1.000000 1.000000 8 | Ks 0.062500 0.062500 0.062500 9 | Ke 0.000000 0.000000 0.000000 10 | Ni 1.000000 11 | d 1.000000 12 | illum 2 13 | map_Kd colors.png 14 | -------------------------------------------------------------------------------- /frankaemika/panda_urdf/meshes/visual/link2.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'link3.blend' 2 | # Material Count: 1 3 | 4 | newmtl Part__Feature024 5 | Ns -1.960784 6 | Ka 1.000000 1.000000 1.000000 7 | Kd 1.000000 1.000000 1.000000 8 | Ks 0.125000 0.125000 0.125000 9 | Ke 0.000000 0.000000 0.000000 10 | Ni 1.000000 11 | d 1.000000 12 | illum 2 13 | map_Kd colors.png 14 | -------------------------------------------------------------------------------- /frankaemika/panda_urdf/meshes/visual/link3.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'link4.blend' 2 | # Material Count: 4 3 | 4 | newmtl Part__Feature001_010_001_002 5 | Ns -1.960784 6 | Ka 1.000000 1.000000 1.000000 7 | Kd 1.000000 1.000000 1.000000 8 | Ks 0.007812 0.007812 0.007812 9 | Ke 0.000000 0.000000 0.000000 10 | Ni 1.000000 11 | d 1.000000 12 | illum 2 13 | map_Kd colors.png 14 | 15 | newmtl Part__Feature002_007_001_002 16 | Ns -1.960784 17 | Ka 1.000000 1.000000 1.000000 18 | Kd 1.000000 1.000000 1.000000 19 | Ks 0.007812 0.007812 0.007812 20 | Ke 0.000000 0.000000 0.000000 21 | Ni 1.000000 22 | d 1.000000 23 | illum 2 24 | map_Kd colors.png 25 | 26 | newmtl Part__Feature003_004_001_002 27 | Ns -1.960784 28 | Ka 1.000000 1.000000 1.000000 29 | Kd 1.000000 1.000000 1.000000 30 | Ks 0.007812 0.007812 0.007812 31 | Ke 0.000000 0.000000 0.000000 32 | Ni 1.000000 33 | d 1.000000 34 | illum 2 35 | map_Kd colors.png 36 | 37 | newmtl Part__Feature_001_001_001_002 38 | Ns -1.960784 39 | Ka 1.000000 1.000000 1.000000 40 | Kd 0.250980 0.250980 0.250980 41 | Ks 0.007812 0.007812 0.007812 42 | Ke 0.000000 0.000000 0.000000 43 | Ni 1.000000 44 | d 1.000000 45 | illum 2 46 | map_Kd colors.png 47 | -------------------------------------------------------------------------------- /frankaemika/panda_urdf/meshes/visual/link4.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'link4.blend' 2 | # Material Count: 4 3 | 4 | newmtl Part__Feature001_001_003_001.001 5 | Ns -1.960784 6 | Ka 1.000000 1.000000 1.000000 7 | Kd 1.000000 1.000000 1.000000 8 | Ks 0.007812 0.007812 0.007812 9 | Ke 0.000000 0.000000 0.000000 10 | Ni 1.000000 11 | d 1.000000 12 | illum 2 13 | 14 | newmtl Part__Feature002_001_003_001.001 15 | Ns -1.960784 16 | Ka 1.000000 1.000000 1.000000 17 | Kd 0.250980 0.250980 0.250980 18 | Ks 0.007812 0.007812 0.007812 19 | Ke 0.000000 0.000000 0.000000 20 | Ni 1.000000 21 | d 1.000000 22 | illum 2 23 | map_Kd colors.png 24 | 25 | newmtl Part__Feature003_001_003_001.001 26 | Ns -1.960784 27 | Ka 1.000000 1.000000 1.000000 28 | Kd 1.000000 1.000000 1.000000 29 | Ks 0.007812 0.007812 0.007812 30 | Ke 0.000000 0.000000 0.000000 31 | Ni 1.000000 32 | d 1.000000 33 | illum 2 34 | map_Kd colors.png 35 | 36 | newmtl Part__Feature_002_003_001.001 37 | Ns -1.960784 38 | Ka 1.000000 1.000000 1.000000 39 | Kd 1.000000 1.000000 1.000000 40 | Ks 0.007812 0.007812 0.007812 41 | Ke 0.000000 0.000000 0.000000 42 | Ni 1.000000 43 | d 1.000000 44 | illum 2 45 | map_Kd colors.png 46 | -------------------------------------------------------------------------------- /frankaemika/panda_urdf/meshes/visual/link5.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'None' 2 | # Material Count: 3 3 | 4 | newmtl Part__Feature_002_004_003.002 5 | Ns -1.960784 6 | Ka 1.000000 1.000000 1.000000 7 | Kd 1.000000 1.000000 1.000000 8 | Ks 0.015625 0.015625 0.015625 9 | Ke 0.000000 0.000000 0.000000 10 | Ni 1.000000 11 | d 1.000000 12 | illum 2 13 | map_Kd colors.png 14 | 15 | newmtl Shell001_001_001_003.002 16 | Ns -1.960784 17 | Ka 1.000000 1.000000 1.000000 18 | Kd 0.250000 0.250000 0.250000 19 | Ks 0.015625 0.015625 0.015625 20 | Ke 0.000000 0.000000 0.000000 21 | Ni 1.000000 22 | d 1.000000 23 | illum 2 24 | map_Kd colors.png 25 | 26 | newmtl Shell_001_001_003.002 27 | Ns -1.960784 28 | Ka 1.000000 1.000000 1.000000 29 | Kd 1.000000 1.000000 1.000000 30 | Ks 0.015625 0.015625 0.015625 31 | Ke 0.000000 0.000000 0.000000 32 | Ni 1.000000 33 | d 1.000000 34 | illum 2 35 | -------------------------------------------------------------------------------- /frankaemika/panda_urdf/meshes/visual/link6.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'link2.blend' 2 | # Material Count: 17 3 | 4 | newmtl Face064_002_001_002_001.001 5 | Ns -1.960784 6 | Ka 1.000000 1.000000 1.000000 7 | Kd 1.000000 0.000000 0.000000 8 | Ks 0.003906 0.003906 0.003906 9 | Ke 0.000000 0.000000 0.000000 10 | Ni 1.000000 11 | d 1.000000 12 | illum 2 13 | 14 | newmtl Face065_002_001_002_001.001 15 | Ns -1.960784 16 | Ka 1.000000 1.000000 1.000000 17 | Kd 0.000000 1.000000 0.000000 18 | Ks 0.003906 0.003906 0.003906 19 | Ke 0.000000 0.000000 0.000000 20 | Ni 1.000000 21 | d 1.000000 22 | illum 2 23 | 24 | newmtl Face374_002_001_002_001.001 25 | Ns -1.960784 26 | Ka 1.000000 1.000000 1.000000 27 | Kd 1.000000 1.000000 1.000000 28 | Ks 0.003906 0.003906 0.003906 29 | Ke 0.000000 0.000000 0.000000 30 | Ni 1.000000 31 | d 1.000000 32 | illum 2 33 | 34 | newmtl Face539_002_001_002_001.001 35 | Ns -1.960784 36 | Ka 1.000000 1.000000 1.000000 37 | Kd 0.250980 0.250980 0.250980 38 | Ks 0.003906 0.003906 0.003906 39 | Ke 0.000000 0.000000 0.000000 40 | Ni 1.000000 41 | d 1.000000 42 | illum 2 43 | map_Kd colors.png 44 | 45 | newmtl Part__Feature001_009_001_002_001.001 46 | Ns -1.960784 47 | Ka 1.000000 1.000000 1.000000 48 | Kd 0.250980 0.250980 0.250980 49 | Ks 0.003906 0.003906 0.003906 50 | Ke 0.000000 0.000000 0.000000 51 | Ni 1.000000 52 | d 1.000000 53 | illum 2 54 | map_Kd colors.png 55 | 56 | newmtl Part__Feature002_006_001_002_001.001 57 | Ns -1.960784 58 | Ka 1.000000 1.000000 1.000000 59 | Kd 0.250980 0.250980 0.250980 60 | Ks 0.003906 0.003906 0.003906 61 | Ke 0.000000 0.000000 0.000000 62 | Ni 1.000000 63 | d 1.000000 64 | illum 2 65 | map_Kd colors.png 66 | 67 | newmtl Shell002_002_001_002_001.001 68 | Ns -1.960784 69 | Ka 1.000000 1.000000 1.000000 70 | Kd 1.000000 1.000000 1.000000 71 | Ks 0.003906 0.003906 0.003906 72 | Ke 0.000000 0.000000 0.000000 73 | Ni 1.000000 74 | d 1.000000 75 | illum 2 76 | 77 | newmtl Shell003_002_001_002_001.001 78 | Ns -1.960784 79 | Ka 1.000000 1.000000 1.000000 80 | Kd 1.000000 1.000000 1.000000 81 | Ks 0.003906 0.003906 0.003906 82 | Ke 0.000000 0.000000 0.000000 83 | Ni 1.000000 84 | d 1.000000 85 | illum 2 86 | 87 | newmtl Shell004_001_001_002_001.001 88 | Ns -1.960784 89 | Ka 1.000000 1.000000 1.000000 90 | Kd 1.000000 1.000000 1.000000 91 | Ks 0.003906 0.003906 0.003906 92 | Ke 0.000000 0.000000 0.000000 93 | Ni 1.000000 94 | d 1.000000 95 | illum 2 96 | 97 | newmtl Shell005_001_001_002_001.001 98 | Ns -1.960784 99 | Ka 1.000000 1.000000 1.000000 100 | Kd 1.000000 1.000000 1.000000 101 | Ks 0.003906 0.003906 0.003906 102 | Ke 0.000000 0.000000 0.000000 103 | Ni 1.000000 104 | d 1.000000 105 | illum 2 106 | 107 | newmtl Shell006_003_002_001.001 108 | Ns -1.960784 109 | Ka 1.000000 1.000000 1.000000 110 | Kd 0.901961 0.921569 0.929412 111 | Ks 0.015625 0.015625 0.015625 112 | Ke 0.000000 0.000000 0.000000 113 | Ni 1.000000 114 | d 1.000000 115 | illum 2 116 | 117 | newmtl Shell007_002_002_001.001 118 | Ns -1.960784 119 | Ka 1.000000 1.000000 1.000000 120 | Kd 0.250000 0.250000 0.250000 121 | Ks 0.003906 0.003906 0.003906 122 | Ke 0.000000 0.000000 0.000000 123 | Ni 1.000000 124 | d 1.000000 125 | illum 2 126 | 127 | newmtl Shell011_002_002_001.001 128 | Ns -1.960784 129 | Ka 1.000000 1.000000 1.000000 130 | Kd 1.000000 1.000000 1.000000 131 | Ks 0.003906 0.003906 0.003906 132 | Ke 0.000000 0.000000 0.000000 133 | Ni 1.000000 134 | d 1.000000 135 | illum 2 136 | 137 | newmtl Shell012_002_002_001.001 138 | Ns -1.960784 139 | Ka 1.000000 1.000000 1.000000 140 | Kd 1.000000 1.000000 1.000000 141 | Ks 0.003906 0.003906 0.003906 142 | Ke 0.000000 0.000000 0.000000 143 | Ni 1.000000 144 | d 1.000000 145 | illum 2 146 | map_Kd colors.png 147 | 148 | newmtl Shell_003_001_002_001.001 149 | Ns -1.960784 150 | Ka 1.000000 1.000000 1.000000 151 | Kd 0.250980 0.250980 0.250980 152 | Ks 0.003906 0.003906 0.003906 153 | Ke 0.000000 0.000000 0.000000 154 | Ni 1.000000 155 | d 1.000000 156 | illum 2 157 | map_Kd colors.png 158 | 159 | newmtl Union001_001_001_002_001.001 160 | Ns -1.960784 161 | Ka 1.000000 1.000000 1.000000 162 | Kd 0.039216 0.541176 0.780392 163 | Ks 0.003906 0.003906 0.003906 164 | Ke 0.000000 0.000000 0.000000 165 | Ni 1.000000 166 | d 1.000000 167 | illum 2 168 | 169 | newmtl Union_001_001_002_001.001 170 | Ns -1.960784 171 | Ka 1.000000 1.000000 1.000000 172 | Kd 0.039216 0.541176 0.780392 173 | Ks 0.003906 0.003906 0.003906 174 | Ke 0.000000 0.000000 0.000000 175 | Ni 1.000000 176 | d 1.000000 177 | illum 2 178 | -------------------------------------------------------------------------------- /frankaemika/panda_urdf/panda.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | 198 | 199 | 200 | 201 | 202 | 203 | 204 | 205 | 206 | 207 | 208 | 209 | 210 | 211 | 212 | 213 | 214 | 215 | 216 | 217 | 218 | 219 | 220 | 221 | 222 | 223 | 224 | 225 | 226 | 227 | 228 | 229 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 239 | 240 | 241 | 242 | 243 | 244 | 245 | 246 | 247 | 248 | 249 | 250 | 251 | 252 | 253 | 254 | 255 | 256 | 257 | 258 | 259 | 260 | 261 | 262 | 263 | 264 | 265 | 266 | 267 | 268 | 269 | 270 | 271 | 272 | 273 | 274 | 275 | 276 | 277 | 278 | 279 | 280 | 281 | 282 | 283 | 284 | 285 | 286 | 287 | 288 | 289 | 290 | 291 | 292 | 293 | 294 | 295 | 296 | 297 | 298 | 299 | 300 | 301 | 302 | 303 | 304 | 305 | 306 | 307 | 308 | 309 | 310 | 311 | 312 | 313 | 314 | 315 | 316 | 317 | 318 | 319 | 320 | 321 | 322 | 323 | 324 | 325 | 326 | 327 | 328 | 329 | 330 | 331 | 332 | 333 | 334 | 335 | 336 | 337 | 338 | 339 | -------------------------------------------------------------------------------- /frankaemika/pybullet_panda_sim.py: -------------------------------------------------------------------------------- 1 | # ----------------------------------------------------------------------------- 2 | # SPDX-License-Identifier: MIT 3 | # This file is part of the CDF project. 4 | # Copyright (c) 2024 Idiap Research Institute 5 | # Contributor: Yimming Li 6 | # ----------------------------------------------------------------------------- 7 | 8 | 9 | import time 10 | import numpy as np 11 | from math import pi 12 | import os 13 | CUR_PATH = os.path.dirname(os.path.realpath(__file__)) 14 | 15 | pandaNumDofs = 7 16 | 17 | # restpose 18 | rp = [0.0,0.0, -1.57461, -1.60788, -0.785175, 1.54666, -0.882595, 0.02, 0.02] 19 | 20 | class PandaSim(): 21 | def __init__(self, bullet_client, base_pos, base_rot): 22 | self.bullet_client = bullet_client 23 | self.bullet_client.setAdditionalSearchPath('content/urdfs') 24 | self.base_pos = np.array(base_pos) 25 | self.base_rot = np.array(base_rot) 26 | # print("offset=",offset) 27 | flags = self.bullet_client.URDF_ENABLE_CACHED_GRAPHICS_SHAPES 28 | table_rot = self.bullet_client.getQuaternionFromEuler([pi / 2, 0, pi]) 29 | self.rp = rp 30 | # self.bullet_client.loadURDF('LAB/lab.urdf', np.array([1.01, -0.28, 0.45]), table_rot, flags=flags) 31 | #self.bullet_client.loadURDF('lab_table/table.urdf', np.array([-0.15, 0.02, -0.1]), table_rot, flags=flags) 32 | # self.bullet_client.loadURDF('plane.urdf', np.array([0, 0, 0]), np.array([0, 0, 0, 1]), flags=flags) 33 | 34 | self.panda = self.bullet_client.loadURDF(os.path.join(CUR_PATH,"panda_urdf/panda.urdf"), self.base_pos, 35 | self.base_rot, useFixedBase=True, flags=flags) 36 | self.reset() 37 | self.t = 0. 38 | # self.set_joint_positions(rp) 39 | 40 | def reset(self): 41 | index = 0 42 | for j in range(self.bullet_client.getNumJoints(self.panda)): 43 | self.bullet_client.changeDynamics(self.panda, j, linearDamping=0, angularDamping=0) 44 | info = self.bullet_client.getJointInfo(self.panda, j) 45 | jointName = info[1] 46 | jointType = info[2] 47 | if (jointType == self.bullet_client.JOINT_PRISMATIC): 48 | self.bullet_client.resetJointState(self.panda, j, self.rp[index]) 49 | index = index + 1 50 | if (jointType == self.bullet_client.JOINT_REVOLUTE): 51 | self.bullet_client.resetJointState(self.panda, j, self.rp[index]) 52 | index = index + 1 53 | 54 | def set_joint_positions(self, joint_positions): 55 | for i in range(pandaNumDofs): 56 | self.bullet_client.setJointMotorControl2(self.panda, i, self.bullet_client.POSITION_CONTROL, 57 | joint_positions[i], force=240.) 58 | self.set_finger_positions(0.04) 59 | 60 | def set_finger_positions(self, gripper_opening): 61 | self.bullet_client.setJointMotorControl2(self.panda, 9, self.bullet_client.POSITION_CONTROL, 62 | gripper_opening/2, force=5 * 240.) 63 | self.bullet_client.setJointMotorControl2(self.panda, 10, self.bullet_client.POSITION_CONTROL, 64 | -gripper_opening/2, force=5 * 240.) 65 | 66 | 67 | def get_joint_positions(self): 68 | joint_state = [] 69 | for i in range(pandaNumDofs): 70 | joint_state.append(self.bullet_client.getJointState(self.panda, i)[0]) 71 | return joint_state 72 | 73 | class SphereManager: 74 | def __init__(self, pybullet_client): 75 | self.pb = pybullet_client 76 | self.spheres = [] 77 | self.color = [.7, .1, .1, 1] 78 | self.color = [.63, .07, .185, 1] 79 | # self.color = [0.8500, 0.3250, 0.0980, 1] 80 | def create_sphere(self, position, radius, color): 81 | sphere = self.pb.createVisualShape(self.pb.GEOM_SPHERE, 82 | radius=radius, 83 | rgbaColor=color, specularColor=[0, 0, 0, 1]) 84 | sphere = self.pb.createCollisionShape(self.pb.GEOM_SPHERE, 85 | radius=radius) 86 | 87 | sphere = self.pb.createMultiBody(baseVisualShapeIndex=sphere, 88 | basePosition=position) 89 | self.spheres.append(sphere) 90 | 91 | def initialize_spheres(self, obstacle_array): 92 | for obstacle in obstacle_array: 93 | self.create_sphere(obstacle[0:3], obstacle[3], self.color) 94 | 95 | def delete_spheres(self): 96 | for sphere in self.spheres: 97 | self.pb.removeBody(sphere) 98 | self.spheres = [] 99 | 100 | def update_spheres(self, obstacle_array): 101 | if (obstacle_array is not None) and (len(self.spheres) == len(obstacle_array)): 102 | for i, sphere in enumerate(self.spheres): 103 | self.pb.resetBasePositionAndOrientation(sphere, 104 | obstacle_array[i, 0:3], 105 | [1, 0, 0, 0]) 106 | else: 107 | print("Number of spheres and obstacles do not match") 108 | self.delete_spheres() 109 | self.initialize_spheres(obstacle_array) 110 | -------------------------------------------------------------------------------- /frankaemika/qp_mp.py: -------------------------------------------------------------------------------- 1 | # ----------------------------------------------------------------------------- 2 | # SPDX-License-Identifier: MIT 3 | # This file is part of the CDF project. 4 | # Copyright (c) 2024 Idiap Research Institute 5 | # Contributor: Yimming Li 6 | # ----------------------------------------------------------------------------- 7 | 8 | 9 | import casadi as ca 10 | import numpy as np 11 | import matplotlib.pyplot as plt 12 | import torch 13 | import torch.nn as nn 14 | import os 15 | import sys 16 | import time 17 | import matplotlib.cm as cm 18 | CUR_PATH = os.path.dirname(os.path.realpath(__file__)) 19 | sys.path.append(os.path.join(CUR_PATH,'../RDF')) 20 | from mlp import MLPRegression 21 | import trimesh 22 | from nn_cdf import CDF 23 | import pybullet as p 24 | import pybullet_data as pd 25 | from pybullet_panda_sim import PandaSim, SphereManager 26 | 27 | def create_system_matrices(n, dt): 28 | # For a single integrator, A is an identity matrix and B is a dt scaled identity matrix 29 | A_d = np.eye(n) 30 | B_d = np.eye(n) * dt 31 | return A_d, B_d 32 | 33 | # set the dtype of numpy and pytorch to float32 34 | torch.set_default_dtype(torch.float32) 35 | np.set_printoptions(precision=4, suppress=True) 36 | PI = 3.14 37 | 38 | def solve_optimization_problem(n_dimensions, x0_2d, xf_2d, cons_u, A, B, distance, gradient, dt, solver=None, safety_buffer=0.6): 39 | """ 40 | Set up and solve the optimization problem. 41 | """ 42 | n_states = n_dimensions 43 | n_controls = n_dimensions 44 | 45 | # Decision variables (states and control inputs) 46 | X_2d = ca.MX.sym('X', n_states, 1+1) # shape = (7, 2) 47 | U_2d = ca.MX.sym('U', n_controls, 1) # shape = (7, 1) 48 | cost_mat_Q = np.diag([150, 190, 80, 70, 70, 90, 100]) # Q matrix 49 | # cost_mat_Q = np.diag([100, 100, 100, 100, 100, 90, 100]) # Q matrix 50 | cost_mat_R = np.diag([0.01, 0.01, 0.01, 0.01, 0.01, 0.01, 0.01]) 51 | 52 | # Objective function (minimize control effort) 53 | obj_2d = 0 54 | x_diff = X_2d[:, 1] - xf_2d 55 | obj_2d += ca.mtimes(x_diff.T, ca.mtimes(cost_mat_Q, x_diff)) + ca.mtimes(U_2d[:, 0].T, ca.mtimes(cost_mat_R, U_2d[:, 0])) 56 | 57 | cons_x = 10 58 | lb_x = [] # Lower bound for the constraints 59 | ub_x = [] # Upper bound for the constraints 60 | lb_u = [] 61 | ub_u = [] 62 | # Adding constraints 63 | g_2d = [] 64 | g_2d.append(X_2d[:, 0] - x0_2d) # Equality constraint 65 | # System dynamics constraint 66 | g_2d.append(X_2d[:, 1] - (ca.mtimes(A, X_2d[:, 0]) + ca.mtimes(B, U_2d[:, 0]))) 67 | # inequality constraints for the collision avoidance 68 | # grad * u * dt <= log(dist + 1) 69 | # grad * u * dt - log(dist + 1) <= 0 70 | g_2d.append(-ca.mtimes(ca.mtimes(gradient, U_2d), dt) - np.log(distance + safety_buffer)) 71 | lbg = [0] * n_states * (1+1) # Lower bound of the constraints, 1 for equality constraints, and 1 for the initial condition 72 | ubg = [0] * n_states * (1+1) # Upper bound of the constraints, 1 for equality constraints, and 1 for the initial condition 73 | lbg.append(-np.inf) 74 | ubg.append(0) 75 | # =============================================== # 76 | # inequality constraints for the velocity 77 | lb_x.append([-cons_x, -cons_x, -cons_x, -cons_x, -cons_x, -cons_x, -cons_x]) 78 | ub_x.append([cons_x, cons_x, cons_x, cons_x, cons_x, cons_x, cons_x]) 79 | # inequality constraints for the control inputs 80 | lb_u.append([-cons_u, -cons_u, -cons_u, -cons_u, -cons_u, -cons_u, -cons_u]) 81 | ub_u.append([cons_u, cons_u, cons_u, cons_u, cons_u, cons_u, cons_u]) 82 | # state is one more than control 83 | lb_x.append([-cons_x, -cons_x, -cons_x, -cons_x, -cons_x, -cons_x, -cons_x]) 84 | ub_x.append([cons_x, cons_x, cons_x, cons_x, cons_x, cons_x, cons_x]) 85 | lbx = ca.vertcat(*lb_x, *lb_u) 86 | ubx = ca.vertcat(*ub_x, *ub_u) 87 | # QP structure 88 | X_2d_long_vector = ca.reshape(X_2d, n_states*(1+1), 1) 89 | U_2d_long_vector = ca.reshape(U_2d, n_controls*1, 1) 90 | qp_x = ca.vertcat(X_2d_long_vector, U_2d_long_vector) 91 | g_sys_vector = ca.vertcat(*g_2d) 92 | 93 | # Create the QP 94 | qp_2d = {'x': qp_x, 95 | 'f': obj_2d, 96 | 'g': g_sys_vector} 97 | 98 | opts = {'print_time': 0,'error_on_fail': False, 'verbose': False} 99 | 100 | # Create the solver 101 | if solver == 'ipopt': 102 | solver_2d = ca.nlpsol('solver', 'ipopt', qp_2d, opts) 103 | elif solver == 'osqp': 104 | solver_2d = ca.qpsol('solver', 'osqp', qp_2d, opts) 105 | elif solver == 'qpOASES': 106 | solver_2d = ca.qpsol('solver', 'qpoases', qp_2d, opts) 107 | elif solver == 'qrqp': 108 | solver_2d = ca.qpsol('solver', 'qrqp', qp_2d, opts) 109 | 110 | 111 | # Solve the problem 112 | sol_2d = solver_2d(lbg=lbg, ubg=ubg, lbx=lbx, ubx=ubx) 113 | # Extract the optimal solution 114 | opt_x_2d = sol_2d['x'][:n_states*(1+1)].full().reshape(1+1, n_states).T 115 | opt_u_2d = sol_2d['x'][n_states*(1+1):].full().reshape(1, n_controls).T 116 | 117 | return opt_x_2d, opt_u_2d 118 | 119 | 120 | def main(): 121 | # 122 | device = torch.device("cuda" if torch.cuda.is_available() else "cpu") 123 | # device = torch.device("cpu") 124 | cdf = CDF(device) 125 | # trainer.train_nn(epoches=20000) 126 | model = MLPRegression(input_dims=10, output_dims=1, mlp_layers=[1024, 512, 256, 128, 128],skips=[], act_fn=torch.nn.ReLU, nerf=True) 127 | # model.load_state_dict(torch.load(os.path.join(CUR_PATH,'model_dict.pt'))[19900]) 128 | model.load_state_dict(torch.load(os.path.join(CUR_PATH,'model_dict.pt'))[49900]) 129 | model.to(device) 130 | 131 | # Parameters of the setup 132 | N = 500 133 | dt = 0.01 134 | T = N * dt 135 | print('The total time is: ', T) 136 | n_dimensions = 7 137 | A, B = create_system_matrices(n_dimensions, dt) 138 | 139 | distance_filed = 'cdf' 140 | 141 | qp_solver_dict = {0: 'ipopt', 1: 'osqp', 2: 'qpOASES', 3: 'qrqp'} 142 | solver = 0 143 | 144 | # set start and goal configuration 145 | x0_7d = np.array([-0.03610672, 0.14759123, 0.60442339, -2.45172895, -0.06231244,2.53993935, 1.10256184]) 146 | xf_7d = np.array([-0.25802498, -0.01593395, -0.35283275, -2.24489454, -0.06160258,2.35934126, 0.34169443]) 147 | 148 | # choose the solver by the number in the dict 149 | solver = qp_solver_dict[solver] 150 | 151 | cons_u = 2.7 # constraints 152 | 153 | #ring obstacle 154 | def ring(radius,center,rot): 155 | theta = torch.arange(0, 2*PI, 0.2).to(device) 156 | x = radius*torch.cos(theta) 157 | y = radius*torch.sin(theta) 158 | z = torch.zeros_like(x).to(device) 159 | points = torch.stack([x,y,z],dim=-1) 160 | points = torch.matmul(points,rot.transpose(0,1)) + center 161 | 162 | obstacle_array = np.zeros((len(points),4)) 163 | obstacle_array[:,:3] = points.detach().cpu().numpy() 164 | obstacle_array[:,3] = 0.05 165 | return points,obstacle_array 166 | #wall obstacle 167 | def wall(size,center,rot): 168 | x = torch.arange(-size[0]/2, size[0]/2, 0.05).to(device) 169 | y = torch.arange(-size[1]/2, size[1]/2, 0.05).to(device) 170 | x,y = torch.meshgrid(x,y) 171 | x,y = x.reshape(-1),y.reshape(-1) 172 | z = torch.zeros_like(x).to(device) 173 | points = torch.stack([x,y,z],dim=-1) 174 | points = torch.matmul(points,rot.transpose(0,1)) + center 175 | 176 | obstacle_array = np.zeros((len(points),4)) 177 | obstacle_array[:,:3] = points.detach().cpu().numpy() 178 | obstacle_array[:,3] = 0.05 179 | return points,obstacle_array 180 | 181 | wall_size = torch.tensor([0.5,0.5]).to(device) 182 | wall_center = torch.tensor([0.5,0.0,0.2]).to(device) 183 | wall_rot = torch.tensor([[1.0,0.0,0.0], 184 | [0.0,0.0,-1.0], 185 | [0.0,1.0,0.0],]).to(device) 186 | ring_radius = 0.25 187 | ring_center = torch.tensor([0.5,0.0,0.45]).to(device) 188 | ring_rot = torch.tensor([[0.0,0.0,-1.0], 189 | [0.0,1.0,0.0], 190 | [1.0,0.0,0.0],]).to(device) 191 | 192 | # QP problem for one time step 193 | log_opt_x_7d = [] 194 | log_opt_u_7d = [] 195 | log_dis_to_obstacle = [] 196 | safety_buffer = 0.3 197 | 198 | 199 | for i in range(N): 200 | log_opt_x_7d.append(x0_7d) 201 | 202 | # set obstacle 203 | pts1,obstacle1 = wall(wall_size,wall_center,wall_rot) 204 | ring_center = torch.tensor([0.3,0.0,0.45]).to(device) 205 | pts2,obstacle2 = ring(0.4,ring_center,ring_rot) 206 | pts = torch.cat([pts1,pts2],dim=0) 207 | 208 | # infer cdf and its gradient 209 | x0_7d_torch = torch.from_numpy(x0_7d).to(device).reshape(1, 7).float() 210 | x0_7d_torch.requires_grad = True 211 | distance_input, gradient_input = cdf.inference_d_wrt_q(pts,x0_7d_torch,model,return_grad = True) 212 | distance_input = distance_input.cpu().detach().numpy() 213 | gradient_input = gradient_input.cpu().detach().numpy() 214 | log_dis_to_obstacle.append(distance_input) 215 | 216 | # optimization 217 | opt_x_7d, opt_u_7d = solve_optimization_problem(n_dimensions, x0_7d, xf_7d, cons_u, A, B, distance_input, gradient_input,dt, solver,safety_buffer) 218 | # update the double integrator system 219 | x0_7d = A @ opt_x_7d[:, 0] + B @ opt_u_7d[:, 0] 220 | log_opt_u_7d.append(opt_u_7d[:, 0]) 221 | 222 | # if the difference between current u and last u is small, we stop 223 | if i > 2: 224 | if np.linalg.norm(opt_u_7d - log_opt_u_7d[-2]) < 0.01: 225 | print(f'The difference is small, we stop, the current step is: {i}') 226 | break 227 | 228 | log_dis_to_obstacle = np.array(log_dis_to_obstacle) 229 | # compute the norm of the final state and the initial state 230 | error = np.linalg.norm(opt_x_7d[:, 0] - xf_7d) 231 | # print('The error is: ', error) 232 | 233 | log_opt_x_7d = np.array(log_opt_x_7d) # shape = (N, 4) 234 | log_opt_u_7d = np.array(log_opt_u_7d) # shape = (N, 2) 235 | 236 | print(log_opt_x_7d.shape) 237 | 238 | p.connect(p.GUI, options='--background_color_red=1 --background_color_green=1' + 239 | ' --background_color_blue=1 --width=1000 --height=1000') 240 | p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0) 241 | p.configureDebugVisualizer(lightPosition=[5, 5, 5]) 242 | p.setPhysicsEngineParameter(maxNumCmdPer1ms=1000) 243 | 244 | p.resetDebugVisualizerCamera(cameraDistance=1.5, cameraYaw=145, cameraPitch=-10, cameraTargetPosition=[0, 0, 0.6]) 245 | 246 | 247 | p.setAdditionalSearchPath(pd.getDataPath()) 248 | timeStep = 0.01 249 | p.setTimeStep(timeStep) 250 | p.setGravity(0, 0, -9.81) 251 | p.setRealTimeSimulation(1) 252 | ## spawn franka robot 253 | base_pos = [0, 0, 0] 254 | base_rot = p.getQuaternionFromEuler([0, 0, 0]) 255 | panda = PandaSim(p, base_pos, base_rot) 256 | # panda_goal = PandaSim(p, base_pos, base_rot) 257 | # panda_goal = panda_goal.set_joint_positions(log_opt_x_7d[-1]) 258 | # time.sleep(5) 259 | q0 = panda.get_joint_positions() 260 | sphere_manager = SphereManager(p) 261 | obstacle = np.concatenate([obstacle1,obstacle2],axis=0) 262 | sphere_manager.initialize_spheres(obstacle) 263 | 264 | for i in range(len(log_opt_x_7d)): 265 | panda.set_joint_positions(log_opt_x_7d[i]) 266 | time.sleep(0.1) 267 | 268 | if __name__ == '__main__': 269 | main() -------------------------------------------------------------------------------- /frankaemika/throw_wik.py: -------------------------------------------------------------------------------- 1 | # ----------------------------------------------------------------------------- 2 | # SPDX-License-Identifier: MIT 3 | # This file is part of the CDF project. 4 | # Copyright (c) 2024 Idiap Research Institute 5 | # Contributor: Yimming Li 6 | # ----------------------------------------------------------------------------- 7 | 8 | 9 | import pybullet as p 10 | import pybullet_data as pd 11 | import numpy as np 12 | import sys 13 | import time 14 | from pybullet_panda_sim import PandaSim, SphereManager 15 | import torch 16 | import os 17 | CUR_PATH = os.path.dirname(os.path.realpath(__file__)) 18 | from mlp import MLPRegression 19 | from nn_cdf import CDF 20 | 21 | def main_loop(): 22 | 23 | # device = torch.device("cuda" if torch.cuda.is_available() else "cpu") 24 | device = torch.device("cpu") 25 | cdf = CDF(device) 26 | 27 | # trainer.train_nn(epoches=20000) 28 | model = MLPRegression(input_dims=10, output_dims=1, mlp_layers=[1024, 512, 256, 128, 128],skips=[], act_fn=torch.nn.ReLU, nerf=True) 29 | model.load_state_dict(torch.load(os.path.join(CUR_PATH,'model_dict.pt'))[49900]) 30 | model.to(device) 31 | 32 | p.connect(p.GUI, options='--background_color_red=0.5 --background_color_green=0.5' + 33 | ' --background_color_blue=0.5 --width=1600 --height=1000') 34 | 35 | 36 | p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0) 37 | p.configureDebugVisualizer(lightPosition=[5, 5, 5]) 38 | p.setPhysicsEngineParameter(maxNumCmdPer1ms=1000) 39 | #p.resetDebugVisualizerCamera(cameraDistance=1.5, cameraYaw=110, cameraPitch=-10, cameraTargetPosition=[0, 0, 0.5]) 40 | # p.resetDebugVisualizerCamera(cameraDistance=1.5, cameraYaw=90, cameraPitch=0, cameraTargetPosition=[0, 0, 0.5]) 41 | #p.resetDebugVisualizerCamera(cameraDistance=1.5, cameraYaw=110, cameraPitch=-25, cameraTargetPosition=[0, 0, 0.5]) 42 | p.resetDebugVisualizerCamera(cameraDistance=1.5, cameraYaw=145, cameraPitch=0, cameraTargetPosition=[0, 0, 0.6]) 43 | p.setAdditionalSearchPath(pd.getDataPath()) 44 | 45 | ## spawn franka robot 46 | base_pos = [0, 0, 0] 47 | base_rot = p.getQuaternionFromEuler([0, 0, 0]) 48 | robot = PandaSim(p, base_pos, base_rot) 49 | q0 = robot.get_joint_positions() 50 | q_init = torch.tensor([q0],requires_grad=True).to(device).float() 51 | # NOTE: need high frequency 52 | hz = 1000 53 | delta_t = 1.0 / hz 54 | p.setGravity(0, 0, -9.81) 55 | p.setTimeStep(delta_t) 56 | p.setRealTimeSimulation(0) 57 | 58 | 59 | p.setAdditionalSearchPath(pd.getDataPath()) 60 | soccerballId = p.loadURDF("soccerball.urdf", [-3.0, 0, 3], globalScaling=0.1) 61 | p.changeDynamics(soccerballId, -1, mass=1.0, linearDamping=0.00, angularDamping=0.00, rollingFriction=0.03, 62 | spinningFriction=0.03, restitution=0.2, lateralFriction=0.03) 63 | 64 | box_size = np.array([0.6,0.01,0.3]) 65 | box_center = np.array([0.0,0.3,0.3]) 66 | box = p.createVisualShape(p.GEOM_BOX, halfExtents=box_size, rgbaColor=[0.8500, 0.3250, 0.0980, 1.0]) 67 | p.createMultiBody(baseVisualShapeIndex=box, 68 | basePosition=box_center) 69 | ball_pos = np.array([0.0, -1.0, 0]) 70 | vec_max = np.array([1.0, 3.0, 4.0]) 71 | vec_min = np.array([-1.0, 2.0, 3.0]) 72 | ball_vec = np.random.rand(100,3) * (vec_max - vec_min) + vec_min 73 | for vec in ball_vec: 74 | for _ in range(300): 75 | robot.set_joint_positions(q0) 76 | p.stepSimulation() 77 | p.resetBasePositionAndOrientation(soccerballId, ball_pos, [0, 0, 0, 1]) 78 | p.resetBaseVelocity(soccerballId, linearVelocity=vec) 79 | defense,proj = False, False 80 | while(True): 81 | # print(robot.get_joint_positions()) 82 | position, orientation = p.getBasePositionAndOrientation(soccerballId) 83 | if (position[0] > box_center[0]-box_size[0]) & (position[0] < box_center[0] + box_size[0]) & \ 84 | (position[1] > -0.5) & (position[1] < -0.1) &(position[2] > 0.0) & (position[2] < box_center[2]+box_size[2] + 0.2): 85 | defense = True 86 | if defense and not proj: 87 | print('defense') 88 | q = q_init 89 | x = torch.from_numpy(np.array([position])).to(device).float() 90 | d,grad = cdf.inference_d_wrt_q(x,q,model) 91 | # print(f'd: {d}, grad: {grad}') 92 | q_proj = cdf.projection(q,d,grad) 93 | q_init = q_proj 94 | robot.set_joint_positions(q_proj[0].data.cpu().numpy()) 95 | proj = True 96 | 97 | p.stepSimulation() 98 | time.sleep(delta_t*2.0) 99 | if position[2] < -0.1 or position[2] > 1.5 or position[1] < -1.0: 100 | break 101 | 102 | p.disconnect() 103 | 104 | 105 | if __name__ == '__main__': 106 | 107 | main_loop() -------------------------------------------------------------------------------- /frankaemika/wik_eval.py: -------------------------------------------------------------------------------- 1 | # ----------------------------------------------------------------------------- 2 | # SPDX-License-Identifier: MIT 3 | # This file is part of the CDF project. 4 | # Copyright (c) 2024 Idiap Research Institute 5 | # Contributor: Yimming Li 6 | # ----------------------------------------------------------------------------- 7 | 8 | import numpy as np 9 | import os 10 | import sys 11 | import torch 12 | import matplotlib.pyplot as plt 13 | import time 14 | import math 15 | CUR_PATH = os.path.dirname(os.path.realpath(__file__)) 16 | sys.path.append(os.path.join(CUR_PATH,'../../RDF')) 17 | from mlp import MLPRegression 18 | from panda_layer.panda_layer import PandaLayer 19 | import bf_sdf 20 | from nn_cdf import CDF 21 | import copy 22 | from data_generator import DataGenerator 23 | 24 | device = torch.device("cuda" if torch.cuda.is_available() else "cpu") 25 | # device = torch.device("cpu") 26 | 27 | # initialize CDF 28 | cdf = CDF(device) 29 | model = MLPRegression(input_dims=10, output_dims=1, mlp_layers=[1024, 512, 256, 128, 128],skips=[], act_fn=torch.nn.ReLU, nerf=True) 30 | model.load_state_dict(torch.load(os.path.join(CUR_PATH,'model_dict.pt'))[49900]) 31 | model.to(device) 32 | 33 | # initialize bp_sdf model 34 | bp_sdf = bf_sdf.BPSDF(8,-1.0,1.0,cdf.panda,device) 35 | bp_sdf_model = torch.load(os.path.join(CUR_PATH,'../../RDF/models/BP_8.pt')) 36 | 37 | # initialize njsdf model 38 | sys.path.append(os.path.join(CUR_PATH,'../../Neural-JSDF/learning/nn-learning')) 39 | from sdf.robot_sdf import RobotSdfCollisionNet 40 | from torchmin import minimize 41 | 42 | tensor_args = {'device': device, 'dtype': torch.float32} 43 | njsdf_model = RobotSdfCollisionNet(in_channels=10, out_channels=9, layers=[256] * 4, skips=[]) 44 | njsdf_model.load_weights('../../Neural-JSDF/learning/nn-learning/sdf_256x5_mesh.pt', tensor_args) 45 | njsdf_model = njsdf_model.model 46 | 47 | def find_q_njsdf(x,q=None,batchsize = 10000): 48 | # x : (N,3) 49 | # scale x to workspace 50 | 51 | def compute_njsdf(x,q): 52 | q = q.unsqueeze(1).expand(len(q),len(x),7) 53 | x = x.unsqueeze(0).expand(len(q),len(x),3) 54 | x_cat = torch.cat([q,x],dim=-1).float().reshape(-1,10) 55 | dist = njsdf_model.forward(x_cat)/100. 56 | d = torch.min(dist[:,:8],dim=1)[0] 57 | return d 58 | 59 | def cost_function(q): 60 | # find q that d(x,q) = 0 61 | # q : B,2 62 | # x : N,3 63 | d = compute_njsdf(x,q) 64 | cost = torch.sum(d**2) 65 | return cost 66 | 67 | t0 = time.time() 68 | # optimizer for data generation 69 | if q is None: 70 | q = torch.rand(batchsize,7).to(device)*(cdf.panda.theta_max-cdf.panda.theta_min)+cdf.panda.theta_min 71 | q0 = copy.deepcopy(q) 72 | res = minimize( 73 | cost_function, 74 | q, 75 | method='l-bfgs', 76 | options=dict(line_search='strong-wolfe'), 77 | max_iter=50, 78 | disp=0 79 | ) 80 | 81 | return q0,res.x 82 | 83 | dg = DataGenerator(device) 84 | 85 | 86 | 87 | def eval_main(): 88 | # method_list = ['SDF','NJSDF','CDF'] 89 | 90 | method = 'CDF' 91 | time_list, num_solutions, avg_dist = [],[],[] 92 | if method == 'CDF': 93 | for t in range (100): 94 | x = torch.rand(1,3).to(device)-torch.tensor([[0.5,0.5,0]]).to(device) 95 | t0 = time.time() 96 | q = cdf.sample_q(batch_q=10000) 97 | q0 = copy.deepcopy(q) 98 | for _ in range (2): 99 | d,grad = cdf.inference_d_wrt_q(x,q,model,return_grad = True) 100 | q = cdf.projection(q,d,grad) 101 | q,grad = q.detach(),grad.detach() # release memory 102 | pose = torch.eye(4).unsqueeze(0).expand(len(q),-1,-1).to(cdf.device).float() 103 | sdf,_ = bp_sdf.get_whole_body_sdf_batch(x, pose, q, bp_sdf_model,use_derivative=False) 104 | 105 | error = sdf.reshape(-1).abs() 106 | mask = error < 0.03 107 | q = q[mask] 108 | dist = torch.norm(q-q0[mask],dim=-1).mean().item() 109 | print('iter:',t, 'avg dist: ',dist) 110 | if t>0: 111 | time_list.append(time.time()-t0) 112 | num_solutions.append(len(q)) 113 | avg_dist.append(dist) 114 | print('average time cost: ',np.mean(time_list),'num solutions: ',np.mean(num_solutions),'avg dist: ',np.mean(avg_dist)) 115 | 116 | if method == 'SDF': 117 | for t in range (1000): 118 | x = torch.rand(1,3).to(device)-torch.tensor([[0.5,0.5,0]]).to(device) 119 | t0 = time.time() 120 | mask,q,idx = dg.given_x_find_q(x,batchsize=1000,return_mask=True,epsilon=0.03) 121 | # pose = torch.eye(4).unsqueeze(0).expand(len(q),-1,-1).to(cdf.device).float() 122 | # sdf,_ = bp_sdf.get_whole_body_sdf_batch(x, pose, q, bp_sdf_model,use_derivative=False) 123 | 124 | # error = sdf.reshape(-1).abs() 125 | # mask = error < 0.03 126 | if t>0: 127 | time_list.append(time.time()-t0) 128 | num_solutions.append(len(q)) 129 | avg_dist.append(dist) 130 | print('average time cost: ',np.mean(time_list),'num solutions: ',np.mean(num_solutions)) 131 | 132 | if method == 'NJSDF': 133 | for t in range (100): 134 | x = torch.rand(1,3).to(device)-torch.tensor([[0.5,0.5,0]]).to(device) 135 | t0 = time.time() 136 | q0,q = find_q_njsdf(x) 137 | pose = torch.eye(4).unsqueeze(0).expand(len(q),-1,-1).to(cdf.device).float() 138 | sdf,_ = bp_sdf.get_whole_body_sdf_batch(x, pose, q, bp_sdf_model,use_derivative=False) 139 | 140 | error = sdf.reshape(-1).abs() 141 | mask = error < 0.03 142 | q0,q = q0[mask],q[mask] 143 | dist = torch.norm(q-q0,dim=-1).mean().item() 144 | if t>0: 145 | time_list.append(time.time()-t0) 146 | num_solutions.append(len(q)) 147 | avg_dist.append(dist) 148 | print('average time cost: ',np.mean(time_list),'num solutions: ',np.mean(num_solutions),'avg dist: ',np.mean(avg_dist)) 149 | 150 | def eval_dist(): 151 | batchsize = 10000 152 | d_list = [] 153 | for t in range(100): 154 | x = torch.rand(1,3).to(device)-torch.tensor([[0.5,0.5,0]]).to(device) 155 | q = torch.torch.rand(batchsize,7).to(device)*(cdf.panda.theta_max-cdf.panda.theta_min)+cdf.panda.theta_min 156 | q0 = copy.deepcopy(q) 157 | # CDF 158 | q.requires_grad = True 159 | for i in range (3): 160 | d,grad = cdf.inference_d_wrt_q(x,q,model,return_grad = True) 161 | q = cdf.projection(q,d,grad) 162 | if i == 0: 163 | q_step1 = q.clone() 164 | if i == 1: 165 | q_step2 = q.clone() 166 | if i ==2: 167 | q_step3 = q.clone() 168 | 169 | q_CDF1 = q_step1.clone() 170 | q_CDF2 = q_step2.clone() 171 | q_CDF3 = q_step3.clone() 172 | 173 | # SDF 174 | mask,q_sdf,_ = dg.given_x_find_q(x,q = q0,batchsize=batchsize,return_mask=True,epsilon=0.03) 175 | # Neural JSDF 176 | q0,q_njsdf = find_q_njsdf(x,q=q0) 177 | d_CDF1 = torch.norm(q_CDF1[mask]-q0[mask],dim=-1).mean() 178 | d_CDF2 = torch.norm(q_CDF2[mask]-q0[mask],dim=-1).mean() 179 | d_CDF3 = torch.norm(q_CDF3[mask]-q0[mask],dim=-1).mean() 180 | d_sdf = torch.norm(q_sdf-q0[mask],dim=-1).mean() 181 | d_njsdf = torch.norm(q_njsdf[mask]-q0[mask],dim=-1).mean() 182 | d_list.append([d_CDF1.item(),d_CDF2.item(),d_CDF3.item(),d_sdf.item(),d_njsdf.item()]) 183 | print('iter:',t,'d_CDF:',d_CDF1.item(),d_CDF2.item(),d_CDF3.item(),'d_sdf:',d_sdf.item(),'d_njsdf:',d_njsdf.item()) 184 | d_list = np.array(d_list) 185 | print('d_CDF1:',np.mean(d_list[:,0]),'d_CDF2:',np.mean(d_list[:,1]),'d_CDF3:',np.mean(d_list[:,2]),'d_sdf:',np.mean(d_list[:,3]),'d_njsdf:',np.mean(d_list[:,4])) 186 | 187 | 188 | # eval_dist() 189 | eval_main() 190 | 191 | -------------------------------------------------------------------------------- /readme.md: -------------------------------------------------------------------------------- 1 | ### Code for paper "Configuration Space Distance Fields for Manipulation Planning" (RSS 2024) 2 | 3 | [[Paper]](https://arxiv.org/abs/2406.01137)[[Project]](https://sites.google.com/view/cdfmp) 4 | 5 | CDF is a differentiable robot representation. It is a function d=f(p,q) where p is the spatial point and q is the joint configuration. DIfferent from SDF that indicates the distance from p to the robot surface defined by q, CDF uses the distance in radians, corresponding to the minimal joint motion required by the robot to contact with the point. Therefore, the distance is evenly spanned in joint space and the gradient points against to the point consistently, and we can solve the inverse kinematics problem through one-step gradient projection without the requirement of iterations. 6 | 7 | 8 | 9 | 10 | #### Dependencies 11 | - Python version: 3.8 (Tested) 12 | - Pytorch version:1.13.0 (Tested) 13 | - Install necessary packages 14 | 15 | ```sh 16 | pip install -r requirements.txt 17 | ``` 18 | 19 | - Install [pytorch3d](https://pytorch3d.org/) (optional) 20 | 21 | #### 2D examples 22 | 23 | Run 24 | 25 | ```sh 26 | python cdf.py 27 | ``` 28 | to see the main difference between SDFs and CDF. 29 | 30 | You can also replace the line 31 | ```python 32 | plot_fig1(scene_1) 33 | ``` 34 | with 35 | ```python 36 | plot_projection(scene_2) 37 | ``` 38 | to see how gradient projection works in solving inverse kinematics in one-step without iterations. 39 | 40 | #### 7 axis Franka robot experiments 41 | 42 | - We provide pretrained neural CDF model at 'model_dict.pt' and precomputed data ['data.pt'](https://drive.google.com/file/d/1vMs9M03gMBC8sZLV336S2bfFvYykO2Qz/view?usp=sharing) for franka emika panda robot. 43 | 44 | - Examples for data generation and neural network training can be found at 'data_generator.py' and 'nn_cdf.py'. 45 | 46 | Note: Many funtions require the RDF library (such as offline data generation, comparison with SDFs). You can run 47 | ```sh 48 | git clone https://github.com/yimingli1998/RDF.git 49 | ``` 50 | and put it in the same folder with CDF. 51 | 52 | Run 53 | ```sh 54 | python mp_ik.py 55 | ``` 56 | to see the gradient projection works on the robot. 57 | 58 | Run 59 | ```sh 60 | python throw_wik.py 61 | ``` 62 | to see the goalkeeper experiment in simulation. 63 | 64 | Run 65 | ```sh 66 | python wik_eval.py 67 | ``` 68 | to evalute the whole-body inverse kinematics and compare it with baseline approaches. 69 | 70 | For motion planning, you can run 71 | 72 | ```sh 73 | python qp_mp.py 74 | ``` 75 | 76 | to see a simple example that uses a reactive QP controller for collision avoidance. this example is in a static scene but it also works for moving objects. 77 | 78 | -------------------------------------------------------- 79 | 80 | CDF is maintained by Yiming LI and licensed under the MIT License. 81 | 82 | Copyright (c) 2024 Idiap Research Institute -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | torch 2 | numpy 3 | matplotlib 4 | pybullet 5 | pytorch-minimize --------------------------------------------------------------------------------