Skip to content

joint_prim

JointPrim

Bases: BasePrim

Provides high level functions to deal with a joint prim and its attributes/ properties. If there is an joint prim present at the path, it will use it. Otherwise, a new joint prim at the specified prim path will be created when self.load(...) is called.

the prim will have "xformOp:orient", "xformOp:translate" and "xformOp:scale" only post init,

unless it is a non-root articulation link.

Parameters:

Name Type Description Default
prim_path str

prim path of the Prim to encapsulate or create.

required
name str

Name for the object. Names need to be unique per scene.

required
load_config None or dict

If specified, should contain keyword-mapped values that are relevant for loading this prim at runtime. For this joint prim, the below values can be specified:

joint_type (str): If specified, should be the joint type to create. Valid options are: {"Joint", "FixedJoint", "PrismaticJoint", "RevoluteJoint", "SphericalJoint"} (equivalently, one of JointType) body0 (None or str): If specified, should be the absolute prim path to the parent body that this joint is connected to. None can also be valid, which corresponds to cases where only a single body may be specified (e.g.: fixed joints) body1 (None or str): If specified, should be the absolute prim path to the child body that this joint is connected to. None can also be valid, which corresponds to cases where only a single body may be specified (e.g.: fixed joints)

None
articulation None or int

if specified, should be handle to pre-existing articulation. This will enable additional features for this joint prim, e.g.: polling / setting this joint's state. Note that in this case, the joint must already exist prior to this class instance. Default is None, which corresponds to a non-articulated joint.

