Skip to content
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.

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

Mark iii dvrk #17

Open
wants to merge 54 commits into
base: master
Choose a base branch
from
Open
Changes from 1 commit
Commits
Show all changes
54 commits
Select commit Hold shift + click to select a range
c6f657f
played with the demo
Feb 7, 2018
4dd2308
fixed primatic offsets
Feb 7, 2018
394ad00
testing Jacobian IK addon
Feb 8, 2018
002a85e
fixed Jacobian IK
Feb 9, 2018
fa12a32
adding fwd test main
Feb 13, 2018
694704e
corrected prismatic offsets
Feb 13, 2018
8da39b4
good magic numbers in the fwd kinematics
Feb 14, 2018
302e144
fixed all DH2 params
Feb 19, 2018
8abe837
fixing the .99 factor
Feb 20, 2018
f40c251
updates from 20180319
Mar 19, 2018
37ec0f2
slight changes
Apr 10, 2018
e6688a5
fixed the frozen bug
Apr 10, 2018
0701608
cleanup
Apr 10, 2018
89c1483
added wrapper that conforms to old format
Apr 10, 2018
b3d4dff
removed adjustments
May 19, 2018
2f95eac
removed the couts
Jun 4, 2018
d0d3457
modified from surgical computer
Jun 18, 2018
3a94eae
fixed small bug
Jun 18, 2018
7c18953
PSM2 dh params
Jun 18, 2018
14582c2
fixing bugs
Jul 13, 2018
34a0501
adding tests
Jul 14, 2018
b9fcaab
added debugging markers
Jul 15, 2018
990a5ea
starting to write Mk2
Jul 16, 2018
5eb5b53
testing..
Jul 16, 2018
684f6c8
a few lines
Jul 16, 2018
f79f250
major refit
Jul 19, 2018
c4d45c8
changed test
Jul 20, 2018
e1d9526
added IK tasks
Jul 20, 2018
8d3a356
some changes
Jul 20, 2018
8436229
debugging
Jul 20, 2018
c8cc10b
added new test
Jul 21, 2018
cbc121f
added test and completed debugging
Jul 22, 2018
3681782
added adjustments for frozen fncs
Jul 23, 2018
f7b84ed
added from tests
Jul 23, 2018
23cbf5e
added more printouts
Jul 23, 2018
2e09da3
dir change
Jul 24, 2018
a6c8e38
removed useless lines
Jul 25, 2018
e664372
added new yaml
Jul 30, 2018
0f2f5ba
added new tests
Jul 30, 2018
494d03d
tested new DH fix
Aug 1, 2018
50729bc
added new yaml
Aug 1, 2018
4d9defa
modified psm1 dh
Aug 6, 2018
8551a3e
psm2 yaml
Aug 11, 2018
a1b578e
added j2 scale factor
Aug 11, 2018
82c1fed
updated from latest test
Aug 13, 2018
2d45103
added joint 1 scale factor and new test result
Aug 13, 2018
eba0225
added from home
Aug 22, 2018
dfb689a
MK 3 upgrades
Oct 29, 2018
5893b9d
MKIII calibration
Oct 29, 2018
858f7b7
testing DH 5 params
Nov 5, 2018
e17b2b8
fixed kinematics issues
Nov 12, 2018
4409429
corrected yaml
Nov 13, 2018
ef3f52b
corrected psm2 yaml
Nov 13, 2018
d1784ff
added new calibration results
Nov 19, 2018
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
Prev Previous commit
Next Next commit
added j2 scale factor
HMS Prince of Wales committed Aug 11, 2018
commit a1b578e360267a3915b1f598195b8c2daf5ce884
2 changes: 2 additions & 0 deletions config/psm1_dh.yaml
Original file line number Diff line number Diff line change
@@ -15,6 +15,8 @@ theta_2: -1.569949
alpha_2: 1.563795
a_2: 0.0002515
d_2: -0.0018725
j2_scale_factor: 0.9732


