new panda testing
@@ -9,6 +9,7 @@ from roboticstoolbox.robot.ELink import ELink
from roboticstoolbox.robot.etsnew import ET
import sympy
import cProfile
+import roboticstoolbox as rtb
class Panda(ERobot):
@@ -87,22 +88,46 @@ if __name__ == "__main__": # pragma nocover
# print(e1.T(0.7))
# print(e2.T())
- ets = e1 * e2
+ # ets = e1 * e2
q = np.array([1.0])
base = np.eye(4)
tool = np.eye(4)
- print(ets.fkine(q, base, tool))
- print(ets.fkine(q, None, None))
- print(e1.T(1.0) @ e2.T())
+ # print(ets.fkine(q, base, tool))
+ # print(ets.fkine(q, None, None))
+ # print(e1.T(1.0) @ e2.T())
+
+ ets = (
+ ET.tz(0.333)
+ * ET.Rz()
+ * ET.Rx(-np.pi / 2)
+ * ET.Rz()
+ * ET.ty(-0.316)
+ * ET.Rx(np.pi / 2)
+ * ET.Rz()
+ * ET.tx(0.0825)
+ * ET.Rx(np.pi / 2)
+ * ET.Rz()
+ * ET.tx(-0.0825)
+ * ET.ty(0.384)
+ * ET.Rx(-np.pi / 2)
+ * ET.Rz()
+ * ET.Rx(np.pi / 2)
+ * ET.Rz()
+ * ET.tx(0.088)
+ * ET.Rx(np.pi / 2)
+ * ET.Rz()
+ * ET.tz(0.107)
+ )
+ print(ets.fkine(q, base, tool))
# print(e1.__axis_to_number(e1.axis))
# print(e1 * e2)
# print(e1.T_OLD(x))
- # num = 1000000
+ num = 1000000
# q = np.random.rand(num)
# def step_old():
@@ -119,6 +144,22 @@ if __name__ == "__main__": # pragma nocover
# # ls = [e1.T(x) for x in q]
+ def step():
+ for i in range(num):
+ ets.fkine(q, base, tool)
+
+ cProfile.run("step()")
+
+ r = rtb.models.Panda()
+
+ q = r.qr
+
+ def step_old():
+ for i in range(num):
+ r.jacob0(q, fast=True)
+
+ cProfile.run("step_old()")
+
# # cProfile.run("step_old()")
# cProfile.run("step_new()")