None
Source code in prims/joint_prim.py
 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
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
class JointPrim(BasePrim):
    """
    Provides high level functions to deal with a joint prim and its attributes/ properties.
    If there is an joint prim present at the path, it will use it. Otherwise, a new joint prim at
    the specified prim path will be created when self.load(...) is called.

    Note: the prim will have "xformOp:orient", "xformOp:translate" and "xformOp:scale" only post init,
            unless it is a non-root articulation link.

    Args:
        prim_path (str): prim path of the Prim to encapsulate or create.
        name (str): Name for the object. Names need to be unique per scene.
        load_config (None or dict): If specified, should contain keyword-mapped values that are relevant for
            loading this prim at runtime. For this joint prim, the below values can be specified:

            joint_type (str): If specified, should be the joint type to create. Valid options are:
                {"Joint", "FixedJoint", "PrismaticJoint", "RevoluteJoint", "SphericalJoint"}
                (equivalently, one of JointType)
            body0 (None or str): If specified, should be the absolute prim path to the parent body that this joint
                is connected to. None can also be valid, which corresponds to cases where only a single body may be
                specified (e.g.: fixed joints)
            body1 (None or str): If specified, should be the absolute prim path to the child body that this joint
                is connected to. None can also be valid, which corresponds to cases where only a single body may be
                specified (e.g.: fixed joints)

        articulation (None or int): if specified, should be handle to pre-existing articulation. This will enable
            additional features for this joint prim, e.g.: polling / setting this joint's state. Note that in this
            case, the joint must already exist prior to this class instance. Default is None,
            which corresponds to a non-articulated joint.
    """

    def __init__(
        self,
        prim_path,
        name,
        load_config=None,
        articulation=None,
    ):
        # Grab dynamic control reference and set properties
        self._art = articulation

        # Other values that will be filled in at runtime
        self._joint_type = None
        self._control_type = None
        self._dof_properties = None

        # The following values will only be valid if this joint is part of an articulation
        self._dc = None
        self._handle = None
        self._n_dof = None
        self._joint_name = None
        self._dof_handles = None

        # Run super method
        super().__init__(
            prim_path=prim_path,
            name=name,
            load_config=load_config,
        )

    def _load(self, simulator=None):
        # Make sure this joint isn't articulated
        assert not self.articulated, "Joint cannot be created, since this is an articulated joint! We are assuming" \
                                     "the joint already exists in the stage."

        # Define a joint prim at the current stage
        stage = get_current_stage()
        prim = create_joint(
            prim_path=self._prim_path,
            joint_type=self._load_config.get("joint_type", JointType.JOINT),
            stage=stage,
        )

        return prim

    def _post_load(self):
        # run super first
        super()._post_load()

        # Possibly set the bodies
        if "body0" in self._load_config and self._load_config["body0"] is not None:
            self.body0 = self._load_config["body0"]
        if "body1" in self._load_config and self._load_config["body1"] is not None:
            self.body1 = self._load_config["body1"]

    def _initialize(self):
        # Always run super first
        super()._initialize()

        # Get joint info
        self._joint_type = JointType.get_type(self._prim.GetTypeName().split("Physics")[-1])

        # Initialize dynamic control references if this joint is articulated
        if self.articulated:
            self._dc = _dynamic_control.acquire_dynamic_control_interface()
            # TODO: A bit hacky way to get the joint handle, ideally we'd simply do dc.get_joint(), but this doesn't seem to work as expected?
            for i in range(self._dc.get_articulation_joint_count(self._art)):
                joint_handle = self._dc.get_articulation_joint(self._art, i)
                joint_path = self._dc.get_joint_path(joint_handle)
                if joint_path == self._prim_path:
                    self._handle = joint_handle
                    break
            assert self._handle is not None, f"Did not find valid articulated joint with path: {self._prim_path}"

            # Grab DOF info / handles
            self._joint_name = self._dc.get_joint_name(self._handle)
            self._n_dof = self._dc.get_joint_dof_count(self._handle)
            self._dof_handles = []
            self._dof_properties = []
            control_types = []
            for i in range(self._n_dof):
                dof_handle = self._dc.get_joint_dof(self._handle, i)
                dof_props = self._dc.get_dof_properties(dof_handle)
                self._dof_handles.append(dof_handle)
                self._dof_properties.append(dof_props)
                # Infer control type based on whether kp and kd are 0 or not
                kp, kd = dof_props.stiffness, dof_props.damping
                if kp == 0.0:
                    control_type = ControlType.EFFORT if kd == 0.0 else ControlType.VELOCITY
                else:
                    control_type = ControlType.POSITION
                control_types.append(control_type)

            # Make sure all the control types are the same -- if not, we had something go wrong!
            assert len(set(control_types)) == 1, f"Got multiple control types for this single joint: {control_types}"
            self._control_type = control_types[0]

    def update_handles(self):
        """
        Updates all internal handles for this prim, in case they change since initialization
        """
        # TODO: A bit hacky way to get the joint handle, ideally we'd simply do dc.get_joint(), but this doesn't seem to work as expected?
        self._handle = None
        for i in range(self._dc.get_articulation_joint_count(self._art)):
            joint_handle = self._dc.get_articulation_joint(self._art, i)
            joint_path = self._dc.get_joint_path(joint_handle)
            if joint_path == self._prim_path:
                self._handle = joint_handle
                break

    def set_control_type(self, control_type, kp=None, kd=None):
        """
        Sets the control type for this joint.

        Args:
            control_type (ControlType): What type of control to use for this joint.
                Valid options are: {ControlType.POSITION, ControlType.VELOCITY, ControlType.EFFORT}
            kp (None or float): If specified, sets the kp gain value for this joint. Should only be set if
                setting ControlType.POSITION
            kd (None or float): If specified, sets the kd gain value for this joint. Should only be set if
                setting ControlType.VELOCITY
        """
        # Sanity check inputs
        assert_valid_key(key=control_type, valid_keys=ControlType.VALID_TYPES, name="control type")
        if control_type == ControlType.POSITION:
            assert kp is not None, "kp gain must be specified for setting POSITION control!"
            assert kd is None, "kd gain must not be specified for setting POSITION control!"
            kd = 0.0
        elif control_type == ControlType.VELOCITY:
            assert kp is None, "kp gain must not be specified for setting VELOCITY control!"
            assert kd is not None, "kd gain must be specified for setting VELOCITY control!"
            kp = 0.0
        else:   # Efforts
            assert kp is None, "kp gain must not be specified for setting EFFORT control!"
            assert kd is None, "kd gain must not be specified for setting EFFORT control!"
            kp, kd = 0.0, 0.0

        # Set values
        if self._dc:
            for dof_handle, dof_property in zip(self._dof_handles, self._dof_properties):
                dof_property.stiffness = kp
                dof_property.damping = kd
                self._dc.set_dof_properties(dof_handle, dof_property)

        # Update control type
        self._control_type = control_type

    @property
    def body0(self):
        """
        Gets this joint's body0 relationship.

        Returns:
            None or str: Absolute prim path to the body prim to set as this joint's parent link, or None if there is
                no body0 specified.
        """
        targets = self._prim.GetRelationship("physics:body0").GetTargets()
        return targets[0].__str__() if len(targets) > 0 else None

    @body0.setter
    def body0(self, body0):
        """
        Sets this joint's body0 relationship.

        Args:
            body0 (str): Absolute prim path to the body prim to set as this joint's parent link.
        """
        # Make sure prim path is valid
        assert is_prim_path_valid(body0), f"Invalid body0 path specified: {body0}"
        self._prim.GetRelationship("physics:body0").SetTargets([Sdf.Path(body0)])

    @property
    def body1(self):
        """
        Gets this joint's body1 relationship.

        Returns:
            None or str: Absolute prim path to the body prim to set as this joint's child link, or None if there is
                no body1 specified.
        """
        targets = self._prim.GetRelationship("physics:body1").GetTargets()
        return targets[0].__str__()

    @body1.setter
    def body1(self, body1):
        """
        Sets this joint's body1 relationship.

        Args:
            body1 (str): Absolute prim path to the body prim to set as this joint's child link.
        """
        # Make sure prim path is valid
        assert is_prim_path_valid(body1), f"Invalid body1 path specified: {body1}"
        self._prim.GetRelationship("physics:body1").SetTargets([Sdf.Path(body1)])

    @property
    def parent_name(self):
        """
        Gets this joint's parent body name, if it exists

        Returns:
            str: Joint's parent body name
        """
        return self._dc.get_rigid_body_name(self._dc.get_joint_parent_body(self._handle))

    @property
    def child_name(self):
        """
        Gets this joint's child body name, if it exists

        Returns:
            str: Joint's child body name
        """
        return self._dc.get_rigid_body_name(self._dc.get_joint_child_body(self._handle))

    @property
    def local_orientation(self):
        """
        Returns:
            4-array: (x,y,z,w) local quaternion orientation of this joint, relative to the parent link
        """
        # Grab local rotation to parent and child links
        quat0 = gf_quat_to_np_array(self.get_attribute("physics:localRot0"))[[1, 2, 3, 0]]
        quat1 = gf_quat_to_np_array(self.get_attribute("physics:localRot1"))[[1, 2, 3, 0]]

        # Invert the child link relationship, and multiply the two rotations together to get the final rotation
        return T.quat_multiply(quaternion1=T.quat_inverse(quat1), quaternion0=quat0)

    @property
    def joint_name(self):
        """
        Returns:
            str: Name of this joint
        """
        return self._joint_name

    @property
    def joint_type(self):
        """
        Gets this joint's type (ignoring the "Physics" prefix)

        Returns:
            str: Joint's type. Should be one of:
                {"FixedJoint", "Joint", "PrismaticJoint", "RevoluteJoint", "SphericalJoint"}
                    (equivalently, one of JointType)
        """
        return self._joint_type

    @property
    def control_type(self):
        """
        Gets the control types for this joint

        Returns:
            ControlType: control type for this joint
        """
        return self._control_type

    @property
    def dof_properties(self):
        """
        Returns:
            list of DOFProperties: Per-DOF properties for this joint.
                See https://docs.omniverse.nvidia.com/py/isaacsim/source/extensions/omni.isaac.dynamic_control/docs/index.html#omni.isaac.dynamic_control._dynamic_control.DofProperties
                for more information.
        """
        return self._dof_properties

    @property
    def max_velocity(self):
        """
        Gets this joint's maximum velocity

        Returns:
            float: maximum velocity for this joint
        """
        # Only support revolute and prismatic joints for now
        assert self.is_single_dof, "Joint properties only supported for a single DOF currently!"
        # We either return the raw value or a default value if there is no max specified
        raw_vel = self._dof_properties[0].max_velocity
        default_max_vel = m.DEFAULT_MAX_REVOLUTE_VEL if self.joint_type == "RevoluteJoint" else m.DEFAULT_MAX_PRISMATIC_VEL
        return default_max_vel if raw_vel is None or np.abs(raw_vel) > m.INF_VEL_THRESHOLD else raw_vel

    @max_velocity.setter
    def max_velocity(self, vel):
        """
        Sets this joint's maximum velocity

        Args:
            vel (float): Velocity to set
        """
        # Only support revolute and prismatic joints for now
        assert self.is_single_dof, "Joint properties only supported for a single DOF currently!"
        self._dof_properties[0].max_velocity = vel
        self._dc.set_dof_properties(self._dof_handles[0], self._dof_properties[0])

    @property
    def max_force(self):
        """
        Gets this joint's maximum force

        Returns:
            float: maximum force for this joint
        """
        # Only support revolute and prismatic joints for now
        assert self.is_single_dof, "Joint properties only supported for a single DOF currently!"
        # We either return the raw value or a default value if there is no max specified
        raw_force = self._dof_properties[0].max_effort
        return m.DEFAULT_MAX_EFFORT if raw_force is None or np.abs(raw_force) > m.INF_EFFORT_THRESHOLD else raw_force

    @max_force.setter
    def max_force(self, force):
        """
        Sets this joint's maximum force

        Args:
            force (float): Force to set
        """
        # Only support revolute and prismatic joints for now
        assert self.is_single_dof, "Joint properties only supported for a single DOF currently!"
        self._dof_properties[0].max_effort = force
        self._dc.set_dof_properties(self._dof_handles[0], self._dof_properties[0])

    @property
    def stiffness(self):
        """
        Gets this joint's stiffness

        Returns:
            float: stiffness for this joint
        """
        # Only support revolute and prismatic joints for now
        assert self.is_single_dof, "Joint properties only supported for a single DOF currently!"
        return self._dof_properties[0].stiffness

    @stiffness.setter
    def stiffness(self, stiffness):
        """
        Sets this joint's stiffness

        Args:
            stiffness (float): stiffness to set
        """
        # Only support revolute and prismatic joints for now
        assert self.is_single_dof, "Joint properties only supported for a single DOF currently!"
        self._dof_properties[0].stiffness = stiffness
        self._dc.set_dof_properties(self._dof_handles[0], self._dof_properties[0])

    @property
    def damping(self):
        """
        Gets this joint's damping

        Returns:
            float: damping for this joint
        """
        # Only support revolute and prismatic joints for now
        assert self.is_single_dof, "Joint properties only supported for a single DOF currently!"
        return self._dof_properties[0].damping

    @damping.setter
    def damping(self, damping):
        """
        Sets this joint's damping

        Args:
            damping (float): damping to set
        """
        # Only support revolute and prismatic joints for now
        assert self.is_single_dof, "Joint properties only supported for a single DOF currently!"
        self._dof_properties[0].damping = damping
        self._dc.set_dof_properties(self._dof_handles[0], self._dof_properties[0])

    @property
    def friction(self):
        """
        Gets this joint's friction

        Returns:
            float: friction for this joint
        """
        return self.get_attribute("physxJoint:jointFriction")

    @friction.setter
    def friction(self, friction):
        """
        Sets this joint's friction

        Args:
            friction (float): friction to set
        """
        self.set_attribute("physxJoint:jointFriction", friction)

    @property
    def lower_limit(self):
        """
        Gets this joint's lower_limit

        Returns:
            float: lower_limit for this joint
        """
        # TODO: Add logic for non Prismatic / Revolute joints (D6, spherical)
        # Only support revolute and prismatic joints for now
        assert self.is_single_dof, "Joint properties only supported for a single DOF currently!"
        # We either return the raw value or a default value if there is no max specified
        raw_pos_lower, raw_pos_upper = self._dof_properties[0].lower, self._dof_properties[0].upper
        return -m.DEFAULT_MAX_POS \
            if raw_pos_lower is None or raw_pos_lower == raw_pos_upper or np.abs(raw_pos_lower) > m.INF_POS_THRESHOLD \
            else raw_pos_lower

    @lower_limit.setter
    def lower_limit(self, lower_limit):
        """
        Sets this joint's lower_limit

        Args:
            lower_limit (float): lower_limit to set
        """
        # Can't use DC because it's read only property
        lower_limit = T.rad2deg(lower_limit) if self.is_revolute else lower_limit
        self.set_attribute("physics:lowerLimit", lower_limit)

    @property
    def upper_limit(self):
        """
        Gets this joint's upper_limit

        Returns:
            float: upper_limit for this joint
        """
        # Only support revolute and prismatic joints for now
        assert self.is_single_dof, "Joint properties only supported for a single DOF currently!"
        # We either return the raw value or a default value if there is no max specified
        raw_pos_lower, raw_pos_upper = self._dof_properties[0].lower, self._dof_properties[0].upper
        return m.DEFAULT_MAX_POS \
            if raw_pos_upper is None or raw_pos_lower == raw_pos_upper or np.abs(raw_pos_upper) > m.INF_POS_THRESHOLD \
            else raw_pos_upper

    @upper_limit.setter
    def upper_limit(self, upper_limit):
        """
        Sets this joint's upper_limit

        Args:
            upper_limit (float): upper_limit to set
        """
        # Can't use DC because it's read only property
        upper_limit = T.rad2deg(upper_limit) if self.is_revolute else upper_limit
        self.set_attribute("physics:upperLimit", upper_limit)

    @property
    def has_limit(self):
        """
        Returns:
            bool: True if this joint has a limit, else False
        """
        # Only support revolute and prismatic joints for now
        assert self.is_single_dof, "Joint properties only supported for a single DOF currently!"
        # We either return the raw value or a default value if there is no max specified
        return self._dof_properties[0].has_limits

    @property
    def n_dof(self):
        """
        Returns:
            int: Number of degrees of freedom this joint has
        """
        return self._n_dof

    @property
    def articulated(self):
        """
        Returns:
             bool: Whether this joint is articulated or not
        """
        return self._art is not None

    @property
    def is_revolute(self):
        """
        Returns:
            bool: Whether this joint is revolute or  not
        """
        return self._joint_type == "RevoluteJoint"

    @property
    def is_single_dof(self):
        """
        Returns:
            bool: Whether this joint has a single DOF or not
        """
        return self._joint_type in {"RevoluteJoint", "PrismaticJoint"}

    def assert_articulated(self):
        """
        Sanity check to make sure this joint is articulated. Used as a gatekeeping function to prevent non-intended
        behavior (e.g.: trying to grab this joint's state if it's not articulated)
        """
        assert self.articulated, "Tried to call method not intended for non-articulated joint!"

    def get_state(self, normalized=False):
        """
        (pos, vel, effort) state of this joint

        Args:
            normalized (bool): If True, will return normalized state of this joint, where pos, vel, and effort values
                are in range [-1, 1].

        Returns:
            3-tuple:
                - n-array: position of this joint, where n = number of DOF for this joint
                - n-array: velocity of this joint, where n = number of DOF for this joint
                - n-array: effort of this joint, where n = number of DOF for this joint
        """
        # Make sure we only call this if we're an articulated joint
        self.assert_articulated()

        # Grab raw states
        pos, vel, effort = np.zeros(self.n_dof), np.zeros(self.n_dof), np.zeros(self.n_dof)
        for i, dof_handle in enumerate(self._dof_handles):
            dof_state = self._dc.get_dof_state(dof_handle, _dynamic_control.STATE_ALL)
            pos[i] = dof_state.pos
            vel[i] = dof_state.vel
            effort[i] = dof_state.effort

        # Potentially normalize if requested
        if normalized:
            pos, vel, effort = self._normalize_pos(pos), self._normalize_vel(vel), self._normalize_effort(effort)

        return pos, vel, effort

    def get_target(self, normalized=False):
        """
        (pos, vel) target of this joint

        Args:
            normalized (bool): If True, will return normalized target of this joint

        Returns:
            2-tuple:
                - n-array: target position of this joint, where n = number of DOF for this joint
                - n-array: target velocity of this joint, where n = number of DOF for this joint
        """
        # Make sure we only call this if we're an articulated joint
        self.assert_articulated()

        # Grab raw states
        pos, vel = np.zeros(self.n_dof), np.zeros(self.n_dof)
        for i, dof_handle in enumerate(self._dof_handles):
            pos[i] = self._dc.get_dof_position_target(dof_handle)
            vel[i] = self._dc.get_dof_velocity_target(dof_handle)

        # Potentially normalize if requested
        if normalized:
            pos, vel = self._normalize_pos(pos), self._normalize_vel(vel)

        return pos, vel

    def _normalize_pos(self, pos):
        """
        Normalizes raw joint positions @pos

        Args:
            pos (n-array): n-DOF raw positions to normalize

        Returns:
            n-array: n-DOF normalized positions in range [-1, 1]
        """
        low, high = self.lower_limit, self.upper_limit
        mean = (low + high) / 2.0
        magnitude = (high - low) / 2.0
        pos = (pos - mean) / magnitude

        return pos

    def _denormalize_pos(self, pos):
        """
        De-normalizes joint positions @pos

        Args:
            pos (n-array): n-DOF normalized positions in range [-1, 1]

        Returns:
            n-array: n-DOF de-normalized positions
        """
        low, high = self.lower_limit, self.upper_limit
        mean = (low + high) / 2.0
        magnitude = (high - low) / 2.0
        pos = pos * magnitude + mean

        return pos

    def _normalize_vel(self, vel):
        """
        Normalizes raw joint velocities @vel

        Args:
            vel (n-array): n-DOF raw velocities to normalize

        Returns:
            n-array: n-DOF normalized velocities in range [-1, 1]
        """
        return vel / self.max_velocity

    def _denormalize_vel(self, vel):
        """
        De-normalizes joint velocities @vel

        Args:
            vel (n-array): n-DOF normalized velocities in range [-1, 1]

        Returns:
            n-array: n-DOF de-normalized velocities
        """
        return vel * self.max_velocity

    def _normalize_effort(self, effort):
        """
        Normalizes raw joint effort @effort

        Args:
            effort (n-array): n-DOF raw effort to normalize

        Returns:
            n-array: n-DOF normalized effort in range [-1, 1]
        """
        return effort / self.max_force

    def _denormalize_effort(self, effort):
        """
        De-normalizes joint effort @effort

        Args:
            effort (n-array): n-DOF normalized effort in range [-1, 1]

        Returns:
            n-array: n-DOF de-normalized effort
        """
        return effort * self.max_force

    def set_pos(self, pos, normalized=False, target=False):
        """
        Set the position of this joint in metric space

        Args:
            pos (float or n-array of float): Set the position(s) for this joint. Can be a single float or 1-array of
                float if the joint only has a single DOF, otherwise it should be an n-array of floats.
            normalized (bool): Whether the input is normalized to [-1, 1] (in this case, the values will be
                de-normalized first before being executed). Default is False
            target (bool): Whether the position being set is a target value or manual value to immediately set. Default
                is False, corresponding to an instantaneous setting of the position
        """
        # Sanity checks -- make sure we're the correct control type if we're setting a target and that we're articulated
        self.assert_articulated()
        if target:
            assert self._control_type == ControlType.POSITION, \
                "Trying to set joint position target, but control type is not position!"

        # Standardize input
        pos = np.array([pos]) if self._n_dof == 1 and not isinstance(pos, Iterable) else np.array(pos)

        # Potentially de-normalize if the input is normalized
        if normalized:
            pos = self._denormalize_pos(pos)

        # Set the DOF(s) in this joint
        for dof_handle, p in zip(self._dof_handles, pos):
            if not target:
                self._dc.set_dof_position(dof_handle, p)
            # We set the position in either case
            self._dc.set_dof_position_target(dof_handle, p)

    def set_vel(self, vel, normalized=False, target=False):
        """
        Set the velocity of this joint in metric space

        Args:
            vel (float or n-array of float): Set the velocity(s) for this joint. Can be a single float or 1-array of
                float if the joint only has a single DOF, otherwise it should be an n-array of floats.
            normalized (bool): Whether the input is normalized to [-1, 1] (in this case, the values will be
                de-normalized first before being executed). Default is False
            target (bool): Whether the velocity being set is a target value or manual value to immediately set. Default
                is False, corresponding to an instantaneous setting of the velocity
        """
        # Sanity checks -- make sure we're the correct control type if we're setting a target and that we're articulated
        self.assert_articulated()
        if target:
            assert self._control_type == ControlType.VELOCITY, \
                f"Trying to set joint velocity target for joint {self.name}, but control type is not velocity!"

        # Standardize input
        vel = np.array([vel]) if self._n_dof == 1 and not isinstance(vel, Iterable) else np.array(vel)

        # Potentially de-normalize if the input is normalized
        if normalized:
            vel = self._denormalize_vel(vel)

        # Set the DOF(s) in this joint
        for dof_handle, v in zip(self._dof_handles, vel):
            if not target:
                self._dc.set_dof_velocity(dof_handle, v)
            # We set the target in either case
            self._dc.set_dof_velocity_target(dof_handle, v)

    def set_effort(self, effort, normalized=False):
        """
        Set the effort of this joint in metric space

        Args:
            effort (float or n-array of float): Set the effort(s) for this joint. Can be a single float or 1-array of
                float if the joint only has a single DOF, otherwise it should be an n-array of floats.
            normalized (bool): Whether the input is normalized to [-1, 1] (in this case, the values will be
                de-normalized first before being executed). Default is False
        """
        # Sanity checks -- make sure that we're articulated (no control type check like position and velocity
        # because we can't set effort targets)
        self.assert_articulated()

        # Standardize input
        effort = np.array([effort]) if self._n_dof == 1 and not isinstance(effort, Iterable) else np.array(effort)

        # Potentially de-normalize if the input is normalized
        if normalized:
            effort = self._denormalize_effort(effort)

        # Set the DOF(s) in this joint
        for dof_handle, e in zip(self._dof_handles, effort):
            self._dc.set_dof_effort(dof_handle, e)

    def keep_still(self):
        """
        Zero out all velocities for this prim
        """
        self.set_vel(np.zeros(self.n_dof))

    def _dump_state(self):
        pos, vel, effort = self.get_state() if self.articulated else (np.array([]), np.array([]), np.array([]))
        target_pos, target_vel = self.get_target() if self.articulated else (np.array([]), np.array([]))
        return OrderedDict(
            pos=pos,
            vel=vel,
            effort=effort,
            target_pos=target_pos,
            target_vel=target_vel,
        )

    def _load_state(self, state):
        if self.articulated:
            self.set_pos(state["pos"], target=False)
            self.set_vel(state["vel"], target=False)
            self.set_effort(state["effort"])
            if self._control_type == ControlType.POSITION:
                self.set_pos(state["target_pos"], target=True)
            elif self._control_type == ControlType.VELOCITY:
                self.set_vel(state["target_vel"], target=True)

    def _serialize(self, state):
        # We serialize by iterating over the keys and adding them to a list that's concatenated at the end
        # This is a deterministic mapping because we assume the state is an OrderedDict
        return np.concatenate(list(state.values())).astype(float)

    def _deserialize(self, state):
        # We deserialize deterministically by knowing the order of values -- pos, vel, effort
        return OrderedDict(
            pos=state[0:self.n_dof],
            vel=state[self.n_dof:2*self.n_dof],
            effort=state[2*self.n_dof:3*self.n_dof],
            target_pos=state[3*self.n_dof:4*self.n_dof],
            target_vel=state[4*self.n_dof:5*self.n_dof],
        ), 5*self.n_dof

    def duplicate(self, simulator, prim_path):
        # Cannot directly duplicate a joint prim
        raise NotImplementedError("Cannot directly duplicate a joint prim!")

