example force_osc_xyz_linear_path_gaussian_velocity works with ur5 and jaco2

Author: LeaStef

abr_control/arms/isaacsim_config.py

@@ -15,7 +15,6 @@ def __init__(self, robot_type):
         robot_type: string
             the name of the arm model to load. 
         """
-
         self.robot_type = robot_type
         self.env_idx = 0  # Assuming only one robot
         self.ee_link_name = "EE"  # name used for virtual end-effector, overwritten if one exists already
@@ -26,8 +25,8 @@ def __init__(self, robot_type):
             self.EE_parent_link = "wrist_3_link"  
             self.ee_offset=[0., 0., 0.] 
             START_ANGLES = "0 -.67 -.67 0 0 0"
-            self.target_min = np.array([-0.6, -0.5, 0.5])
-            self.target_range = np.array([0.9, 1.1, 0.3])
+            self.target_min = np.array([-0.4, -0.4, 0.3]) # np.array([-0.6, -0.5, 0.5])
+            self.target_max = np.array([0.4, 0.4, 0.6]) 
             self.controlled_dof = ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint', 'wrist_1_joint', 'wrist_2_joint', 'wrist_3_joint']
             print(f"Virtual end effector with name '{self.ee_link_name}' is attached, as robot has none.")
 
@@ -37,8 +36,8 @@ def __init__(self, robot_type):
             self.EE_parent_link = "j2n6s300_link_6"  
             self.ee_link_name = "j2n6s300_end_effector"  
             START_ANGLES = "2.0 3.14 1.57 4.71 0.0 3.04"
-            self.target_min = np.array([-0.5, -0.5, 0.5])
-            self.target_range = [1, 1, 0.5]
+            self.target_min = np.array([-0.4, -0.4, 0.3]) # np.array([-0.5, -0.5, 0.5])
+            self.target_max = np.array([0.4, 0.4, 0.6]) 
             self.controlled_dof = ['j2n6s300_joint_1', 'j2n6s300_joint_2', 'j2n6s300_joint_3', 'j2n6s300_joint_4', 'j2n6s300_joint_5', 'j2n6s300_joint_6']
             print(f"End effector with name '{self.ee_link_name}' specified in UDS, using it ...")
 
@@ -49,11 +48,7 @@ def __init__(self, robot_type):
             self.ee_offset=[0.26455, 0.00118, -0.0209] # for H1 
             START_ANGLES = "0. 0. 0. 0."
             self.target_min = np.array([0.12, -0.4, 1.4])
-            self.target_range = np.array([
-                0.4 - 0.12,  # x range
-                0.05 - (-0.4), # y range
-                2.2 - 1.4     # z range
-            ])
+            self.target_max = np.array([0.4, 0.05, 2.2]) 
             self.controlled_dof = ['right_shoulder_pitch_joint','right_shoulder_roll_joint', 'right_shoulder_yaw_joint','right_elbow_joint']
             self.lock_prim_standing = '/World/robot/pelvis'
             print(f"Virtual end effector with name '{self.ee_link_name}' is attached, as robot has none.")
@@ -65,11 +60,7 @@ def __init__(self, robot_type):
             self.ee_link_name = "right_hand_link"  
             START_ANGLES = "0. 0. 0. 0. 0."
             self.target_min = np.array([0.12, -0.4, 1.4])
-            self.target_range = np.array([
-                0.49 - 0.12,  # x range
-                0.05 - (-0.4), # y range
-                2.2 - 1.4     # z range
-            ])
+            self.target_max = np.array([0.49, 0.05, 2.2]) 
             self.controlled_dof = ['right_shoulder_pitch_joint','right_shoulder_roll_joint', 'right_shoulder_yaw_joint','right_elbow_joint','right_hand_joint']
             self.lock_prim_standing = '/World/robot/pelvis'
             print(f"End effector with name '{self.ee_link_name}' specified in UDS, using it ...")

abr_control/interfaces/nv_isaacsim.py

@@ -157,7 +157,7 @@ def get_feedback(self):
     def set_xyz(self, name, xyz):
         """Set the position of an object in the environment.
 
-        prim_path : string
+        name : string
             the prim_path of the prim
         xyz : np.array
             the [x,y,z] location of the target [meters]
@@ -166,6 +166,23 @@ def set_xyz(self, name, xyz):
         xformable = UsdGeom.Xformable(prim)
         transform_matrix = Gf.Matrix4d().SetTranslate(Gf.Vec3d(xyz[0], xyz[1], xyz[2]))
         xformable.MakeMatrixXform().Set(transform_matrix)
+
+
+    def set_orientation(self, name, quat_wxyz):
+        """Set the position of an object in the environment.
+
+        name : string
+            the prim_path of the prim
+        quat_wxyz : np.array
+            the [w,x,y,z] quaternion representation of the target orientation
+        """     
+        prim = self.robot_config._get_prim(name)
+        xformable = UsdGeom.Xformable(prim)
+        quat = Gf.Quatd(quat_wxyz[0], Gf.Vec3d(quat_wxyz[1],
+                                           quat_wxyz[2],
+                                           quat_wxyz[3]))
+        transform_matrix = Gf.Matrix4d().SetRotate(quat)
+        xformable.MakeMatrixXform().Set(transform_matrix)
 
 
     # method that humanoid does not fall over
