diff --git a/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/config/common.sh b/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/config/common.sh
index f4da4dcd..3a2d560d 100644
--- a/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/config/common.sh
+++ b/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/config/common.sh
@@ -9,4 +9,5 @@ config="big_collision_box_height:=0.5 big_collision_box_on_top:=1 big_collision_
mobilicom_shift_y:=0.01 mobilicom_shift_z:=0.2 mobilicom_yaw:=3.14152 ptuxthermo_x_offset:=0.2
ptuxthermo_y_offset:=0.18 realsense_holder_version:=5 realsense_parent_link:=omnicam_sensor_mount
realsense_roll:=0.0 realsense_shift_x:=0.0 realsense_shift_y:=0.0 realsense_shift_z:=0.0 realsense_tilt:=-1.15
- realsense_upside_down:=0 realsense_yaw:=0.62831853 t265_parent_link:=rear_sensor_item top_box_j_x:=0.472"
\ No newline at end of file
+ realsense_upside_down:=0 realsense_width:=640 realsense_yaw:=0.62831853 t265_parent_link:=rear_sensor_item
+ top_box_j_x:=0.472"
\ No newline at end of file
diff --git a/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/launch/vehicle_topics.launch b/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/launch/vehicle_topics.launch
index 83c80ba9..95b61dd1 100644
--- a/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/launch/vehicle_topics.launch
+++ b/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/launch/vehicle_topics.launch
@@ -70,6 +70,13 @@
args="/model/$(arg name)/joint/front_left_flipper_j/cmd_pos@std_msgs/Float64]ignition.msgs.Double">
+
+
+
+
+
+
+
+
+
-
+
+
+
+
+
0.50017 0.24093 0.139
+
+
+
+
+
+
+
+
0.157 0 0.118 0 -0 0
@@ -30,6 +38,14 @@
0.056 0.12093 0.139
+
+
+
+
+
+
+
+
0.144 -0.076 0.118 0 -0 0.785398
@@ -38,6 +54,14 @@
0.083 0.04 0.139
+
+
+
+
+
+
+
+
0.144 0.076 0.118 0 0 -0.785398
@@ -46,6 +70,14 @@
0.083 0.04 0.139
+
+
+
+
+
+
+
+
0 0.135 0.09 0 -0 0
@@ -54,6 +86,14 @@
0.055 0.04 0.08
+
+
+
+
+
+
+
+
0 0.155 0.013 0 -0 0
@@ -62,6 +102,14 @@
0.055 0.02 0.075
+
+
+
+
+
+
+
+
0 -0.135 0.09 0 -0 0
@@ -70,6 +118,14 @@
0.055 0.04 0.08
+
+
+
+
+
+
+
+
0 -0.155 0.013 0 -0 0
@@ -78,6 +134,14 @@
0.055 0.02 0.075
+
+
+
+
+
+
+
+
-0.39064 0.0789 0.1551 3.1415926535897931 1.5707963267948966 3.1415926535897931
@@ -179,6 +243,14 @@
0.068
+
+
+
+
+
+
+
+
0.111811 -0.018928 0.440133 0 0.504467 -0.000319
@@ -187,6 +259,14 @@
0.008 0.075 0.025
+
+
+
+
+
+
+
+
0.099551 -0.028924 0.4469 0 0.504467 -0.000319
@@ -195,6 +275,14 @@
0.035 0.01 0.028
+
+
+
+
+
+
+
+
0.074085 -0.043916 0.415266 -0 0.004467 -0.000319
@@ -203,6 +291,14 @@
0.035 0.045 0.073
+
+
+
+
+
+
+
+
0.125348 -0.010971 0.43571 0.015 0.499467 -0.000319
@@ -211,10 +307,20 @@
0.02505 0.09 0.025
+
+
+
+
+
+
+
+
+ 0 0 0 0 -0 0
+ 1 1 1
meshes/body.dae
@@ -223,6 +329,7 @@
-0.372 0.0789 0.1551 0 -0 0
+ 1 1 1
meshes/antenna.dae
@@ -231,6 +338,7 @@
-0.2836 -0.0221 0.0722 0 -0 0
+ 1 1 1
meshes/battery.dae
@@ -440,7 +548,7 @@
- 0.088894 -0.073921 0.3871 -5e-06 -0.002418 1.57486
+ 0.084785 -0.073919 0.387078 -5e-06 -0.002418 1.57486
0.001 0.001 0.001
@@ -448,23 +556,10 @@
- .5 0.5 0.5 1
- .1 0.1 0.1 1
-
-
-
- 0.133882 -0.000937 0.420959 2.07031 -0.013169 1.5633
-
-
- meshes/realsense_d435.dae
-
-
-
- .5 0.5 0.5 1
- .1 0.1 0.1 1
+ 0.5 0.5 0.5 1
+ 0.1 0.1 0.1 1
-
0.115312 -0.018929 0.4382 0.015 0.499467 -0.000319
@@ -489,7 +584,20 @@
-
+
+ 0.138428 -0.010975 0.428574 2.07031 -0.013168 1.56329
+
+
+ 1 1 1
+ meshes/realsense_d435.dae
+
+
+
+ 0.5 0.5 0.5 1
+ 0.1 0.1 0.1 1
+
+
+
0.125479 0.006527 0.435941 0.015 0.499467 -0.000319
@@ -497,7 +605,7 @@
-
+
0.125592 0.021526 0.436138 0.015 0.499467 -0.000319
@@ -505,7 +613,7 @@
-
+
0.125592 0.021526 0.436138 -2.07031 0.013167 -1.5783
@@ -513,7 +621,7 @@
-
+
0.125479 0.006527 0.435941 -2.07031 0.013167 -1.5783
@@ -521,7 +629,7 @@
-
+
0.125479 0.006527 0.435941 0.015 0.499467 -0.000319
@@ -529,7 +637,7 @@
-
+
0.125479 0.006527 0.435941 -2.07031 0.013167 -1.5783
@@ -537,7 +645,7 @@
-
+
0.125104 -0.043467 0.435282 0.015 0.499467 -0.000319
@@ -545,7 +653,7 @@
-
+
0.125104 -0.043467 0.435282 -2.07031 0.013167 -1.5783
@@ -710,6 +818,8 @@
+
+
0 0 0.15 -3.1415926535897931 -0 0
@@ -875,6 +985,7 @@
0.012561 -0.017068 0.314417 -0.000579 0.005611 0.313689
+
6
@@ -914,6 +1025,7 @@
0.058951 -0.03438 0.313899 -0.010815 0.001034 -0.943109
+
6
@@ -953,6 +1065,8 @@
0.024083 -0.058382 0.376328 3.1415926535897931 -1.5707963267948966 0.945143
+
+
1.21126
@@ -981,7 +1095,7 @@
RGB_INT8
- 0.01
+ 0.1
100
@@ -999,8 +1113,10 @@
1
6
- 0.136479 -0.016527 0.419441 0.015 0.499467 -0.000319
+ 0.125479 0.006527 0.435941 0.015 0.499467 -0.000319
+ 1
+
0.2502 0 0.1407 -3.1415926535897931 -0 0
@@ -1120,18 +1236,22 @@
+
+
+ 1e+07
+ 1
+
+
0.7
150
+ 0.035
+ 0
0 0 1
-
-
-
- 20
0 0 -0.0705 1.5707963267948966 -0 1.5707963267948966
@@ -1182,6 +1302,7 @@
0.25 0.272 0.0195 0 0.193732 0
+ 0 0 0 0 -0 0
1e-05
0.001
@@ -1193,16 +1314,25 @@
+ 0 0 0 0 -0 0
0.01 0.01 0.01
+
+
+ 1e+07
+ 1
+
+
0.7
150
+ 0.035
+ 0
0 0 1
@@ -1212,10 +1342,15 @@
0 0 0 -2.95744 0 1.5707963267948966
+ 1 1 1
meshes/flipper.dae
+ 1
+
+ 1
+ 0
front_left_flipper
@@ -1257,7 +1392,7 @@
0.010941
-
+
0 0 0 1.5707963267948966 -0 0
@@ -1274,8 +1409,8 @@
- 1
- 1
+ 0.7
+ 150
0.035
0
0 0 1
@@ -1283,6 +1418,10 @@
+ 1
+
+ 1
+ 0
front_left_flipper_wheel1
@@ -1314,7 +1453,7 @@
0.010941
-
+
0 0 0 1.5707963267948966 -0 0
@@ -1331,16 +1470,19 @@
- 1
- 1
+ 0.7
+ 150
0.035
0
0 0 1
-
+ 1
+
+ 1
+ 0
front_left_flipper_wheel2
@@ -1372,7 +1514,7 @@
0.010941
-
+
0 0 0 1.5707963267948966 -0 0
@@ -1389,16 +1531,19 @@
- 1
- 1
+ 0.7
+ 150
0.035
0
0 0 1
-
+ 1
+
+ 1
+ 0
front_left_flipper_wheel3
@@ -1430,7 +1575,7 @@
0.010941
-
+
0 0 0 1.5707963267948966 -0 0
@@ -1447,16 +1592,19 @@
- 1
- 1
+ 0.7
+ 150
0.035
0
0 0 1
-
+ 1
+
+ 1
+ 0
front_left_flipper_wheel4
@@ -1488,7 +1636,7 @@
0.010941
-
+
0 0 0 1.5707963267948966 -0 0
@@ -1505,16 +1653,19 @@
- 1
- 1
+ 0.7
+ 150
0.035
0
0 0 1
-
+ 1
+
+ 1
+ 0
front_left_flipper_wheel5
@@ -1563,16 +1714,18 @@
- 1
- 1
+ 0.7
+ 150
0.035
0
0 0 1
-
+ 1
+
+ 0
left_track_wheel1
@@ -1621,16 +1774,18 @@
- 1
- 1
+ 0.7
+ 150
0.035
0
0 0 1
-
+ 1
+
+ 0
left_track_wheel2
@@ -1679,16 +1834,18 @@
- 1
- 1
+ 0.7
+ 150
0.035
0
0 0 1
-
+ 1
+
+ 0
left_track_wheel3
@@ -1737,16 +1894,18 @@
- 1
- 1
+ 0.7
+ 150
0.035
0
0 0 1
-
+ 1
+
+ 0
left_track_wheel4
@@ -1795,16 +1954,18 @@
- 1
- 1
+ 0.7
+ 150
0.035
0
0 0 1
-
+ 1
+
+ 0
left_track_wheel5
@@ -1853,16 +2014,18 @@
- 1
- 1
+ 0.7
+ 150
0.035
0
0 0 1
-
+ 1
+
+ 0
left_track_wheel6
@@ -1911,16 +2074,18 @@
- 1
- 1
+ 0.7
+ 150
0.035
0
0 0 1
-
+ 1
+
+ 0
left_track_wheel7
@@ -1969,17 +2134,18 @@
- 1
- 1
+ 0.7
+ 150
0.035
0
0 0 1
-
-
+ 1
+
+ 0
left_track_wheel8
@@ -2019,18 +2185,22 @@
+
+
+ 1e+07
+ 1
+
+
0.7
150
+ 0.035
+ 0
0 0 1
-
-
-
- 20
0 0 0 -2.95744 0 1.5707963267948966
@@ -2086,7 +2256,7 @@
0.010941
-
+
0 0 0 1.5707963267948966 -0 0
@@ -2103,16 +2273,19 @@
- 1
- 1
+ 0.7
+ 150
0.035
0
0 0 1
-
+ 1
+
+ 1
+ 0
rear_left_flipper_wheel1
@@ -2144,7 +2317,7 @@
0.010941
-
+
0 0 0 1.5707963267948966 -0 0
@@ -2161,16 +2334,19 @@
- 1
- 1
+ 0.7
+ 150
0.035
0
0 0 1
-
+ 1
+
+ 1
+ 0
rear_left_flipper_wheel2
@@ -2202,7 +2378,7 @@
0.010941
-
+
0 0 0 1.5707963267948966 -0 0
@@ -2219,16 +2395,19 @@
- 1
- 1
+ 0.7
+ 150
0.035
0
0 0 1
-
+ 1
+
+ 1
+ 0
rear_left_flipper_wheel3
@@ -2260,7 +2439,7 @@
0.010941
-
+
0 0 0 1.5707963267948966 -0 0
@@ -2277,16 +2456,19 @@
- 1
- 1
+ 0.7
+ 150
0.035
0
0 0 1
-
+ 1
+
+ 1
+ 0
rear_left_flipper_wheel4
@@ -2318,7 +2500,7 @@
0.010941
-
+
0 0 0 1.5707963267948966 -0 0
@@ -2335,16 +2517,19 @@
- 1
- 1
+ 0.7
+ 150
0.035
0
0 0 1
-
+ 1
+
+ 1
+ 0
rear_left_flipper_wheel5
@@ -2384,18 +2569,22 @@
+
+
+ 1e+07
+ 1
+
+
0.7
150
+ 0.035
+ 0
0 0 1
-
-
-
- 20
0 0 -0.0705 1.5707963267948966 0 -1.5707963267948966
@@ -2465,18 +2654,22 @@
+
+
+ 1e+07
+ 1
+
+
0.7
150
+ 0.035
+ 0
0 0 1
-
-
-
- 20
0 0 0 -2.95744 -0 -1.5707963267948966
@@ -2532,7 +2725,7 @@
0.010941
-
+
0 0 0 1.5707963267948966 -0 0
@@ -2549,16 +2742,19 @@
- 1
- 1
+ 0.7
+ 150
0.035
0
0 0 1
-
+ 1
+
+ 1
+ 0
front_right_flipper_wheel1
@@ -2590,14 +2786,13 @@
0.010941
-
+
0 0 0 1.5707963267948966 -0 0
0.04981
0.074
-
@@ -2608,16 +2803,19 @@
- 1
- 1
+ 0.7
+ 150
0.035
0
0 0 1
-
+ 1
+
+ 1
+ 0
front_right_flipper_wheel2
@@ -2649,7 +2847,7 @@
0.010941
-
+
0 0 0 1.5707963267948966 -0 0
@@ -2666,16 +2864,19 @@
- 1
- 1
+ 0.7
+ 150
0.035
0
0 0 1
-
+ 1
+
+ 1
+ 0
front_right_flipper_wheel3
@@ -2707,7 +2908,7 @@
0.010941
-
+
0 0 0 1.5707963267948966 -0 0
@@ -2724,16 +2925,19 @@
- 1
- 1
+ 0.7
+ 150
0.035
0
0 0 1
-
+ 1
+
+ 1
+ 0
front_right_flipper_wheel4
@@ -2765,7 +2969,7 @@
0.010941
-
+
0 0 0 1.5707963267948966 -0 0
@@ -2782,16 +2986,19 @@
- 1
- 1
+ 0.7
+ 150
0.035
0
0 0 1
-
+ 1
+
+ 1
+ 0
front_right_flipper_wheel5
@@ -2831,18 +3038,22 @@
+
+
+ 1e+07
+ 1
+
+
0.7
150
+ 0.035
+ 0
0 0 1
-
-
-
- 20
0 0 0 -2.95744 -0 -1.5707963267948966
@@ -2898,7 +3109,7 @@
0.010941
-
+
0 0 0 1.5707963267948966 -0 0
@@ -2915,16 +3126,19 @@
- 1
- 1
+ 0.7
+ 150
0.035
0
0 0 1
-
+ 1
+
+ 1
+ 0
rear_right_flipper_wheel1
@@ -2956,7 +3170,7 @@
0.010941
-
+
0 0 0 1.5707963267948966 -0 0
@@ -2973,16 +3187,19 @@
- 1
- 1
+ 0.7
+ 150
0.035
0
0 0 1
-
+ 1
+
+ 1
+ 0
rear_right_flipper_wheel2
@@ -3014,7 +3231,7 @@
0.010941
-
+
0 0 0 1.5707963267948966 -0 0
@@ -3031,16 +3248,19 @@
- 1
- 1
+ 0.7
+ 150
0.035
0
0 0 1
-
+ 1
+
+ 1
+ 0
rear_right_flipper_wheel3
@@ -3072,7 +3292,7 @@
0.010941
-
+
0 0 0 1.5707963267948966 -0 0
@@ -3089,16 +3309,19 @@
- 1
- 1
+ 0.7
+ 150
0.035
0
0 0 1
-
+ 1
+
+ 1
+ 0
rear_right_flipper_wheel4
@@ -3130,7 +3353,7 @@
0.010941
-
+
0 0 0 1.5707963267948966 -0 0
@@ -3147,16 +3370,19 @@
- 1
- 1
+ 0.7
+ 150
0.035
0
0 0 1
-
+ 1
+
+ 1
+ 0
rear_right_flipper_wheel5
@@ -3205,16 +3431,18 @@
- 1
- 1
+ 0.7
+ 150
0.035
0
0 0 1
-
+ 1
+
+ 0
right_track_wheel1
@@ -3263,16 +3491,18 @@
- 1
- 1
+ 0.7
+ 150
0.035
0
0 0 1
-
+ 1
+
+ 0
right_track_wheel2
@@ -3321,16 +3551,18 @@
- 1
- 1
+ 0.7
+ 150
0.035
0
0 0 1
-
+ 1
+
+ 0
right_track_wheel3
@@ -3379,16 +3611,18 @@
- 1
- 1
+ 0.7
+ 150
0.035
0
0 0 1
-
+ 1
+
+ 0
right_track_wheel4
@@ -3437,16 +3671,18 @@
- 1
- 1
+ 0.7
+ 150
0.035
0
0 0 1
-
+ 1
+
+ 0
right_track_wheel5
@@ -3495,16 +3731,18 @@
- 1
- 1
+ 0.7
+ 150
0.035
0
0 0 1
-
+ 1
+
+ 0
right_track_wheel6
@@ -3553,16 +3791,18 @@
- 1
- 1
+ 0.7
+ 150
0.035
0
0 0 1
-
+ 1
+
+ 0
right_track_wheel7
@@ -3611,16 +3851,18 @@
- 1
- 1
+ 0.7
+ 150
0.035
0
0 0 1
-
+ 1
+
+ 0
right_track_wheel8
@@ -3638,41 +3880,18 @@
0
+ 0
front_left_flipper_j
-
+
front_right_flipper_j
- 1
- 30.0
- 0.1
- 0.01
- 10
- -10
- 100
- -100
-
+
rear_left_flipper_j
- 1
- 30.0
- 0.1
- 0.01
- 10
- -10
- 100
- -100
-
+
rear_right_flipper_j
- 1
- 30.0
- 0.1
- 0.01
- 10
- -10
- 100
- -100
laser_j
diff --git a/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/src/flipper_control_plugin.cpp b/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/src/flipper_control_plugin.cpp
index e4301ca7..2fa1b239 100644
--- a/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/src/flipper_control_plugin.cpp
+++ b/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/src/flipper_control_plugin.cpp
@@ -35,7 +35,7 @@ namespace cras
///
/// `{topic_vel}` (`ignition::msgs::Double`): The desired rotation velocity of the flipper.
/// `{topic_pos}` (`ignition::msgs::Double`): The positional setpoint of the flipper.
-class FlipperControlPlugin : public System, public ISystemConfigure, public ISystemPreUpdate, public ISystemPostUpdate
+class FlipperControlPlugin : public System, public ISystemConfigure, public ISystemPreUpdate
{
public: void Configure(const Entity& _entity, const std::shared_ptr& _sdf,
EntityComponentManager& _ecm, EventManager& _eventMgr) override
@@ -69,13 +69,18 @@ class FlipperControlPlugin : public System, public ISystemConfigure, public ISys
topicVel = _sdf->Get("topic_vel");
this->node.Subscribe(topicVel, &FlipperControlPlugin::OnCmdVel, this);
- std::string topicPos {"/model/" + this->model.Name(_ecm) + "/joint/" + this->jointName + "/cmd_pos"};
- if (_sdf->HasElement("topic_pos"))
- topicPos = _sdf->Get("topic_pos");
- this->node.Subscribe(topicPos, &FlipperControlPlugin::OnCmdPos, this);
+ std::string topicPosAbs {"/model/" + this->model.Name(_ecm) + "/joint/" + this->jointName + "/cmd_pos"};
+ if (_sdf->HasElement("topic_pos_abs"))
+ topicPosAbs = _sdf->Get("topic_pos_abs");
+ this->node.Subscribe(topicPosAbs, &FlipperControlPlugin::OnCmdPosAbs, this);
+
+ std::string topicPosRel {"/model/" + this->model.Name(_ecm) + "/joint/" + this->jointName + "/cmd_pos_rel"};
+ if (_sdf->HasElement("topic_pos_rel"))
+ topicPosRel = _sdf->Get("topic_pos_rel");
+ this->node.Subscribe(topicPosRel, &FlipperControlPlugin::OnCmdPosRel, this);
ignmsg << "FlipperControlPlugin subscribing to cmd_vel messages on [" << topicVel << "] and cmd_pos messages on ["
- << topicPos << "]" << std::endl;
+ << topicPosAbs << "] and cmd_pos_rel messages on [" << topicPosRel << "]" << std::endl;
}
public: void PreUpdate(const UpdateInfo& _info, EntityComponentManager& _ecm) override
@@ -102,75 +107,63 @@ class FlipperControlPlugin : public System, public ISystemConfigure, public ISys
auto pos = _ecm.Component(this->joint);
if (!pos)
- _ecm.CreateComponent(this->joint, components::JointPosition());
-
- auto vel = _ecm.Component(this->joint);
- if (!vel)
{
- _ecm.CreateComponent(this->joint, components::JointVelocityCmd({this->angularSpeed}));
- }
- else
- {
- *vel = components::JointVelocityCmd({this->angularSpeed});
+ _ecm.CreateComponent(this->joint, components::JointPosition());
+ pos = _ecm.Component(this->joint);
}
- if (this->angularSpeed == 0.0)
- this->correctStaticAnglePosition(this->joint, this->staticAngle, _ecm);
- }
-
- public: void PostUpdate(const UpdateInfo& _info, const EntityComponentManager& _ecm) override
- {
if (this->cmdVel.has_value())
{
const auto velocity = this->cmdVel.value();
- if (this->joint != kNullEntity) {
- if (this->angularSpeed != 0.0 && velocity == 0.0)
- {
- auto pos = _ecm.Component(this->joint);
- if (pos)
- {
- this->staticAngle = pos->Data()[0];
- }
- }
- this->angularSpeed = velocity;
+ this->staticAngle.reset();
+ if (this->angularSpeed != 0.0 && velocity == 0.0)
+ {
+ this->staticAngle = pos->Data()[0];
}
+ this->angularSpeed = velocity;
this->cmdVel.reset();
- } else if (this->cmdPos.has_value()) {
- const auto position = this->cmdPos.value();
- if (this->joint != kNullEntity) {
- this->staticAngle = position;
- }
- this->cmdPos.reset();
+ } else if (this->cmdPosAbs.has_value()) {
+ const auto position = this->cmdPosAbs.value();
+ this->staticAngle = position;
+ this->cmdPosAbs.reset();
+ } else if (this->cmdPosRel.has_value()) {
+ const auto position = pos->Data()[0] + this->cmdPosRel.value();
+ this->staticAngle = position;
+ this->cmdPosRel.reset();
}
if (this->cmdTorque.has_value()) {
this->UpdateMaxTorque(this->cmdTorque.value(), _ecm);
this->cmdTorque.reset();
}
+
+ auto velocityCommand = this->angularSpeed;
+ if (this->staticAngle.has_value())
+ velocityCommand = this->correctStaticAnglePosition(this->joint, this->staticAngle.value(), _ecm);
+
+ auto vel = _ecm.Component(this->joint);
+ if (!vel)
+ {
+ _ecm.CreateComponent(this->joint, components::JointVelocityCmd({velocityCommand}));
+ }
+ else
+ {
+ *vel = components::JointVelocityCmd({velocityCommand});
+ }
}
// To mitigate integrating small velocity errors, if the flipper is said to be stationary, we check that its position
// does not drift over time.
- protected: void correctStaticAnglePosition(const Entity& joint, const math::Angle& staticPos, EntityComponentManager& _ecm) {
+ protected: double correctStaticAnglePosition(const Entity& joint, const math::Angle& staticPos, EntityComponentManager& _ecm) {
auto pos = _ecm.Component(this->joint);
- if (!pos)
- return;
-
const math::Angle currentPos{pos->Data()[0]};
if (fabs((currentPos - staticPos).Degree()) > 1.0)
{
const auto correctingVelocity = this->positionCorrectionGain * (staticPos - currentPos).Radian();
const auto sanitizedVelocity = math::clamp(correctingVelocity, -this->maxAngularVelocity, this->maxAngularVelocity);
- auto vel = _ecm.Component(this->joint);
- if (!vel)
- {
- _ecm.CreateComponent(this->joint, components::JointVelocityCmd({sanitizedVelocity}));
- }
- else
- {
- *vel = components::JointVelocityCmd({sanitizedVelocity});
- }
+ return sanitizedVelocity;
}
+ return 0.0;
}
protected: void UpdateMaxTorque(const double maxTorque, const EntityComponentManager& _ecm)
@@ -188,15 +181,20 @@ class FlipperControlPlugin : public System, public ISystemConfigure, public ISys
this->cmdVel = _msg.data();
}
- public: void OnCmdPos(const msgs::Double &_msg)
+ public: void OnCmdPosAbs(const msgs::Double &_msg)
+ {
+ this->cmdPosAbs = _msg.data();
+ }
+
+ public: void OnCmdPosRel(const msgs::Double &_msg)
{
- this->cmdPos = _msg.data();
+ this->cmdPosRel = _msg.data();
}
public: void Reset(EntityComponentManager& _ecm)
{
this->angularSpeed = 0;
- this->staticAngle = ignition::math::Angle(0);
+ this->staticAngle.reset();
if (this->joint != kNullEntity)
{
@@ -220,13 +218,14 @@ class FlipperControlPlugin : public System, public ISystemConfigure, public ISys
protected: std::string jointName;
protected: Entity joint{kNullEntity};
protected: transport::Node node;
- protected: std::optional cmdPos;
+ protected: std::optional cmdPosAbs;
+ protected: std::optional cmdPosRel;
protected: std::optional cmdVel;
protected: std::optional cmdTorque;
- protected: ignition::math::Angle staticAngle{0.0};
+ protected: std::optional staticAngle{0.0};
protected: double angularSpeed{0.0};
protected: double maxTorque{30.0};
- protected: double positionCorrectionGain{0.2};
+ protected: double positionCorrectionGain{20.0};
protected: double maxAngularVelocity{0.5};
};
@@ -235,7 +234,6 @@ class FlipperControlPlugin : public System, public ISystemConfigure, public ISys
IGNITION_ADD_PLUGIN(cras::FlipperControlPlugin,
System,
ISystemConfigure,
- ISystemPreUpdate,
- ISystemPostUpdate)
+ ISystemPreUpdate)
IGNITION_ADD_PLUGIN_ALIAS(cras::FlipperControlPlugin, "cras::FlipperControlPlugin")
\ No newline at end of file
diff --git a/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/urdf/_d435.xacro b/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/urdf/_d435.xacro
index 39824695..aeb39867 100644
--- a/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/urdf/_d435.xacro
+++ b/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/urdf/_d435.xacro
@@ -56,18 +56,16 @@ aluminum peripherial evaluation case.
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
diff --git a/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/urdf/nifti_robot.xacro b/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/urdf/nifti_robot.xacro
index 622b7931..c3e1e483 100644
--- a/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/urdf/nifti_robot.xacro
+++ b/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/urdf/nifti_robot.xacro
@@ -866,7 +866,7 @@
-
+
@@ -1115,38 +1115,14 @@
front_left_flipper_j
-
+
front_right_flipper_j
- true
- 30.0
- 0.1
- 0.01
- 10
- -10
- 100
- -100
-
+
rear_left_flipper_j
- true
- 30.0
- 0.1
- 0.01
- 10
- -10
- 100
- -100
-
+
rear_right_flipper_j
- true
- 30.0
- 0.1
- 0.01
- 10
- -10
- 100
- -100
@@ -1183,21 +1159,25 @@
false
-
- true
-
-
-
-
- $(arg track_mu)
- $(arg track_mu2)
- 0 1 0
-
-
+
+
+ 1e+07
+ 1
+
+
+
+
+ $(arg track_mu)
+ $(arg track_mu2)
+ 0.035
+ 0
+ 0 0 1
+
+
- 20
+
@@ -1206,54 +1186,69 @@
-
+
-
-
-
-
-
-
+
+
+
+
+
+
+
-
-
-
-
-
+ true
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -1287,7 +1282,6 @@
-
diff --git a/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/urdf/realsense.gazebo.xacro b/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/urdf/realsense.gazebo.xacro
index 33a11077..83ee23cc 100644
--- a/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/urdf/realsense.gazebo.xacro
+++ b/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/urdf/realsense.gazebo.xacro
@@ -42,12 +42,12 @@ This is the Gazebo URDF model for the Intel RealSense D435 camera
RGB_INT8
- 0.01
+ 0.1
100
- 0.1
+ 0.1
10
@@ -62,4 +62,22 @@ This is the Gazebo URDF model for the Intel RealSense D435 camera
0.0 0 0.0 0 0.0 0
+
+
+
+
+ .5 0.5 0.5 1
+ .1 0.1 0.1 1
+
+
+
+
+
+
+
+ .5 0.5 0.5 1
+ .1 0.1 0.1 1
+
+
+
diff --git a/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/urdf/realsense_d435_with_holder.xacro b/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/urdf/realsense_d435_with_holder.xacro
index 5908202e..8d9ceefe 100644
--- a/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/urdf/realsense_d435_with_holder.xacro
+++ b/submitted_models/ctu_cras_norlab_absolem_sensor_config_1/urdf/realsense_d435_with_holder.xacro
@@ -170,7 +170,7 @@
-
+