Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- Code:
- ```python
- theta5 = atan2(norm(R3_6[0,2], R3_6[2,2]), R3_6[1,2])
- # prevent wrist flaps by using consistently one of the multiple solutions
- if sin(theta5) < 0.:
- theta4 = atan2(-R3_6[2,2], R3_6[0,2])
- theta6 = atan2(R3_6[1,1], -R3_6[1,0])
- rospy.loginfo("Negative branch, theta4=%04.8f, theta5= %04.8f, theta6=%04.8f" % (theta4, theta5, theta6))
- else:
- theta4 = atan2(R3_6[2,2], -R3_6[0,2])
- theta6 = atan2(-R3_6[1,1], R3_6[1,0])
- rospy.loginfo("Positive branch, theta4=%04.8f, theta5= %04.8f, theta6=%04.8f" % (theta4, theta5, theta6))
- ```
- Result:
- ```bash
- [INFO] [1515296620.711290]: Received 10 eef-poses from the plan
- [INFO] [1515296622.091448]: Positive branch, theta4=0.00000000, theta5= 1.57079633, theta6=0.00000015
- [INFO] [1515296622.336308]: Positive branch, theta4=3.04619374, theta5= 0.07653129, theta6=2.48070150
- [INFO] [1515296622.814664]: Positive branch, theta4=2.88591735, theta5= 0.11432489, theta6=-2.89691175
- [INFO] [1515296623.151579]: Positive branch, theta4=2.76165392, theta5= 0.17294709, theta6=-2.77831331
- [INFO] [1515296623.537406]: Positive branch, theta4=3.10917248, theta5= 1.39209392, theta6=1.59041056
- [INFO] [1515296624.011607]: Positive branch, theta4=2.04795072, theta5= 0.19098979, theta6=2.76478810
- [INFO] [1515296624.227802]: Positive branch, theta4=1.64354934, theta5= 0.23452094, theta6=2.83982764
- [INFO] [1515296624.571575]: Positive branch, theta4=2.26014052, theta5= 0.40430168, theta6=-2.30169624
- [INFO] [1515296624.873025]: Positive branch, theta4=2.13303477, theta5= 0.46109383, theta6=-2.18133842
- [INFO] [1515296625.194482]: Positive branch, theta4=0.09899684, theta5= 1.15552980, theta6=-1.68595180
- [INFO] [1515296625.197542]: length of Joint Trajectory List: 10
- [INFO] [1515296819.804877]: Received 34 eef-poses from the plan
- [INFO] [1515296820.493005]: Positive branch, theta4=0.09940830, theta5= 1.15546806, theta6=-3.13828782
- [INFO] [1515296820.810797]: Positive branch, theta4=3.02433816, theta5= 1.57012099, theta6=-3.13923899
- [INFO] [1515296821.064625]: Positive branch, theta4=0.90623955, theta5= 0.51392552, theta6=-3.13058199
- [INFO] [1515296821.354005]: Positive branch, theta4=1.87483665, theta5= 0.46755769, theta6=-1.93742377
- [INFO] [1515296821.661567]: Positive branch, theta4=1.77785699, theta5= 0.42915022, theta6=-1.84564470
- [INFO] [1515296822.011201]: Positive branch, theta4=1.51646121, theta5= 0.32776982, theta6=-1.59796114
- [INFO] [1515296822.320521]: Positive branch, theta4=0.67555900, theta5= 0.32062229, theta6=0.88177037
- [INFO] [1515296822.664673]: Positive branch, theta4=1.06471786, theta5= 0.16322217, theta6=-1.16900148
- [INFO] [1515296822.966211]: Positive branch, theta4=0.02606275, theta5= 0.97514972, theta6=0.59660692
- [INFO] [1515296823.282765]: Positive branch, theta4=-2.25885420, theta5= 0.03338622, theta6=2.12703898
- [INFO] [1515296823.624056]: Positive branch, theta4=-2.67713859, theta5= 0.12642208, theta6=2.53310800
- [INFO] [1515296824.029298]: Positive branch, theta4=-0.01104541, theta5= 0.95565049, theta6=0.98480602
- [INFO] [1515296824.276248]: Positive branch, theta4=-3.04277505, theta5= 0.09457323, theta6=2.67425932
- [INFO] [1515296824.535916]: Positive branch, theta4=2.80369387, theta5= 0.15178119, theta6=3.01032135
- [INFO] [1515296824.805015]: Positive branch, theta4=2.68716364, theta5= 0.19375563, theta6=-3.12142687
- [INFO] [1515296825.165374]: Positive branch, theta4=2.84980073, theta5= 0.46551786, theta6=-3.03571392
- [INFO] [1515296825.416462]: Positive branch, theta4=2.81793174, theta5= 0.53198907, theta6=-3.00240362
- [INFO] [1515296825.662737]: Positive branch, theta4=3.07823853, theta5= 1.75247140, theta6=-1.76330273
- [INFO] [1515296825.999230]: Positive branch, theta4=3.07572899, theta5= 1.84768804, theta6=-1.77098064
- [INFO] [1515296826.395250]: Positive branch, theta4=2.68611674, theta5= 0.88755618, theta6=-2.86039418
- [INFO] [1515296826.730216]: Positive branch, theta4=2.70010727, theta5= 1.05505045, theta6=-2.86151165
- [INFO] [1515296827.109111]: Positive branch, theta4=2.94943277, theta5= 1.70691599, theta6=2.95304055
- [INFO] [1515296827.398127]: Positive branch, theta4=2.81966476, theta5= 1.22071125, theta6=2.63089577
- [INFO] [1515296827.821995]: Positive branch, theta4=3.08710850, theta5= 1.75758232, theta6=2.97161351
- [INFO] [1515296828.162249]: Positive branch, theta4=-2.83732393, theta5= 1.11502729, theta6=2.59243100
- [INFO] [1515296828.456853]: Positive branch, theta4=-2.77730073, theta5= 1.71373069, theta6=2.77510751
- [INFO] [1515296828.717037]: Positive branch, theta4=-2.53229213, theta5= 1.72747883, theta6=2.56041837
- [INFO] [1515296829.029624]: Positive branch, theta4=-2.32159317, theta5= 1.74613129, theta6=2.32697032
- [INFO] [1515296829.455979]: Positive branch, theta4=-0.00651232, theta5= 1.36596995, theta6=-2.87513722
- [INFO] [1515296829.893171]: Positive branch, theta4=-1.06147598, theta5= 1.62170686, theta6=-2.83781771
- [INFO] [1515296830.234233]: Positive branch, theta4=-1.78805480, theta5= 1.76541313, theta6=1.87712627
- [INFO] [1515296830.538411]: Positive branch, theta4=-1.68085903, theta5= 1.77028451, theta6=1.77554188
- [INFO] [1515296830.783437]: Positive branch, theta4=-1.61771887, theta5= 1.77087114, theta6=1.71549611
- [INFO] [1515296831.219722]: Positive branch, theta4=-1.55460159, theta5= 1.77172240, theta6=1.65544093
- [INFO] [1515296831.220440]: length of Joint Trajectory List: 34
- ```
Add Comment
Please, Sign In to add comment