articulated property

Returns:

Name Type Description
bool

Whether this joint is articulated or not

body0 property writable

Gets this joint's body0 relationship.

Returns:

Type Description

None or str: Absolute prim path to the body prim to set as this joint's parent link, or None if there is no body0 specified.

body1 property writable

Gets this joint's body1 relationship.

Returns:

Type Description

None or str: Absolute prim path to the body prim to set as this joint's child link, or None if there is no body1 specified.

child_name property

Gets this joint's child body name, if it exists

Returns:

Name Type Description
str

Joint's child body name

control_type property

Gets the control types for this joint

Returns:

Name Type Description
ControlType

control type for this joint

damping property writable

Gets this joint's damping

Returns:

Name Type Description
float

damping for this joint

dof_properties property

Returns:

Type Description

list of DOFProperties: Per-DOF properties for this joint. See https://docs.omniverse.nvidia.com/py/isaacsim/source/extensions/omni.isaac.dynamic_control/docs/index.html#omni.isaac.dynamic_control._dynamic_control.DofProperties for more information.

friction property writable

Gets this joint's friction

Returns:

Name Type Description
float

friction for this joint

has_limit property

Returns:

Name Type Description
bool

True if this joint has a limit, else False

is_revolute property

Returns:

Name Type Description
bool

