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

Fix hinge2 joint and windows distro #18

Draft
wants to merge 11 commits into
base: master
Choose a base branch
from
Draft
1 change: 1 addition & 0 deletions docs/reference/changelog-r2024.md
Original file line number Diff line number Diff line change
Expand Up @@ -14,3 +14,4 @@ Released on December **th, 2023.
- Fixed length of arrays returned by `getPose()` in Java ([#6556](https://github.com/cyberbotics/webots/pull/6556)).
- Fixed length of arrays returned by `CameraRecognitionObject.getColors()` in Java ([#6564](https://github.com/cyberbotics/webots/pull/6564)).
- Fixed handling of device objects with the same name in the controller API ([#6579](https://github.com/cyberbotics/webots/pull/6579)).
- Fixed `Brake`s added to the second axis of a `Hinge2Joint` being applied to the non-transformed axis ([#6584](https://github.com/cyberbotics/webots/pull/6584)).
1 change: 0 additions & 1 deletion scripts/packaging/files_msys64.txt
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,6 @@
/mingw64/share/qt6/plugins/imageformats/qjpeg.dll
/mingw64/bin/libjpeg-8.dll
/mingw64/share/qt6/plugins/platforms/qwindows.dll
/mingw64/share/qt6/plugins/styles/qwindowsvistastyle.dll
/mingw64/share/qt6/plugins/tls/qcertonlybackend.dll
/mingw64/share/qt6/plugins/tls/qopensslbackend.dll
/mingw64/share/qt6/plugins/tls/qschannelbackend.dll
Expand Down
3 changes: 0 additions & 3 deletions scripts/packaging/publish_release.py
Original file line number Diff line number Diff line change
Expand Up @@ -47,9 +47,6 @@
if now.hour <= 5:
# Publish nightly build with previous day date even if it completes in the morning
now = now - datetime.timedelta(hours=6)
if now.weekday() >= 5:
print('Skipping nightly build for Saturday and Sunday.')
sys.exit(0)

warningMessage = '\nIt might be unstable, for a stable version of Webots, please use the [latest official release]' \
'(https://github.com/cyberbotics/webots/releases/latest).'
Expand Down
4 changes: 3 additions & 1 deletion scripts/packaging/windows_distro.py
Original file line number Diff line number Diff line change
Expand Up @@ -205,7 +205,9 @@ def create_webots_bundle(self, include_commit_file):
else:
INNO_SETUP_HOME = "/C/Program Files (x86)/Inno Setup 6"
print('creating webots_setup.exe (takes long)\n')
subprocess.run([INNO_SETUP_HOME + '/iscc', '-Q', 'webots.iss'])
subprocess.run(
[INNO_SETUP_HOME + "/iscc", "-Q", "webots.iss"]
).check_returncode()

print('Done.')

Expand Down
6 changes: 6 additions & 0 deletions src/webots/maths/WbMathsUtilities.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -107,7 +107,13 @@ int WbMathsUtilities::convexHull(const QVector<WbVector2> &points, QVector<int>
iMin = i;
}

// Ignore false positive warning produced by some versions of GCC.
#pragma GCC diagnostic push
#if __GNUC__ >= 7
#pragma GCC diagnostic ignored "-Wstringop-overflow"
#endif
QVector<int> index(size);
#pragma GCC diagnostic pop
for (i = 0; i < size; ++i)
index[i] = i;

Expand Down
14 changes: 5 additions & 9 deletions src/webots/nodes/WbHinge2Joint.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -197,20 +197,20 @@ void WbHinge2Joint::applyToOdeAxis() {
if (mSpringAndDampingConstantsAxis1On && mSpringAndDampingConstantsAxis2On) {
// axes 0 and 1 of the AMotorAngle are enabled
dJointSetAMotorAxis(mSpringAndDamperMotor, 0, 1, a1.x(), a1.y(), a1.z());
dJointSetAMotorAxis(mSpringAndDamperMotor, 1, 1, a2.x(), a2.y(), a2.z());
dJointSetAMotorAxis(mSpringAndDamperMotor, 1, 2, a2.x(), a2.y(), a2.z());
} else if (mSpringAndDampingConstantsAxis1On) {
// only axis 0 of the AMotorAngle is enabled
dJointSetAMotorAxis(mSpringAndDamperMotor, 0, 1, a1.x(), a1.y(), a1.z());
} else
dJointSetAMotorAxis(mSpringAndDamperMotor, 0, 1, a2.x(), a2.y(), a2.z());
dJointSetAMotorAxis(mSpringAndDamperMotor, 0, 2, a2.x(), a2.y(), a2.z());
}
} else {
parsingWarn(tr("Hinge axes are aligned: using x and z axes instead."));
dJointSetHinge2Axis1(mJoint, 1.0, 0.0, 0.0);
dJointSetHinge2Axis2(mJoint, 0.0, 0.0, 1.0);
if (mSpringAndDamperMotor) {
dJointSetAMotorAxis(mSpringAndDamperMotor, 0, 1, 1.0, 0.0, 0.0);
dJointSetAMotorAxis(mSpringAndDamperMotor, 1, 1, 0.0, 0.0, 1.0);
dJointSetAMotorAxis(mSpringAndDamperMotor, 1, 2, 0.0, 0.0, 1.0);
}
}
}
Expand Down Expand Up @@ -295,12 +295,11 @@ void WbHinge2Joint::applyToOdeSpringAndDampingConstants(dBodyID body, dBodyID pa
dJointSetAMotorNumAxes(mSpringAndDamperMotor, numberOfAxes);
dJointSetAMotorMode(mSpringAndDamperMotor, dAMotorUser);

applyToOdeAxis();

// Axis dependent settings
const WbMatrix4 &m4 = upperPose()->matrix();
if (mSpringAndDampingConstantsAxis1On) {
const double clamped = WbMathsUtilities::normalizeAngle(mOdePositionOffset);
const WbVector3 &a1 = m4.sub3x3MatrixDot(axis());
dJointSetAMotorAxis(mSpringAndDamperMotor, 0, 1, a1.x(), a1.y(), a1.z());
dJointSetAMotorAngle(mSpringAndDamperMotor, 0, 0.0);
dJointSetAMotorParam(mSpringAndDamperMotor, dParamLoStop, clamped);
dJointSetAMotorParam(mSpringAndDamperMotor, dParamHiStop, clamped);
Expand All @@ -310,16 +309,13 @@ void WbHinge2Joint::applyToOdeSpringAndDampingConstants(dBodyID body, dBodyID pa

if (mSpringAndDampingConstantsAxis2On) {
const double clamped2 = WbMathsUtilities::normalizeAngle(mOdePositionOffset2);
const WbVector3 &a2 = m4.sub3x3MatrixDot(axis2());
if (bothAxes) { // axes 0 and 1 of the AMotorAngle are enabled
dJointSetAMotorAxis(mSpringAndDamperMotor, 1, 1, a2.x(), a2.y(), a2.z());
dJointSetAMotorAngle(mSpringAndDamperMotor, 1, 0.0);
dJointSetAMotorParam(mSpringAndDamperMotor, dParamLoStop2, clamped2);
dJointSetAMotorParam(mSpringAndDamperMotor, dParamHiStop2, clamped2);
dJointSetAMotorParam(mSpringAndDamperMotor, dParamStopCFM2, cfm2);
dJointSetAMotorParam(mSpringAndDamperMotor, dParamStopERP2, erp2);
} else { // only axis 0 of the AMotorAngle is enabled
dJointSetAMotorAxis(mSpringAndDamperMotor, 0, 1, a2.x(), a2.y(), a2.z());
dJointSetAMotorAngle(mSpringAndDamperMotor, 0, 0.0);
dJointSetAMotorParam(mSpringAndDamperMotor, dParamLoStop, clamped2);
dJointSetAMotorParam(mSpringAndDamperMotor, dParamHiStop, clamped2);
Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1 @@
/hinge_2_joint_damping_axis
25 changes: 25 additions & 0 deletions tests/physics/controllers/hinge_2_joint_damping_axis/Makefile
Original file line number Diff line number Diff line change
@@ -0,0 +1,25 @@
# Copyright 1996-2023 Cyberbotics Ltd.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# https://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

# Webots Makefile system
#
# You may add some variable definitions hereafter to customize the build process
# See documentation in $(WEBOTS_HOME_PATH)/resources/Makefile.include


# Do not modify the following: this includes Webots global Makefile.include
null :=
space := $(null) $(null)
WEBOTS_HOME_PATH?=$(subst $(space),\ ,$(strip $(subst \,/,$(WEBOTS_HOME))))
include $(WEBOTS_HOME_PATH)/resources/Makefile.include
Original file line number Diff line number Diff line change
@@ -0,0 +1,77 @@
#include <webots/brake.h>
#include <webots/motor.h>
#include <webots/nodes.h>
#include <webots/robot.h>
#include <webots/supervisor.h>

#include "../../../lib/ts_assertion.h"
#include "../../../lib/ts_utils.h"

#include <math.h>
#include <stdio.h>

#define TIME_STEP 32

double angular_speed_of(WbNodeRef node) {
const double *v = wb_supervisor_node_get_velocity(node);
return sqrt(v[3] * v[3] + v[4] * v[4] + v[5] * v[5]);
}

struct HingeJoint2Data {
WbDeviceTag driveMotor;
WbNodeRef shaftNode;
};

void init(struct HingeJoint2Data *jd, char *driveMotorName, char *shaftDef) {
jd->driveMotor = wb_robot_get_device(driveMotorName);
wb_motor_set_position(jd->driveMotor, INFINITY);
wb_motor_set_velocity(jd->driveMotor, 0.0);
wb_brake_set_damping_constant(wb_motor_get_brake(jd->driveMotor), 28.0);
jd->shaftNode = wb_supervisor_node_get_from_def(shaftDef);
}

void assert_same_speed_for_secs(struct HingeJoint2Data j1, struct HingeJoint2Data j2, double durationSecs) {
const double tolerance = 0.001;

double t = 0;
while (wb_robot_step(TIME_STEP) != -1 && t < durationSecs) {
t += (double)TIME_STEP / 1000.0;
double expSpeed = angular_speed_of(j1.shaftNode);
double actSpeed = angular_speed_of(j2.shaftNode);
ts_assert_double_in_delta(actSpeed, expSpeed, tolerance, "Unexpected angle rate (expected = %lf, measured = %lf)", expSpeed,
actSpeed);
printf("angle_rate: %lf expected: %lf\n", actSpeed, expSpeed);
}
}

int main(int argc, char **argv) {
ts_setup(argv[0]);

struct HingeJoint2Data joint1, joint2;

init(&joint1, "joint1motor", "HINGE2JOINT1_SHAFT");
init(&joint2, "joint2motor", "HINGE2JOINT2_SHAFT");

// Rotate the first axis of the second joint by 90 degrees
// (takes about 1 second)
wb_motor_set_position(wb_robot_get_device("joint2turnmotor"), 1.57);
double t = 0.0; // elapsed simulation time
while (wb_robot_step(TIME_STEP) != -1 && t < 1.0) {
t += (double)TIME_STEP / 1000.0;
}

// Accerate toward max velocity for 1 second.
wb_motor_set_velocity(joint1.driveMotor, 6.28);
wb_motor_set_velocity(joint2.driveMotor, 6.28);
assert_same_speed_for_secs(joint1, joint2, 1.0);

// Coast to a stop
wb_motor_set_velocity(joint1.driveMotor, 0.0);
wb_motor_set_available_torque(joint1.driveMotor, 0.0);
wb_motor_set_velocity(joint2.driveMotor, 0.0);
wb_motor_set_available_torque(joint2.driveMotor, 0.0);
assert_same_speed_for_secs(joint1, joint2, 1.0);

ts_send_success();
return EXIT_SUCCESS;
}
159 changes: 159 additions & 0 deletions tests/physics/worlds/hinge_2_joint_damping_axis.wbt
Original file line number Diff line number Diff line change
@@ -0,0 +1,159 @@
#VRML_SIM R2024a utf8

EXTERNPROTO "webots://projects//objects/backgrounds/protos/TexturedBackground.proto"
EXTERNPROTO "webots://projects//objects/backgrounds/protos/TexturedBackgroundLight.proto"
EXTERNPROTO "webots://projects//objects/floors/protos/RectangleArena.proto"
EXTERNPROTO "webots://tests/default/protos/TestSuiteEmitter.proto"
EXTERNPROTO "webots://tests/default/protos/TestSuiteSupervisor.proto"

WorldInfo {
}
Viewpoint {
orientation -0.21603302555552628 0.4161981327199255 0.8832377065036189 1.0896384095552925
position -4.162691183645749 -6.376519530609339 5.213071959373613
}
TexturedBackground {
}
TexturedBackgroundLight {
}
RectangleArena {
floorSize 10 10
}
DEF ROBOT Robot {
translation 0 0 0.06
children [
DEF HINGE2JOINT1 Solid {
translation -1 0 0
rotation 0 1 0 0
children [
DEF HINGE2JOINT_BASE Pose {
children [
Shape {
appearance PBRAppearance {
}
geometry Box {
size 1 1 0.1
}
}
]
}
Hinge2Joint {
jointParameters HingeJointParameters {
axis 0 1 0
anchor 0 0 2
}
jointParameters2 JointParameters {
axis 1 0 0
}
device2 [
Brake {
name "joint1brake"
}
RotationalMotor {
name "joint1motor"
maxVelocity 6.28
maxTorque 137
}
]
endPoint DEF HINGE2JOINT1_SHAFT Solid {
translation 0 0 2
children [
DEF HINGE2JOINT_SHAFT_GEOM Pose {
rotation 0 1 0 1.5701
children [
Shape {
appearance PBRAppearance {
}
geometry Box {
size 1 1 0.215
}
}
]
}
]
boundingObject USE HINGE2JOINT_SHAFT_GEOM
physics Physics {
}
}
}
]
name "solid(3)"
boundingObject USE HINGE2JOINT_BASE
physics Physics {
}
}
DEF HINGE2JOINT2 Solid {
translation 1 0 0
rotation 0 1 0 0
children [
DEF HINGE2JOINT_BASE Pose {
children [
Shape {
appearance PBRAppearance {
}
geometry Box {
size 1 1 0.1
}
}
]
}
Hinge2Joint {
jointParameters HingeJointParameters {
axis 0 1 0
anchor 0 0 2
}
jointParameters2 JointParameters {
axis 1 0 0
}
device [
RotationalMotor {
name "joint2turnmotor"
maxPosition 1.5707
maxTorque 10000
}
]
device2 [
Brake {
name "joint2brake"
}
RotationalMotor {
name "joint2motor"
maxVelocity 6.28
maxTorque 137
}
]
endPoint DEF HINGE2JOINT2_SHAFT Solid {
translation 0 0 2
children [
DEF HINGE2JOINT_SHAFT_GEOM Pose {
rotation 0 1 0 1.5701
children [
Shape {
appearance PBRAppearance {
}
geometry Box {
size 1 1 0.215
}
}
]
}
]
boundingObject USE HINGE2JOINT_SHAFT_GEOM
physics Physics {
}
}
}
]
name "solid(1)"
boundingObject USE HINGE2JOINT_BASE
physics Physics {
}
}
TestSuiteEmitter {
}
]
controller "hinge_2_joint_damping_axis"
supervisor TRUE
}
TestSuiteSupervisor {
}
Loading