AUV-Coop-Assembly
Master Thesis for Robotics Engineering. Cooperative peg-in-hole assembly with two underwater vehicles guided by vision
collisionPropagator.cpp
1 #include "header/collisionPropagator.h"
2 
3 CollisionPropagator::CollisionPropagator(){} //private costructor not to be used
4 
5 
6 std::vector<double> CollisionPropagator::calculateCollisionDisturbArmVeh(Eigen::Matrix<double, 6, TOT_DOF>world_J_tip,
7  Eigen::Matrix4d wTpegtip, Eigen::Vector3d forces, Eigen::Vector3d torques){
8 
9  Eigen::Vector3d w_forces = wTpegtip.topLeftCorner<3,3>() * forces;
10  Eigen::Vector3d w_torques = wTpegtip.topLeftCorner<3,3>() * torques;
11 
12  Eigen::Matrix<double, TOT_DOF, 1> deltayDot_eigen =
13  CollisionPropagator::fromPegTipToWholeRobot(world_J_tip, w_forces, w_torques);
14 
15  return CONV::vector_eigen2std(deltayDot_eigen);
16 
17 }
18 
19 Eigen::Matrix<double, TOT_DOF, 1> CollisionPropagator::fromPegTipToWholeRobot(
20  Eigen::Matrix<double, 6, TOT_DOF>world_J_tip, Eigen::Vector3d w_forces, Eigen::Vector3d w_torques){
21 
22  Eigen::Matrix<double, TOT_DOF, 1> deltayDot_eigen;
23 
24  deltayDot_eigen = ( (world_J_tip.topRows<3>().transpose()) * w_forces ) +
25  ( (world_J_tip.bottomRows<3>().transpose()) * w_torques );
26 
27  return deltayDot_eigen;
28 }
29 
30 
31 std::vector<double> CollisionPropagator::calculateCollisionDisturb(Eigen::Matrix<double, 6, ARM_DOF>world_J_veh_tip,
32  Eigen::Matrix4d wTpegtip, Eigen::Vector3d forces, Eigen::Vector3d torques){
33 
34  Eigen::Matrix<double, 6, ARM_DOF>tip_J_veh_tip;
35  Eigen::Matrix3d pegtipRw = wTpegtip.topLeftCorner<3,3>().transpose();
36  tip_J_veh_tip.topRows<3>() = pegtipRw * world_J_veh_tip.topRows<3>();
37  tip_J_veh_tip.bottomRows<3>() = pegtipRw * world_J_veh_tip.bottomRows<3>();
38 
39  Eigen::Matrix<double, ARM_DOF, 1> deltayDot_eigen =
40  CollisionPropagator::fromPegTipToWholeArm(tip_J_veh_tip, forces, torques);
41 
42  return CONV::vector_eigen2std(deltayDot_eigen);
43 
44 }
45 
46 std::vector<double> CollisionPropagator::calculateCollisionDisturbVeh(Eigen::Matrix<double, 6, VEHICLE_DOF> world_J_veh_tip,
47  Eigen::Matrix4d wTpegtip, Eigen::Vector3d forces,
48  Eigen::Vector3d torques){
49 
50  Eigen::Matrix<double, 6, VEHICLE_DOF>tip_J_veh_tip;
51  Eigen::Matrix3d pegtipRw = wTpegtip.topLeftCorner<3,3>().transpose();
52  tip_J_veh_tip.topRows<3>() = pegtipRw * world_J_veh_tip.topRows<3>();
53  tip_J_veh_tip.bottomRows<3>() = pegtipRw * world_J_veh_tip.bottomRows<3>();
54 
55  Eigen::Matrix<double, VEHICLE_DOF, 1> deltayDotVeh_eigen =
56  CollisionPropagator::fromPegTipToVehicle(tip_J_veh_tip, forces, torques);
57 
58  return CONV::vector_eigen2std(deltayDotVeh_eigen);
59 
60 }
61 
62 
64 //std::vector<double> CollisionPropagator::calculateCollisionDisturb(Eigen::Matrix<double, 6, ARM_DOF>world_J_veh_tip,
65 // Eigen::Matrix4d wTpegtip, Eigen::Vector3d forces){
66 
67 // Eigen::Matrix<double, 6, ARM_DOF>tip_J_veh_tip;
68 // Eigen::Matrix3d pegtipRw = wTpegtip.topLeftCorner<3,3>().transpose();
69 // tip_J_veh_tip.topRows<3>() = pegtipRw * world_J_veh_tip.topRows<3>();
70 // tip_J_veh_tip.bottomRows<3>() = pegtipRw * world_J_veh_tip.bottomRows<3>();
71 
72 // Eigen::Matrix<double, ARM_DOF, 1> deltayDot_eigen =
73 // CollisionPropagator::fromPegTipToWholeArm(tip_J_veh_tip, forces);
74 
75 // return CONV::vector_eigen2std(deltayDot_eigen);
76 
77 //}
78 
79 
80 
81 
82 
83 
96 Eigen::Matrix<double, ARM_DOF, 1> CollisionPropagator::fromPegTipToWholeArm(
97  Eigen::Matrix<double, 6, ARM_DOF>tip_J_veh_tip, Eigen::Vector3d forces, Eigen::Vector3d torques){
98 
99  Eigen::Matrix<double, ARM_DOF, 1> deltayDot_eigen;
100 
101  deltayDot_eigen = ( (tip_J_veh_tip.topRows<3>().transpose()) * forces ) +
102  ( (tip_J_veh_tip.bottomRows<3>().transpose()) * torques );
103 
104  return deltayDot_eigen;
105 }
106 
114 Eigen::Matrix<double, VEHICLE_DOF, 1> CollisionPropagator::fromPegTipToVehicle(
115  Eigen::Matrix<double, 6, VEHICLE_DOF>tip_J_veh_tip, Eigen::Vector3d forces, Eigen::Vector3d torques){
116 
117  Eigen::Matrix<double, VEHICLE_DOF, 1> deltayDotVeh_eigen;
118 
119  deltayDotVeh_eigen = ( (tip_J_veh_tip.topRows<3>().transpose()) * forces ) +
120  ( (tip_J_veh_tip.bottomRows<3>().transpose()) * torques );
121 
122  return deltayDotVeh_eigen;
123 
124 }
Generated by   doxygen 1.8.11