Whether this joint is revolute or not

is_single_dof property

Returns:

Name Type Description
bool

Whether this joint has a single DOF or not

joint_name property

Returns:

Name Type Description
str

Name of this joint

joint_type property

Gets this joint's type (ignoring the "Physics" prefix)

Returns:

Name Type Description
str

Joint's type. Should be one of: {"FixedJoint", "Joint", "PrismaticJoint", "RevoluteJoint", "SphericalJoint"} (equivalently, one of JointType)

local_orientation property

Returns:

Type Description

4-array: (x,y,z,w) local quaternion orientation of this joint, relative to the parent link

lower_limit property writable

Gets this joint's lower_limit

Returns:

Name Type Description
float

lower_limit for this joint

max_force property writable

Gets this joint's maximum force

Returns:

Name Type Description
float

maximum force for this joint

max_velocity property writable

Gets this joint's maximum velocity

Returns:

Name Type Description
float

maximum velocity for this joint

n_dof property

Returns:

Name Type Description
int

Number of degrees of freedom this joint has

parent_name property

Gets this joint's parent body name, if it exists

Returns:

Name Type Description
str

Joint's parent body name

stiffness property writable

Gets this joint's stiffness

Returns:

Name Type Description
float

stiffness for this joint

upper_limit property writable

Gets this joint's upper_limit

