progress on any axis
@@ -1692,7 +1692,40 @@ class URDF(URDFType):
# TODO, find where reverse is used and change
# it to [::-1] or do that here
rot = joint.rpy
+
+ # Check if axis of rotation/tanslation is not 1
+ # if np.sum(np.abs(np.array(joint.axis))) == 1 and 1 in np.abs(
+ # np.array(joint.axis)
+ # ):
+ # print("not")
childlink._ets = rtb.ETS.SE3(trans, rot)
+ # else:
+
+ # phi = np.arctan2(
+ # np.sqrt(joint.axis[0] * joint.axis[0] + joint.axis[1] * joint.axis[1]),
+ # joint.axis[2],
+ # )
+ # print(phi)
+ # theta = np.arctan2(joint.axis[1], -joint.axis[0])
+ # print(theta)
+
+ # joint.axis = [1, 0, 0]
+
+ # new_axis = [0, 0, 1]
+
+ # v = np.cross(joint.axis, new_axis)
+ # theta = np.arccos(np.dot(joint.axis, new_axis))
+ # new_rot = sm.SO3.AngVec(theta, v)
+
+ # print(joint.axis)
+ # print(new_rot.rpy())
+
+ # childlink._ets = rtb.ETS.SE3(trans, rot) * rtb.ETS.SE3(
+ # [0, 0, 0], new_rot.rpy()
+ # )
+
+ # joint.axis = [0, 0, 1]
+
childlink._init_Ts()
# variable part of link transform