# DH_2 TO DH_3 FRAME
d_3: -0.0080085
2 changes: 2 additions & 0 deletions config/psm1_dh_sim.yaml
Original file line number Diff line number Diff line change
@@ -14,6 +14,8 @@ theta_2: -1.57079632679
alpha_2: 1.57079632679
a_2: 0.0008
d_2: -0.0029
j2_scale_factor: 0.9732 # Temp


# DH_2 TO DH_3 FRAME
d_3: -0.0087
1 change: 1 addition & 0 deletions config/psm2_dh.yaml
Original file line number Diff line number Diff line change
@@ -12,6 +12,7 @@ theta_2: 1.5928065
alpha_2: -1.575889
a_2: 0.000337
d_2: 0.001284
j2_scale_factor: 0.9732

# DH_2 TO DH_3 FRAME
d_3: -0.0089265
1 change: 1 addition & 0 deletions config/psm_generic.yaml
Original file line number Diff line number Diff line change
@@ -11,6 +11,7 @@ theta_2: 1.570796
alpha_2: 1.570796
a_2: 0
d_2: 0
j2_scale_factor: 1.0
# DH_2 TO DH_3 FRAME
d_3: -0.0156
j3_scale_factor: 1.0
12 changes: 6 additions & 6 deletions examples/psm_intrinsic_calibration_auxiliary.cpp
Original file line number Diff line number Diff line change
@@ -23,8 +23,8 @@ Eigen::Affine3d printInfo(Eigen::Vector3d test_pt) {
dvrk_inverse.resetDhOffsetsMaps();
dvrk_inverse.loadDHyamlfiles("psm1_dh","psm1_dh");
dvrk_inverse.loadDHyamlfiles("psm2_dh","psm2_dh");
dvrk_inverse.loadDHyamlfiles("psm1_dh_sim","psm1_dh_sim");
dvrk_inverse.loadDHyamlfiles("psm_generic","psm_generic");
// dvrk_inverse.loadDHyamlfiles("psm1_dh_sim","psm1_dh_sim");
// dvrk_inverse.loadDHyamlfiles("psm_generic","psm_generic");

if (dvrk_inverse.ik_solve_frozen_refined(test_pt, "psm2_dh") > 0) {

@@ -59,15 +59,15 @@ Eigen::Affine3d printInfo(davinci_kinematics::Vectorq7x1 q_vec) {

Eigen::Affine3d affine_wrist_wrt_base, affine_tip_wrt_base;

davinci_kinematics::Vectorq7x1 q_vec_ik;
// davinci_kinematics::Vectorq7x1 q_vec_ik;

dvrk_inverse.resetDhOffsetsMaps();
dvrk_inverse.loadDHyamlfiles("psm1_dh","psm1_dh");
dvrk_inverse.loadDHyamlfiles("psm2_dh","psm2_dh");
dvrk_inverse.loadDHyamlfiles("psm1_dh_sim","psm1_dh_sim");
dvrk_inverse.loadDHyamlfiles("psm_generic","psm_generic");
// dvrk_inverse.loadDHyamlfiles("psm1_dh_sim","psm1_dh_sim");
// dvrk_inverse.loadDHyamlfiles("psm_generic","psm_generic");

std::cout << "Your input q_vec: " << std::endl << q_vec_ik.transpose() << std::endl;
std::cout << "Your input q_vec: " << std::endl << q_vec.transpose() << std::endl;

affine_tip_wrt_base = dvrk_inverse.fwd_kin_solve(q_vec, "psm2_dh");

2 changes: 2 additions & 0 deletions include/cwru_davinci_kinematics/davinci_fwd_kinematics.h
Original file line number Diff line number Diff line change
@@ -231,11 +231,13 @@ class Forward
Eigen::VectorXd dval_DH_offsets_generic_;
Eigen::VectorXd DH_a_params_generic_;
Eigen::VectorXd DH_alpha_params_generic_;
double j2_scale_factor_generic_;
double j3_scale_factor_generic_;
std::map<std::string, Eigen::VectorXd> theta_DH_offsets_map_;
std::map<std::string, Eigen::VectorXd> dval_DH_offsets_map_;
std::map<std::string, Eigen::VectorXd> DH_a_params_map_;
std::map<std::string, Eigen::VectorXd> DH_alpha_params_map_;
std::map<std::string, double> j2_scale_factor_map_;
std::map<std::string, double> j3_scale_factor_map_;

std::map<std::string, Vectorq7x1> current_joint_state__map_;
32 changes: 29 additions & 3 deletions src/davinci_fwd_kinematics.cpp
Original file line number Diff line number Diff line change
@@ -376,6 +376,7 @@ bool Forward::loadDHyamlfiles(std::string yaml_name, std::string kinematic_set_n
std::string psm_yaml_path;
YAML::Node psm_dh_param_node;

double j2_scale_factor;
double j3_scale_factor;
Eigen::VectorXd theta_DH_offsets;
Eigen::VectorXd dval_DH_offsets;
@@ -391,6 +392,7 @@ bool Forward::loadDHyamlfiles(std::string yaml_name, std::string kinematic_set_n
dval_DH_offsets = dval_DH_offsets_generic_;
DH_a_params = DH_a_params_generic_;
DH_alpha_params = DH_alpha_params_generic_;
j2_scale_factor = j2_scale_factor_generic_;
j3_scale_factor = j3_scale_factor_generic_;

ros_pkg_path_ = ros::package::getPath("cwru_davinci_kinematics");
@@ -461,6 +463,14 @@ bool Forward::loadDHyamlfiles(std::string yaml_name, std::string kinematic_set_n
ROS_WARN("Failed to load DH d_2, will use generic instead.");
}

try {
j2_scale_factor = psm_dh_param_node["j2_scale_factor"].as<double>();
} catch(const std::exception& e) {
std::cout << e.what() << std::endl;
ROS_WARN("Failed to load j2_scale_factor, will use generic instead.");
}


try {
dval_DH_offsets[2] = psm_dh_param_node["d_3"].as<double>();
} catch(const std::exception& e) {
@@ -475,13 +485,16 @@ bool Forward::loadDHyamlfiles(std::string yaml_name, std::string kinematic_set_n
ROS_WARN("Failed to load j3_scale_factor, will use generic instead.");
}



ROS_INFO("Yaml loading complete!");

// Add to maps
theta_DH_offsets_map_[kinematic_set_name] = theta_DH_offsets;
dval_DH_offsets_map_[kinematic_set_name] = dval_DH_offsets;
DH_a_params_map_[kinematic_set_name] = DH_a_params;
DH_alpha_params_map_[kinematic_set_name] = DH_alpha_params;
j2_scale_factor_map_[kinematic_set_name] = j2_scale_factor;
j3_scale_factor_map_[kinematic_set_name] = j3_scale_factor;

// Also initialise related variables if needed.
@@ -526,7 +539,7 @@ void Forward::resetDhGenericParams() {
}

dval_DH_offsets_generic_<< 0, 0, DH_q_offsets[2], 0, 0, 0, 0;

j2_scale_factor_generic_ = 1.0;
j3_scale_factor_generic_ = 1.0;

}
@@ -605,14 +618,27 @@ void Forward::convert_qvec_to_DH_vecs(const Vectorq7x1& q_vec,
thetas_DH_vec.resize(7);
thetas_DH_vec = theta_DH_offsets_map_[kinematic_set_name];


// TODO this is experimental. Include this into the yaml file and modify the code to read it.
double j2_scale_factor = 0.9732;

for (int i = 0; i < 7; i++)
{

// skip the linear joint.
if (i == 1) {

// Joint 2 has a scale factor to be added here.
thetas_DH_vec(i)+= j2_scale_factor_map_[kinematic_set_name] * q_vec(i);

} else {

thetas_DH_vec(i)+= q_vec(i);

}


if (i == 2) continue;
thetas_DH_vec(i)+= q_vec(i);

}