Returns:

Name Type Description
float

upper_limit for this joint

assert_articulated()

Sanity check to make sure this joint is articulated. Used as a gatekeeping function to prevent non-intended behavior (e.g.: trying to grab this joint's state if it's not articulated)

Source code in prims/joint_prim.py
def assert_articulated(self):
    """
    Sanity check to make sure this joint is articulated. Used as a gatekeeping function to prevent non-intended
    behavior (e.g.: trying to grab this joint's state if it's not articulated)
    """
    assert self.articulated, "Tried to call method not intended for non-articulated joint!"

get_state(normalized=False)

(pos, vel, effort) state of this joint

Parameters:

Name Type Description Default
normalized bool

If True, will return normalized state of this joint, where pos, vel, and effort values are in range [-1, 1].

False

Returns:

Type Description

3-tuple: - n-array: position of this joint, where n = number of DOF for this joint - n-array: velocity of this joint, where n = number of DOF for this joint - n-array: effort of this joint, where n = number of DOF for this joint

Source code in prims/joint_prim.py
def get_state(self, normalized=False):
    """
    (pos, vel, effort) state of this joint

    Args:
        normalized (bool): If True, will return normalized state of this joint, where pos, vel, and effort values
            are in range [-1, 1].

    Returns:
        3-tuple:
            - n-array: position of this joint, where n = number of DOF for this joint
            - n-array: velocity of this joint, where n = number of DOF for this joint
            - n-array: effort of this joint, where n = number of DOF for this joint
    """
    # Make sure we only call this if we're an articulated joint
    self.assert_articulated()

    # Grab raw states
    pos, vel, effort = np.zeros(self.n_dof), np.zeros(self.n_dof), np.zeros(self.n_dof)
    for i, dof_handle in enumerate(self._dof_handles):
        dof_state = self._dc.get_dof_state(dof_handle, _dynamic_control.STATE_ALL)
        pos[i] = dof_state.pos
        vel[i] = dof_state.vel
        effort[i] = dof_state.effort

    # Potentially normalize if requested
    if normalized:
        pos, vel, effort = self._normalize_pos(pos), self._normalize_vel(vel), self._normalize_effort(effort)

    return pos, vel, effort

