├── .gitignore ├── LICENSE ├── bbo_planning.py ├── bf_sdf.py ├── collision_avoidance_example ├── collision_avoidance_example.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 ├── data └── sdf_points │ └── test_data.npy ├── eval.py ├── figs ├── BBO.png ├── SDF01.png ├── SDF02.png └── SDF03.png ├── mlp.py ├── models ├── BP_24.pt ├── BP_8.pt ├── NN_AD.pt └── NN_LD.pt ├── nn_sdf.py ├── panda_layer ├── franka_sphere.yaml ├── mesh_test.py ├── meshes │ ├── visual │ │ ├── finger_vis.stl │ │ ├── link0_vis.stl │ │ ├── link1_vis.stl │ │ ├── link2_vis.stl │ │ ├── link3_vis.stl │ │ ├── link4_vis.stl │ │ ├── link5_vis.stl │ │ ├── link6_vis.stl │ │ ├── link7_vis.stl │ │ └── link8_vis.stl │ └── voxel_128 │ │ ├── finger.stl │ │ ├── link0.stl │ │ ├── link1.stl │ │ ├── link2.stl │ │ ├── link3.stl │ │ ├── link4.stl │ │ ├── link5.stl │ │ ├── link6.stl │ │ ├── link7.stl │ │ └── link8.stl ├── panda_layer.py ├── process_stl.py ├── remesh.py └── renormolize.py ├── readme.md ├── requirements.txt ├── robot_sdf.gif ├── sample_sdf_points.py ├── sphere.py ├── utils.py └── vis.py /.gitignore: -------------------------------------------------------------------------------- 1 | __pycache__ 2 | panda_layer/__pycache__ 3 | joint_conf.pt 4 | data/sdf_points/voxel* 5 | /output_meshes 6 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2023 Idiap Research Institute 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 | -------------------------------------------------------------------------------- /bbo_planning.py: -------------------------------------------------------------------------------- 1 | # ----------------------------------------------------------------------------- 2 | # SPDX-License-Identifier: MIT 3 | # This file is part of the RDF project. 4 | # Copyright (c) 2023 Idiap Research Institute 5 | # Contributor: Yimming Li 6 | # ----------------------------------------------------------------------------- 7 | 8 | import torch 9 | import trimesh 10 | import numpy as np 11 | import math 12 | from panda_layer.panda_layer import PandaLayer 13 | import bf_sdf 14 | 15 | class BBOPlanner(): 16 | def __init__(self, n_func,domain_min,domain_max,robot,device): 17 | self.n_func = n_func 18 | self.domain_min = domain_min 19 | self.domain_max = domain_max 20 | self.device = device 21 | self.robot = robot 22 | self.theta_max = self.robot.theta_max_soft 23 | self.theta_min = self.robot.theta_min_soft 24 | self.bp_sdf = bf_sdf.BPSDF(n_func,domain_min,domain_max,robot,device) 25 | self.model = torch.load(f'models/BP_8.pt') 26 | self.object_mesh = self.load_box_object() 27 | self.object_internal_points = self.compute_internal_points(num=5,use_surface_points=False) 28 | self.contact_points = self.compute_contact_points() 29 | self.pose_l = torch.from_numpy(np.identity(4)).to(self.device).reshape(-1, 4, 4).float() 30 | self.pose_r = torch.from_numpy(np.identity(4)).to(self.device).reshape(-1, 4, 4).float() 31 | self.pose_r[0,:3,3] = torch.tensor([0.6198, -0.7636, 0]).to(self.device) 32 | # self.mid_l = torch.tensor([ 0.42704887, 0.17838557, 0.10469598, -1.74670609, -0.05181788 , 2.16040988,-2.29006758]).reshape(-1,7).to(self.device) 33 | # self.mid_r = torch.tensor( [ 0.79520689 , 0.37705809, -0.01953359, -1.50133787, 0.14086509, 1.87535585, 1.05259796]).reshape(-1,7).to(self.device) 34 | 35 | def load_box_object(self): 36 | """load a box with size .3*.3*.3 based on urdf path""" 37 | self.box_pos = np.array([0.7934301890820722, 0.05027181819137394, 0.2246743147850761]) 38 | self.box_size = np.array([0.38, 0.24, 0.26]) 39 | self.box_rotation = np.array([[ 0.707, 0.707, 0], 40 | [-0.707, 0.707, 0], 41 | [0, 0, 1]]) 42 | # internal points 43 | mesh = trimesh.creation.box(self.box_size) 44 | mat = np.eye(4) 45 | mat[:3,3] = self.box_pos 46 | mat[:3,:3] = self.box_rotation 47 | mesh.apply_transform(mat) 48 | return mesh 49 | 50 | def compute_internal_points(self, num=10,use_surface_points=False): 51 | if use_surface_points: 52 | grid_x,grid_z = torch.meshgrid(torch.linspace(-self.box_size[0]/2.0,self.box_size[0]/2.0,num).to(self.device), 53 | torch.linspace(-self.box_size[2]/2.0,self.box_size[2]/2.0,num).to(self.device)) 54 | grid_y = torch.zeros_like(grid_x) + self.box_size[1]/2.0 55 | iternal_points_l = torch.cat([grid_x.reshape(-1,1),grid_y.reshape(-1,1),grid_z.reshape(-1,1)],dim=1) 56 | iternal_points_r = torch.cat([grid_x.reshape(-1,1),-grid_y.reshape(-1,1),grid_z.reshape(-1,1)],dim=1) 57 | iternal_points = torch.cat([iternal_points_l,iternal_points_r],dim=0).float() 58 | 59 | else: 60 | grid_x,grid_y,grid_z = torch.meshgrid(torch.linspace(-self.box_size[0]/2.0,self.box_size[0]/2.0,num).to(self.device), 61 | torch.linspace(-self.box_size[1]/2.0,self.box_size[1]/2.0,num).to(self.device), 62 | torch.linspace(-self.box_size[2]/2.0,self.box_size[2]/2.0,num).to(self.device)) 63 | 64 | iternal_points = torch.cat([grid_x.reshape(-1,1),grid_y.reshape(-1,1),grid_z.reshape(-1,1)],dim=1).float() 65 | iternal_points = torch.mm(iternal_points,torch.from_numpy(self.box_rotation).float().to(self.device).T) + torch.from_numpy(self.box_pos).float().to(self.device) 66 | return iternal_points 67 | 68 | def compute_contact_points(self): 69 | # # surface contact 70 | p_y = torch.linspace(-0.03,0.03,5,device=self.device) 71 | p_x = torch.ones_like(p_y).float().to(self.device)*(self.box_size[0]/2) 72 | p_z_h = torch.ones_like(p_y).float().to(self.device)*0.005 73 | p_z_l = -p_z_h 74 | p_l_h = torch.cat([-p_x.reshape(-1,1),p_y.reshape(-1,1),p_z_h.reshape(-1,1)],dim=1).float() 75 | p_l_l = torch.cat([-p_x.reshape(-1,1),p_y.reshape(-1,1),p_z_l.reshape(-1,1)],dim=1).float() 76 | p_l = torch.cat([p_l_h,p_l_l],dim=0) 77 | p_l = torch.mm(p_l, torch.from_numpy(self.box_rotation).float().to(self.device).T) + torch.from_numpy(self.box_pos).float().to(self.device) 78 | p_r_h = torch.cat([p_x.reshape(-1,1),p_y.reshape(-1,1),p_z_h.reshape(-1,1)],dim=1).float() 79 | p_r_l = torch.cat([p_x.reshape(-1,1),p_y.reshape(-1,1),p_z_l.reshape(-1,1)],dim=1).float() 80 | p_r = torch.cat([p_r_h,p_r_l],dim=0) 81 | p_r = torch.mm(p_r, torch.from_numpy(self.box_rotation).float().to(self.device).T) + torch.from_numpy(self.box_pos).float().to(self.device) 82 | n_r = torch.tensor([[-0.707,0.707,0]]).expand(len(p_r),3).float().to(self.device) 83 | n_l = torch.tensor([[0.707,-0.707,0]]).expand(len(p_l),3).float().to(self.device) 84 | return p_l,p_r,n_l,n_r 85 | 86 | def reaching_cost(self,pose, theta,p): 87 | B = theta.shape[0] 88 | sdf, joint_grad = self.bp_sdf.get_whole_body_sdf_with_joints_grad_batch(p, pose, theta, self.model,used_links=[5,6,7,8]) 89 | sdf, joint_grad = sdf.squeeze(0), joint_grad.squeeze(0) 90 | # reaching multiple points 91 | dist = sdf.mean(dim=1) 92 | cost = (sdf**2).mean(dim=1) 93 | grad = (2*sdf.unsqueeze(-1).expand_as(joint_grad)*joint_grad).mean(dim=1) 94 | return cost.reshape(B,1),grad.reshape(B,1,7),dist.reshape(B,1) 95 | 96 | def collision_cost(self,pose,theta,p): 97 | B = theta.shape[0] 98 | sdf, joint_grad = self.bp_sdf.get_whole_body_sdf_with_joints_grad_batch(p, pose, theta, self.model,used_links=[5,6,7,8]) 99 | sdf, joint_grad = sdf.squeeze(), joint_grad.squeeze() 100 | coll_mask = sdf<0 101 | sdf[~coll_mask] = 0 102 | joint_grad[~coll_mask] = 0 103 | cost = (sdf**2).mean(dim=1) 104 | grad = (2*sdf.unsqueeze(-1).expand_as(joint_grad)*joint_grad).mean(dim=1) 105 | penetration = -sdf.sum(dim=1) 106 | return cost.reshape(B,1),grad.reshape(B,1,7),penetration.reshape(B,1) 107 | 108 | def normal_cost(self,pose, theta,p,tgt_normal): 109 | B = theta.shape[0] 110 | delta = 0.001 111 | normal = self.bp_sdf.get_whole_body_normal_with_joints_grad_batch(p, pose, theta, self.model,used_links=[5,6,7,8]) 112 | tgt_normal = tgt_normal.unsqueeze(1).unsqueeze(0).expand_as(normal) 113 | cosine_similarities = 1 - torch.sum(normal*tgt_normal,dim=-1) 114 | cost = cosine_similarities[:,:,0].mean(dim=1) 115 | grad = ((cosine_similarities[:,:,1:] - cosine_similarities[:,:,:1])/delta).mean(dim=1) 116 | return cost.reshape(B,1),grad.reshape(B,1,7) 117 | 118 | def limit_angles(self,theta): 119 | theta = theta % (2*math.pi) # Wrap angles between 0 and 2*pi 120 | theta[theta > math.pi] -= 2 * math.pi # Shift angles to -pi to pi range 121 | theta_5_mask = (theta[:,5]>-math.pi)*(theta[:,5]self.theta_min).all(dim=1)).unsqueeze(1) 158 | accept = (dist<0.005) * (penetration < 0.01)* (c_normal<0.2) * joint_accept 159 | # accept = (c_reaching<0.01) * (c_normal<0.1) 160 | if accept.sum()>0: 161 | num_accept += accept.sum() 162 | print('num_accept:',num_accept) 163 | accept = accept.squeeze() 164 | theta_accept = theta[accept] 165 | # print('theta_accept',theta_accept) 166 | cost_accept = c[accept] 167 | # print('cost_accept',cost_accept) 168 | theta = theta[~accept] 169 | for (i,th_a) in enumerate(theta_accept): 170 | valid_theta_list.append(th_a) 171 | 172 | d_theta = (torch.matmul(torch.linalg.pinv(-J), c.unsqueeze(-1)) *10. *0.01)[:,:,0] # Gauss-Newton 173 | # d_theta = torch.clamp(d_theta_l, -0.05, 0.05) 174 | theta += d_theta[~accept] # Update state 175 | theta = self.limit_angles(theta) 176 | if len(valid_theta_list)>=3: 177 | valid_theta_list = valid_theta_list[:10] 178 | break 179 | else: 180 | d_theta = (torch.matmul(torch.linalg.pinv(-J), c.unsqueeze(-1)) *10. *0.01)[:,:,0] # Gauss-Newton 181 | # d_theta = torch.clamp(d_theta_l, -0.05, 0.05) 182 | theta += d_theta # Update state 183 | theta = self.limit_angles(theta) 184 | valid_theta_list = torch.cat(valid_theta_list,dim=0).reshape(-1,7) 185 | return valid_theta_list 186 | 187 | if __name__ == '__main__': 188 | device = 'cuda' 189 | panda = PandaLayer(device,mesh_path = "panda_layer/meshes/visual/*.stl") 190 | bbo_planner = BBOPlanner(n_func=8,domain_min=-1.0,domain_max=1.0,robot=panda,device=device) 191 | 192 | contact_points = bbo_planner.contact_points 193 | p_l,p_r,n_l,n_r = contact_points[0],contact_points[1],contact_points[2],contact_points[3] 194 | pose_l,pose_r = bbo_planner.pose_l,bbo_planner.pose_r 195 | mid_l = torch.tensor([ 0.42704887, 0.17838557, 0.10469598, -1.74670609, -0.05181788 , 2.16040988,-2.29006758]).reshape(-1,7).to(device) 196 | mid_r = torch.tensor( [ 0.79520689 , 0.37705809, -0.01953359, -1.50133787, 0.14086509, 1.87535585, 1.05259796]).reshape(-1,7).to(device) 197 | 198 | # planning for both arm 199 | theta_left = bbo_planner.optimizer(bbo_planner.pose_l,p_l,n_l,mid_l,batch = 64) 200 | theta_right = bbo_planner.optimizer(bbo_planner.pose_r,p_r,n_r,mid_r,batch = 64) 201 | joint_conf = { 202 | 'theta_left':theta_left, 203 | 'theta_right':theta_right 204 | } 205 | torch.save(joint_conf,'joint_conf.pt') 206 | 207 | # load planned joint conf 208 | data = torch.load('joint_conf.pt') 209 | theta_left = data['theta_left'] 210 | theta_right = data['theta_right'] 211 | print('theta_left',theta_left.shape,'theta_right',theta_right.shape) 212 | 213 | # visualize planning results 214 | scene = trimesh.Scene() 215 | pc1 = trimesh.PointCloud(bbo_planner.object_internal_points.detach().cpu().numpy(),colors = [0,255,0]) 216 | pc2 = trimesh.PointCloud(p_l.detach().cpu().numpy(),colors=[255,0,0]) 217 | pc3 = trimesh.PointCloud(p_r.detach().cpu().numpy(),colors=[255,0,0]) 218 | scene.add_geometry([pc1,pc2,pc3]) 219 | # mesh.visual.face_colors = [200,200,200,255] 220 | scene.add_geometry(bbo_planner.object_mesh) 221 | 222 | # # visualize the final joint configuration 223 | for t_l,t_r in zip(theta_left,theta_right): 224 | print('t left:',t_l) 225 | robot_l = panda.get_forward_robot_mesh(pose_l, t_l.reshape(-1,7))[0] 226 | robot_l = np.sum(robot_l) 227 | robot_l.visual.face_colors = [150,150,200,200] 228 | scene.add_geometry(robot_l,node_name='robot_l') 229 | 230 | print('t right:',t_r) 231 | robot_r = panda.get_forward_robot_mesh(pose_r, t_r.reshape(-1,7))[0] 232 | robot_r = np.sum(robot_r) 233 | robot_r.visual.face_colors = [150,200,150,200] 234 | scene.add_geometry(robot_r,node_name='robot_r') 235 | scene.show() 236 | 237 | scene.delete_geometry('robot_l') 238 | scene.delete_geometry('robot_r') 239 | 240 | -------------------------------------------------------------------------------- /bf_sdf.py: -------------------------------------------------------------------------------- 1 | # ----------------------------------------------------------------------------- 2 | # SPDX-License-Identifier: MIT 3 | # This file is part of the RDF project. 4 | # Copyright (c) 2023 Idiap Research Institute 5 | # Contributor: Yimming Li 6 | # ----------------------------------------------------------------------------- 7 | 8 | import torch 9 | import os 10 | import numpy as np 11 | np.set_printoptions(threshold=np.inf) 12 | import glob 13 | import trimesh 14 | import utils 15 | import mesh_to_sdf 16 | import skimage 17 | from panda_layer.panda_layer import PandaLayer 18 | import argparse 19 | 20 | CUR_DIR = os.path.dirname(os.path.abspath(__file__)) 21 | 22 | class BPSDF(): 23 | def __init__(self, n_func,domain_min,domain_max,robot,device): 24 | self.n_func = n_func 25 | self.domain_min = domain_min 26 | self.domain_max = domain_max 27 | self.device = device 28 | self.robot = robot 29 | self.model_path = os.path.join(CUR_DIR, 'models') 30 | 31 | def binomial_coefficient(self, n, k): 32 | return torch.exp(torch.lgamma(n + 1) - torch.lgamma(k + 1) - torch.lgamma(n - k + 1)) 33 | 34 | def build_bernstein_t(self,t, use_derivative=False): 35 | # t is normalized to [0,1] 36 | t =torch.clamp(t, min=1e-4, max=1-1e-4) 37 | n = self.n_func - 1 38 | i = torch.arange(self.n_func, device=self.device) 39 | comb = self.binomial_coefficient(torch.tensor(n, device=self.device), i) 40 | phi = comb * (1 - t).unsqueeze(-1) ** (n - i) * t.unsqueeze(-1) ** i 41 | if not use_derivative: 42 | return phi.float(),None 43 | else: 44 | dphi = -comb * (n - i) * (1 - t).unsqueeze(-1) ** (n - i - 1) * t.unsqueeze(-1) ** i + comb * i * (1 - t).unsqueeze(-1) ** (n - i) * t.unsqueeze(-1) ** (i - 1) 45 | dphi = torch.clamp(dphi, min=-1e4, max=1e4) 46 | return phi.float(),dphi.float() 47 | 48 | def build_basis_function_from_points(self,p,use_derivative=False): 49 | N = len(p) 50 | p = ((p - self.domain_min)/(self.domain_max-self.domain_min)).reshape(-1) 51 | phi,d_phi = self.build_bernstein_t(p,use_derivative) 52 | phi = phi.reshape(N,3,self.n_func) 53 | phi_x = phi[:,0,:] 54 | phi_y = phi[:,1,:] 55 | phi_z = phi[:,2,:] 56 | phi_xy = torch.einsum("ij,ik->ijk",phi_x,phi_y).view(-1,self.n_func**2) 57 | phi_xyz = torch.einsum("ij,ik->ijk",phi_xy,phi_z).view(-1,self.n_func**3) 58 | if use_derivative ==False: 59 | return phi_xyz,None 60 | else: 61 | d_phi = d_phi.reshape(N,3,self.n_func) 62 | d_phi_x_1D= d_phi[:,0,:] 63 | d_phi_y_1D = d_phi[:,1,:] 64 | d_phi_z_1D = d_phi[:,2,:] 65 | d_phi_x = torch.einsum("ij,ik->ijk",torch.einsum("ij,ik->ijk",d_phi_x_1D,phi_y).view(-1,self.n_func**2),phi_z).view(-1,self.n_func**3) 66 | d_phi_y = torch.einsum("ij,ik->ijk",torch.einsum("ij,ik->ijk",phi_x,d_phi_y_1D).view(-1,self.n_func**2),phi_z).view(-1,self.n_func**3) 67 | d_phi_z = torch.einsum("ij,ik->ijk",phi_xy,d_phi_z_1D).view(-1,self.n_func**3) 68 | d_phi_xyz = torch.cat((d_phi_x.unsqueeze(-1),d_phi_y.unsqueeze(-1),d_phi_z.unsqueeze(-1)),dim=-1) 69 | return phi_xyz,d_phi_xyz 70 | 71 | def train_bf_sdf(self,epoches=200): 72 | # represent SDF using basis functions 73 | mesh_path = os.path.join(CUR_DIR,"panda_layer/meshes/voxel_128/*") 74 | mesh_files = glob.glob(mesh_path) 75 | mesh_files = sorted(mesh_files)[1:] #except finger 76 | mesh_dict = {} 77 | for i,mf in enumerate(mesh_files): 78 | mesh_name = mf.split('/')[-1].split('.')[0] 79 | mesh = trimesh.load(mf) 80 | offset = mesh.bounding_box.centroid 81 | scale = np.max(np.linalg.norm(mesh.vertices-offset, axis=1)) 82 | mesh = mesh_to_sdf.scale_to_unit_sphere(mesh) 83 | mesh_dict[i] = {} 84 | mesh_dict[i]['mesh_name'] = mesh_name 85 | # load data 86 | data = np.load(f'./data/sdf_points/voxel_128_{mesh_name}.npy',allow_pickle=True).item() 87 | point_near_data = data['near_points'] 88 | sdf_near_data = data['near_sdf'] 89 | point_random_data = data['random_points'] 90 | sdf_random_data = data['random_sdf'] 91 | sdf_random_data[sdf_random_data <-1] = -sdf_random_data[sdf_random_data <-1] 92 | wb = torch.zeros(self.n_func**3).float().to(self.device) 93 | B = (torch.eye(self.n_func**3)/1e-4).float().to(self.device) 94 | # loss_list = [] 95 | for iter in range(epoches): 96 | choice_near = np.random.choice(len(point_near_data),1024,replace=False) 97 | p_near,sdf_near = torch.from_numpy(point_near_data[choice_near]).float().to(self.device),torch.from_numpy(sdf_near_data[choice_near]).float().to(self.device) 98 | choice_random = np.random.choice(len(point_random_data),256,replace=False) 99 | p_random,sdf_random = torch.from_numpy(point_random_data[choice_random]).float().to(self.device),torch.from_numpy(sdf_random_data[choice_random]).float().to(self.device) 100 | p = torch.cat([p_near,p_random],dim=0) 101 | sdf = torch.cat([sdf_near,sdf_random],dim=0) 102 | phi_xyz, _ = self.build_basis_function_from_points(p.float().to(self.device),use_derivative=False) 103 | 104 | K = torch.matmul(B,phi_xyz.T).matmul(torch.linalg.inv((torch.eye(len(p)).float().to(self.device)+torch.matmul(torch.matmul(phi_xyz,B),phi_xyz.T)))) 105 | B -= torch.matmul(K,phi_xyz).matmul(B) 106 | delta_wb = torch.matmul(K,(sdf - torch.matmul(phi_xyz,wb)).squeeze()) 107 | # loss = torch.nn.functional.mse_loss(torch.matmul(phi_xyz,wb).squeeze(), sdf, reduction='mean').item() 108 | # loss_list.append(loss) 109 | wb += delta_wb 110 | 111 | print(f'mesh name {mesh_name} finished!') 112 | mesh_dict[i] ={ 113 | 'mesh_name': mesh_name, 114 | 'weights': wb, 115 | 'offset': torch.from_numpy(offset), 116 | 'scale': scale, 117 | 118 | } 119 | if os.path.exists(self.model_path) is False: 120 | os.mkdir(self.model_path) 121 | torch.save(mesh_dict,f'{self.model_path}/BP_{self.n_func}.pt') # save the robot sdf model 122 | print(f'{self.model_path}/BP_{self.n_func}.pt model saved!') 123 | 124 | def sdf_to_mesh(self, model, nbData,use_derivative=False): 125 | verts_list, faces_list, mesh_name_list = [], [], [] 126 | for i, k in enumerate(model.keys()): 127 | mesh_dict = model[k] 128 | mesh_name = mesh_dict['mesh_name'] 129 | print(f'{mesh_name}') 130 | mesh_name_list.append(mesh_name) 131 | weights = mesh_dict['weights'].to(self.device) 132 | 133 | domain = torch.linspace(self.domain_min,self.domain_max,nbData).to(self.device) 134 | grid_x, grid_y, grid_z= torch.meshgrid(domain,domain,domain) 135 | grid_x, grid_y, grid_z = grid_x.reshape(-1,1), grid_y.reshape(-1,1), grid_z.reshape(-1,1) 136 | p = torch.cat([grid_x, grid_y, grid_z],dim=1).float().to(self.device) 137 | 138 | # split data to deal with memory issues 139 | p_split = torch.split(p, 10000, dim=0) 140 | d =[] 141 | for p_s in p_split: 142 | phi_p,d_phi_p = self.build_basis_function_from_points(p_s,use_derivative) 143 | d_s = torch.matmul(phi_p,weights) 144 | d.append(d_s) 145 | d = torch.cat(d,dim=0) 146 | 147 | verts, faces, normals, values = skimage.measure.marching_cubes( 148 | d.view(nbData,nbData,nbData).detach().cpu().numpy(), level=0.0, spacing=np.array([(self.domain_max-self.domain_min)/nbData] * 3) 149 | ) 150 | verts = verts - [1,1,1] 151 | verts_list.append(verts) 152 | faces_list.append(faces) 153 | return verts_list, faces_list,mesh_name_list 154 | 155 | def create_surface_mesh(self,model, nbData,vis =False, save_mesh_name=None): 156 | verts_list, faces_list,mesh_name_list = self.sdf_to_mesh(model, nbData) 157 | for verts, faces,mesh_name in zip(verts_list, faces_list,mesh_name_list): 158 | rec_mesh = trimesh.Trimesh(verts,faces) 159 | if vis: 160 | rec_mesh.show() 161 | if save_mesh_name != None: 162 | save_path = os.path.join(CUR_DIR,"output_meshes") 163 | if os.path.exists(save_path) is False: 164 | os.mkdir(save_path) 165 | trimesh.exchange.export.export_mesh(rec_mesh, os.path.join(save_path,f"{save_mesh_name}_{mesh_name}.stl")) 166 | 167 | def get_whole_body_sdf_batch(self,x,pose,theta,model,use_derivative = True, used_links = [0,1,2,3,4,5,6,7,8],return_index=False): 168 | 169 | B = len(theta) 170 | N = len(x) 171 | K = len(used_links) 172 | offset = torch.cat([model[i]['offset'].unsqueeze(0) for i in used_links],dim=0).to(self.device) 173 | offset = offset.unsqueeze(0).expand(B,K,3).reshape(B*K,3).float() 174 | scale = torch.tensor([model[i]['scale'] for i in used_links],device=self.device) 175 | scale = scale.unsqueeze(0).expand(B,K).reshape(B*K).float() 176 | trans_list = self.robot.get_transformations_each_link(pose,theta) 177 | 178 | fk_trans = torch.cat([t.unsqueeze(1) for t in trans_list],dim=1)[:,used_links,:,:].reshape(-1,4,4) # B,K,4,4 179 | x_robot_frame_batch = utils.transform_points(x.float(),torch.linalg.inv(fk_trans).float(),device=self.device) # B*K,N,3 180 | x_robot_frame_batch_scaled = x_robot_frame_batch - offset.unsqueeze(1) 181 | x_robot_frame_batch_scaled = x_robot_frame_batch_scaled/scale.unsqueeze(-1).unsqueeze(-1) #B*K,N,3 182 | 183 | x_bounded = torch.where(x_robot_frame_batch_scaled>1.0-1e-2,1.0-1e-2,x_robot_frame_batch_scaled) 184 | x_bounded = torch.where(x_bounded<-1.0+1e-2,-1.0+1e-2,x_bounded) 185 | res_x = x_robot_frame_batch_scaled - x_bounded 186 | 187 | if not use_derivative: 188 | phi,_ = self.build_basis_function_from_points(x_bounded.reshape(B*K*N,3), use_derivative=False) 189 | phi = phi.reshape(B,K,N,-1).transpose(0,1).reshape(K,B*N,-1) # K,B*N,-1 190 | weights_near = torch.cat([model[i]['weights'].unsqueeze(0) for i in used_links],dim=0).to(self.device) 191 | # sdf 192 | sdf = torch.einsum('ijk,ik->ij',phi,weights_near).reshape(K,B,N).transpose(0,1).reshape(B*K,N) # B,K,N 193 | sdf = sdf + res_x.norm(dim=-1) 194 | sdf = sdf.reshape(B,K,N) 195 | sdf = sdf*scale.reshape(B,K).unsqueeze(-1) 196 | sdf_value, idx = sdf.min(dim=1) 197 | if return_index: 198 | return sdf_value, None, idx 199 | return sdf_value, None 200 | else: 201 | phi,dphi = self.build_basis_function_from_points(x_bounded.reshape(B*K*N,3), use_derivative=True) 202 | phi_cat = torch.cat([phi.unsqueeze(-1),dphi],dim=-1) 203 | phi_cat = phi_cat.reshape(B,K,N,-1,4).transpose(0,1).reshape(K,B*N,-1,4) # K,B*N,-1,4 204 | 205 | weights_near = torch.cat([model[i]['weights'].unsqueeze(0) for i in used_links],dim=0).to(self.device) 206 | 207 | output = torch.einsum('ijkl,ik->ijl',phi_cat,weights_near).reshape(K,B,N,4).transpose(0,1).reshape(B*K,N,4) 208 | sdf = output[:,:,0] 209 | gradient = output[:,:,1:] 210 | # sdf 211 | sdf = sdf + res_x.norm(dim=-1) 212 | sdf = sdf.reshape(B,K,N) 213 | sdf = sdf*(scale.reshape(B,K).unsqueeze(-1)) 214 | sdf_value, idx = sdf.min(dim=1) 215 | # derivative 216 | gradient = res_x + torch.nn.functional.normalize(gradient,dim=-1) 217 | gradient = torch.nn.functional.normalize(gradient,dim=-1).float() 218 | # gradient = gradient.reshape(B,K,N,3) 219 | fk_rotation = fk_trans[:,:3,:3] 220 | gradient_base_frame = torch.einsum('ijk,ikl->ijl',fk_rotation,gradient.transpose(1,2)).transpose(1,2).reshape(B,K,N,3) 221 | # norm_gradient_base_frame = torch.linalg.norm(gradient_base_frame,dim=-1) 222 | 223 | # exit() 224 | # print(norm_gradient_base_frame) 225 | 226 | idx_grad = idx.unsqueeze(1).unsqueeze(-1).expand(B,K,N,3) 227 | gradient_value = torch.gather(gradient_base_frame,1,idx_grad)[:,0,:,:] 228 | # gradient_value = None 229 | if return_index: 230 | return sdf_value, gradient_value, idx 231 | return sdf_value, gradient_value 232 | 233 | def get_whole_body_sdf_with_joints_grad_batch(self,x,pose,theta,model,used_links = [0,1,2,3,4,5,6,7,8]): 234 | 235 | delta = 0.001 236 | B = theta.shape[0] 237 | theta = theta.unsqueeze(1) 238 | d_theta = (theta.expand(B,7,7)+ torch.eye(7,device=self.device).unsqueeze(0).expand(B,7,7)*delta).reshape(B,-1,7) 239 | theta = torch.cat([theta,d_theta],dim=1).reshape(B*8,7) 240 | pose = pose.unsqueeze(1).expand(B,8,4,4).reshape(B*8,4,4) 241 | sdf,_ = self.get_whole_body_sdf_batch(x,pose,theta,model,use_derivative = False, used_links = used_links) 242 | sdf = sdf.reshape(B,8,-1) 243 | d_sdf = (sdf[:,1:,:]-sdf[:,:1,:])/delta 244 | return sdf[:,0,:],d_sdf.transpose(1,2) 245 | 246 | def get_whole_body_normal_with_joints_grad_batch(self,x,pose,theta,model,used_links = [0,1,2,3,4,5,6,7,8]): 247 | delta = 0.001 248 | B = theta.shape[0] 249 | theta = theta.unsqueeze(1) 250 | d_theta = (theta.expand(B,7,7)+ torch.eye(7,device=self.device).unsqueeze(0).expand(B,7,7)*delta).reshape(B,-1,7) 251 | theta = torch.cat([theta,d_theta],dim=1).reshape(B*8,7) 252 | pose = pose.unsqueeze(1).expand(B,8,4,4).reshape(B*8,4,4) 253 | sdf, normal = self.get_whole_body_sdf_batch(x,pose,theta,model,use_derivative = True, used_links = used_links) 254 | normal = normal.reshape(B,8,-1,3).transpose(1,2) 255 | return normal # normal size: (B,N,8,3) normal[:,:,0,:] origin normal vector normal[:,:,1:,:] derivatives with respect to joints 256 | 257 | if __name__ =='__main__': 258 | 259 | parser = argparse.ArgumentParser() 260 | parser.add_argument('--device', default='cuda', type=str) 261 | parser.add_argument('--domain_max', default=1.0, type=float) 262 | parser.add_argument('--domain_min', default=-1.0, type=float) 263 | parser.add_argument('--n_func', default=8, type=int) 264 | parser.add_argument('--train', action='store_true') 265 | args = parser.parse_args() 266 | 267 | panda = PandaLayer(args.device) 268 | bp_sdf = BPSDF(args.n_func,args.domain_min,args.domain_max,panda,args.device) 269 | 270 | # # train Bernstein Polynomial model 271 | if args.train: 272 | bp_sdf.train_bf_sdf() 273 | 274 | # load trained model 275 | model_path = f'models/BP_{args.n_func}.pt' 276 | model = torch.load(model_path) 277 | 278 | # visualize the Bernstein Polynomial model for each robot link 279 | bp_sdf.create_surface_mesh(model,nbData=128,vis=True,save_mesh_name=f'BP_{args.n_func}') 280 | 281 | # visualize the Bernstein Polynomial model for the whole body 282 | theta = torch.tensor([0, -0.3, 0, -2.2, 0, 2.0, np.pi/4]).float().to(args.device).reshape(-1,7) 283 | pose = torch.from_numpy(np.identity(4)).to(args.device).reshape(-1, 4, 4).expand(len(theta),4,4).float() 284 | trans_list = panda.get_transformations_each_link(pose,theta) 285 | utils.visualize_reconstructed_whole_body(model, trans_list, tag=f'BP_{args.n_func}') 286 | 287 | # run RDF 288 | x = torch.rand(128,3).to(args.device)*2.0 - 1.0 289 | theta = torch.rand(2,7).to(args.device).float() 290 | pose = torch.from_numpy(np.identity(4)).unsqueeze(0).to(args.device).expand(len(theta),4,4).float() 291 | sdf,gradient = bp_sdf.get_whole_body_sdf_batch(x,pose,theta,model,use_derivative=True) 292 | print('sdf:',sdf.shape,'gradient:',gradient.shape) 293 | sdf,joint_grad = bp_sdf.get_whole_body_sdf_with_joints_grad_batch(x,pose,theta,model) 294 | print('sdf:',sdf.shape,'joint gradient:',joint_grad.shape) 295 | 296 | 297 | 298 | 299 | 300 | -------------------------------------------------------------------------------- /collision_avoidance_example/collision_avoidance_example.py: -------------------------------------------------------------------------------- 1 | # ----------------------------------------------------------------------------- 2 | #SPDX-License-Identifier: MIT 3 | # This file is part of the RDF project. 4 | # Copyright (c) 2023 Idiap Research Institute 5 | # Contributor: Yimming Li 6 | # ----------------------------------------------------------------------------- 7 | 8 | # ----------------------------------------------------------------------------- 9 | # SPDX-License-Identifier: MIT 10 | # This file is part of the RDF project. 11 | # Copyright (c) 2023 Idiap Research Institute 12 | # Contributor: Yimming Li 13 | # ----------------------------------------------------------------------------- 14 | 15 | 16 | import pybullet as p 17 | import pybullet_data as pd 18 | import numpy as np 19 | import sys 20 | import time 21 | from pybullet_panda_sim import PandaSim, SphereManager 22 | import torch 23 | import os 24 | import math 25 | CUR_PATH = os.path.dirname(os.path.realpath(__file__)) 26 | sys.path.append(os.path.join(CUR_PATH,'../')) 27 | import bf_sdf 28 | from panda_layer.panda_layer import PandaLayer 29 | 30 | device = torch.device("cuda" if torch.cuda.is_available() else "cpu") 31 | robot = PandaLayer(device) 32 | bp_sdf = bf_sdf.BPSDF(8,-1.0,1.0,robot,device) 33 | bp_sdf_model = torch.load(os.path.join(CUR_PATH,'../models/BP_8.pt')) 34 | 35 | def main_loop(): 36 | 37 | # p.connect(p.GUI, options='--background_color_red=0.5 --background_color_green=0.5' + 38 | # ' --background_color_blue=0.5 --width=1600 --height=1000') 39 | p.connect(p.GUI, options='--background_color_red=1 --background_color_green=1' + 40 | ' --background_color_blue=1 --width=1000 --height=1000') 41 | 42 | p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0) 43 | p.configureDebugVisualizer(lightPosition=[5, 5, 5]) 44 | p.setPhysicsEngineParameter(maxNumCmdPer1ms=1000) 45 | #p.resetDebugVisualizerCamera(cameraDistance=1.5, cameraYaw=110, cameraPitch=-10, cameraTargetPosition=[0, 0, 0.5]) 46 | # p.resetDebugVisualizerCamera(cameraDistance=1.5, cameraYaw=90, cameraPitch=0, cameraTargetPosition=[0, 0, 0.5]) 47 | #p.resetDebugVisualizerCamera(cameraDistance=1.5, cameraYaw=110, cameraPitch=-25, cameraTargetPosition=[0, 0, 0.5]) 48 | p.resetDebugVisualizerCamera(cameraDistance=1.5, cameraYaw=145, cameraPitch=-10, cameraTargetPosition=[0, 0, 0.6]) 49 | 50 | p.setAdditionalSearchPath(pd.getDataPath()) 51 | timeStep = 0.01 52 | p.setTimeStep(timeStep) 53 | p.setGravity(0, 0, -9.81) 54 | p.setRealTimeSimulation(1) 55 | ## spawn franka robot 56 | base_pos = [0, 0, 0] 57 | base_rot = p.getQuaternionFromEuler([0, 0, 0]) 58 | panda = PandaSim(p, base_pos, base_rot) 59 | q_target = np.array([1.34643478, -0.06273081, 0.16822745, -2.54468149, -0.32664142,3.57070459, 1.14682636]) 60 | q_init= np.array([-0.23643478, -0.06273081, 0.16822745, -2.54468149, -0.32664142,3.57070459, 1.14682636]) 61 | panda.set_joint_positions(q_init) 62 | time.sleep(1) 63 | 64 | # obstacle 65 | sphere_manager = SphereManager(p) 66 | sphere_center = [0.3, 0.4, 0.5] 67 | pose = torch.eye(4).unsqueeze(0).to(device).float() 68 | while True: 69 | sphere_manager.create_sphere(sphere_center, 0.05, [0.8500, 0.3250, 0.0980, 1]) 70 | pts = torch.tensor(sphere_center).unsqueeze(0).to(device).float() 71 | q_current = panda.get_joint_positions() 72 | theta = torch.tensor(q_current).unsqueeze(0).to(device).float() 73 | sdf,joint_grad = bp_sdf.get_whole_body_sdf_with_joints_grad_batch(pts,pose,theta,bp_sdf_model) 74 | # print(f'sdf: {sdf.shape}, joint_grad: {joint_grad.shape}') 75 | sdf,joint_grad = sdf.squeeze(0),joint_grad.squeeze(0) 76 | sdf_min,sdf_min_idx = torch.min(sdf,dim=0) 77 | sdf_min_grad = joint_grad[sdf_min_idx] 78 | # print(f'sdf_min: {sdf_min.shape}, sdf_min_grad: {sdf_min_grad.shape}') 79 | 80 | goal_reaching_vec = q_target - q_current 81 | goal_reaching_vec = goal_reaching_vec/np.linalg.norm(goal_reaching_vec) 82 | 83 | collision_avoidance_vec = torch.nn.functional.normalize(sdf_min_grad,dim=-1).detach().cpu().numpy() 84 | print(sdf_min) 85 | if sdf_min > 0.2: 86 | vec = goal_reaching_vec 87 | else: 88 | vec = collision_avoidance_vec 89 | 90 | q_current = q_current + 0.05*vec 91 | print(f'q_current: {q_current}') 92 | panda.set_joint_positions(q_current) 93 | sphere_center = sphere_center + np.array([0.0,-0.01,0.0]) 94 | time.sleep(0.01) 95 | sphere_manager.delete_spheres() 96 | 97 | 98 | 99 | if __name__ == '__main__': 100 | main_loop() -------------------------------------------------------------------------------- /collision_avoidance_example/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 -------------------------------------------------------------------------------- /collision_avoidance_example/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 | -------------------------------------------------------------------------------- /collision_avoidance_example/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 | -------------------------------------------------------------------------------- /collision_avoidance_example/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 | -------------------------------------------------------------------------------- /collision_avoidance_example/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 | -------------------------------------------------------------------------------- /collision_avoidance_example/panda_urdf/meshes/visual/colors.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yimingli1998/RDF/a1cd0d503831fc2b8b921ebb251d8bc714921522/collision_avoidance_example/panda_urdf/meshes/visual/colors.png -------------------------------------------------------------------------------- /collision_avoidance_example/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 | -------------------------------------------------------------------------------- /collision_avoidance_example/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 | -------------------------------------------------------------------------------- /collision_avoidance_example/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 | -------------------------------------------------------------------------------- /collision_avoidance_example/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 | -------------------------------------------------------------------------------- /collision_avoidance_example/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 | -------------------------------------------------------------------------------- /collision_avoidance_example/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 | -------------------------------------------------------------------------------- /collision_avoidance_example/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 | -------------------------------------------------------------------------------- /collision_avoidance_example/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 | -------------------------------------------------------------------------------- /collision_avoidance_example/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 | -------------------------------------------------------------------------------- /collision_avoidance_example/pybullet_panda_sim.py: -------------------------------------------------------------------------------- 1 | # ----------------------------------------------------------------------------- 2 | #SPDX-License-Identifier: MIT 3 | # This file is part of the RDF project. 4 | # Copyright (c) 2023 Idiap Research Institute 5 | # Contributor: Yimming Li 6 | # ----------------------------------------------------------------------------- 7 | 8 | # ----------------------------------------------------------------------------- 9 | # SPDX-License-Identifier: MIT 10 | # This file is part of the RDF project. 11 | # Copyright (c) 2023 Idiap Research Institute 12 | # Contributor: Yimming Li 13 | # ----------------------------------------------------------------------------- 14 | 15 | 16 | import time 17 | import numpy as np 18 | from math import pi 19 | import os 20 | import sys 21 | 22 | CUR_PATH = os.path.dirname(os.path.realpath(__file__)) 23 | pandaNumDofs = 7 24 | 25 | # restpose 26 | rp = [0.0,0.0, -1.57461, -1.60788, -0.785175, 1.54666, -0.882595, 0.02, 0.02] 27 | 28 | class PandaSim(): 29 | def __init__(self, bullet_client, base_pos, base_rot): 30 | self.bullet_client = bullet_client 31 | self.bullet_client.setAdditionalSearchPath('content/urdfs') 32 | self.base_pos = np.array(base_pos) 33 | self.base_rot = np.array(base_rot) 34 | # print("offset=",offset) 35 | flags = self.bullet_client.URDF_ENABLE_CACHED_GRAPHICS_SHAPES 36 | table_rot = self.bullet_client.getQuaternionFromEuler([pi / 2, 0, pi]) 37 | self.rp = rp 38 | self.panda = self.bullet_client.loadURDF(os.path.join(CUR_PATH,"panda_urdf/panda.urdf"), self.base_pos, 39 | self.base_rot, useFixedBase=True, flags=flags) 40 | self.reset() 41 | self.t = 0. 42 | # self.set_joint_positions(rp) 43 | 44 | def reset(self): 45 | index = 0 46 | for j in range(self.bullet_client.getNumJoints(self.panda)): 47 | self.bullet_client.changeDynamics(self.panda, j, linearDamping=0, angularDamping=0) 48 | info = self.bullet_client.getJointInfo(self.panda, j) 49 | jointName = info[1] 50 | jointType = info[2] 51 | if (jointType == self.bullet_client.JOINT_PRISMATIC): 52 | self.bullet_client.resetJointState(self.panda, j, self.rp[index]) 53 | index = index + 1 54 | if (jointType == self.bullet_client.JOINT_REVOLUTE): 55 | self.bullet_client.resetJointState(self.panda, j, self.rp[index]) 56 | index = index + 1 57 | 58 | def set_joint_positions(self, joint_positions): 59 | for i in range(pandaNumDofs): 60 | self.bullet_client.setJointMotorControl2(self.panda, i, self.bullet_client.POSITION_CONTROL, 61 | joint_positions[i], force=240.) 62 | self.set_finger_positions(0.04) 63 | 64 | def set_finger_positions(self, gripper_opening): 65 | self.bullet_client.setJointMotorControl2(self.panda, 9, self.bullet_client.POSITION_CONTROL, 66 | gripper_opening/2, force=5 * 240.) 67 | self.bullet_client.setJointMotorControl2(self.panda, 10, self.bullet_client.POSITION_CONTROL, 68 | -gripper_opening/2, force=5 * 240.) 69 | 70 | 71 | def get_joint_positions(self): 72 | joint_state = [] 73 | for i in range(pandaNumDofs): 74 | joint_state.append(self.bullet_client.getJointState(self.panda, i)[0]) 75 | return joint_state 76 | 77 | class SphereManager: 78 | def __init__(self, pybullet_client): 79 | self.pb = pybullet_client 80 | self.spheres = [] 81 | self.color = [.7, .1, .1, 1] 82 | self.color = [.63, .07, .185, 1] 83 | # self.color = [0.8500, 0.3250, 0.0980, 1] 84 | def create_sphere(self, position, radius, color): 85 | sphere = self.pb.createVisualShape(self.pb.GEOM_SPHERE, 86 | radius=radius, 87 | rgbaColor=color, specularColor=[0, 0, 0, 1]) 88 | sphere = self.pb.createCollisionShape(self.pb.GEOM_SPHERE, 89 | radius=radius) 90 | 91 | sphere = self.pb.createMultiBody(baseVisualShapeIndex=sphere, 92 | basePosition=position) 93 | self.spheres.append(sphere) 94 | 95 | def initialize_spheres(self, obstacle_array): 96 | for obstacle in obstacle_array: 97 | self.create_sphere(obstacle[0:3], obstacle[3], self.color) 98 | 99 | def delete_spheres(self): 100 | for sphere in self.spheres: 101 | self.pb.removeBody(sphere) 102 | self.spheres = [] 103 | 104 | def update_spheres(self, obstacle_array): 105 | if (obstacle_array is not None) and (len(self.spheres) == len(obstacle_array)): 106 | for i, sphere in enumerate(self.spheres): 107 | self.pb.resetBasePositionAndOrientation(sphere, 108 | obstacle_array[i, 0:3], 109 | [1, 0, 0, 0]) 110 | else: 111 | print("Number of spheres and obstacles do not match") 112 | self.delete_spheres() 113 | self.initialize_spheres(obstacle_array) 114 | -------------------------------------------------------------------------------- /data/sdf_points/test_data.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yimingli1998/RDF/a1cd0d503831fc2b8b921ebb251d8bc714921522/data/sdf_points/test_data.npy -------------------------------------------------------------------------------- /eval.py: -------------------------------------------------------------------------------- 1 | 2 | # ----------------------------------------------------------------------------- 3 | # SPDX-License-Identifier: MIT 4 | # This file is part of the RDF project. 5 | # Copyright (c) 2023 Idiap Research Institute 6 | # Contributor: Yimming Li 7 | # ----------------------------------------------------------------------------- 8 | 9 | import os 10 | import numpy as np 11 | import torch 12 | import time 13 | import bf_sdf,nn_sdf,sphere 14 | from panda_layer.panda_layer import PandaLayer 15 | import argparse 16 | import utils 17 | import yaml 18 | 19 | CUR_DIR = os.path.dirname(os.path.abspath(__file__)) 20 | 21 | if __name__ =='__main__': 22 | 23 | parser = argparse.ArgumentParser() 24 | parser.add_argument('--device', default='cuda', type=str) 25 | parser.add_argument('--method', default='BP_8', type=str) 26 | parser.add_argument('--type', default='RDF', type=str) 27 | parser.add_argument('--vis', action='store_true') 28 | parser.add_argument('--vis_rec_robot_surface', action='store_true') 29 | args = parser.parse_args() 30 | 31 | data = np.load(f'./data/sdf_points/test_data.npy',allow_pickle=True).item() 32 | panda = PandaLayer(args.device) 33 | 34 | if args.method == 'BP_8': 35 | bpSdf = bf_sdf.BPSDF(8,-1,1,panda,args.device) 36 | model = torch.load(f'models/{args.method}.pt') 37 | elif args.method == 'BP_24': 38 | bpSdf = bf_sdf.BPSDF(24,-1,1,panda,args.device) 39 | model = torch.load(f'models/{args.method}.pt') 40 | elif args.method == 'NN_LD' or args.method == 'NN_AD': 41 | nnSdf = nn_sdf.NNSDF(panda,device=args.device) 42 | model = torch.load(f'models/{args.method}.pt') 43 | elif args.method == 'Sphere': 44 | sphere_sdf = sphere.SphereSDF(args.device) 45 | with open(os.path.join(CUR_DIR,'panda_layer/franka_sphere.yaml'), 'r') as f: 46 | conf = yaml.load(f,Loader=yaml.FullLoader)['collision_spheres'] 47 | rs, cs = sphere_sdf.get_sphere_param(conf) 48 | 49 | # eval EACH LINK 50 | if args.type == 'LINK': 51 | # save reconstructed mesh for each robo link 52 | print('save mesh from sdfs...') 53 | if args.method == 'BP_8' or args.method == 'BP_24': 54 | bpSdf.create_surface_mesh(model,nbData=256,vis = args.vis, save_mesh_name=args.method) 55 | elif args.method == 'NN_LD' or args.method == 'NN_AD': 56 | nnSdf.create_surface_mesh(model,nbData=256,vis = args.vis, save_mesh_name=args.method) 57 | 58 | # eval chamfer distance 59 | cd_mean,cd_max = utils.eval_chamfer_distance(tag=args.method) 60 | print(f'eval chamfer distance:\tmethod:{args.method} cd mean:{cd_mean}\tcd max:{cd_max}') 61 | 62 | # eval RDF 63 | if args.type =='RDF': 64 | # eval quality 65 | res = [] 66 | for k in data.keys(): 67 | x = data[k]['pts'] 68 | sdf = data[k]['sdf'] 69 | pose = data[k]['pose'] 70 | theta = data[k]['theta'] 71 | if args.method == 'BP_8' or args.method == 'BP_24': 72 | pred_sdf, _ = bpSdf.get_whole_body_sdf_batch(x,pose,theta,model,use_derivative=False) 73 | elif args.method == 'NN_LD' or args.method == 'NN_AD': 74 | pred_sdf = nnSdf.whole_body_nn_sdf(x,pose,theta,model) 75 | elif args.method == 'Sphere': 76 | pred_sdf, _ = sphere_sdf.get_sdf(x, pose, theta, rs, cs) 77 | res.append(utils.print_eval(pred_sdf,sdf)) 78 | res = np.mean(res,axis=0)*1000. 79 | print(f'Evaluate produced robot distance field:\t' 80 | f'Method:{args.method}\t' 81 | f'MAE:{res[0]:.3f}\t' 82 | f'RMSE:{res[2]:.3f}\t' 83 | f'MAE_NEAR:{res[3]:.3f}\t' 84 | f'RMSE_NEAR:{res[5]:.3f}\t' 85 | f'MAE_FAR:{res[6]:.3f}\t' 86 | f'RMSE_FAR:{res[8]:.3f}\t' 87 | ) 88 | 89 | # eval time 90 | t = [] 91 | x = torch.rand(1024,3).to(args.device)*2.0 - 1.0 92 | panda = PandaLayer(args.device) 93 | theta = torch.rand(1,7).to(args.device).float() 94 | pose = torch.from_numpy(np.identity(4)).unsqueeze(0).to(args.device).expand(len(theta),4,4).float() 95 | for _ in range(100): 96 | time0 = time.time() 97 | if args.method == 'BP_8' or args.method == 'BP_24': 98 | pred_sdf, _ = bpSdf.get_whole_body_sdf_batch(x,pose,theta,model,use_derivative=False) 99 | elif args.method == 'NN_LD' or args.method == 'NN_AD': 100 | pred_sdf = nnSdf.whole_body_nn_sdf(x,pose,theta,model) 101 | elif args.method == 'Sphere': 102 | pred_sdf, _ = sphere_sdf.get_sdf(x, pose, theta, rs, cs) 103 | t.append(time.time()-time0) 104 | print(f'Method:{args.method}\t Time Cost:{np.mean(t[1:])*1000.}ms\t') 105 | 106 | # vis reconstructed robot surface (please make sure you have saved the reconstructed mesh before) 107 | if args.vis_rec_robot_surface: 108 | theta = torch.tensor([0, -0.3, 0, -2.2, 0, 2.0, np.pi/4]).float().to(args.device).reshape(-1,7) 109 | pose = torch.from_numpy(np.identity(4)).to(args.device).reshape(-1, 4, 4).expand(len(theta),4,4).float() 110 | trans_list = panda.get_transformations_each_link(pose,theta) 111 | utils.visualize_reconstructed_whole_body(model, trans_list, tag=args.method) 112 | 113 | 114 | -------------------------------------------------------------------------------- /figs/BBO.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yimingli1998/RDF/a1cd0d503831fc2b8b921ebb251d8bc714921522/figs/BBO.png -------------------------------------------------------------------------------- /figs/SDF01.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yimingli1998/RDF/a1cd0d503831fc2b8b921ebb251d8bc714921522/figs/SDF01.png -------------------------------------------------------------------------------- /figs/SDF02.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yimingli1998/RDF/a1cd0d503831fc2b8b921ebb251d8bc714921522/figs/SDF02.png -------------------------------------------------------------------------------- /figs/SDF03.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yimingli1998/RDF/a1cd0d503831fc2b8b921ebb251d8bc714921522/figs/SDF03.png -------------------------------------------------------------------------------- /mlp.py: -------------------------------------------------------------------------------- 1 | # ----------------------------------------------------------------------------- 2 | #SPDX-License-Identifier: MIT 3 | # This file is part of the RDF project. 4 | # Copyright (c) 2023 Idiap Research Institute 5 | # Contributor: Yimming Li 6 | # ----------------------------------------------------------------------------- 7 | 8 | import torch 9 | from torch import nn 10 | from torch.nn import Sequential as Seq, Linear as Lin, ReLU, ReLU6, ELU, Dropout, BatchNorm1d as BN, LayerNorm as LN, Tanh 11 | import numpy as np 12 | 13 | def xavier(param): 14 | """ initialize weights with xavier. 15 | Args: 16 | param (network params): params to initialize. 17 | """ 18 | nn.init.xavier_uniform(param) 19 | 20 | def he_init(param): 21 | """initialize weights with he. 22 | Args: 23 | param (network params): params to initialize. 24 | """ 25 | nn.init.kaiming_uniform_(param,nonlinearity='relu') 26 | nn.init.normal(param) 27 | 28 | def weights_init(m): 29 | """Function to initialize weights of a nn. 30 | Args: 31 | m (network params): pass in model.parameters() 32 | """ 33 | fn = he_init 34 | if isinstance(m, nn.Conv2d): 35 | fn(m.weight.data) 36 | m.bias.data.zero_() 37 | elif isinstance(m, nn.Conv3d): 38 | fn(m.weight.data) 39 | m.bias.data.zero_() 40 | elif isinstance(m, nn.Linear): 41 | fn(m.weight.data) 42 | if(m.bias is not None): 43 | m.bias.data.zero_() 44 | 45 | def MLP(channels, act_fn=ReLU, islast = False): 46 | """Automatic generation of mlp given some 47 | Args: 48 | channels (int): number of channels in input 49 | dropout_ratio (float, optional): dropout used after every layer. Defaults to 0.0. 50 | batch_norm (bool, optional): batch norm after every layer. Defaults to False. 51 | act_fn ([type], optional): activation function after every layer. Defaults to ReLU. 52 | layer_norm (bool, optional): layer norm after every layer. Defaults to False. 53 | nerf (bool, optional): use positional encoding (x->[sin(x),cos(x)]). Defaults to True. 54 | Returns: 55 | nn sequential layers 56 | """ 57 | if not islast: 58 | layers = [Seq(Lin(channels[i - 1], channels[i]), act_fn()) 59 | for i in range(1, len(channels))] 60 | else: 61 | layers = [Seq(Lin(channels[i - 1], channels[i]), act_fn()) 62 | for i in range(1, len(channels)-1)] 63 | layers.append(Seq(Lin(channels[-2], channels[-1]))) 64 | 65 | layers = Seq(*layers) 66 | 67 | return layers 68 | 69 | 70 | class MLPRegression(nn.Module): 71 | def __init__(self, input_dims=10, output_dims=1, mlp_layers=[128, 128, 128, 128, 128],skips=[2], act_fn=ReLU, nerf=True): 72 | """Create an instance of mlp nn model 73 | Args: 74 | input_dims (int): number of channels 75 | output_dims (int): output channel size 76 | mlp_layers (list, optional): perceptrons in each layer. Defaults to [256, 128, 128]. 77 | dropout_ratio (float, optional): dropout after every layer. Defaults to 0.0. 78 | batch_norm (bool, optional): batch norm after every layer. Defaults to False. 79 | 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. 80 | act_fn ([type], optional): activation function after every layer. Defaults to ELU. 81 | layer_norm (bool, optional): layer norm after every layer. Defaults to False. 82 | nerf (bool, optional): use positional encoding (x->[sin(x),cos(x)]). Defaults to False. 83 | """ 84 | super(MLPRegression, self).__init__() 85 | 86 | mlp_arr = [] 87 | if (nerf): 88 | input_dims = 3*input_dims 89 | if len(skips)>0: 90 | mlp_arr.append(mlp_layers[0:skips[0]]) 91 | mlp_arr[0][-1]-=input_dims 92 | for s in range(1,len(skips)): 93 | mlp_arr.append(mlp_layers[skips[s-1]:skips[s]]) 94 | mlp_arr[-1][-1]-=input_dims 95 | mlp_arr.append(mlp_layers[skips[-1]:]) 96 | else: 97 | mlp_arr.append(mlp_layers) 98 | 99 | mlp_arr[-1].append(output_dims) 100 | 101 | mlp_arr[0].insert(0,input_dims) 102 | self.layers = nn.ModuleList() 103 | for arr in mlp_arr[0:-1]: 104 | self.layers.append(MLP(arr,act_fn=act_fn, islast=False)) 105 | self.layers.append(MLP(mlp_arr[-1],act_fn=act_fn, islast=True)) 106 | 107 | self.nerf = nerf 108 | 109 | def forward(self, x): 110 | """forward pass on network.""" 111 | if(self.nerf): 112 | x_nerf = torch.cat((x, torch.sin(x), torch.cos(x)), dim=-1) 113 | else: 114 | x_nerf = x 115 | y = self.layers[0](x_nerf) 116 | for layer in self.layers[1:]: 117 | y = layer(torch.cat((y, x_nerf), dim=1)) 118 | return y 119 | 120 | def reset_parameters(self): 121 | """Use this function to initialize weights. Doesn't help much for mlp. 122 | """ 123 | self.apply(weights_init) -------------------------------------------------------------------------------- /models/BP_24.pt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yimingli1998/RDF/a1cd0d503831fc2b8b921ebb251d8bc714921522/models/BP_24.pt -------------------------------------------------------------------------------- /models/BP_8.pt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yimingli1998/RDF/a1cd0d503831fc2b8b921ebb251d8bc714921522/models/BP_8.pt -------------------------------------------------------------------------------- /models/NN_AD.pt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yimingli1998/RDF/a1cd0d503831fc2b8b921ebb251d8bc714921522/models/NN_AD.pt -------------------------------------------------------------------------------- /models/NN_LD.pt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yimingli1998/RDF/a1cd0d503831fc2b8b921ebb251d8bc714921522/models/NN_LD.pt -------------------------------------------------------------------------------- /nn_sdf.py: -------------------------------------------------------------------------------- 1 | # ----------------------------------------------------------------------------- 2 | # SPDX-License-Identifier: MIT 3 | # This file is part of the RDF project. 4 | # Copyright (c) 2023 Idiap Research Institute 5 | # Contributor: Yimming Li 6 | # ----------------------------------------------------------------------------- 7 | 8 | 9 | import os 10 | import numpy as np 11 | import glob 12 | import torch 13 | import trimesh 14 | from mlp import MLPRegression 15 | import torch.nn.functional as F 16 | import mesh_to_sdf 17 | import skimage 18 | from panda_layer.panda_layer import PandaLayer 19 | import utils 20 | 21 | CUR_DIR = os.path.dirname(os.path.abspath(__file__)) 22 | 23 | class NNSDF(): 24 | def __init__(self,robot,lr=0.002,device='cuda'): 25 | self.device = device 26 | self.robot = robot 27 | self.lr = lr 28 | self.model_path = os.path.join(CUR_DIR, 'models') 29 | 30 | def train_nn(self,epoches=2000): 31 | mesh_path = os.path.dirname(os.path.realpath(__file__)) + "/panda_layer/meshes/voxel_128/*" 32 | mesh_files = glob.glob(mesh_path) 33 | mesh_files = sorted(mesh_files)[1:] #except finger 34 | mesh_dict = {} 35 | for i,mf in enumerate(mesh_files): 36 | mesh_name = mf.split('/')[-1].split('.')[0] 37 | print('mesh_name: ',mesh_name) 38 | mesh = trimesh.load(mf) 39 | mesh = mesh_to_sdf.scale_to_unit_sphere(mesh) 40 | offset = mesh.bounding_box.centroid 41 | scale = np.max(np.linalg.norm(mesh.vertices-offset, axis=1)) 42 | 43 | mesh_dict[i] = {} 44 | mesh_dict[i]['mesh_name'] = mesh_name 45 | 46 | # load data 47 | data = np.load(f'./data/sdf_points/voxel_128_{mesh_name}.npy',allow_pickle=True).item() 48 | point_near_data = data['near_points'] 49 | sdf_near_data = data['near_sdf'] 50 | point_random_data = data['random_points'] 51 | sdf_random_data = data['random_sdf'] 52 | sdf_random_data[sdf_random_data <-1] = -sdf_random_data[sdf_random_data <-1] 53 | model = MLPRegression(input_dims=3, output_dims=1, mlp_layers=[128, 128, 128, 128, 128],skips=[], act_fn=torch.nn.ReLU, nerf=True) 54 | model.to(device) 55 | optimizer = torch.optim.Adam(model.parameters(), lr=lr) 56 | scheduler = torch.optim.lr_scheduler.ReduceLROnPlateau(optimizer, mode='min', factor=0.5, patience=5000, 57 | threshold=0.01, threshold_mode='rel', 58 | cooldown=0, min_lr=0, eps=1e-04, verbose=True) 59 | scaler = torch.cuda.amp.GradScaler(enabled=True) 60 | for iter in range(epoches): 61 | model.train() 62 | with torch.cuda.amp.autocast(): 63 | choice_near = np.random.choice(len(point_near_data),1024,replace=False) 64 | p_near,sdf_near = torch.from_numpy(point_near_data[choice_near]).float().to(device),torch.from_numpy(sdf_near_data[choice_near]).float().to(device) 65 | choice_random = np.random.choice(len(point_random_data),256,replace=False) 66 | p_random,sdf_random = torch.from_numpy(point_random_data[choice_random]).float().to(device),torch.from_numpy(sdf_random_data[choice_random]).float().to(device) 67 | p = torch.cat([p_near,p_random],dim=0) 68 | sdf = torch.cat([sdf_near,sdf_random],dim=0) 69 | sdf_pred = model.forward(p) 70 | loss = F.mse_loss(sdf_pred[:,0], sdf, reduction='mean') 71 | scaler.scale(loss).backward() 72 | scaler.step(optimizer) 73 | scaler.update() 74 | optimizer.zero_grad() 75 | scheduler.step(loss) 76 | if iter % 100 == 0: 77 | print(loss.item()) 78 | 79 | mesh_dict[i]['model'] = model 80 | mesh_dict[i]['offset'] = torch.from_numpy(offset) 81 | mesh_dict[i]['scale'] = scale 82 | 83 | if os.path.exists(self.model_path) is False: 84 | os.mkdir(self.model_path) 85 | torch.save(mesh_dict,f'{self.model_path}/NN_{epoches}.pt') # save nn sdf model 86 | print(f'{self.model_path}/NN_{epoches}.pt model saved!') 87 | 88 | def sdf_to_mesh(self, model, nbData): 89 | verts_list, faces_list, mesh_name_list = [], [], [] 90 | for i, k in enumerate(model.keys()): 91 | mesh_dict = model[k] 92 | mesh_name = mesh_dict['mesh_name'] 93 | mesh_name_list.append(mesh_name) 94 | model_k = mesh_dict['model'].to(self.device) 95 | model_k.eval() 96 | domain = torch.linspace(-1.0,1.0,nbData).to(self.device) 97 | grid_x, grid_y, grid_z= torch.meshgrid(domain,domain,domain) 98 | grid_x, grid_y, grid_z = grid_x.reshape(-1,1), grid_y.reshape(-1,1), grid_z.reshape(-1,1) 99 | p = torch.cat([grid_x, grid_y, grid_z],dim=1).float().to(self.device) 100 | # split data to deal with memory issues 101 | p_split = torch.split(p, 100000, dim=0) 102 | d =[] 103 | with torch.no_grad(): 104 | for p_s in p_split: 105 | d_s = model_k.forward(p_s) 106 | d.append(d_s) 107 | d = torch.cat(d,dim=0) 108 | # scene.add_geometry(mesh) 109 | verts, faces, normals, values = skimage.measure.marching_cubes( 110 | d.view(nbData,nbData,nbData).detach().cpu().numpy(), level=0.0, spacing=np.array([(2.0)/nbData] * 3) 111 | ) 112 | verts = verts - [1,1,1] 113 | verts_list.append(verts) 114 | faces_list.append(faces) 115 | return verts_list, faces_list,mesh_name_list 116 | 117 | def create_surface_mesh(self,model, nbData,vis=False,save_mesh_name=None): 118 | verts_list, faces_list,mesh_name_list = self.sdf_to_mesh(model, nbData) 119 | for verts, faces,mesh_name in zip(verts_list, faces_list,mesh_name_list): 120 | rec_mesh = trimesh.Trimesh(verts,faces) 121 | if vis: 122 | rec_mesh.show() 123 | if save_mesh_name !=None: 124 | save_path = os.path.join(CUR_DIR,"output_meshes") 125 | if os.path.exists(save_path) is False: 126 | os.mkdir(save_path) 127 | trimesh.exchange.export.export_mesh(rec_mesh, os.path.join(save_path,f"{save_mesh_name}_{mesh_name}.stl")) 128 | 129 | def whole_body_nn_sdf(self,x,pose,theta,model,used_links = [0,1,2,3,4,5,6,7,8]): 130 | B = len(theta) 131 | N = len(x) 132 | K = len(used_links) 133 | offset = torch.cat([model[i]['offset'].unsqueeze(0) for i in used_links],dim=0).to(self.device) 134 | offset = offset.unsqueeze(0).expand(B,K,3).reshape(B*K,3).float() 135 | scale = torch.tensor([model[i]['scale'] for i in used_links],device=self.device) 136 | scale = scale.unsqueeze(0).expand(B,K).reshape(B*K).float() 137 | trans_list = self.robot.get_transformations_each_link(pose,theta) 138 | 139 | fk_trans = torch.cat([t.unsqueeze(1) for t in trans_list],dim=1)[:,used_links,:,:].reshape(-1,4,4) # B,K,4,4 140 | x_robot_frame_batch = utils.transform_points(x.float(),torch.linalg.inv(fk_trans).float(),device=self.device) # B*K,N,3 141 | x_robot_frame_batch_scaled = x_robot_frame_batch - offset.unsqueeze(1) 142 | x_robot_frame_batch_scaled = x_robot_frame_batch_scaled/scale.unsqueeze(-1).unsqueeze(-1) #B*K,N,3 143 | x_robot_frame_batch_scaled = x_robot_frame_batch_scaled.reshape(B,K,N,3).transpose(0,1) #K,B,N,3 144 | 145 | x_bounded = torch.where(x_robot_frame_batch_scaled>1.0-1e-2,1.0-1e-2,x_robot_frame_batch_scaled) 146 | x_bounded = torch.where(x_bounded<-1.0+1e-2,-1.0+1e-2,x_bounded) 147 | res_x = x_robot_frame_batch_scaled - x_bounded 148 | 149 | # sdf 150 | sdf = [] 151 | for i in model.keys(): 152 | model_i = model[i]['model'].eval().to(self.device) 153 | sdf.append(model_i.forward(x_bounded[i])) 154 | sdf = torch.cat(sdf,dim=0).reshape(K,B,N) 155 | 156 | sdf = sdf + res_x.norm(dim=-1) 157 | sdf = sdf.transpose(0,1) 158 | 159 | sdf = sdf*scale.reshape(B,K).unsqueeze(-1) 160 | sdf_value, idx = sdf.min(dim=1) 161 | return sdf_value 162 | 163 | def whole_body_nn_sdf_with_joints_grad_batch(self,x,pose,theta,model,used_links = [0,1,2,3,4,5,6,7,8]): 164 | delta = 0.001 165 | B = theta.shape[0] 166 | theta = theta.unsqueeze(1) 167 | d_theta = (theta.expand(B,7,7)+ torch.eye(7,device=self.device).unsqueeze(0).expand(B,7,7)*delta).reshape(B,-1,7) 168 | theta = torch.cat([theta,d_theta],dim=1).reshape(B*8,7) 169 | pose = pose.expand(B*8,4,4) 170 | sdf = self.whole_body_nn_sdf(x,pose,theta,model, used_links = used_links).reshape(B,8,-1) 171 | d_sdf = (sdf[:,1:,:]-sdf[:,:1,:])/delta 172 | return sdf[:,0,:],d_sdf.transpose(1,2) 173 | 174 | if __name__ =='__main__': 175 | device = 'cuda' 176 | lr = 0.002 177 | panda = PandaLayer(device) 178 | nn_sdf = NNSDF(panda,lr,device) 179 | 180 | # # train neural network model 181 | # nn_sdf.train_nn(epoches=200) 182 | 183 | # visualize the Bernstein Polynomial model for each robot link 184 | model_path = f'models/NN_AD.pt' 185 | model = torch.load(model_path) 186 | nn_sdf.create_surface_mesh(model,nbData=128,vis=True) 187 | 188 | # run nn sdf model 189 | x = torch.rand(128,3).to(device)*2.0 - 1.0 190 | theta = torch.rand(1,7).to(device).float() 191 | pose = torch.from_numpy(np.identity(4)).unsqueeze(0).to(device).expand(len(theta),4,4).float() 192 | sdf_value = nn_sdf.whole_body_nn_sdf(x,pose,theta,model) 193 | sdf,joint_grad = nn_sdf.whole_body_nn_sdf_with_joints_grad_batch(x,pose,theta,model) 194 | print(sdf_value.shape) 195 | print(sdf_value.shape,joint_grad.shape) -------------------------------------------------------------------------------- /panda_layer/franka_sphere.yaml: -------------------------------------------------------------------------------- 1 | collision_spheres: 2 | panda_link0: 3 | - "center": [0.0, 0.0, 0.05] 4 | "radius": 0.08 5 | panda_link1: 6 | - "center": [0.0, -0.08, 0.0] 7 | "radius": 0.06 8 | - "center": [0.0, -0.03, 0.0] 9 | "radius": 0.06 10 | - "center": [0.0, 0.0, -0.12] 11 | "radius": 0.06 12 | - "center": [0.0, 0.0, -0.17] 13 | "radius": 0.06 14 | panda_link2: 15 | - "center": [0.0, 0.0, 0.03] 16 | "radius": 0.06 17 | - "center": [0.0, 0.0, 0.08] 18 | "radius": 0.06 19 | - "center": [0.0, -0.12, 0.0] 20 | "radius": 0.06 21 | - "center": [0.0, -0.17, 0.0] 22 | "radius": 0.06 23 | panda_link3: 24 | - "center": [0.0, 0.0, -0.06] 25 | "radius": 0.05 26 | - "center": [0.0, 0.0, -0.1] 27 | "radius": 0.06 28 | - "center": [0.08, 0.06, 0.0] 29 | "radius": 0.055 30 | - "center": [0.08, 0.02, 0.0] 31 | "radius": 0.055 32 | panda_link4: 33 | - "center": [0.0, 0.0, 0.02] 34 | "radius": 0.055 35 | - "center": [0.0, 0.0, 0.06] 36 | "radius": 0.055 37 | - "center": [-0.08, 0.095, 0.0] 38 | "radius": 0.06 39 | - "center": [-0.08, 0.06, 0.0] 40 | "radius": 0.055 41 | panda_link5: 42 | - "center": [0.0, 0.055, 0.0] 43 | "radius": 0.06 44 | - "center": [0.0, 0.075, 0.0] 45 | "radius": 0.06 46 | - "center": [0.0, 0.000, -0.22] 47 | "radius": 0.06 48 | - "center": [0.0, 0.05, -0.18] 49 | "radius": 0.05 50 | - "center": [0.01, 0.08, -0.14] 51 | "radius": 0.025 52 | - "center": [0.01, 0.085, -0.11] 53 | "radius": 0.025 54 | - "center": [0.01, 0.09, -0.08] 55 | "radius": 0.025 56 | - "center": [0.01, 0.095, -0.05] 57 | "radius": 0.025 58 | - "center": [-0.01, 0.08, -0.14] 59 | "radius": 0.025 60 | - "center": [-0.01, 0.085, -0.11] 61 | "radius": 0.025 62 | - "center": [-0.01, 0.09, -0.08] 63 | "radius": 0.025 64 | - "center": [-0.01, 0.095, -0.05] 65 | "radius": 0.025 66 | panda_link6: 67 | - "center": [0.0, 0.0, 0.0] 68 | "radius": 0.05 69 | - "center": [0.08, 0.035, 0.0] 70 | "radius": 0.052 71 | - "center": [0.08, -0.01, 0.0] 72 | "radius": 0.05 73 | panda_link7: 74 | - "center": [0.0, 0.0, 0.07] 75 | "radius": 0.05 76 | - "center": [0.02, 0.04, 0.08] 77 | "radius": 0.025 78 | - "center": [0.04, 0.02, 0.08] 79 | "radius": 0.025 80 | - "center": [0.04, 0.06, 0.085] 81 | "radius": 0.02 82 | - "center": [0.06, 0.04, 0.085] 83 | "radius": 0.02 84 | panda_hand: 85 | - "center": [0.0, -0.075, 0.01] 86 | "radius": 0.028 87 | - "center": [0.0, -0.045, 0.01] 88 | "radius": 0.028 89 | - "center": [0.0, -0.015, 0.01] 90 | "radius": 0.028 91 | - "center": [0.0, 0.015, 0.01] 92 | "radius": 0.028 93 | - "center": [0.0, 0.045, 0.01] 94 | "radius": 0.028 95 | - "center": [0.0, 0.075, 0.01] 96 | "radius": 0.028 97 | - "center": [0.0, -0.075, 0.03] 98 | "radius": 0.026 99 | - "center": [0.0, -0.045, 0.03] 100 | "radius": 0.026 101 | - "center": [0.0, -0.015, 0.03] 102 | "radius": 0.026 103 | - "center": [0.0, 0.015, 0.03] 104 | "radius": 0.026 105 | - "center": [0.0, 0.045, 0.03] 106 | "radius": 0.026 107 | - "center": [0.0, 0.075, 0.03] 108 | "radius": 0.026 109 | - "center": [0.0, -0.075, 0.05] 110 | "radius": 0.024 111 | - "center": [0.0, -0.045, 0.05] 112 | "radius": 0.024 113 | - "center": [0.0, -0.015, 0.05] 114 | "radius": 0.024 115 | - "center": [0.0, 0.015, 0.05] 116 | "radius": 0.024 117 | - "center": [0.0, 0.045, 0.05] 118 | "radius": 0.024 119 | - "center": [0.0, 0.075, 0.05] 120 | "radius": 0.024 -------------------------------------------------------------------------------- /panda_layer/mesh_test.py: -------------------------------------------------------------------------------- 1 | # ----------------------------------------------------------------------------- 2 | #SPDX-License-Identifier: MIT 3 | # This file is part of the RDF project. 4 | # Copyright (c) 2023 Idiap Research Institute 5 | # Contributor: Yimming Li 6 | # ----------------------------------------------------------------------------- 7 | 8 | import trimesh 9 | import glob 10 | import os 11 | import numpy as np 12 | from mesh_to_sdf import mesh_to_voxels 13 | import skimage 14 | from mesh_to_sdf import surface_point_cloud 15 | from mesh_to_sdf.utils import scale_to_unit_cube, scale_to_unit_sphere, get_raster_points, check_voxels 16 | 17 | 18 | mesh_path = os.path.dirname(os.path.realpath(__file__)) + "/meshes/visual/*.stl" 19 | mesh_files = glob.glob(mesh_path) 20 | print(mesh_files) 21 | mesh_files = sorted(mesh_files)[1:] #except finger 22 | voxel_resolution = 128 23 | for mf in mesh_files: 24 | mesh_name = mf.split('/')[-1].split('_')[0] 25 | print(mesh_name) 26 | scene = trimesh.Scene() 27 | mesh_origin = trimesh.load(mf) 28 | # mesh_origin.visual.face_colors = [255,0,0,150] 29 | center = mesh_origin.bounding_box.centroid 30 | scale = 2 / np.max(mesh_origin.bounding_box.extents) 31 | voxels = mesh_to_voxels(mesh_origin, 32 | voxel_resolution=voxel_resolution, 33 | surface_point_method='scan', 34 | sign_method='normal', 35 | scan_count=100, 36 | scan_resolution=400, 37 | sample_point_count=10000000, 38 | normal_sample_count=100, 39 | pad=True, 40 | check_result=False) 41 | vertices, faces, normals, _ = skimage.measure.marching_cubes(voxels, level=0.0,spacing=(2/voxel_resolution,2/voxel_resolution,2/voxel_resolution)) 42 | mesh_voxelized = trimesh.Trimesh(vertices=vertices, faces=faces, vertex_normals=normals) 43 | mesh_voxelized.visual.face_colors = [0,0,255,150] 44 | mesh_voxelized.vertices = mesh_voxelized.vertices/scale 45 | mesh_voxelized.vertices = mesh_voxelized.vertices - mesh_voxelized.bounding_box.centroid +center 46 | print(mesh_voxelized.vertices.shape) 47 | scene.add_geometry(mesh_voxelized) 48 | scene.show() 49 | # save_path = os.path.join(os.path.dirname(os.path.realpath(__file__)),f'meshes/voxel_{str(voxel_resolution)}') 50 | # if os.path.exists(save_path) is not True: 51 | # os.mkdir(save_path) 52 | # trimesh.exchange.export.export_mesh(mesh_voxelized, os.path.join(save_path,f'{mesh_name}.stl')) 53 | -------------------------------------------------------------------------------- /panda_layer/meshes/visual/finger_vis.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yimingli1998/RDF/a1cd0d503831fc2b8b921ebb251d8bc714921522/panda_layer/meshes/visual/finger_vis.stl -------------------------------------------------------------------------------- /panda_layer/meshes/visual/link0_vis.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yimingli1998/RDF/a1cd0d503831fc2b8b921ebb251d8bc714921522/panda_layer/meshes/visual/link0_vis.stl -------------------------------------------------------------------------------- /panda_layer/meshes/visual/link1_vis.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yimingli1998/RDF/a1cd0d503831fc2b8b921ebb251d8bc714921522/panda_layer/meshes/visual/link1_vis.stl -------------------------------------------------------------------------------- /panda_layer/meshes/visual/link2_vis.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yimingli1998/RDF/a1cd0d503831fc2b8b921ebb251d8bc714921522/panda_layer/meshes/visual/link2_vis.stl -------------------------------------------------------------------------------- /panda_layer/meshes/visual/link3_vis.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yimingli1998/RDF/a1cd0d503831fc2b8b921ebb251d8bc714921522/panda_layer/meshes/visual/link3_vis.stl -------------------------------------------------------------------------------- /panda_layer/meshes/visual/link4_vis.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yimingli1998/RDF/a1cd0d503831fc2b8b921ebb251d8bc714921522/panda_layer/meshes/visual/link4_vis.stl -------------------------------------------------------------------------------- /panda_layer/meshes/visual/link5_vis.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yimingli1998/RDF/a1cd0d503831fc2b8b921ebb251d8bc714921522/panda_layer/meshes/visual/link5_vis.stl -------------------------------------------------------------------------------- /panda_layer/meshes/visual/link6_vis.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yimingli1998/RDF/a1cd0d503831fc2b8b921ebb251d8bc714921522/panda_layer/meshes/visual/link6_vis.stl -------------------------------------------------------------------------------- /panda_layer/meshes/visual/link7_vis.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yimingli1998/RDF/a1cd0d503831fc2b8b921ebb251d8bc714921522/panda_layer/meshes/visual/link7_vis.stl -------------------------------------------------------------------------------- /panda_layer/meshes/visual/link8_vis.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yimingli1998/RDF/a1cd0d503831fc2b8b921ebb251d8bc714921522/panda_layer/meshes/visual/link8_vis.stl -------------------------------------------------------------------------------- /panda_layer/meshes/voxel_128/finger.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yimingli1998/RDF/a1cd0d503831fc2b8b921ebb251d8bc714921522/panda_layer/meshes/voxel_128/finger.stl -------------------------------------------------------------------------------- /panda_layer/meshes/voxel_128/link0.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yimingli1998/RDF/a1cd0d503831fc2b8b921ebb251d8bc714921522/panda_layer/meshes/voxel_128/link0.stl -------------------------------------------------------------------------------- /panda_layer/meshes/voxel_128/link1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yimingli1998/RDF/a1cd0d503831fc2b8b921ebb251d8bc714921522/panda_layer/meshes/voxel_128/link1.stl -------------------------------------------------------------------------------- /panda_layer/meshes/voxel_128/link2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yimingli1998/RDF/a1cd0d503831fc2b8b921ebb251d8bc714921522/panda_layer/meshes/voxel_128/link2.stl -------------------------------------------------------------------------------- /panda_layer/meshes/voxel_128/link3.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yimingli1998/RDF/a1cd0d503831fc2b8b921ebb251d8bc714921522/panda_layer/meshes/voxel_128/link3.stl -------------------------------------------------------------------------------- /panda_layer/meshes/voxel_128/link4.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yimingli1998/RDF/a1cd0d503831fc2b8b921ebb251d8bc714921522/panda_layer/meshes/voxel_128/link4.stl -------------------------------------------------------------------------------- /panda_layer/meshes/voxel_128/link5.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yimingli1998/RDF/a1cd0d503831fc2b8b921ebb251d8bc714921522/panda_layer/meshes/voxel_128/link5.stl -------------------------------------------------------------------------------- /panda_layer/meshes/voxel_128/link6.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yimingli1998/RDF/a1cd0d503831fc2b8b921ebb251d8bc714921522/panda_layer/meshes/voxel_128/link6.stl -------------------------------------------------------------------------------- /panda_layer/meshes/voxel_128/link7.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yimingli1998/RDF/a1cd0d503831fc2b8b921ebb251d8bc714921522/panda_layer/meshes/voxel_128/link7.stl -------------------------------------------------------------------------------- /panda_layer/meshes/voxel_128/link8.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yimingli1998/RDF/a1cd0d503831fc2b8b921ebb251d8bc714921522/panda_layer/meshes/voxel_128/link8.stl -------------------------------------------------------------------------------- /panda_layer/panda_layer.py: -------------------------------------------------------------------------------- 1 | 2 | # ----------------------------------------------------------------------------- 3 | # SPDX-License-Identifier: MIT 4 | # This file is part of the RDF project. 5 | # Copyright (c) 2023 Idiap Research Institute 6 | # Contributor: Yimming Li 7 | # ----------------------------------------------------------------------------- 8 | 9 | import torch 10 | import trimesh 11 | import glob 12 | import os 13 | import numpy as np 14 | import math 15 | 16 | def save_to_mesh(vertices, faces, output_mesh_path=None): 17 | assert output_mesh_path is not None 18 | with open(output_mesh_path, 'w') as fp: 19 | for vert in vertices: 20 | fp.write('v %f %f %f\n' % (vert[0], vert[1], vert[2])) 21 | for face in faces+1: 22 | fp.write('f %d %d %d\n' % (face[0], face[1], face[2])) 23 | print('Output mesh save to: ', os.path.abspath(output_mesh_path)) 24 | 25 | 26 | class PandaLayer(torch.nn.Module): 27 | def __init__(self, device='cpu',mesh_path = os.path.dirname(os.path.realpath(__file__)) + "/meshes/visual/*.stl"): 28 | # The forward kinematics equations implemented here are robot_mesh.show()from 29 | super().__init__() 30 | dir_path = os.path.split(os.path.abspath(__file__))[0] 31 | self.device = device 32 | self.mesh_path = mesh_path 33 | self.meshes = self.load_meshes() 34 | 35 | # self.theta_min = [-2.3093, -1.5133, -2.4937, -2.7478, -2.4800, 0.8521, -2.6895] 36 | # self.theta_max = [ 2.3093, 1.5133, 2.4937, -0.4461, 2.4800, 4.2094, 2.6895] 37 | 38 | self.theta_min = torch.tensor([-2.8973, -1.7628, -2.8973, -3.0718, -2.8973, -0.0175, -2.8973]).to(self.device) 39 | self.theta_max = torch.tensor([ 2.8973, 1.7628, 2.8973, -0.0698, 2.8973, 3.7525, 2.8973]).to(self.device) 40 | self.theta_mid = (self.theta_min + self.theta_max) / 2.0 41 | self.theta_min_soft = (self.theta_min-self.theta_mid)*0.8 + self.theta_mid 42 | self.theta_max_soft = (self.theta_max-self.theta_mid)*0.8 + self.theta_mid 43 | self.dof = len(self.theta_min) 44 | 45 | # meshes 46 | self.link0 = self.meshes["link0"][0] 47 | self.link0_normals = self.meshes["link0"][-1] 48 | 49 | self.link1 = self.meshes["link1"][0] 50 | self.link1_normals = self.meshes["link1"][-1] 51 | 52 | self.link2 = self.meshes["link2"][0] 53 | self.link2_normals = self.meshes["link2"][-1] 54 | 55 | self.link3 = self.meshes["link3"][0] 56 | self.link3_normals = self.meshes["link3"][-1] 57 | 58 | self.link4 = self.meshes["link4"][0] 59 | self.link4_normals = self.meshes["link4"][-1] 60 | 61 | self.link5 = self.meshes["link5"][0] 62 | self.link5_normals = self.meshes["link5"][-1] 63 | 64 | self.link6 = self.meshes["link6"][0] 65 | self.link6_normals = self.meshes["link6"][-1] 66 | self.link7 = self.meshes["link7"][0] 67 | self.link7_normals = self.meshes["link7"][-1] 68 | self.link8 = self.meshes["link8"][0] 69 | self.link8_normals = self.meshes["link8"][-1] 70 | self.finger = self.meshes["finger"][0] 71 | self.finger_normals = self.meshes["finger"][-1] 72 | 73 | # mesh faces 74 | self.robot_faces = [ 75 | self.meshes["link0"][1], self.meshes["link1"][1], self.meshes["link2"][1], 76 | self.meshes["link3"][1], self.meshes["link4"][1], self.meshes["link5"][1], 77 | self.meshes["link6"][1], self.meshes["link7"][1], self.meshes["link8"][1], 78 | self.meshes["finger"][1] 79 | ] 80 | 81 | # self.vertices_face_areas = [ 82 | # self.meshes["link0"][2], self.meshes["link1"][2], self.meshes["link2"][2], 83 | # self.meshes["link3"][2], self.meshes["link4"][2], self.meshes["link5"][2], 84 | # self.meshes["link6"][2], self.meshes["link7"][2], self.meshes["link8"][2], 85 | # self.meshes["finger"][2] 86 | # ] 87 | 88 | self.num_vertices_per_part = [ 89 | self.meshes["link0"][0].shape[0], self.meshes["link1"][0].shape[0], self.meshes["link2"][0].shape[0], 90 | self.meshes["link3"][0].shape[0], self.meshes["link4"][0].shape[0], self.meshes["link5"][0].shape[0], 91 | self.meshes["link6"][0].shape[0], self.meshes["link7"][0].shape[0], self.meshes["link8"][0].shape[0], 92 | self.meshes["finger"][0].shape[0] 93 | ] 94 | 95 | self.A0 = torch.tensor(0.0, dtype=torch.float32, device=device) 96 | self.A1 = torch.tensor(0.0, dtype=torch.float32, device=device) 97 | self.A2 = torch.tensor(0.0, dtype=torch.float32, device=device) 98 | self.A3 = torch.tensor(0.0825, dtype=torch.float32, device=device) 99 | self.A4 = torch.tensor(-0.0825, dtype=torch.float32, device=device) 100 | self.A5 = torch.tensor(0.0, dtype=torch.float32, device=device) 101 | self.A6 = torch.tensor(0.088, dtype=torch.float32, device=device) 102 | self.A7 = torch.tensor(0.0, dtype=torch.float32, device=device) 103 | 104 | # def check_normal(self,verterices, normals): 105 | # center = np.mean(verterices,axis=0) 106 | # verts = torch.from_numpy(verterices-center).float() 107 | # normals = torch.from_numpy(normals).float() 108 | # cosine = torch.cosine_similarity(verts,normals).float() 109 | # normals[cosine<0] = -normals[cosine<0] 110 | # return normals 111 | 112 | def load_meshes(self): 113 | check_normal = False 114 | # mesh_path = os.path.dirname(os.path.realpath(__file__)) + "/meshes/visual/*.stl" 115 | mesh_files = glob.glob(self.mesh_path) 116 | mesh_files = [f for f in mesh_files if os.path.isfile(f)] 117 | meshes = {} 118 | 119 | for mesh_file in mesh_files: 120 | if self.mesh_path.split('/')[-2]=='visual': 121 | name = os.path.basename(mesh_file)[:-4].split('_')[0] 122 | else: 123 | name = os.path.basename(mesh_file)[:-4] 124 | mesh = trimesh.load(mesh_file) 125 | triangle_areas = trimesh.triangles.area(mesh.triangles) 126 | vert_area_weight = [] 127 | # for i in range(mesh.vertices.shape[0]): 128 | # vert_neighour_face = np.where(mesh.faces == i)[0] 129 | # vert_area_weight.append(1000000*triangle_areas[vert_neighour_face].mean()) 130 | temp = torch.ones(mesh.vertices.shape[0], 1).float() 131 | meshes[name] = [ 132 | torch.cat((torch.FloatTensor(np.array(mesh.vertices)), temp), dim=-1).to(self.device), 133 | # torch.LongTensor(np.asarray(mesh.faces)).to(self.device), 134 | mesh.faces, 135 | # torch.FloatTensor(np.asarray(vert_area_weight)).to(self.device), 136 | # vert_area_weight, 137 | # torch.FloatTensor(mesh.vertex_normals) 138 | torch.cat((torch.FloatTensor(np.array(mesh.vertex_normals)), temp), dim=-1).to(self.device).to(torch.float), 139 | ] 140 | return meshes 141 | 142 | def forward(self, pose, theta): 143 | batch_size = theta.shape[0] 144 | link0_vertices = self.link0.repeat(batch_size, 1, 1) 145 | # print(link0_vertices.shape) 146 | link0_vertices = torch.matmul(pose, 147 | link0_vertices.transpose(2, 1)).transpose(1, 2)[:, :, :3] 148 | link0_normals = self.link0_normals.repeat(batch_size, 1, 1) 149 | link0_normals = torch.matmul(pose, 150 | link0_normals.transpose(2, 1)).transpose(1, 2)[:, :, :3] 151 | 152 | link1_vertices = self.link1.repeat(batch_size, 1, 1) 153 | T01 = self.forward_kinematics(self.A0, torch.tensor(0, dtype=torch.float32, device=self.device), 154 | 0.333, theta[:, 0], batch_size).float() 155 | 156 | 157 | link2_vertices = self.link2.repeat(batch_size, 1, 1) 158 | T12 = self.forward_kinematics(self.A1, torch.tensor(-np.pi/2., dtype=torch.float32, device=self.device), 159 | 0, theta[:, 1], batch_size).float() 160 | link3_vertices = self.link3.repeat(batch_size, 1, 1) 161 | T23 = self.forward_kinematics(self.A2, torch.tensor(np.pi/2., dtype=torch.float32, device=self.device), 162 | 0.316, theta[:, 2], batch_size).float() 163 | link4_vertices = self.link4.repeat(batch_size, 1, 1) 164 | T34 = self.forward_kinematics(self.A3, torch.tensor(np.pi/2., dtype=torch.float32, device=self.device), 165 | 0, theta[:, 3], batch_size).float() 166 | link5_vertices = self.link5.repeat(batch_size, 1, 1) 167 | T45 = self.forward_kinematics(self.A4, torch.tensor(-np.pi/2., dtype=torch.float32, device=self.device), 168 | 0.384, theta[:, 4], batch_size).float() 169 | link6_vertices = self.link6.repeat(batch_size, 1, 1) 170 | T56 = self.forward_kinematics(self.A5, torch.tensor(np.pi/2., dtype=torch.float32, device=self.device), 171 | 0, theta[:, 5], batch_size).float() 172 | link7_vertices = self.link7.repeat(batch_size, 1, 1) 173 | T67 = self.forward_kinematics(self.A6, torch.tensor(np.pi/2., dtype=torch.float32, device=self.device), 174 | 0, theta[:, 6], batch_size).float() 175 | link8_vertices = self.link8.repeat(batch_size, 1, 1) 176 | T78 = self.forward_kinematics(self.A7, torch.tensor(0, dtype=torch.float32, device=self.device), 177 | 0.107, -np.pi/4*torch.ones_like(theta[:,0],device=self.device), batch_size).float() 178 | # finger_vertices = self.finger.repeat(batch_size, 1, 1) 179 | 180 | pose_to_Tw0 = pose 181 | pose_to_T01 = torch.matmul(pose_to_Tw0, T01) 182 | link1_vertices= torch.matmul( 183 | pose_to_T01, 184 | link1_vertices.transpose(2, 1)).transpose(1, 2)[:, :, :3] 185 | link1_normals = self.link1_normals.repeat(batch_size, 1, 1) 186 | link1_normals = torch.matmul(pose_to_T01, 187 | link1_normals.transpose(2, 1)).transpose(1, 2)[:, :, :3] 188 | 189 | pose_to_T12 = torch.matmul(pose_to_T01, T12) 190 | link2_vertices= torch.matmul( 191 | pose_to_T12, 192 | link2_vertices.transpose(2, 1)).transpose(1, 2)[:, :, :3] 193 | link2_normals = self.link2_normals.repeat(batch_size, 1, 1) 194 | link2_normals = torch.matmul(pose_to_T12, 195 | link2_normals.transpose(2, 1)).transpose(1, 2)[:, :, :3] 196 | 197 | pose_to_T23 = torch.matmul(pose_to_T12, T23) 198 | link3_vertices= torch.matmul( 199 | pose_to_T23, 200 | link3_vertices.transpose(2, 1)).transpose(1, 2)[:, :, :3] 201 | link3_normals = self.link3_normals.repeat(batch_size, 1, 1) 202 | link3_normals = torch.matmul(pose_to_T23, 203 | link3_normals.transpose(2, 1)).transpose(1, 2)[:, :, :3] 204 | 205 | pose_to_T34 = torch.matmul(pose_to_T23, T34) 206 | link4_vertices= torch.matmul( 207 | pose_to_T34, 208 | link4_vertices.transpose(2, 1)).transpose(1, 2)[:, :, :3] 209 | link4_normals = self.link4_normals.repeat(batch_size, 1, 1) 210 | link4_normals = torch.matmul(pose_to_T34, 211 | link4_normals.transpose(2, 1)).transpose(1, 2)[:, :, :3] 212 | 213 | pose_to_T45 = torch.matmul(pose_to_T34, T45) 214 | link5_vertices= torch.matmul( 215 | pose_to_T45, 216 | link5_vertices.transpose(2, 1)).transpose(1, 2)[:, :, :3] 217 | link5_normals = self.link5_normals.repeat(batch_size, 1, 1) 218 | link5_normals = torch.matmul(pose_to_T45, 219 | link5_normals.transpose(2, 1)).transpose(1, 2)[:, :, :3] 220 | 221 | pose_to_T56 = torch.matmul(pose_to_T45, T56) 222 | link6_vertices= torch.matmul( 223 | pose_to_T56, 224 | link6_vertices.transpose(2, 1)).transpose(1, 2)[:, :, :3] 225 | link6_normals = self.link6_normals.repeat(batch_size, 1, 1) 226 | link6_normals = torch.matmul(pose_to_T56, 227 | link6_normals.transpose(2, 1)).transpose(1, 2)[:, :, :3] 228 | 229 | pose_to_T67 = torch.matmul(pose_to_T56, T67) 230 | link7_vertices= torch.matmul( 231 | pose_to_T67, 232 | link7_vertices.transpose(2, 1)).transpose(1, 2)[:, :, :3] 233 | link7_normals = self.link7_normals.repeat(batch_size, 1, 1) 234 | link7_normals = torch.matmul(pose_to_T67, 235 | link7_normals.transpose(2, 1)).transpose(1, 2)[:, :, :3] 236 | 237 | 238 | pose_to_T78 = torch.matmul(pose_to_T67, T78) 239 | link8_vertices= torch.matmul( 240 | pose_to_T78, 241 | link8_vertices.transpose(2, 1)).transpose(1, 2)[:, :, :3] 242 | link8_normals = self.link8_normals.repeat(batch_size, 1, 1) 243 | link8_normals = torch.matmul(pose_to_T78, 244 | link8_normals.transpose(2, 1)).transpose(1, 2)[:, :, :3] 245 | 246 | return [link0_vertices, link1_vertices, link2_vertices, \ 247 | link3_vertices, link4_vertices, link5_vertices, \ 248 | link6_vertices, link7_vertices, link8_vertices, \ 249 | link0_normals, link1_normals, link2_normals, \ 250 | link3_normals, link4_normals, link5_normals, \ 251 | link6_normals, link7_normals, link8_normals] 252 | 253 | def get_transformations_each_link(self,pose, theta): 254 | batch_size = theta.shape[0] 255 | T01 = self.forward_kinematics(self.A0, torch.tensor(0, dtype=torch.float32, device=self.device), 256 | 0.333, theta[:, 0], batch_size).float() 257 | 258 | # link2_vertices = self.link2.repeat(batch_size, 1, 1) 259 | T12 = self.forward_kinematics(self.A1, torch.tensor(-np.pi/2., dtype=torch.float32, device=self.device), 260 | 0, theta[:, 1], batch_size).float() 261 | # link3_vertices = self.link3.repeat(batch_size, 1, 1) 262 | T23 = self.forward_kinematics(self.A2, torch.tensor(np.pi/2., dtype=torch.float32, device=self.device), 263 | 0.316, theta[:, 2], batch_size).float() 264 | # link4_vertices = self.link4.repeat(batch_size, 1, 1) 265 | T34 = self.forward_kinematics(self.A3, torch.tensor(np.pi/2., dtype=torch.float32, device=self.device), 266 | 0, theta[:, 3], batch_size).float() 267 | # link5_vertices = self.link5.repeat(batch_size, 1, 1) 268 | T45 = self.forward_kinematics(self.A4, torch.tensor(-np.pi/2., dtype=torch.float32, device=self.device), 269 | 0.384, theta[:, 4], batch_size).float() 270 | # link6_vertices = self.link6.repeat(batch_size, 1, 1) 271 | T56 = self.forward_kinematics(self.A5, torch.tensor(np.pi/2., dtype=torch.float32, device=self.device), 272 | 0, theta[:, 5], batch_size).float() 273 | # link7_vertices = self.link7.repeat(batch_size, 1, 1) 274 | T67 = self.forward_kinematics(self.A6, torch.tensor(np.pi/2., dtype=torch.float32, device=self.device), 275 | 0, theta[:, 6], batch_size).float() 276 | # link8_vertices = self.link8.repeat(batch_size, 1, 1) 277 | T78 = self.forward_kinematics(self.A7, torch.tensor(0, dtype=torch.float32, device=self.device), 278 | 0.107, -np.pi/4*torch.ones_like(theta[:,0],device=self.device), batch_size).float() 279 | # finger_vertices = self.finger.repeat(batch_size, 1, 1) 280 | pose_to_Tw0 = pose 281 | pose_to_T01 = torch.matmul(pose_to_Tw0, T01) 282 | pose_to_T12 = torch.matmul(pose_to_T01, T12) 283 | pose_to_T23 = torch.matmul(pose_to_T12, T23) 284 | pose_to_T34 = torch.matmul(pose_to_T23, T34) 285 | pose_to_T45 = torch.matmul(pose_to_T34, T45) 286 | pose_to_T56 = torch.matmul(pose_to_T45, T56) 287 | pose_to_T67 = torch.matmul(pose_to_T56, T67) 288 | pose_to_T78 = torch.matmul(pose_to_T67, T78) 289 | 290 | return [pose_to_Tw0,pose_to_T01,pose_to_T12,pose_to_T23,pose_to_T34,pose_to_T45,pose_to_T56,pose_to_T67,pose_to_T78] 291 | 292 | def get_eef(self,pose, theta,link=-1): 293 | poses = self.get_transformations_each_link(pose, theta) 294 | pos = poses[link][:, :3, 3] 295 | rot = poses[link][:, :3, :3] 296 | return pos, rot 297 | 298 | def forward_kinematics(self, A, alpha, D, theta, batch_size=1): 299 | theta = theta.view(batch_size, -1) 300 | alpha = alpha*torch.ones_like(theta) 301 | c_theta = torch.cos(theta) 302 | s_theta = torch.sin(theta) 303 | c_alpha = torch.cos(alpha) 304 | s_alpha = torch.sin(alpha) 305 | 306 | l_1_to_l = torch.cat([c_theta, -s_theta, torch.zeros_like(s_theta), A * torch.ones_like(c_theta), 307 | s_theta * c_alpha, c_theta * c_alpha, -s_alpha, -s_alpha * D, 308 | s_theta * s_alpha, c_theta * s_alpha, c_alpha, c_alpha * D, 309 | torch.zeros_like(s_theta), torch.zeros_like(s_theta), torch.zeros_like(s_theta), torch.ones_like(s_theta)], dim=1).reshape(batch_size, 4, 4) 310 | # l_1_to_l = torch.zeros((batch_size, 4, 4), device=self.device) 311 | # l_1_to_l[:, 0, 0] = c_theta 312 | # l_1_to_l[:, 0, 1] = -s_theta 313 | # l_1_to_l[:, 0, 3] = A 314 | # l_1_to_l[:, 1, 0] = s_theta * c_alpha 315 | # l_1_to_l[:, 1, 1] = c_theta * c_alpha 316 | # l_1_to_l[:, 1, 2] = -s_alpha 317 | # l_1_to_l[:, 1, 3] = -s_alpha * D 318 | # l_1_to_l[:, 2, 0] = s_theta * s_alpha 319 | # l_1_to_l[:, 2, 1] = c_theta * s_alpha 320 | # l_1_to_l[:, 2, 2] = c_alpha 321 | # l_1_to_l[:, 2, 3] = c_alpha * D 322 | # l_1_to_l[:, 3, 3] = 1 323 | return l_1_to_l 324 | 325 | def get_robot_mesh(self, vertices_list, faces): 326 | 327 | link0_verts = vertices_list[0] 328 | link0_faces = faces[0] 329 | 330 | link1_verts = vertices_list[1] 331 | link1_faces = faces[1] 332 | 333 | link2_verts = vertices_list[2] 334 | link2_faces = faces[2] 335 | 336 | link3_verts = vertices_list[3] 337 | link3_faces = faces[3] 338 | 339 | link4_verts = vertices_list[4] 340 | link4_faces = faces[4] 341 | 342 | link5_verts = vertices_list[5] 343 | link5_faces = faces[5] 344 | 345 | link6_verts = vertices_list[6] 346 | link6_faces = faces[6] 347 | 348 | link7_verts = vertices_list[7] 349 | link7_faces = faces[7] 350 | 351 | link8_verts = vertices_list[8] 352 | link8_faces = faces[8] 353 | 354 | link0_mesh = trimesh.Trimesh(link0_verts, link0_faces) 355 | # link0_mesh.visual.face_colors = [150,150,150] 356 | link1_mesh = trimesh.Trimesh(link1_verts, link1_faces) 357 | # link1_mesh.visual.face_colors = [150,150,150] 358 | link2_mesh = trimesh.Trimesh(link2_verts, link2_faces) 359 | # link2_mesh.visual.face_colors = [150,150,150] 360 | link3_mesh = trimesh.Trimesh(link3_verts, link3_faces) 361 | # link3_mesh.visual.face_colors = [150,150,150] 362 | link4_mesh = trimesh.Trimesh(link4_verts, link4_faces) 363 | # link4_mesh.visual.face_colors = [150,150,150] 364 | link5_mesh = trimesh.Trimesh(link5_verts, link5_faces) 365 | # link5_mesh.visual.face_colors = [250,150,150] 366 | link6_mesh = trimesh.Trimesh(link6_verts, link6_faces) 367 | # link6_mesh.visual.face_colors = [250,150,150] 368 | link7_mesh = trimesh.Trimesh(link7_verts, link7_faces) 369 | # link7_mesh.visual.face_colors = [250,150,150] 370 | link8_mesh = trimesh.Trimesh(link8_verts, link8_faces) 371 | # link8_mesh.visual.face_colors = [250,150,150] 372 | 373 | robot_mesh = [ 374 | link0_mesh, 375 | link1_mesh, 376 | link2_mesh, 377 | link3_mesh, 378 | link4_mesh, 379 | link5_mesh, 380 | link6_mesh, 381 | link7_mesh, 382 | link8_mesh 383 | ] 384 | # robot_mesh = np.sum(robot_mesh) 385 | return robot_mesh 386 | 387 | def get_forward_robot_mesh(self, pose, theta): 388 | batch_size = pose.size()[0] 389 | outputs = self.forward(pose, theta) 390 | 391 | vertices_list = [[ 392 | outputs[0][i].detach().cpu().numpy(), 393 | outputs[1][i].detach().cpu().numpy(), 394 | outputs[2][i].detach().cpu().numpy(), 395 | outputs[3][i].detach().cpu().numpy(), 396 | outputs[4][i].detach().cpu().numpy(), 397 | outputs[5][i].detach().cpu().numpy(), 398 | outputs[6][i].detach().cpu().numpy(), 399 | outputs[7][i].detach().cpu().numpy(), 400 | outputs[8][i].detach().cpu().numpy()] for i in range(batch_size)] 401 | 402 | mesh = [self.get_robot_mesh(vertices, self.robot_faces) for vertices in vertices_list] 403 | return mesh 404 | 405 | def get_forward_vertices(self, pose, theta): 406 | batch_size = pose.size()[0] 407 | outputs = self.forward(pose, theta) 408 | 409 | robot_vertices = torch.cat(( 410 | outputs[0].view(batch_size, -1, 3), 411 | outputs[1].view(batch_size, -1, 3), 412 | outputs[2].view(batch_size, -1, 3), 413 | outputs[3].view(batch_size, -1, 3), 414 | outputs[4].view(batch_size, -1, 3), 415 | outputs[5].view(batch_size, -1, 3), 416 | outputs[6].view(batch_size, -1, 3), 417 | outputs[7].view(batch_size, -1, 3), 418 | outputs[8].view(batch_size, -1, 3)), 1) # .squeeze() 419 | 420 | robot_vertices_normal = torch.cat(( 421 | outputs[9].view(batch_size, -1, 3), 422 | outputs[10].view(batch_size, -1, 3), 423 | outputs[11].view(batch_size, -1, 3), 424 | outputs[12].view(batch_size, -1, 3), 425 | outputs[13].view(batch_size, -1, 3), 426 | outputs[14].view(batch_size, -1, 3), 427 | outputs[15].view(batch_size, -1, 3), 428 | outputs[16].view(batch_size, -1, 3), 429 | outputs[17].view(batch_size, -1, 3)), 1) # .squeeze() 430 | 431 | return robot_vertices,robot_vertices_normal 432 | 433 | 434 | 435 | if __name__ == "__main__": 436 | device = 'cuda' 437 | panda = PandaLayer(device).to(device) 438 | scene = trimesh.Scene() 439 | 440 | # show robot 441 | # theta = panda.theta_min + (panda.theta_max-panda.theta_min)*0.5 442 | # theta = torch.tensor([0, 0.8, -0.0, -2.3, -2.8, 1.5, np.pi/4.0]).float().to(device).reshape(-1,7) 443 | theta = torch.rand(1,7).float().to(device) 444 | pose = torch.from_numpy(np.identity(4)).to(device).reshape(-1, 4, 4).expand(len(theta),-1,-1).float() 445 | robot_mesh = panda.get_forward_robot_mesh(pose, theta) 446 | robot_mesh = np.sum(robot_mesh) 447 | trimesh.exchange.export.export_mesh(robot_mesh, os.path.join('output_meshes',f"whole_body_levelset_0.stl")) 448 | robot_mesh.show() -------------------------------------------------------------------------------- /panda_layer/process_stl.py: -------------------------------------------------------------------------------- 1 | 2 | # ----------------------------------------------------------------------------- 3 | # SPDX-License-Identifier: MIT 4 | # This file is part of the RDF project. 5 | # Copyright (c) 2023 Idiap Research Institute 6 | # Contributor: Yimming Li 7 | # ----------------------------------------------------------------------------- 8 | 9 | import trimesh 10 | import glob 11 | import os 12 | 13 | 14 | for i in range(9): 15 | mesh_path = os.path.dirname(os.path.realpath(__file__)) + f"/meshes/visual/link{i}_vis.stl" 16 | mesh = trimesh.load(mesh_path) 17 | mesh.show() 18 | vertices = mesh.vertices 19 | -------------------------------------------------------------------------------- /panda_layer/remesh.py: -------------------------------------------------------------------------------- 1 | # ----------------------------------------------------------------------------- 2 | # SPDX-License-Identifier: MIT 3 | # This file is part of the RDF project. 4 | # Copyright (c) 2023 Idiap Research Institute 5 | # Contributor: Yimming Li 6 | # ----------------------------------------------------------------------------- 7 | 8 | 9 | import trimesh 10 | import glob 11 | import os 12 | import numpy as np 13 | # mesh_path = os.path.dirname(os.path.realpath(__file__)) + "/meshes/*" 14 | # mesh_files = glob.glob(mesh_path) 15 | # save_mesh = False 16 | # 17 | # for mf in mesh_files: 18 | # name = os.path.basename(mf)[:-4] 19 | # mesh = trimesh.load(mf) 20 | # new_mesh = mesh.simplify_quadratic_decimation(100) 21 | # if save_mesh: 22 | # trimesh.exchange.export.export_mesh(new_mesh, os.path.join(os.path.dirname(os.path.realpath(__file__)),f'meshes/{name}_face_100.stl')) 23 | 24 | 25 | # mesh_path = os.path.dirname(os.path.realpath(__file__)) + "/meshes/face_100/*.stl" 26 | # mesh_files = glob.glob(mesh_path) 27 | # save_mesh = True 28 | # 29 | # for mf in mesh_files: 30 | # name = os.path.basename(mf)[:-13] 31 | # print(name) 32 | # mesh = trimesh.load(mf) 33 | # verts = mesh.vertices 34 | # faces = mesh.faces 35 | # new_verts,new_faces = trimesh.remesh.subdivide_to_size(verts, faces, max_edge=0.1, max_iter=10, return_index=False) 36 | # new_mesh = trimesh.Trimesh(new_verts,new_faces) 37 | # # new_mesh = new_mesh.simplify_quadratic_decimation(500) 38 | # print(len(new_mesh.vertices)) 39 | # # new_mesh.show() 40 | # if save_mesh: 41 | # save_path = os.path.join(os.path.dirname(os.path.realpath(__file__)),f'meshes/smooth/') 42 | # if os.path.exists(save_path) is not True: 43 | # os.mkdir(save_path) 44 | # trimesh.exchange.export.export_mesh(new_mesh, os.path.join(os.path.dirname(os.path.realpath(__file__)),f'meshes/smooth/{name}.stl')) 45 | mesh_path = os.path.dirname(os.path.realpath(__file__)) + "/meshes/visual/*.stl" 46 | mesh_files = glob.glob(mesh_path) 47 | for mf in mesh_files: 48 | mesh_name = mf.split('/')[-1].split('_')[0] 49 | print(mesh_name) 50 | if mesh_name =='link0': 51 | continue 52 | scene = trimesh.Scene() 53 | mesh = trimesh.load(mf) 54 | mesh.show() 55 | print(mesh.vertices.shape) 56 | vertices,faces = trimesh.remesh.subdivide_to_size(mesh.vertices, mesh.faces, max_edge=0.005, max_iter=10, return_index=False) 57 | print(vertices.shape) 58 | mesh = trimesh.Trimesh(vertices,faces) 59 | # ray_visualize = trimesh.load_path(np.hstack((mesh.vertices, mesh.vertices + mesh.vertex_normals / 100)).reshape(-1, 2, 3)) 60 | # scene.add_geometry([mesh,ray_visualize]) 61 | # scene.show() 62 | # trimesh.exchange.export.export_mesh(mesh, os.path.dirname(os.path.realpath(__file__)) + f"/meshes/high_quality/{mesh_name}.stl") -------------------------------------------------------------------------------- /panda_layer/renormolize.py: -------------------------------------------------------------------------------- 1 | # ----------------------------------------------------------------------------- 2 | # SPDX-License-Identifier: MIT 3 | # This file is part of the RDF project. 4 | # Copyright (c) 2023 Idiap Research Institute 5 | # Contributor: Yimming Li 6 | # ----------------------------------------------------------------------------- 7 | import trimesh 8 | import glob 9 | import os 10 | import numpy as np 11 | import torch 12 | 13 | mesh_path = os.path.dirname(os.path.realpath(__file__)) + "/meshes/visual/link8_vis.stl" 14 | mesh_files = glob.glob(mesh_path) 15 | save_mesh = True 16 | 17 | for mf in mesh_files: 18 | scene = trimesh.Scene() 19 | mesh = trimesh.load(mf) 20 | vertices,faces = trimesh.remesh.subdivide_to_size(mesh.vertices, mesh.faces, max_edge=0.05, max_iter=10, return_index=False) 21 | mesh = trimesh.Trimesh(vertices,faces) 22 | print(mesh.vertices.shape) 23 | center = np.mean(mesh.vertices,axis=0) 24 | verts = torch.from_numpy(mesh.vertices-center) 25 | normals = torch.from_numpy(mesh.vertex_normals) 26 | cosine = torch.cosine_similarity(verts,normals) 27 | print(cosine) 28 | normals[cosine<0] = -normals[cosine<0] 29 | normals = normals.numpy() 30 | ray_visualize = trimesh.load_path(np.hstack((mesh.vertices, mesh.vertices + normals / 100)).reshape(-1, 2, 3)) 31 | scene.add_geometry(mesh) 32 | scene.add_geometry(ray_visualize) 33 | scene.show() 34 | # new_verts,new_faces = trimesh.remesh.subdivide_to_size(verts, faces, max_edge=0.1, max_iter=10, return_index=False) 35 | # new_mesh = trimesh.Trimesh(new_verts,new_faces) 36 | # # new_mesh = new_mesh.simplify_quadratic_decimation(500) 37 | # print(len(new_mesh.vertices)) 38 | # # new_mesh.show() 39 | # if save_mesh: 40 | # save_path = os.path.join(os.path.dirname(os.path.realpath(__file__)),f'meshes/smooth/') 41 | # if os.path.exists(save_path) is not True: 42 | # os.mkdir(save_path) 43 | # trimesh.exchange.export.export_mesh(new_mesh, os.path.join(os.path.dirname(os.path.realpath(__file__)),f'meshes/smooth/{name}.stl')) -------------------------------------------------------------------------------- /readme.md: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | ### Code for paper "Learning Robot Geometry as Distance Fields: Applications to Whole-body Manipulation" 5 | 6 | [[Paper]](https://arxiv.org/pdf/2307.00533.pdf)[[Project]](https://sites.google.com/view/lrdf) 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 [Chamfer Distance](https://github.com/otaheri/chamfer_distance) (optional, only used for evaluating the chamfer distance) 20 | 21 | #### Usage 22 | 23 | ##### Run RDF represented with basis functions 24 | ```sh 25 | python bf_sdf.py --n_func 24 --device cuda 26 | ``` 27 | 28 | Given points with size (N,3) and joint configurations (B,7), it will output SDF values (B,N) and gradients w.r.t. both points (analytical, with shape(B,N,3)) and joints (numerical, with shape(B,N,7)). 29 | 30 | You can also uncomment the code 31 | ``` python 32 | # # visualize the Bernstein Polynomial model for each robot link 33 | # bp_sdf.create_surface_mesh(model,nbData=128,vis=True) 34 | 35 | # # visualize the Bernstein Polynomial model for the whole body 36 | # theta = torch.tensor([0, -0.3, 0, -2.2, 0, 2.0, np.pi/4]).float().to(args.device).reshape(-1,7) 37 | # pose = torch.from_numpy(np.identity(4)).to(args.device).reshape(-1, 4, 4).expand(len(theta),4,4).float() 38 | # trans_list = panda.get_transformations_each_link(pose,theta) 39 | # utils.visualize_reconstructed_whole_body(model, trans_list, tag=f'BP_{args.n_func}') 40 | ``` 41 | to visualize the reconstructed meshes for each robot link and whole body, respectively. 42 | 43 | We provide some pretrained models in ```models```. 44 | 45 | ##### Visualize the SDF 46 | We provide 2D and 3D visualization for produced SDF and gradient, just run 47 | ```sh 48 | python vis.py 49 | ``` 50 | to see the results. 51 | 52 | ##### Train RDF model with basis functions 53 | Generate SDF data for each robot link: 54 | ```sh 55 | python sample_sdf_points.py 56 | ``` 57 | It computes the SDF value based on [mesh_to_sdf](https://github.com/marian42/mesh_to_sdf). The sampled points and sdf values are saved in ```data/sdf_points```. You can also download the data [here](https://drive.google.com/file/d/1lsdJzxECFOILhYiCJydOcruKoqB6QiJR/view?usp=sharing). After this, please put '*.npy' files in ```data/sdf_points/``` 58 | 59 | 60 | Then just run 61 | ```sh 62 | python bf_sdf.py --train --n_func 8 --device cuda 63 | ``` 64 | 65 | to learn weights of basis functions. Normally it will take 1~2 minutes to train a model when using 8 basis functions. 66 | 67 | ##### Evaluation 68 | For instance, you can run 69 | ```sh 70 | # method:[BP_8,BP_24,NN_LD,NN_AD,Sphere] 71 | # type: [RDF,LINK] 72 | python eval.py --device cuda --method BP_8 --type RDF 73 | ``` 74 | to evaluate the quality of RDF. BP_8 and BP_24 donate numbers of basis functios we use, while NN_LD and NN_AD donate nn models trained with limited data and argumented data. 75 | 76 | ##### Dual arm grasp planning 77 | 78 | You can run 79 | ```sh 80 | python bbo_planning.py 81 | ``` 82 | to see how our RDF model can be used for whole arm lifting task with Gauss-Newton algorithm. It will plan 3 valid joint configurations for both arms. 83 | 84 | ##### Train RDF for your own robot 85 | 86 | - Build a differentiable robot layer for forward kinematics (see ```panda_layer/panda_layer.py``` for details) 87 | 88 | - Train RDF model using basis functions (We use .stl file for SDF computation and reconstruction, which can be found in the URDF file) 89 | 90 | - Use it! 91 | 92 | Note: Another option is to use the pytorch kinematics library to parse the urdf file automatically to build RDF for your own robot: https://github.com/UM-ARM-Lab/pytorch_kinematics 93 | 94 | RDF is maintained by Yiming LI and licensed under the MIT License. 95 | 96 | The urdf file we used is licensed under the Apache License. 97 | 98 | Copyright (c) 2023 Idiap Research Institute 99 | -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | numpy 2 | pyyaml 3 | trimesh 4 | argparse 5 | pyglet==1.5.0 6 | matplotlib 7 | git+https://github.com/marian42/mesh_to_sdf.git #mesh-to-sdf 8 | -------------------------------------------------------------------------------- /robot_sdf.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/yimingli1998/RDF/a1cd0d503831fc2b8b921ebb251d8bc714921522/robot_sdf.gif -------------------------------------------------------------------------------- /sample_sdf_points.py: -------------------------------------------------------------------------------- 1 | 2 | # ----------------------------------------------------------------------------- 3 | # This file is part of the RDF project. 4 | # Copyright (c) 2023 Idiap Research Institute 5 | # Contributor: Yimming Li 6 | # ----------------------------------------------------------------------------- 7 | 8 | import trimesh 9 | import glob 10 | import os 11 | import numpy as np 12 | import mesh_to_sdf 13 | import skimage 14 | import pyrender 15 | import torch 16 | 17 | mesh_path = os.path.dirname(os.path.realpath(__file__)) + "/panda_layer/meshes/voxel_128/*.stl" 18 | mesh_files = glob.glob(mesh_path) 19 | mesh_files = sorted(mesh_files)[1:] #except finger 20 | 21 | for mf in mesh_files: 22 | mesh_name = mf.split('/')[-1].split('.')[0] 23 | print(mesh_name) 24 | mesh = trimesh.load(mf) 25 | mesh = mesh_to_sdf.scale_to_unit_sphere(mesh) 26 | 27 | center = mesh.bounding_box.centroid 28 | scale = np.max(np.linalg.norm(mesh.vertices-center, axis=1)) 29 | 30 | # sample points near surface (as same as deepSDF) 31 | near_points, near_sdf = mesh_to_sdf.sample_sdf_near_surface(mesh, 32 | number_of_points = 500000, 33 | surface_point_method='scan', 34 | sign_method='normal', 35 | scan_count=100, 36 | scan_resolution=400, 37 | sample_point_count=10000000, 38 | normal_sample_count=100, 39 | min_size=0.015, 40 | return_gradients=False) 41 | # # sample points randomly within the bounding box [-1,1] 42 | random_points = np.random.rand(500000,3)*2.0-1.0 43 | random_sdf = mesh_to_sdf.mesh_to_sdf(mesh, 44 | random_points, 45 | surface_point_method='scan', 46 | sign_method='normal', 47 | bounding_radius=None, 48 | scan_count=100, 49 | scan_resolution=400, 50 | sample_point_count=10000000, 51 | normal_sample_count=100) 52 | 53 | # save data 54 | data = { 55 | 'near_points': near_points, 56 | 'near_sdf': near_sdf, 57 | 'random_points': random_points, 58 | 'random_sdf': random_sdf, 59 | 'center': center, 60 | 'scale': scale 61 | } 62 | save_path = os.path.join(os.path.dirname(os.path.realpath(__file__)),f'data/sdf_points') 63 | if os.path.exists(save_path) is not True: 64 | os.mkdir(save_path) 65 | np.save(os.path.join(save_path,f'voxel_128_{mesh_name}.npy'), data) 66 | 67 | # # # for visualization 68 | # data = np.load(os.path.join(os.path.join(os.path.dirname(os.path.realpath(__file__)),f'data/sdf_points/voxel_128_{mesh_name}.npy')), allow_pickle=True).item() 69 | # random_points = data['random_points'] 70 | # random_sdf = data['random_sdf'] 71 | # near_points = data['near_points'] 72 | # near_sdf = data['near_sdf'] 73 | # colors = np.zeros(random_points.shape) 74 | # colors[random_sdf < 0, 2] = 1 75 | # colors[random_sdf > 0, 0] = 1 76 | # cloud = pyrender.Mesh.from_points(random_points, colors=colors) 77 | # scene = pyrender.Scene() 78 | # scene.add(cloud) 79 | # viewer = pyrender.Viewer(scene, use_raymond_lighting=True, point_size=2) 80 | -------------------------------------------------------------------------------- /sphere.py: -------------------------------------------------------------------------------- 1 | 2 | # ----------------------------------------------------------------------------- 3 | # SPDX-License-Identifier: MIT 4 | # This file is part of the RDF project. 5 | # Copyright (c) 2023 Idiap Research Institute 6 | # Contributor: Yimming Li 7 | # ----------------------------------------------------------------------------- 8 | 9 | import time 10 | import torch 11 | import os 12 | import numpy as np 13 | import trimesh 14 | import yaml 15 | from panda_layer.panda_layer import PandaLayer 16 | import utils 17 | 18 | CUR_DIR = os.path.dirname(os.path.abspath(__file__)) 19 | 20 | class SphereSDF(): 21 | def __init__(self, device): 22 | self.device = device 23 | self.panda = PandaLayer(self.device) 24 | self.conf = self.load_conf() 25 | self.r,self.c = self.get_sphere_param(self.conf) 26 | 27 | def load_conf(self): 28 | with open(os.path.join(CUR_DIR,'panda_layer/franka_sphere.yaml'), 'r') as f: 29 | conf = yaml.load(f,Loader=yaml.FullLoader)['collision_spheres'] 30 | return conf 31 | 32 | def get_sphere_param(self, sphere_conf): 33 | rs,cs = [],[] 34 | for i,k in enumerate(sphere_conf.keys()): 35 | sphere_each_link = sphere_conf[k] 36 | r_list,c_list = [],[] 37 | for sphere in sphere_each_link: 38 | r_list.append(torch.tensor(sphere['radius']).unsqueeze(0).to(self.device)) 39 | c_list.append(torch.tensor(sphere['center']).unsqueeze(0).to(self.device)) 40 | radius = torch.cat(r_list) 41 | center = torch.cat(c_list) 42 | rs.append(radius) 43 | cs.append(center) 44 | rs = torch.cat(rs, dim=0) 45 | # cs = torch.cat(cs, dim=0) 46 | return rs,cs 47 | 48 | def get_sdf(self, x, pose, theta, rs, cs): 49 | B,N = theta.shape[0],x.shape[0] 50 | pose =pose.reshape(-1, 4, 4).expand(len(theta),4,4).float() 51 | trans = self.panda.get_transformations_each_link(pose,theta) 52 | c_list = [] 53 | for c,t in zip(cs,trans): 54 | trans_c = utils.transform_points(c,t,self.device) 55 | c_list.append(trans_c) 56 | c = torch.cat(c_list,dim=1) 57 | N_s = c.shape[1] 58 | x = x.unsqueeze(0).expand(B,N,3) 59 | x_ = x.unsqueeze(2).expand(B,N, N_s,3) 60 | c_ = c.unsqueeze(1).expand(B,N,N_s,3) 61 | 62 | dist = torch.norm(x_ - c_, dim=-1) - rs.unsqueeze(0).unsqueeze(0).expand(B,N,N_s) 63 | sdf,idx = dist.min(dim=-1) 64 | c_idx = c_.gather(2,idx.unsqueeze(-1).unsqueeze(-1).expand(B,N,N_s,3))[:,:,0,:] 65 | grad = torch.nn.functional.normalize(c_idx-x,dim =-1) 66 | return sdf,grad 67 | 68 | def get_sdf_with_joint_grad(self, x, pose, theta, rs, cs,delta =0.001): 69 | # t0 = time.time() 70 | B = theta.shape[0] 71 | theta = theta.unsqueeze(1) 72 | d_theta = (theta.expand(B,7,7)+ torch.eye(7,device=self.device).unsqueeze(0).expand(B,7,7)*delta).reshape(B,-1,7) 73 | theta = torch.cat([theta,d_theta],dim=1).reshape(B*8,7) 74 | pose = pose.expand(B*8,4,4) 75 | # t1 = time.time() 76 | sdf,_ = self.get_sdf(x,pose,theta,rs,cs) 77 | sdf = sdf.reshape(B,8,-1) 78 | d_sdf = (sdf[:,1:,:]-sdf[:,:1,:])/delta 79 | # t2 = time.time() 80 | # print(t2-t1,t1-t0) 81 | return sdf[:,0,:],d_sdf.transpose(1,2) 82 | 83 | def get_sdf_normal_with_joint_grad(self,x,pose,theta, rs, cs,delta =0.001): 84 | B = theta.shape[0] 85 | theta = theta.unsqueeze(1) 86 | d_theta = (theta.expand(B,7,7)+ torch.eye(7,device=self.device).unsqueeze(0).expand(B,7,7)*delta).reshape(B,-1,7) 87 | theta = torch.cat([theta,d_theta],dim=1).reshape(B*8,7) 88 | pose = pose.expand(B*8,4,4) 89 | sdf, normal = self.get_sdf(x,pose,theta,rs,cs) 90 | normal = normal.reshape(B,8,-1,3).transpose(1,2) 91 | return normal # normal size: (B,N,8,3) normal[:,:,0,:] origin normal vector normakl[:,:,1:,:] derivatives with respect to joints 92 | 93 | if __name__ == "__main__": 94 | device = "cuda" 95 | sphere_sdf = SphereSDF(device) 96 | 97 | with open(os.path.join(CUR_DIR,'panda_layer/franka_sphere.yaml'), 'r') as f: 98 | conf = yaml.load(f,Loader=yaml.FullLoader)['collision_spheres'] 99 | 100 | x = torch.rand(128,3).to(device)*2.0 - 1.0 101 | theta = torch.rand(1,7).to(device).float() 102 | pose = torch.from_numpy(np.identity(4)).unsqueeze(0).to(device).expand(len(theta),4,4).float() 103 | rs, cs = sphere_sdf.get_sphere_param(conf) 104 | sdf,grad = sphere_sdf.get_sdf(x, pose, theta, rs, cs) 105 | sdf,grad = sphere_sdf.get_sdf_with_joint_grad(x, pose, theta, rs, cs) 106 | print(sdf.shape, grad.shape) 107 | -------------------------------------------------------------------------------- /utils.py: -------------------------------------------------------------------------------- 1 | # ----------------------------------------------------------------------------- 2 | # SPDX-License-Identifier: MIT 3 | # This file is part of the RDF project. 4 | # Copyright (c) 2023 Idiap Research Institute 5 | # Contributor: Yimming Li 6 | # ----------------------------------------------------------------------------- 7 | 8 | import copy 9 | import trimesh 10 | import numpy as np 11 | import torch 12 | import os 13 | import glob 14 | import mesh_to_sdf 15 | CUR_DIR = os.path.dirname(os.path.abspath(__file__)) 16 | 17 | def transform_points(points, trans,device): 18 | # transfrom points in SE(3) points:(N,3) trans:(B,4,4) 19 | B,N = trans.shape[0],points.shape[0] 20 | ones = torch.ones([B, N, 1],device =device).float() 21 | points_ = torch.cat([points.unsqueeze(0).expand(B,N,3), ones], dim=-1) 22 | points_ = torch.matmul(trans, points_.permute(0, 2, 1)).permute(0, 2, 1) 23 | return points_[:, :, :3].float() 24 | 25 | def mse(yhat,y): 26 | return torch.nn.MSELoss(reduction='mean')(yhat,y) 27 | 28 | def rmse(yhat,y): 29 | return torch.sqrt(mse(yhat,y)) 30 | 31 | def print_eval(yhat,y,string='default'): 32 | yhat,y = yhat.view(-1),y.view(-1) 33 | y_near = (y.abs()<0.03) 34 | y_far = (y.abs()>0.03) 35 | MAE = (yhat-y).abs().mean() 36 | MSE = mse(yhat,y) 37 | RMSE = rmse(yhat,y) 38 | MAE_near = (yhat[y_near]-y[y_near]).abs().mean() 39 | MSE_near = mse(yhat[y_near],y[y_near]) 40 | RMSE_near = rmse(yhat[y_near],y[y_near]) 41 | MAE_far = (yhat[y_far]-y[y_far]).abs().mean() 42 | MSE_far = mse(yhat[y_far],y[y_far]) 43 | RMSE_far = rmse(yhat[y_far],y[y_far]) 44 | # print(f'{string}\t' 45 | # f'abs:{MAE:.6f}\t' 46 | # f'mse:{MSE:.6f}\t' 47 | # f'rmse:{RMSE:.6f}\t' 48 | # f'abs_near:{MAE_near:.6f}\t' 49 | # f'mse_near:{MSE_near:.6f}\t' 50 | # f'rmse_near:{RMSE_near:.6f}\t' 51 | # f'abs_far:{MAE_far:.6f}\t' 52 | # f'mse_far:{MSE_far:.6f}\t' 53 | # f'rmse_far:{RMSE_far:.6f}\t') 54 | res = [MAE,MSE,RMSE,MAE_near,MSE_near,RMSE_near,MAE_far,MSE_far,RMSE_far] 55 | return [r.item() for r in res] 56 | 57 | def eval_chamfer_distance(tag): 58 | from chamfer_distance import ChamferDistance as chamfer_dist 59 | mesh_path = os.path.join(CUR_DIR,"panda_layer/meshes/voxel_128/*") 60 | mesh_files = glob.glob(mesh_path) 61 | mesh_files = sorted(mesh_files)[1:] #except finger 62 | res = [] 63 | for i,mf in enumerate(mesh_files): 64 | scene = trimesh.Scene() 65 | mesh_name = mf.split('/')[-1].split('.')[0] 66 | mesh = trimesh.load(mf) 67 | mesh = mesh_to_sdf.scale_to_unit_sphere(mesh) 68 | # scene.add_geometry(mesh) 69 | rec_mesh = trimesh.load(CUR_DIR +f'/output_meshes/{tag}_{mesh_name}.stl') 70 | # rec_mesh.vertices = rec_mesh.vertices + [2.0,0,0] 71 | # rec_mesh.visual.face_colors= [255,0,0,100] 72 | # scene.add_geometry(rec_mesh) 73 | 74 | mesh_points = trimesh.sample.sample_surface_even(mesh,30000)[0] 75 | rec_mesh_points = trimesh.sample.sample_surface_even(rec_mesh,30000)[0] 76 | 77 | chamfer = chamfer_dist() 78 | x_near, y_near, xidx_near, yidx_near = chamfer(torch.from_numpy(mesh_points).float().unsqueeze(0).to('cuda'), 79 | torch.from_numpy(rec_mesh_points).float().unsqueeze(0).to('cuda')) 80 | cd_mean = (torch.mean(x_near) + torch.mean(y_near)).item()*1000.0 81 | cd_max = (torch.max(x_near) + torch.max(y_near)).item()*1000.0 82 | res.append(np.asarray([cd_mean, cd_max])) 83 | cd_mean,cd_max =np.mean(res,axis=0) 84 | return cd_mean,cd_max 85 | 86 | def visualize_reconstructed_whole_body(model, trans_list,tag): 87 | mesh_path = os.path.join(CUR_DIR,f"output_meshes/{tag}_*.stl") 88 | mesh_files = glob.glob(mesh_path) 89 | mesh_files.sort() 90 | view_mat = np.array([[1,0,0,0],[0,0,1,0],[0,-1,0,0],[0,0,0,1]]) 91 | scene = trimesh.Scene() 92 | for i,mf in enumerate(mesh_files): 93 | mesh = trimesh.load(mf) 94 | mesh_dict = model[i] 95 | offset = mesh_dict['offset'].cpu().numpy() 96 | scale = mesh_dict['scale'] 97 | mesh.vertices = mesh.vertices*scale + offset 98 | mesh.apply_transform(trans_list[i].squeeze().cpu().numpy()) 99 | mesh.apply_transform(view_mat) 100 | scene.add_geometry(mesh) 101 | scene.show() 102 | 103 | def rotation_matrix_from_vectors(vec1, vec2): 104 | """ Find the rotation matrix that aligns vec1 to vec2 105 | :param vec1: A 3d "source" vector 106 | :param vec2: A 3d "destination" vector 107 | :return mat: A transform matrix (3x3) which when applied to vec1, aligns it with vec2. 108 | """ 109 | a, b = (vec1 / np.linalg.norm(vec1)).reshape(3), (vec2 / np.linalg.norm(vec2)).reshape(3) 110 | v = np.cross(a, b) 111 | if any(v): #if not all zeros then 112 | c = np.dot(a, b) 113 | s = np.linalg.norm(v) 114 | kmat = np.array([[0, -v[2], v[1]], [v[2], 0, -v[0]], [-v[1], v[0], 0]]) 115 | return np.eye(3) + kmat + kmat.dot(kmat) * ((1 - c) / (s ** 2)) 116 | 117 | elif np.linalg.norm(a-b)<1e-6: 118 | return np.eye(3) #cross of all zeros only occurs on identical directions 119 | else: 120 | return -np.eye(3) 121 | 122 | 123 | def create_arrow(vector,point,vec_length = 0.05,color =[255,0,0]): 124 | v_norm = np.linalg.norm(vector) 125 | r = vec_length/12.0 126 | h = vec_length/2.0 127 | cy = trimesh.creation.cylinder(r/2.0,h) 128 | cy.vertices[:,2] = cy.vertices[:,2] + h/2.0 129 | cc = trimesh.creation.cone(r,h) 130 | cc.vertices[:,2] = cc.vertices[:,2] + h 131 | arrow = np.sum([cy,cc]) 132 | 133 | transformation = np.eye(4) 134 | 135 | rot = rotation_matrix_from_vectors(np.array([0,0,1]),vector/v_norm) 136 | 137 | transformation[:3,:3] = rot 138 | transformation[:3,3] = point 139 | arrow.apply_transform(transformation) 140 | arrow.visual.face_colors = np.array(color,dtype=object) 141 | return arrow 142 | -------------------------------------------------------------------------------- /vis.py: -------------------------------------------------------------------------------- 1 | 2 | 3 | # ----------------------------------------------------------------------------- 4 | # SPDX-License-Identifier: MIT 5 | # This file is part of the RDF project. 6 | # Copyright (c) 2023 Idiap Research Institute 7 | # Contributor: Yimming Li 8 | # ----------------------------------------------------------------------------- 9 | 10 | 11 | import torch 12 | import os 13 | from panda_layer.panda_layer import PandaLayer 14 | import bf_sdf 15 | import matplotlib.pyplot as plt 16 | import numpy as np 17 | import trimesh 18 | import utils 19 | import argparse 20 | 21 | def plot_2D_panda_sdf(pose,theta,bp_sdf,nbData,model,device): 22 | domain_0 = torch.linspace(-1.0,1.0,nbData).to(device) 23 | domain_1 = torch.linspace(-1.0,1.0,nbData).to(device) 24 | grid_x, grid_y= torch.meshgrid(domain_0,domain_1) 25 | p1 = torch.stack([grid_x.reshape(-1),grid_y.reshape(-1),torch.zeros_like(grid_x.reshape(-1))],dim=1) 26 | p2 = torch.stack([torch.zeros_like(grid_x.reshape(-1)),grid_x.reshape(-1)*0.4, grid_y.reshape(-1)*0.4+0.375],dim=1) 27 | p3 = torch.stack([grid_x.reshape(-1)*0.4 + 0.2,torch.zeros_like(grid_x.reshape(-1)),grid_y.reshape(-1)*0.4+0.375],dim=1) 28 | grid_x, grid_y= grid_x.detach().cpu().numpy(), grid_y.detach().cpu().numpy() 29 | 30 | plt.figure(figsize=(10,10)) 31 | plt.rc('font', size=25) 32 | p2_split = torch.split(p2,1000,dim=0) 33 | sdf,ana_grad = [],[] 34 | for p_2 in p2_split: 35 | sdf_split,ana_grad_split = bp_sdf.get_whole_body_sdf_batch(p_2,pose,theta,model,use_derivative=True) 36 | sdf_split,ana_grad_split = sdf_split.squeeze(),ana_grad_split.squeeze() 37 | sdf.append(sdf_split) 38 | ana_grad.append(ana_grad_split) 39 | sdf = torch.cat(sdf,dim=0) 40 | ana_grad = torch.cat(ana_grad,dim=0) 41 | p2 = p2.detach().cpu().numpy() 42 | sdf =sdf.squeeze().reshape(nbData,nbData).detach().cpu().numpy() 43 | ct1 = plt.contour(grid_x*0.4,grid_y*0.4+0.375,sdf,levels=12) 44 | plt.clabel(ct1, inline=False, fontsize=10) 45 | ana_grad_2d = -torch.nn.functional.normalize(ana_grad[:,[1,2]],dim=-1)*0.01 46 | p2_3d = p2.reshape(nbData,nbData,3) 47 | ana_grad_3d = ana_grad_2d.reshape(nbData,nbData,2) 48 | plt.quiver(p2_3d[0:-1:4,0:-1:4,1],p2_3d[0:-1:4,0:-1:4,2],ana_grad_3d[0:-1:4,0:-1:4,0].detach().cpu().numpy(),ana_grad_3d[0:-1:4,0:-1:4,1].detach().cpu().numpy(),scale=0.5,color = [0.1,0.1,0.1]) 49 | plt.title('YoZ') 50 | plt.show() 51 | 52 | # plt.subplot(1,3,3) 53 | plt.figure(figsize=(10,10)) 54 | plt.rc('font', size=25) 55 | p3_split = torch.split(p3,1000,dim=0) 56 | sdf,ana_grad = [],[] 57 | for p_3 in p3_split: 58 | sdf_split,ana_grad_split = bp_sdf.get_whole_body_sdf_batch(p_3,pose,theta,model,use_derivative=True) 59 | sdf_split,ana_grad_split = sdf_split.squeeze(),ana_grad_split.squeeze() 60 | sdf.append(sdf_split) 61 | ana_grad.append(ana_grad_split) 62 | sdf = torch.cat(sdf,dim=0) 63 | ana_grad = torch.cat(ana_grad,dim=0) 64 | p3 = p3.detach().cpu().numpy() 65 | sdf =sdf.squeeze().reshape(nbData,nbData).detach().cpu().numpy() 66 | ct1 = plt.contour(grid_x*0.4+0.2,grid_y*0.4+0.375,sdf,levels=12) 67 | plt.clabel(ct1, inline=False, fontsize=10) 68 | ana_grad_2d = -torch.nn.functional.normalize(ana_grad[:,[0,2]],dim=-1)*0.01 69 | p3_3d = p3.reshape(nbData,nbData,3) 70 | ana_grad_3d = ana_grad_2d.reshape(nbData,nbData,2) 71 | plt.quiver(p3_3d[0:-1:4,0:-1:4,0],p3_3d[0:-1:4,0:-1:4,2],ana_grad_3d[0:-1:4,0:-1:4,0].detach().cpu().numpy(),ana_grad_3d[0:-1:4,0:-1:4,1].detach().cpu().numpy(),scale=0.5,color = [0.1,0.1,0.1]) 72 | plt.title('XoZ') 73 | plt.show() 74 | 75 | def plot_3D_panda_with_gradient(pose,theta,bp_sdf,model,device): 76 | robot_mesh = panda.get_forward_robot_mesh(pose, theta)[0] 77 | robot_mesh = np.sum(robot_mesh) 78 | 79 | surface_points = robot_mesh.vertices 80 | scene = trimesh.Scene() 81 | # robot mesh 82 | scene.add_geometry(robot_mesh) 83 | choice = np.random.choice(len(surface_points), 1024, replace=False) 84 | surface_points = surface_points[choice] 85 | p =torch.from_numpy(surface_points).float().to(device) 86 | ball_query = trimesh.creation.uv_sphere(1).vertices 87 | choice_ball = np.random.choice(len(ball_query), 1024, replace=False) 88 | ball_query = ball_query[choice_ball] 89 | p = p + torch.from_numpy(ball_query).float().to(device)*0.5 90 | sdf,ana_grad = bp_sdf.get_whole_body_sdf_batch(p,pose,theta,model,use_derivative=True,used_links = [0,1,2,3,4,5,6,7,8]) 91 | sdf,ana_grad = sdf.squeeze().detach().cpu().numpy(),ana_grad.squeeze().detach().cpu().numpy() 92 | # points 93 | pts = p.detach().cpu().numpy() 94 | colors = np.zeros_like(pts,dtype=object) 95 | colors[:,0] = np.abs(sdf)*400 96 | # pc =trimesh.PointCloud(pts,colors) 97 | # scene.add_geometry(pc) 98 | 99 | # gradients 100 | for i in range(len(pts)): 101 | dg = ana_grad[i] 102 | if dg.sum() ==0: 103 | continue 104 | c = colors[i] 105 | print(c) 106 | m = utils.create_arrow(-dg,pts[i],vec_length = 0.05,color=c) 107 | scene.add_geometry(m) 108 | scene.show() 109 | 110 | def generate_panda_mesh_sdf_points(max_dist =0.10): 111 | # represent SDF using basis functions 112 | import glob 113 | import mesh_to_sdf 114 | mesh_path = os.path.dirname(os.path.realpath(__file__)) + "/panda_layer/meshes/voxel_128/*" 115 | mesh_files = glob.glob(mesh_path) 116 | mesh_files = sorted(mesh_files)[1:] #except finger 117 | mesh_dict = {} 118 | 119 | for i,mf in enumerate(mesh_files): 120 | mesh_name = mf.split('/')[-1].split('.')[0] 121 | print(mesh_name) 122 | mesh = trimesh.load(mf) 123 | mesh_dict[i] = {} 124 | mesh_dict[i]['mesh_name'] = mesh_name 125 | 126 | vert = mesh.vertices 127 | points = vert + np.random.uniform(-max_dist,max_dist,size=vert.shape) 128 | sdf = random_sdf = mesh_to_sdf.mesh_to_sdf(mesh, 129 | points, 130 | surface_point_method='scan', 131 | sign_method='normal', 132 | bounding_radius=None, 133 | scan_count=100, 134 | scan_resolution=400, 135 | sample_point_count=10000000, 136 | normal_sample_count=100) 137 | mesh_dict[i]['points'] = points 138 | mesh_dict[i]['sdf'] = sdf 139 | np.save('data/panda_mesh_sdf.npy',mesh_dict) 140 | 141 | def vis_panda_sdf(pose, theta,device): 142 | data = np.load('data/panda_mesh_sdf.npy',allow_pickle=True).item() 143 | trans = panda.get_transformations_each_link(pose,theta) 144 | pts = [] 145 | for i,k in enumerate(data.keys()): 146 | points = data[k]['points'] 147 | sdf = data[k]['sdf'] 148 | print(points.shape, sdf.shape) 149 | choice = (sdf <0.05) * (sdf>0.045) 150 | points = points[choice] 151 | sdf = sdf[choice] 152 | 153 | sample = np.random.choice(len(points), 128, replace=True) 154 | points,sdf = points[sample], sdf[sample] 155 | 156 | points = torch.from_numpy(points).float().to(device) 157 | ones = torch.ones([len(points), 1],device =device).float() 158 | points = torch.cat([points, ones], dim=-1) 159 | t = trans[i].squeeze() 160 | print(points.shape,t.shape) 161 | 162 | trans_points = torch.matmul(t,points.t()).t()[:,:3] 163 | pts.append(trans_points) 164 | pts = torch.cat(pts,dim=0).detach().cpu().numpy() 165 | print(pts.shape) 166 | scene = trimesh.Scene() 167 | robot_mesh = panda.get_forward_robot_mesh(pose, theta)[0] 168 | robot_mesh = np.sum(robot_mesh) 169 | scene.add_geometry(robot_mesh) 170 | pc =trimesh.PointCloud(pts,colors = [255,0,0]) 171 | scene.add_geometry(pc) 172 | scene.show() 173 | 174 | if __name__ =='__main__': 175 | 176 | parser = argparse.ArgumentParser() 177 | parser.add_argument('--device', default='cuda', type=str) 178 | parser.add_argument('--domain_max', default=1.0, type=float) 179 | parser.add_argument('--domain_min', default=-1.0, type=float) 180 | parser.add_argument('--n_func', default=8, type=float) 181 | parser.add_argument('--train', action='store_true') 182 | args = parser.parse_args() 183 | 184 | panda = PandaLayer(args.device) 185 | bp_sdf = bf_sdf.BPSDF(args.n_func,args.domain_min,args.domain_max,panda,args.device) 186 | 187 | # load model 188 | model = torch.load(f'models/BP_{args.n_func}.pt') 189 | 190 | # initial the robot configuration 191 | theta = torch.tensor([0, -0.3, 0, -2.2, 0, 2.0, np.pi/4]).float().to(args.device).reshape(-1,7) 192 | pose = torch.from_numpy(np.identity(4)).unsqueeze(0).to(args.device).expand(len(theta),4,4).float() 193 | 194 | # # vis 2D SDF with gradient 195 | # plot_2D_panda_sdf(pose,theta,bp_sdf,nbData=80,model=model,device=args.device) 196 | 197 | # vis 3D SDF with gradient 198 | plot_3D_panda_with_gradient(pose,theta,bp_sdf,model=model,device=args.device) 199 | --------------------------------------------------------------------------------