Skip to content

Commit bd575b4

Browse files
brettlebrettle
committed
Add test case to test suite.
1 parent 673558a commit bd575b4

4 files changed

Lines changed: 262 additions & 0 deletions

File tree

‎tests/physics/controllers/hinge_2_joint_damping_axis/.gitignore‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1 @@
1+
/hinge_2_joint_damping_axis

‎tests/physics/controllers/hinge_2_joint_damping_axis/Makefile‎

Lines changed: 25 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,25 @@
1+
# Copyright 1996-2023 Cyberbotics Ltd.
2+
#
3+
# Licensed under the Apache License, Version 2.0 (the "License");
4+
# you may not use this file except in compliance with the License.
5+
# You may obtain a copy of the License at
6+
#
7+
# https://www.apache.org/licenses/LICENSE-2.0
8+
#
9+
# Unless required by applicable law or agreed to in writing, software
10+
# distributed under the License is distributed on an "AS IS" BASIS,
11+
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
# See the License for the specific language governing permissions and
13+
# limitations under the License.
14+
15+
# Webots Makefile system
16+
#
17+
# You may add some variable definitions hereafter to customize the build process
18+
# See documentation in $(WEBOTS_HOME_PATH)/resources/Makefile.include
19+
20+
21+
# Do not modify the following: this includes Webots global Makefile.include
22+
null :=
23+
space := $(null) $(null)
24+
WEBOTS_HOME_PATH?=$(subst $(space),\ ,$(strip $(subst \,/,$(WEBOTS_HOME))))
25+
include $(WEBOTS_HOME_PATH)/resources/Makefile.include

‎tests/physics/controllers/hinge_2_joint_damping_axis/hinge_2_joint_damping_axis.c‎

Lines changed: 77 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,77 @@
1+
#include <webots/brake.h>
2+
#include <webots/motor.h>
3+
#include <webots/nodes.h>
4+
#include <webots/robot.h>
5+
#include <webots/supervisor.h>
6+
7+
#include "../../../lib/ts_assertion.h"
8+
#include "../../../lib/ts_utils.h"
9+
10+
#include <math.h>
11+
#include <stdio.h>
12+
13+
#define TIME_STEP 32
14+
15+
double angular_speed_of(WbNodeRef node) {
16+
const double *v = wb_supervisor_node_get_velocity(node);
17+
return sqrt(v[3] * v[3] + v[4] * v[4] + v[5] * v[5]);
18+
}
19+
20+
struct HingeJoint2Data {
21+
WbDeviceTag driveMotor;
22+
WbNodeRef shaftNode;
23+
};
24+
25+
void init(struct HingeJoint2Data *jd, char *driveMotorName, char *shaftDef) {
26+
jd->driveMotor = wb_robot_get_device(driveMotorName);
27+
wb_motor_set_position(jd->driveMotor, INFINITY);
28+
wb_motor_set_velocity(jd->driveMotor, 0.0);
29+
wb_brake_set_damping_constant(wb_motor_get_brake(jd->driveMotor), 28.0);
30+
jd->shaftNode = wb_supervisor_node_get_from_def(shaftDef);
31+
}
32+
33+
void assert_same_speed_for_secs(struct HingeJoint2Data j1, struct HingeJoint2Data j2, double durationSecs) {
34+
const double tolerance = 0.001;
35+
36+
double t = 0;
37+
while (wb_robot_step(TIME_STEP) != -1 && t < durationSecs) {
38+
t += (double)TIME_STEP / 1000.0;
39+
double expSpeed = angular_speed_of(j1.shaftNode);
40+
double actSpeed = angular_speed_of(j2.shaftNode);
41+
ts_assert_double_in_delta(actSpeed, expSpeed, tolerance, "Unexpected angle rate (expected = %lf, measured = %lf)", expSpeed,
42+
actSpeed);
43+
printf("angle_rate: %lf expected: %lf\n", actSpeed, expSpeed);
44+
}
45+
}
46+
47+
int main(int argc, char **argv) {
48+
ts_setup(argv[0]);
49+
50+
struct HingeJoint2Data joint1, joint2;
51+
52+
init(&joint1, "joint1motor", "HINGE2JOINT1_SHAFT");
53+
init(&joint2, "joint2motor", "HINGE2JOINT2_SHAFT");
54+
55+
// Rotate the first axis of the second joint by 90 degrees
56+
// (takes about 1 second)
57+
wb_motor_set_position(wb_robot_get_device("joint2turnmotor"), 1.57);
58+
double t = 0.0; // elapsed simulation time
59+
while (wb_robot_step(TIME_STEP) != -1 && t < 1.0) {
60+
t += (double)TIME_STEP / 1000.0;
61+
}
62+
63+
// Accerate toward max velocity for 1 second.
64+
wb_motor_set_velocity(joint1.driveMotor, 6.28);
65+
wb_motor_set_velocity(joint2.driveMotor, 6.28);
66+
assert_same_speed_for_secs(joint1, joint2, 1.0);
67+
68+
// Coast to a stop
69+
wb_motor_set_velocity(joint1.driveMotor, 0.0);
70+
wb_motor_set_available_torque(joint1.driveMotor, 0.0);
71+
wb_motor_set_velocity(joint2.driveMotor, 0.0);
72+
wb_motor_set_available_torque(joint2.driveMotor, 0.0);
73+
assert_same_speed_for_secs(joint1, joint2, 1.0);
74+
75+
ts_send_success();
76+
return EXIT_SUCCESS;
77+
}

