initial updates to enable fixed joint kinematic targets

Author: plancherb1

.gitignore

@@ -7,6 +7,12 @@ grid.cuh
 t.txt
 t2.txt
 t3.txt
+t4.txt
+t5.txt
+t6.txt
+t7.txt
+t8.txt
+t9.txt
 
 # Prerequisites
 *.d

GRiDCodeGenerator

@@ -78,7 +78,7 @@ def generate_matlab_model(robot, floating_base):
     f.close()
 
 def main():
-    URDF_PATH, DEBUG_MODE, FILE_NAMESPACE_NAME, FLOATING_BASE = parseInputs()
+    URDF_PATH, DEBUG_MODE, FILE_NAMESPACE_NAME, FLOATING_BASE, FIXED_TARGET_NAMES = parseInputs()
     parser = URDFParser()
     robot = parser.parse(URDF_PATH, floating_base=FLOATING_BASE)
 
@@ -91,7 +91,7 @@ def main():
     codegen = GRiDCodeGenerator(robot, DEBUG_MODE, True, FILE_NAMESPACE = FILE_NAMESPACE_NAME)
     if FLOATING_BASE: include_homogenous_transforms = False
     else: include_homogenous_transforms = True
-    codegen.gen_all_code(include_homogenous_transforms = include_homogenous_transforms)
+    codegen.gen_all_code(include_homogenous_transforms = include_homogenous_transforms, fixed_target_name = FIXED_TARGET_NAMES)
     print("New code generated and saved to grid.cuh!")
 
 if __name__ == "__main__":

RBDReference

@@ -71,6 +71,10 @@ void test(){
     grid::end_effector_pose<T,false>(hd_data,d_robotModel,1,dim3(1,1,1),dimms,streams);
     printMat<T,1,6*grid::NUM_EES>(hd_data->h_eePos,1);
 
+    // printf("eePos - for panda_grasptarget_hand\n");
+    // grid::end_effector_pose_panda_grasptarget_hand<T,false>(hd_data,d_robotModel,1,dim3(1,1,1),dimms,streams);
+    // printMat<T,1,6*grid::NUM_EES>(hd_data->h_eePos,1);
+
     printf("deePos\n");
     grid::end_effector_pose_gradient<T,false>(hd_data,d_robotModel,1,dim3(1,1,1),dimms,streams);
     for(int ee=0; ee < grid::NUM_EES; ee++){
@@ -89,8 +93,6 @@ void test(){
             printf("[%d]\n",i); printMat<T,grid::NUM_JOINTS,grid::NUM_JOINTS>(&hd_data->h_d2eePos[offset],grid::NUM_JOINTS);
         }
     }
-    // printMat<T,grid::NUM_JOINTS,grid::NUM_EES*grid::NUM_JOINTS>(hd_data->h_d2eePos,6);
-
     grid::close_grid<T>(streams,d_robotModel,hd_data);
 }
 

URDFParser

@@ -8,26 +8,28 @@
 def main():
     inputs = parseInputs(NO_ARG_OPTION = True)
     if not inputs is None:
-	URDF_PATH, DEBUG_MODE, FILE_NAMESPACE_NAME, FLOATING_BASE = inputs
+        URDF_PATH, DEBUG_MODE, FILE_NAMESPACE_NAME, FLOATING_BASE, FIXED_TARGET_NAMES = inputs
         parser = URDFParser()
         robot = parser.parse(URDF_PATH, floating_base = FLOATING_BASE)
 
         validateRobot(robot, NO_ARG_OPTION = True)
 
-        codegen = GRiDCodeGenerator(robot, DEBUG_MODE, True, FILE_NAMESPACE = FILE_NAMESPACE_NAME)
         print("-----------------")
         print("Generating GRiD.cuh")
         print("-----------------")
-        codegen.gen_all_code(include_homogenous_transforms = True)
+        codegen = GRiDCodeGenerator(robot, DEBUG_MODE, True, FILE_NAMESPACE = FILE_NAMESPACE_NAME)
+        if FLOATING_BASE: include_homogenous_transforms = False
+        else: include_homogenous_transforms = True
+        codegen.gen_all_code(include_homogenous_transforms = include_homogenous_transforms, fixed_target_name = FIXED_TARGET_NAMES)
         print("New code generated and saved to grid.cuh!")
 
     print("-----------------")
     print("Compiling printGRiD")
     print("-----------------")
     result = subprocess.run( \
         ["nvcc", "-std=c++11", "-o", "printGRiD.exe", "printGRiD.cu", \
-         "-gencode", "arch=compute_86,code=sm_86", \
-         "-O3", "-ftz=true", "-prec-div=false", "-prec-sqrt=false"], \
+         "-gencode", "arch=compute_120,code=sm_120", \
+         "-ftz=true", "-prec-div=false", "-prec-sqrt=false"], \
         capture_output=True, text=True \
     )
     if result.stderr:

dev_notes/zp_notes

@@ -6,7 +6,7 @@
 import numpy as np
 
 def main():
-    URDF_PATH, DEBUG_MODE, FILE_NAMESPACE_NAME, FLOATING_BASE = parseInputs()
+    URDF_PATH, DEBUG_MODE, FILE_NAMESPACE_NAME, FLOATING_BASE, FIXED_TARGET_NAMES = parseInputs()
 
     parser = URDFParser()
     robot = parser.parse(URDF_PATH, floating_base=FLOATING_BASE)
@@ -55,9 +55,15 @@ def main():
     if not FLOATING_BASE:
         ee_pos = reference.end_effector_pose(q)
         print("eepos\n",ee_pos)
+        for fixed_target_name in FIXED_TARGET_NAMES:
+            ee_pos2 = reference.end_effector_pose(q, ee_joint_names=fixed_target_name)
+            print("eepos-" + fixed_target_name + "\n",ee_pos2)
 
         dee_pos = reference.end_effector_pose_gradient(q)
         print("deepos\n",dee_pos)
+        for fixed_target_name in FIXED_TARGET_NAMES:
+            dee_pos2 = reference.end_effector_pose_gradient(q, ee_joint_names=fixed_target_name)
+            print("deepos-" + fixed_target_name + "\n",dee_pos2)
 
         d2ee_pos = reference.end_effector_pose_hessian(q)
         print("d2eepos\n", d2ee_pos)

generateGRiD.py

@@ -1,3 +1,4 @@
 from RBDReference import RBDReference
 from URDFParser import URDFParser
-from GRiDCodeGenerator import GRiDCodeGenerator
+from GRiDCodeGenerator import GRiDCodeGenerator
+from util import printUsage, fileExists, validateFile, parseInputs, validateRobot, printErr, rand3_to_quat, initializeValues