diff --git a/graphik/robots/urdfs/panda_arm_truncated.urdf b/graphik/robots/urdfs/panda_arm_truncated.urdf new file mode 100644 index 0000000..86fd924 --- /dev/null +++ b/graphik/robots/urdfs/panda_arm_truncated.urdf @@ -0,0 +1,287 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/graphik/utils/dgp.py b/graphik/utils/dgp.py index 919ea81..1a4768f 100644 --- a/graphik/utils/dgp.py +++ b/graphik/utils/dgp.py @@ -229,3 +229,14 @@ def bound_smoothing(G: nx.DiGraph) -> tuple: upper_bounds[ids.index(u), ids.index(v)] = bounds[u][v] return lower_bounds, upper_bounds + +def normalize_positions(Y: ArrayLike, scale=False): + Y_c = Y - Y.mean(0) + C = Y_c.T.dot(Y_c) + e, v = np.linalg.eig(C) + Y_cr = Y_c.dot(v) + if scale: + Y_crs = Y_cr / (1 / abs(Y_cr).max()) + return Y_crs + else: + return Y_cr diff --git a/graphik/utils/roboturdf.py b/graphik/utils/roboturdf.py index 2cbc5c5..5b4a144 100644 --- a/graphik/utils/roboturdf.py +++ b/graphik/utils/roboturdf.py @@ -579,6 +579,19 @@ def load_truncated_ur10(n: int): graph = ProblemGraphRevolute(robot) return robot, graph +def load_panda(limits=None): + fname = graphik.__path__[0] + "/robots/urdfs/panda_arm.urdf" + urdf_robot = RobotURDF(fname) + n = urdf_robot.n_q_joints + if limits is None: + ub = np.ones(n) * np.pi + lb = -ub + else: + lb = limits[0] + ub = limits[1] + robot = urdf_robot.make_Revolute3d(ub, lb) # make the Revolute class from a URDF + graph = ProblemGraphRevolute(robot) + return robot, graph def load_9_dof(limits=None): """ diff --git a/setup.py b/setup.py index b8c1721..a6c9ddf 100644 --- a/setup.py +++ b/setup.py @@ -11,7 +11,7 @@ url="https://github.com/utiasSTARS/graphIK", packages=find_packages(), install_requires=[ - "numpy >= 1.16", + "numpy == 1.20", "scipy >= 1.3.0", "sympy >= 1.5", "matplotlib >= 3.1", diff --git a/tests/test_joint_variables.py b/tests/test_joint_variables.py index d66e685..528bc55 100644 --- a/tests/test_joint_variables.py +++ b/tests/test_joint_variables.py @@ -15,58 +15,67 @@ from graphik.utils.roboturdf import RobotURDF from graphik.utils import * +ROBOT_NAMES = [ + "ur10_mod", + "panda_arm", + "kuka_iiwr", + "kuka_lwr", + "ur10_mod", + "jaco2arm6DOF_no_hand", + "panda_arm_truncated", +] class TestJointVariables(unittest.TestCase): + def test_scale_invariance(self): - n = 7 - ub = (pi) * np.ones(n) - lb = -ub - # fname = graphik.__path__[0] + "/robots/urdfs/ur10_mod.urdf" - fname = graphik.__path__[0] + "/robots/urdfs/panda_arm.urdf" - # fname = graphik.__path__[0] + "/robots/urdfs/kuka_iiwr.urdf" - # fname = graphik.__path__[0] + "/robots/urdfs/kuka_lwr.urdf" - # fname = graphik.__path__[0] + "/robots/urdfs/jaco2arm6DOF_no_hand.urdf" - urdf_robot = RobotURDF(fname) - robot = urdf_robot.make_Revolute3d( - ub, lb - ) # make the Revolute class from a URDF - graph = ProblemGraphRevolute(robot) - for _ in range(100): - q_goal = robot.random_configuration() - T_goal = {} - T_goal[f"p{n}"] = robot.pose(q_goal, "p" + str(n)) - X = graph.realization(q_goal) - P = pos_from_graph(X) - q_rec = graph.joint_variables(graph_from_pos(P, node_ids=list(X)), T_goal) - self.assertIsNone( - assert_allclose(list(q_goal.values()), list(q_rec.values()), rtol=1e-5) - ) + for name in ROBOT_NAMES: + fname = graphik.__path__[0] + "/robots/urdfs/" + name + ".urdf" + urdf_robot = RobotURDF(fname) + n = urdf_robot.n_q_joints + ub = (pi) * np.ones(n) + lb = -ub + robot = urdf_robot.make_Revolute3d( + ub, lb + ) # make the Revolute class from a URDF + + graph = ProblemGraphRevolute(robot) + + for _ in range(10): + q_goal = robot.random_configuration() + T_goal = {} + T_goal[f"p{n}"] = robot.pose(q_goal, "p" + str(n)) + X = graph.realization(q_goal) + P = normalize_positions(pos_from_graph(X)) + q_rec = graph.joint_variables(graph_from_pos(P, node_ids=list(X)), T_goal) + self.assertIsNone( + assert_allclose(list(q_goal.values()), list(q_rec.values()), rtol=1e-5) + ) def test_urdf_params_3d_chain(self): - n = 7 - ub = (pi) * np.ones(n) - lb = -ub - # fname = graphik.__path__[0] + "/robots/urdfs/ur10_mod.urdf" - fname = graphik.__path__[0] + "/robots/urdfs/panda_arm.urdf" - # fname = graphik.__path__[0] + "/robots/urdfs/kuka_iiwr.urdf" - # fname = graphik.__path__[0] + "/robots/urdfs/kuka_lwr.urdf" - # fname = graphik.__path__[0] + "/robots/urdfs/jaco2arm6DOF_no_hand.urdf" - urdf_robot = RobotURDF(fname) - robot = urdf_robot.make_Revolute3d( - ub, lb - ) # make the Revolute class from a URDF - graph = ProblemGraphRevolute(robot) - for _ in range(100): - q_goal = robot.random_configuration() - T_goal = {} - T_goal[f"p{n}"] = robot.pose(q_goal, "p" + str(n)) - X = graph.realization(q_goal) - q_rec = graph.joint_variables(X, T_goal) - self.assertIsNone( - assert_allclose(list(q_goal.values()), list(q_rec.values()), rtol=1e-5) - ) + for name in ROBOT_NAMES: + fname = graphik.__path__[0] + "/robots/urdfs/" + name + ".urdf" + urdf_robot = RobotURDF(fname) + n = urdf_robot.n_q_joints + ub = (pi) * np.ones(n) + lb = -ub + robot = urdf_robot.make_Revolute3d( + ub, lb + ) # make the Revolute class from a URDF + + graph = ProblemGraphRevolute(robot) + for _ in range(10): + q_goal = robot.random_configuration() + T_goal = {} + T_goal[f"p{n}"] = robot.pose(q_goal, "p" + str(n)) + X = graph.realization(q_goal) + q_rec = graph.joint_variables(X, T_goal) + self.assertIsNone( + assert_allclose( + list(q_goal.values()), list(q_rec.values()), rtol=1e-5 + ) + ) def test_random_params_3d_chain(self): # TODO include randomized theta to FK and such