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 @@ - +