get_target(normalized=False)

(pos, vel) target of this joint

Parameters:

Name Type Description Default
normalized bool

If True, will return normalized target of this joint

False

Returns:

Type Description

2-tuple: - n-array: target position of this joint, where n = number of DOF for this joint - n-array: target velocity of this joint, where n = number of DOF for this joint

Source code in prims/joint_prim.py
def get_target(self, normalized=False):
    """
    (pos, vel) target of this joint

    Args:
        normalized (bool): If True, will return normalized target of this joint

    Returns:
        2-tuple:
            - n-array: target position of this joint, where n = number of DOF for this joint
            - n-array: target velocity of this joint, where n = number of DOF for this joint
    """
    # Make sure we only call this if we're an articulated joint
    self.assert_articulated()

    # Grab raw states
    pos, vel = np.zeros(self.n_dof), np.zeros(self.n_dof)
    for i, dof_handle in enumerate(self._dof_handles):
        pos[i] = self._dc.get_dof_position_target(dof_handle)
        vel[i] = self._dc.get_dof_velocity_target(dof_handle)

    # Potentially normalize if requested
    if normalized:
        pos, vel = self._normalize_pos(pos), self._normalize_vel(vel)

    return pos, vel

keep_still()

Zero out all velocities for this prim

Source code in prims/joint_prim.py
def keep_still(self):
    """
    Zero out all velocities for this prim
    """
    self.set_vel(np.zeros(self.n_dof))