@@ -200,12 +217,16 @@ def create_target_prim(self, prim_path="/World/target", size = .1, color=np.arra
         cube_prim.GetPrim().CreateAttribute("physics:collisionEnabled", Sdf.ValueTypeNames.Bool).Set(False)
         return cube_prim
 
-
-    def set_target_random(self, name="target"):
-        target_min = self.robot_config.target_min
-        target_range = self.robot_config.target_range
-        target_xyz = target_min + np.random.rand(3) * target_range
-        self.set_xyz(name, target_xyz)
+    
+    def create_random_pos(self):
+        pos_target = np.array(
+            [
+                np.random.uniform(low=self.robot_config.target_min[0], high=self.robot_config.target_max[0]),
+                np.random.uniform(low=self.robot_config.target_min[1], high=self.robot_config.target_max[1]),
+                np.random.uniform(low=self.robot_config.target_min[2], high=self.robot_config.target_max[2]),
+            ]
+        )
+        return pos_target
 
 
     # setting max_force via PhysX DriveAPI

examples/IsaacSim/force_osc_xyz.py

@@ -15,10 +15,10 @@
 if len(sys.argv) > 1:
     arm_model = sys.argv[1]
 else:
-    arm_model = "ur5" # "h1_hands" / "h1" / jaco2 / ur5
+    arm_model = "h1" # works with "h1_hands" / "h1" / jaco2 / ur5
 robot_config = arm(arm_model)
 
-dt = 0.005
+dt = 0.005 # 200 Hz
 target_name="target"
 
 # create our IsaacSim interface
@@ -51,10 +51,7 @@
 try:
     # get the end-effector's initial position
     feedback = interface.get_feedback()
-    start = robot_config.Tx(robot_config.ee_link_name, feedback["q"])
-    
-    # make the target offset from that start position
-    interface.set_target_random()
+    interface.set_xyz("target", interface.create_random_pos())
 
     count = 0
     print("\nSimulation starting...\n")

examples/IsaacSim/force_osc_xyz_linear_path_gaussian_velocity_WIP.py renamed to examples/IsaacSim/force_osc_xyz_linear_path_gaussian_velocity.py

@@ -21,8 +21,7 @@
 if len(sys.argv) > 1:
     arm_model = sys.argv[1]
 else:
-    arm_model = "jaco2"
-# initialize our robot config for the jaco2
+    arm_model = "jaco2" # works with jaco2 / ur5 
 robot_config = arm(arm_model)
 
 ctrlr_dof = [True, True, True, False, False, False]
@@ -32,16 +31,14 @@
 print(stars)
 print(dof_print)
 print(stars)
-dt = 0.007  # 143 Hz 
-#dt = 0.001 # 1000 Hz
+dt = 0.005 # 200 Hz
 target_prim_path="/World/target"
 
 # create our interface
-interface = IsaacSim(robot_config, dt=dt)
+interface = IsaacSim(robot_config, dt)
 interface.connect()
 interface.send_target_angles(robot_config.START_ANGLES)
 isaac_target = interface.create_target_prim(prim_path=target_prim_path)
-interface.fix_arm_joint_gains()
 
 # damp the movements of the arm
 damping = Damping(robot_config, kv=10)
@@ -66,35 +63,17 @@
 target_track = []
 
 
-
-
-
-
-
-
-
-
-
-
 print("\nSimulation starting...\n")
 for ii in range(0, n_targets):
         feedback = interface.get_feedback()
-        hand_xyz = robot_config.Tx("EE", feedback["q"])
-
-        pos_target = np.array(
-            [
-                np.random.uniform(low=-0.4, high=0.4),
-                np.random.uniform(low=-0.4, high=0.4),
-                np.random.uniform(low=0.3, high=0.6),
-            ]
-        )
-
+        hand_xyz = robot_config.Tx(robot_config.ee_link_name, feedback["q"])
+        pos_target = interface.create_random_pos()
         path_planner.generate_path(
             start_position=hand_xyz, target_position=pos_target, max_velocity=2
         )
 
-        #interface.set_xyz("target", pos_target)
-        interface.set_xyz(target_prim_path, pos_target)
+        interface.set_xyz("target", pos_target)
+        #interface.set_xyz(target_prim_path, pos_target)
         at_target = 0
         count = 0
 
@@ -103,10 +82,9 @@
                 break
             filtered_target = path_planner.next()
             interface.set_xyz(target_prim_path, filtered_target[:3])
-            #interface.set_xyz("target_orientation", filtered_target[:3])
 
             feedback = interface.get_feedback()
-            hand_xyz = robot_config.Tx("EE", feedback["q"])
+            hand_xyz = robot_config.Tx(robot_config.ee_link_name, feedback["q"])
 
             u = ctrlr.generate(
                 q=feedback["q"],
@@ -128,26 +106,11 @@
             count += 1
 
 
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
 try:
     print("\nSimulation starting...\n")
     for ii in range(0, n_targets):
         feedback = interface.get_feedback()
-        hand_xyz = robot_config.Tx("EE", feedback["q"])
+        hand_xyz = robot_config.Tx(robot_config.ee_link_name, feedback["q"])
 
         pos_target = np.array(
             [
@@ -174,7 +137,7 @@
             #interface.set_xyz("target_orientation", filtered_target[:3])
 
             feedback = interface.get_feedback()
-            hand_xyz = robot_config.Tx("EE", feedback["q"])
+            hand_xyz = robot_config.Tx(robot_config.ee_link_name, feedback["q"])
 
             u = ctrlr.generate(
                 q=feedback["q"],