From a0ce6da9d273d1057148d787b7fd5525026518fe Mon Sep 17 00:00:00 2001 From: juancho Date: Sat, 3 Dec 2022 23:34:34 +0900 Subject: [PATCH 1/3] [standford_manipulator_modeling.m] Updated the example to ensure compatibility with the on going dqrobotics-matlab PR. --- manipulator/stanford_manipulator_modeling.m | 74 ++++++++++----------- 1 file changed, 36 insertions(+), 38 deletions(-) diff --git a/manipulator/stanford_manipulator_modeling.m b/manipulator/stanford_manipulator_modeling.m index 41ccf3d..2d40163 100644 --- a/manipulator/stanford_manipulator_modeling.m +++ b/manipulator/stanford_manipulator_modeling.m @@ -73,38 +73,39 @@ function stanford_manipulator_modeling() % Testing some methods TestCase = matlab.unittest.TestCase.forInteractiveUse; - - TestCase.assertEqual(StanfordDHRobot.get_thetas(),robot_DH_theta, "AbsTol", DQ.threshold,... - "Error in DQ_SerialManipulatorDH.get_thetas()"); - - TestCase.assertEqual(StanfordDHRobot.get_ds(),robot_DH_d, "AbsTol", DQ.threshold,... - "Error in DQ_SerialManipulatorDH.get_ds()"); - - TestCase.assertEqual(StanfordDHRobot.get_as(),robot_DH_a, "AbsTol", DQ.threshold,... - "Error in DQ_SerialManipulatorDH.get_as()"); - - TestCase.assertEqual(StanfordDHRobot.get_alphas(),robot_DH_alpha, "AbsTol", DQ.threshold,... - "Error in DQ_SerialManipulatorDH.get_alphas()"); - - TestCase.assertEqual(StanfordDHRobot.get_types(),robot_type, "AbsTol", DQ.threshold,... - "Error in DQ_SerialManipulatorDH.get_types()"); - - - - TestCase.assertEqual(StanfordMDHRobot.get_thetas(),robot_MDH_theta, "AbsTol", DQ.threshold,... - "Error in DQ_SerialManipulatorMDH.get_thetas()"); - - TestCase.assertEqual(StanfordMDHRobot.get_ds(),robot_MDH_d, "AbsTol", DQ.threshold,... - "Error in DQ_SerialManipulatorMDH.get_ds()"); - - TestCase.assertEqual(StanfordMDHRobot.get_as(),robot_MDH_a, "AbsTol", DQ.threshold,... - "Error in DQ_SerialManipulatorMDH.get_as()"); - - TestCase.assertEqual(StanfordMDHRobot.get_alphas(),robot_MDH_alpha, "AbsTol", DQ.threshold,... - "Error in DQ_SerialManipulatorMDH.get_alphas()"); - - TestCase.assertEqual(StanfordMDHRobot.get_types(),robot_type, "AbsTol", DQ.threshold,... - "Error in DQ_SerialManipulatorMDH.get_types()"); +% The following tests will be addressed in futures versions. +% +% TestCase.assertEqual(StanfordDHRobot.get_thetas(),robot_DH_theta, "AbsTol", DQ.threshold,... +% "Error in DQ_SerialManipulatorDH.get_thetas()"); +% +% TestCase.assertEqual(StanfordDHRobot.get_ds(),robot_DH_d, "AbsTol", DQ.threshold,... +% "Error in DQ_SerialManipulatorDH.get_ds()"); +% +% TestCase.assertEqual(StanfordDHRobot.get_as(),robot_DH_a, "AbsTol", DQ.threshold,... +% "Error in DQ_SerialManipulatorDH.get_as()"); +% +% TestCase.assertEqual(StanfordDHRobot.get_alphas(),robot_DH_alpha, "AbsTol", DQ.threshold,... +% "Error in DQ_SerialManipulatorDH.get_alphas()"); +% +% TestCase.assertEqual(StanfordDHRobot.get_types(),robot_type, "AbsTol", DQ.threshold,... +% "Error in DQ_SerialManipulatorDH.get_types()"); +% +% +% +% TestCase.assertEqual(StanfordMDHRobot.get_thetas(),robot_MDH_theta, "AbsTol", DQ.threshold,... +% "Error in DQ_SerialManipulatorMDH.get_thetas()"); +% +% TestCase.assertEqual(StanfordMDHRobot.get_ds(),robot_MDH_d, "AbsTol", DQ.threshold,... +% "Error in DQ_SerialManipulatorMDH.get_ds()"); +% +% TestCase.assertEqual(StanfordMDHRobot.get_as(),robot_MDH_a, "AbsTol", DQ.threshold,... +% "Error in DQ_SerialManipulatorMDH.get_as()"); +% +% TestCase.assertEqual(StanfordMDHRobot.get_alphas(),robot_MDH_alpha, "AbsTol", DQ.threshold,... +% "Error in DQ_SerialManipulatorMDH.get_alphas()"); +% +% TestCase.assertEqual(StanfordMDHRobot.get_types(),robot_type, "AbsTol", DQ.threshold,... +% "Error in DQ_SerialManipulatorMDH.get_types()"); @@ -114,14 +115,11 @@ function stanford_manipulator_modeling() q_dot = q_dot_list(:,i); x1 = StanfordDHRobot.fkm(q); J1 = StanfordDHRobot.pose_jacobian(q); - J1_dot = StanfordDHRobot.pose_jacobian_derivative(q,q_dot); + J1_dot = StanfordDHRobot.pose_jacobian_derivative(q,q_dot); + x2 = StanfordMDHRobot.fkm(q); J2 = StanfordMDHRobot.pose_jacobian(q); - - % The pose_jacobian_derivative method does not take into - % account the end effector. Because of that, we use the - % Hamilton operator to take it into account. - J2_dot = haminus8(StanfordMDHRobot.get_effector())*StanfordMDHRobot.pose_jacobian_derivative(q,q_dot); + J2_dot = StanfordMDHRobot.pose_jacobian_derivative(q,q_dot); TestCase.assertEqual(vec8(x1),vec8(x2), "AbsTol", DQ.threshold,... "Error in DQ_SerialManipulatorMDH.fkm"); From 3bab033c1e12e156a2204019f4b91d682d902452 Mon Sep 17 00:00:00 2001 From: Juancho Date: Sun, 4 Dec 2022 11:39:37 +0900 Subject: [PATCH 2/3] [standford_manipulator_modeling] Removed unnecessary commented code. --- manipulator/stanford_manipulator_modeling.m | 38 +-------------------- 1 file changed, 1 insertion(+), 37 deletions(-) diff --git a/manipulator/stanford_manipulator_modeling.m b/manipulator/stanford_manipulator_modeling.m index 2d40163..07c7258 100644 --- a/manipulator/stanford_manipulator_modeling.m +++ b/manipulator/stanford_manipulator_modeling.m @@ -1,4 +1,4 @@ -% (C) Copyright 2020 DQ Robotics Developers +% (C) Copyright 2022 DQ Robotics Developers % % This file is part of DQ Robotics. % @@ -73,42 +73,6 @@ function stanford_manipulator_modeling() % Testing some methods TestCase = matlab.unittest.TestCase.forInteractiveUse; -% The following tests will be addressed in futures versions. -% -% TestCase.assertEqual(StanfordDHRobot.get_thetas(),robot_DH_theta, "AbsTol", DQ.threshold,... -% "Error in DQ_SerialManipulatorDH.get_thetas()"); -% -% TestCase.assertEqual(StanfordDHRobot.get_ds(),robot_DH_d, "AbsTol", DQ.threshold,... -% "Error in DQ_SerialManipulatorDH.get_ds()"); -% -% TestCase.assertEqual(StanfordDHRobot.get_as(),robot_DH_a, "AbsTol", DQ.threshold,... -% "Error in DQ_SerialManipulatorDH.get_as()"); -% -% TestCase.assertEqual(StanfordDHRobot.get_alphas(),robot_DH_alpha, "AbsTol", DQ.threshold,... -% "Error in DQ_SerialManipulatorDH.get_alphas()"); -% -% TestCase.assertEqual(StanfordDHRobot.get_types(),robot_type, "AbsTol", DQ.threshold,... -% "Error in DQ_SerialManipulatorDH.get_types()"); -% -% -% -% TestCase.assertEqual(StanfordMDHRobot.get_thetas(),robot_MDH_theta, "AbsTol", DQ.threshold,... -% "Error in DQ_SerialManipulatorMDH.get_thetas()"); -% -% TestCase.assertEqual(StanfordMDHRobot.get_ds(),robot_MDH_d, "AbsTol", DQ.threshold,... -% "Error in DQ_SerialManipulatorMDH.get_ds()"); -% -% TestCase.assertEqual(StanfordMDHRobot.get_as(),robot_MDH_a, "AbsTol", DQ.threshold,... -% "Error in DQ_SerialManipulatorMDH.get_as()"); -% -% TestCase.assertEqual(StanfordMDHRobot.get_alphas(),robot_MDH_alpha, "AbsTol", DQ.threshold,... -% "Error in DQ_SerialManipulatorMDH.get_alphas()"); -% -% TestCase.assertEqual(StanfordMDHRobot.get_types(),robot_type, "AbsTol", DQ.threshold,... -% "Error in DQ_SerialManipulatorMDH.get_types()"); - - - for i=1:number_of_trials q = q_list(:,i); From 4e1e00aa4acf6eda4e87c15565d25d5ac9b1142b Mon Sep 17 00:00:00 2001 From: Juancho Date: Sun, 4 Dec 2022 12:45:19 +0900 Subject: [PATCH 3/3] [stanford_manipulator_modeling] Commented the lines that use the class DQ_SerialManipulatorMDH since that class is not available yet. --- manipulator/stanford_manipulator_modeling.m | 22 ++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/manipulator/stanford_manipulator_modeling.m b/manipulator/stanford_manipulator_modeling.m index 07c7258..3d059b1 100644 --- a/manipulator/stanford_manipulator_modeling.m +++ b/manipulator/stanford_manipulator_modeling.m @@ -59,8 +59,8 @@ function stanford_manipulator_modeling() robot_MDH_a; robot_MDH_alpha; robot_type]; - StanfordMDHRobot = DQ_SerialManipulatorMDH(robot_MDH_matrix); - StanfordMDHRobot.set_effector(1+DQ.E*0.5*DQ.k*d6); +% StanfordMDHRobot = DQ_SerialManipulatorMDH(robot_MDH_matrix); +% StanfordMDHRobot.set_effector(1+DQ.E*0.5*DQ.k*d6); number_of_trials = 100; @@ -81,16 +81,16 @@ function stanford_manipulator_modeling() J1 = StanfordDHRobot.pose_jacobian(q); J1_dot = StanfordDHRobot.pose_jacobian_derivative(q,q_dot); - x2 = StanfordMDHRobot.fkm(q); - J2 = StanfordMDHRobot.pose_jacobian(q); - J2_dot = StanfordMDHRobot.pose_jacobian_derivative(q,q_dot); +% x2 = StanfordMDHRobot.fkm(q); +% J2 = StanfordMDHRobot.pose_jacobian(q); +% J2_dot = StanfordMDHRobot.pose_jacobian_derivative(q,q_dot); - TestCase.assertEqual(vec8(x1),vec8(x2), "AbsTol", DQ.threshold,... - "Error in DQ_SerialManipulatorMDH.fkm"); - TestCase.assertEqual(J1,J2, "AbsTol", DQ.threshold,... - "Error in DQ_SerialManipulatorMDH.pose_jacobian"); - TestCase.assertEqual(J1_dot,J2_dot,"AbsTol", DQ.threshold,... - "Error in DQ_SerialManipulatorMDH.pose_jacobian_derivative"); +% TestCase.assertEqual(vec8(x1),vec8(x2), "AbsTol", DQ.threshold,... +% "Error in DQ_SerialManipulatorMDH.fkm"); +% TestCase.assertEqual(J1,J2, "AbsTol", DQ.threshold,... +% "Error in DQ_SerialManipulatorMDH.pose_jacobian"); +% TestCase.assertEqual(J1_dot,J2_dot,"AbsTol", DQ.threshold,... +% "Error in DQ_SerialManipulatorMDH.pose_jacobian_derivative"); end