set_control_type(control_type, kp=None, kd=None)

Sets the control type for this joint.

Parameters:

Name Type Description Default
control_type ControlType

What type of control to use for this joint. Valid options are: {ControlType.POSITION, ControlType.VELOCITY, ControlType.EFFORT}

required
kp None or float

If specified, sets the kp gain value for this joint. Should only be set if setting ControlType.POSITION

None
kd None or float

If specified, sets the kd gain value for this joint. Should only be set if setting ControlType.VELOCITY

None
Source code in prims/joint_prim.py
def set_control_type(self, control_type, kp=None, kd=None):
    """
    Sets the control type for this joint.

    Args:
        control_type (ControlType): What type of control to use for this joint.
            Valid options are: {ControlType.POSITION, ControlType.VELOCITY, ControlType.EFFORT}
        kp (None or float): If specified, sets the kp gain value for this joint. Should only be set if
            setting ControlType.POSITION
        kd (None or float): If specified, sets the kd gain value for this joint. Should only be set if
            setting ControlType.VELOCITY
    """
    # Sanity check inputs
    assert_valid_key(key=control_type, valid_keys=ControlType.VALID_TYPES, name="control type")
    if control_type == ControlType.POSITION:
        assert kp is not None, "kp gain must be specified for setting POSITION control!"
        assert kd is None, "kd gain must not be specified for setting POSITION control!"
        kd = 0.0
    elif control_type == ControlType.VELOCITY:
        assert kp is None, "kp gain must not be specified for setting VELOCITY control!"
        assert kd is not None, "kd gain must be specified for setting VELOCITY control!"
        kp = 0.0
    else:   # Efforts
        assert kp is None, "kp gain must not be specified for setting EFFORT control!"
        assert kd is None, "kd gain must not be specified for setting EFFORT control!"
        kp, kd = 0.0, 0.0

    # Set values
    if self._dc:
        for dof_handle, dof_property in zip(self._dof_handles, self._dof_properties):
            dof_property.stiffness = kp
            dof_property.damping = kd
            self._dc.set_dof_properties(dof_handle, dof_property)

    # Update control type
    self._control_type = control_type

set_effort(effort, normalized=False)

Set the effort of this joint in metric space

Parameters:

Name Type Description Default
effort float or n-array of float

Set the effort(s) for this joint. Can be a single float or 1-array of float if the joint only has a single DOF, otherwise it should be an n-array of floats.

required
normalized bool

Whether the input is normalized to [-1, 1] (in this case, the values will be de-normalized first before being executed). Default is False

