Skip to content

The standford_manipulator_modeling example is only modeled with DH convention. #2

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
merged 3 commits into from
Dec 11, 2022
Merged

The standford_manipulator_modeling example is only modeled with DH convention. #2

Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
a0ce6da
[standford_manipulator_modeling.m] Updated the example to ensure comp…
juanjqo Dec 3, 2022
3bab033
juanjqo Dec 4, 2022
4e1e00a
juanjqo Dec 4, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
66 changes: 14 additions & 52 deletions manipulator/stanford_manipulator_modeling.m
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
% (C) Copyright 2020 DQ Robotics Developers
% (C) Copyright 2022 DQ Robotics Developers
%
% This file is part of DQ Robotics.
%
Expand Down Expand Up @@ -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;
Expand All @@ -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

Expand Down