‎tests/physics/worlds/hinge_2_joint_damping_axis.wbt‎

Lines changed: 159 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,159 @@
1+
#VRML_SIM R2024a utf8
2+
3+
EXTERNPROTO "webots://projects//objects/backgrounds/protos/TexturedBackground.proto"
4+
EXTERNPROTO "webots://projects//objects/backgrounds/protos/TexturedBackgroundLight.proto"
5+
EXTERNPROTO "webots://projects//objects/floors/protos/RectangleArena.proto"
6+
EXTERNPROTO "webots://tests/default/protos/TestSuiteEmitter.proto"
7+
EXTERNPROTO "webots://tests/default/protos/TestSuiteSupervisor.proto"
8+
9+
WorldInfo {
10+
}
11+
Viewpoint {
12+
orientation -0.21603302555552628 0.4161981327199255 0.8832377065036189 1.0896384095552925
13+
position -4.162691183645749 -6.376519530609339 5.213071959373613
14+
}
15+
TexturedBackground {
16+
}
17+
TexturedBackgroundLight {
18+
}
19+
RectangleArena {
20+
floorSize 10 10
21+
}
22+
DEF ROBOT Robot {
23+
translation 0 0 0.06
24+
children [
25+
DEF HINGE2JOINT1 Solid {
26+
translation -1 0 0
27+
rotation 0 1 0 0
28+
children [
29+
DEF HINGE2JOINT_BASE Pose {
30+
children [
31+
Shape {
32+
appearance PBRAppearance {
33+
}
34+
geometry Box {
35+
size 1 1 0.1
36+
}
37+
}
38+
]
39+
}
40+
Hinge2Joint {
41+
jointParameters HingeJointParameters {
42+
axis 0 1 0
43+
anchor 0 0 2
44+
}
45+
jointParameters2 JointParameters {
46+
axis 1 0 0
47+
}
48+
device2 [
49+
Brake {
50+
name "joint1brake"
51+
}
52+
RotationalMotor {
53+
name "joint1motor"
54+
maxVelocity 6.28
55+
maxTorque 137
56+
}
57+
]
58+
endPoint DEF HINGE2JOINT1_SHAFT Solid {
59+
translation 0 0 2
60+
children [
61+
DEF HINGE2JOINT_SHAFT_GEOM Pose {
62+
rotation 0 1 0 1.5701
63+
children [
64+
Shape {
65+
appearance PBRAppearance {
66+
}
67+
geometry Box {
68+
size 1 1 0.215
69+
}
70+
}
71+
]
72+
}
73+
]
74+
boundingObject USE HINGE2JOINT_SHAFT_GEOM
75+
physics Physics {
76+
}
77+
}
78+
}
79+
]
80+
name "solid(3)"
81+
boundingObject USE HINGE2JOINT_BASE
82+
physics Physics {
83+
}
84+
}
85+
DEF HINGE2JOINT2 Solid {
86+
translation 1 0 0
87+
rotation 0 1 0 0
88+
children [
89+
DEF HINGE2JOINT_BASE Pose {
90+
children [
91+
Shape {
92+
appearance PBRAppearance {
93+
}
94+
geometry Box {
95+
size 1 1 0.1
96+
}
97+
}
98+
]
99+
}
100+
Hinge2Joint {
101+
jointParameters HingeJointParameters {
102+
axis 0 1 0
103+
anchor 0 0 2
104+
}
105+
jointParameters2 JointParameters {
106+
axis 1 0 0
107+
}
108+
device [
109+
RotationalMotor {
110+
name "joint2turnmotor"
111+
maxPosition 1.5707
112+
maxTorque 10000
113+
}
114+
]
115+
device2 [
116+
Brake {
117+
name "joint2brake"
118+
}
119+
RotationalMotor {
120+
name "joint2motor"
121+
maxVelocity 6.28
122+
maxTorque 137
123+
}
124+
]
125+
endPoint DEF HINGE2JOINT2_SHAFT Solid {
126+
translation 0 0 2
127+
children [
128+
DEF HINGE2JOINT_SHAFT_GEOM Pose {
129+
rotation 0 1 0 1.5701
130+
children [
131+
Shape {
132+
appearance PBRAppearance {
133+
}
134+
geometry Box {
135+
size 1 1 0.215
136+
}
137+
}
138+
]
139+
}
140+
]
141+
boundingObject USE HINGE2JOINT_SHAFT_GEOM
142+
physics Physics {
143+
}
144+
}
145+
}
146+
]
147+
name "solid(1)"
148+
boundingObject USE HINGE2JOINT_BASE
149+
physics Physics {
150+
}
151+
}
152+
TestSuiteEmitter {
153+
}
154+
]
155+
controller "hinge_2_joint_damping_axis"
156+
supervisor TRUE
157+
}
158+
TestSuiteSupervisor {
159+
}

0 commit comments

Comments
 (0)