@@ -656,7 +656,14 @@ <h1>Source code for roboticstoolbox.robot.IK</h1><div class="highlight"><pre>
656
656
< span class ="n "> tcount</ span > < span class ="o "> =</ span > < span class ="mi "> 0</ span > < span class ="c1 "> # Total iteration count</ span >
657
657
658
658
< span class ="c1 "> # bool vector indicating revolute joints</ span >
659
- < span class ="n "> revolutes</ span > < span class ="o "> =</ span > < span class ="n "> np</ span > < span class ="o "> .</ span > < span class ="n "> array</ span > < span class ="p "> ([</ span > < span class ="n "> link</ span > < span class ="o "> .</ span > < span class ="n "> isrevolute</ span > < span class ="k "> for</ span > < span class ="n "> link</ span > < span class ="ow "> in</ span > < span class ="bp "> self</ span > < span class ="p "> ])</ span >
659
+ < span class ="c1 "> # joints = np.array([link.isjoint for link in self])</ span >
660
+ < span class ="c1 "> # revolutes = np.array([link.isrevolute for link in self])</ span >
661
+ < span class ="n "> revolutes</ span > < span class ="o "> =</ span > < span class ="p "> []</ span >
662
+
663
+ < span class ="k "> for</ span > < span class ="n "> link</ span > < span class ="ow "> in</ span > < span class ="bp "> self</ span > < span class ="p "> :</ span >
664
+ < span class ="k "> if</ span > < span class ="n "> link</ span > < span class ="o "> .</ span > < span class ="n "> isjoint</ span > < span class ="p "> :</ span >
665
+ < span class ="n "> revolutes</ span > < span class ="o "> .</ span > < span class ="n "> append</ span > < span class ="p "> (</ span > < span class ="n "> link</ span > < span class ="o "> .</ span > < span class ="n "> isrevolute</ span > < span class ="p "> )</ span >
666
+ < span class ="n "> revolutes</ span > < span class ="o "> =</ span > < span class ="n "> np</ span > < span class ="o "> .</ span > < span class ="n "> array</ span > < span class ="p "> (</ span > < span class ="n "> revolutes</ span > < span class ="p "> )</ span >
660
667
661
668
< span class ="n "> q</ span > < span class ="o "> =</ span > < span class ="n "> q0</ span >
662
669
< span class ="k "> for</ span > < span class ="n "> Tk</ span > < span class ="ow "> in</ span > < span class ="n "> T</ span > < span class ="p "> :</ span >
@@ -692,6 +699,8 @@ <h1>Source code for roboticstoolbox.robot.IK</h1><div class="highlight"><pre>
692
699
< span class ="c1 "> # print(J)</ span >
693
700
694
701
< span class ="c1 "> # Wrap angles for revolute joints</ span >
702
+ < span class ="nb "> print</ span > < span class ="p "> (</ span > < span class ="n "> revolutes</ span > < span class ="p "> )</ span >
703
+ < span class ="nb "> print</ span > < span class ="p "> (</ span > < span class ="n "> q</ span > < span class ="p "> )</ span >
695
704
< span class ="n "> k</ span > < span class ="o "> =</ span > < span class ="n "> np</ span > < span class ="o "> .</ span > < span class ="n "> logical_and</ span > < span class ="p "> (</ span > < span class ="n "> q</ span > < span class ="o "> ></ span > < span class ="n "> np</ span > < span class ="o "> .</ span > < span class ="n "> pi</ span > < span class ="p "> ,</ span > < span class ="n "> revolutes</ span > < span class ="p "> )</ span >
696
705
< span class ="n "> q</ span > < span class ="p "> [</ span > < span class ="n "> k</ span > < span class ="p "> ]</ span > < span class ="o "> -=</ span > < span class ="mi "> 2</ span > < span class ="o "> *</ span > < span class ="n "> np</ span > < span class ="o "> .</ span > < span class ="n "> pi</ span >
697
706
0 commit comments