diff --git a/manipulator/stanford_manipulator_modeling.m b/manipulator/stanford_manipulator_modeling.m index 41ccf3d..3d059b1 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. % @@ -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; @@ -74,61 +74,23 @@ function stanford_manipulator_modeling() 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()"); - - - - for i=1:number_of_trials q = q_list(:,i); q_dot = q_dot_list(:,i); x1 = StanfordDHRobot.fkm(q); J1 = StanfordDHRobot.pose_jacobian(q); - 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); + 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); - 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