False
Source code in prims/joint_prim.py
def set_effort(self, effort, normalized=False):
    """
    Set the effort of this joint in metric space

    Args:
        effort (float or n-array of float): Set the effort(s) for this joint. Can be a single float or 1-array of
            float if the joint only has a single DOF, otherwise it should be an n-array of floats.
        normalized (bool): Whether the input is normalized to [-1, 1] (in this case, the values will be
            de-normalized first before being executed). Default is False
    """
    # Sanity checks -- make sure that we're articulated (no control type check like position and velocity
    # because we can't set effort targets)
    self.assert_articulated()

    # Standardize input
    effort = np.array([effort]) if self._n_dof == 1 and not isinstance(effort, Iterable) else np.array(effort)

    # Potentially de-normalize if the input is normalized
    if normalized:
        effort = self._denormalize_effort(effort)

    # Set the DOF(s) in this joint
    for dof_handle, e in zip(self._dof_handles, effort):
        self._dc.set_dof_effort(dof_handle, e)

set_pos(pos, normalized=False, target=False)

Set the position of this joint in metric space

Parameters:

Name Type Description Default
pos float or n-array of float

Set the position(s) for this joint. Can be a single float or 1-array of float if the joint only has a single DOF, otherwise it should be an n-array of floats.

required
normalized bool

Whether the input is normalized to [-1, 1] (in this case, the values will be de-normalized first before being executed). Default is False

False
target bool

Whether the position being set is a target value or manual value to immediately set. Default is False, corresponding to an instantaneous setting of the position

False
Source code in prims/joint_prim.py
def set_pos(self, pos, normalized=False, target=False):
    """
    Set the position of this joint in metric space

    Args:
        pos (float or n-array of float): Set the position(s) for this joint. Can be a single float or 1-array of
            float if the joint only has a single DOF, otherwise it should be an n-array of floats.
        normalized (bool): Whether the input is normalized to [-1, 1] (in this case, the values will be
            de-normalized first before being executed). Default is False
        target (bool): Whether the position being set is a target value or manual value to immediately set. Default
            is False, corresponding to an instantaneous setting of the position
    """
    # Sanity checks -- make sure we're the correct control type if we're setting a target and that we're articulated
    self.assert_articulated()
    if target:
        assert self._control_type == ControlType.POSITION, \
            "Trying to set joint position target, but control type is not position!"

    # Standardize input
    pos = np.array([pos]) if self._n_dof == 1 and not isinstance(pos, Iterable) else np.array(pos)

    # Potentially de-normalize if the input is normalized
    if normalized:
        pos = self._denormalize_pos(pos)

    # Set the DOF(s) in this joint
    for dof_handle, p in zip(self._dof_handles, pos):
        if not target:
            self._dc.set_dof_position(dof_handle, p)
        # We set the position in either case
        self._dc.set_dof_position_target(dof_handle, p)

set_vel(vel, normalized=False, target=False)

Set the velocity of this joint in metric space

Parameters:

Name Type Description Default
vel float or n-array of float

Set the velocity(s) for this joint. Can be a single float or 1-array of float if the joint only has a single DOF, otherwise it should be an n-array of floats.

required
normalized bool

Whether the input is normalized to [-1, 1] (in this case, the values will be de-normalized first before being executed). Default is False

False
target bool

Whether the velocity being set is a target value or manual value to immediately set. Default is False, corresponding to an instantaneous setting of the velocity

False
Source code in prims/joint_prim.py
def set_vel(self, vel, normalized=False, target=False):
    """
    Set the velocity of this joint in metric space

    Args:
        vel (float or n-array of float): Set the velocity(s) for this joint. Can be a single float or 1-array of
            float if the joint only has a single DOF, otherwise it should be an n-array of floats.
        normalized (bool): Whether the input is normalized to [-1, 1] (in this case, the values will be
            de-normalized first before being executed). Default is False
        target (bool): Whether the velocity being set is a target value or manual value to immediately set. Default
            is False, corresponding to an instantaneous setting of the velocity
    """
    # Sanity checks -- make sure we're the correct control type if we're setting a target and that we're articulated
    self.assert_articulated()
    if target:
        assert self._control_type == ControlType.VELOCITY, \
            f"Trying to set joint velocity target for joint {self.name}, but control type is not velocity!"

    # Standardize input
    vel = np.array([vel]) if self._n_dof == 1 and not isinstance(vel, Iterable) else np.array(vel)

    # Potentially de-normalize if the input is normalized
    if normalized:
        vel = self._denormalize_vel(vel)

    # Set the DOF(s) in this joint
    for dof_handle, v in zip(self._dof_handles, vel):
        if not target:
            self._dc.set_dof_velocity(dof_handle, v)
        # We set the target in either case
        self._dc.set_dof_velocity_target(dof_handle, v)

update_handles()

Updates all internal handles for this prim, in case they change since initialization

Source code in prims/joint_prim.py
def update_handles(self):
    """
    Updates all internal handles for this prim, in case they change since initialization
    """
    # TODO: A bit hacky way to get the joint handle, ideally we'd simply do dc.get_joint(), but this doesn't seem to work as expected?
    self._handle = None
    for i in range(self._dc.get_articulation_joint_count(self._art)):
        joint_handle = self._dc.get_articulation_joint(self._art, i)
        joint_path = self._dc.get_joint_path(joint_handle)
        if joint_path == self._prim_path:
            self._handle = joint_handle
            break