From 93e497e080f395a9f9e08bcb04706848a7f98fa1 Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Tue, 13 Jun 2023 18:27:51 +0530 Subject: [PATCH 01/88] Copy sdf 1.10 to 1.11 Signed-off-by: Jasmeet Singh --- sdf/1.11/1_9.convert | 2 + sdf/1.11/CMakeLists.txt | 88 +++++++++++ sdf/1.11/actor.sdf | 86 ++++++++++ sdf/1.11/air_pressure.sdf | 15 ++ sdf/1.11/air_speed.sdf | 11 ++ sdf/1.11/altimeter.sdf | 18 +++ sdf/1.11/atmosphere.sdf | 21 +++ sdf/1.11/audio_sink.sdf | 4 + sdf/1.11/audio_source.sdf | 30 ++++ sdf/1.11/battery.sdf | 12 ++ sdf/1.11/box_shape.sdf | 6 + sdf/1.11/camera.sdf | 230 +++++++++++++++++++++++++++ sdf/1.11/capsule_shape.sdf | 9 ++ sdf/1.11/collision.sdf | 22 +++ sdf/1.11/contact.sdf | 12 ++ sdf/1.11/cylinder_shape.sdf | 9 ++ sdf/1.11/ellipsoid_shape.sdf | 6 + sdf/1.11/forcetorque.sdf | 54 +++++++ sdf/1.11/frame.sdf | 35 +++++ sdf/1.11/geometry.sdf | 20 +++ sdf/1.11/gps.sdf | 40 +++++ sdf/1.11/gripper.sdf | 30 ++++ sdf/1.11/gui.sdf | 60 +++++++ sdf/1.11/heightmap_shape.sdf | 44 ++++++ sdf/1.11/image_shape.sdf | 18 +++ sdf/1.11/imu.sdf | 120 ++++++++++++++ sdf/1.11/inertial.sdf | 209 ++++++++++++++++++++++++ sdf/1.11/joint.sdf | 240 ++++++++++++++++++++++++++++ sdf/1.11/lidar.sdf | 78 +++++++++ sdf/1.11/light.sdf | 67 ++++++++ sdf/1.11/light_state.sdf | 10 ++ sdf/1.11/link.sdf | 51 ++++++ sdf/1.11/link_state.sdf | 40 +++++ sdf/1.11/logical_camera.sdf | 19 +++ sdf/1.11/magnetometer.sdf | 21 +++ sdf/1.11/material.sdf | 166 +++++++++++++++++++ sdf/1.11/mesh_shape.sdf | 20 +++ sdf/1.11/model.sdf | 92 +++++++++++ sdf/1.11/model_state.sdf | 41 +++++ sdf/1.11/navsat.sdf | 40 +++++ sdf/1.11/noise.sdf | 43 +++++ sdf/1.11/particle_emitter.sdf | 120 ++++++++++++++ sdf/1.11/physics.sdf | 238 ++++++++++++++++++++++++++++ sdf/1.11/plane_shape.sdf | 9 ++ sdf/1.11/plugin.sdf | 13 ++ sdf/1.11/polyline_shape.sdf | 14 ++ sdf/1.11/population.sdf | 58 +++++++ sdf/1.11/pose.sdf | 43 +++++ sdf/1.11/projector.sdf | 28 ++++ sdf/1.11/ray.sdf | 78 +++++++++ sdf/1.11/rfid.sdf | 2 + sdf/1.11/rfidtag.sdf | 2 + sdf/1.11/road.sdf | 16 ++ sdf/1.11/root.sdf | 21 +++ sdf/1.11/scene.sdf | 86 ++++++++++ sdf/1.11/schema/types.xsd | 46 ++++++ sdf/1.11/sensor.sdf | 82 ++++++++++ sdf/1.11/sonar.sdf | 16 ++ sdf/1.11/sphere_shape.sdf | 6 + sdf/1.11/spherical_coordinates.sdf | 65 ++++++++ sdf/1.11/state.sdf | 41 +++++ sdf/1.11/surface.sdf | 245 +++++++++++++++++++++++++++++ sdf/1.11/transceiver.sdf | 34 ++++ sdf/1.11/visual.sdf | 38 +++++ sdf/1.11/world.sdf | 72 +++++++++ 65 files changed, 3512 insertions(+) create mode 100644 sdf/1.11/1_9.convert create mode 100644 sdf/1.11/CMakeLists.txt create mode 100644 sdf/1.11/actor.sdf create mode 100644 sdf/1.11/air_pressure.sdf create mode 100644 sdf/1.11/air_speed.sdf create mode 100644 sdf/1.11/altimeter.sdf create mode 100644 sdf/1.11/atmosphere.sdf create mode 100644 sdf/1.11/audio_sink.sdf create mode 100644 sdf/1.11/audio_source.sdf create mode 100644 sdf/1.11/battery.sdf create mode 100644 sdf/1.11/box_shape.sdf create mode 100644 sdf/1.11/camera.sdf create mode 100644 sdf/1.11/capsule_shape.sdf create mode 100644 sdf/1.11/collision.sdf create mode 100644 sdf/1.11/contact.sdf create mode 100644 sdf/1.11/cylinder_shape.sdf create mode 100644 sdf/1.11/ellipsoid_shape.sdf create mode 100644 sdf/1.11/forcetorque.sdf create mode 100644 sdf/1.11/frame.sdf create mode 100644 sdf/1.11/geometry.sdf create mode 100644 sdf/1.11/gps.sdf create mode 100644 sdf/1.11/gripper.sdf create mode 100644 sdf/1.11/gui.sdf create mode 100644 sdf/1.11/heightmap_shape.sdf create mode 100644 sdf/1.11/image_shape.sdf create mode 100644 sdf/1.11/imu.sdf create mode 100644 sdf/1.11/inertial.sdf create mode 100644 sdf/1.11/joint.sdf create mode 100644 sdf/1.11/lidar.sdf create mode 100644 sdf/1.11/light.sdf create mode 100644 sdf/1.11/light_state.sdf create mode 100644 sdf/1.11/link.sdf create mode 100644 sdf/1.11/link_state.sdf create mode 100644 sdf/1.11/logical_camera.sdf create mode 100644 sdf/1.11/magnetometer.sdf create mode 100644 sdf/1.11/material.sdf create mode 100644 sdf/1.11/mesh_shape.sdf create mode 100644 sdf/1.11/model.sdf create mode 100644 sdf/1.11/model_state.sdf create mode 100644 sdf/1.11/navsat.sdf create mode 100644 sdf/1.11/noise.sdf create mode 100755 sdf/1.11/particle_emitter.sdf create mode 100644 sdf/1.11/physics.sdf create mode 100644 sdf/1.11/plane_shape.sdf create mode 100644 sdf/1.11/plugin.sdf create mode 100644 sdf/1.11/polyline_shape.sdf create mode 100644 sdf/1.11/population.sdf create mode 100644 sdf/1.11/pose.sdf create mode 100644 sdf/1.11/projector.sdf create mode 100644 sdf/1.11/ray.sdf create mode 100644 sdf/1.11/rfid.sdf create mode 100644 sdf/1.11/rfidtag.sdf create mode 100644 sdf/1.11/road.sdf create mode 100644 sdf/1.11/root.sdf create mode 100644 sdf/1.11/scene.sdf create mode 100644 sdf/1.11/schema/types.xsd create mode 100644 sdf/1.11/sensor.sdf create mode 100644 sdf/1.11/sonar.sdf create mode 100644 sdf/1.11/sphere_shape.sdf create mode 100644 sdf/1.11/spherical_coordinates.sdf create mode 100644 sdf/1.11/state.sdf create mode 100644 sdf/1.11/surface.sdf create mode 100644 sdf/1.11/transceiver.sdf create mode 100644 sdf/1.11/visual.sdf create mode 100644 sdf/1.11/world.sdf diff --git a/sdf/1.11/1_9.convert b/sdf/1.11/1_9.convert new file mode 100644 index 000000000..380267d62 --- /dev/null +++ b/sdf/1.11/1_9.convert @@ -0,0 +1,2 @@ + + diff --git a/sdf/1.11/CMakeLists.txt b/sdf/1.11/CMakeLists.txt new file mode 100644 index 000000000..531202e38 --- /dev/null +++ b/sdf/1.11/CMakeLists.txt @@ -0,0 +1,88 @@ +set (sdfs + actor.sdf + air_pressure.sdf + air_speed.sdf + altimeter.sdf + atmosphere.sdf + audio_source.sdf + audio_sink.sdf + battery.sdf + box_shape.sdf + camera.sdf + capsule_shape.sdf + collision.sdf + contact.sdf + cylinder_shape.sdf + ellipsoid_shape.sdf + frame.sdf + forcetorque.sdf + geometry.sdf + gps.sdf + gripper.sdf + gui.sdf + heightmap_shape.sdf + image_shape.sdf + imu.sdf + inertial.sdf + joint.sdf + lidar.sdf + light.sdf + light_state.sdf + link.sdf + link_state.sdf + logical_camera.sdf + magnetometer.sdf + material.sdf + mesh_shape.sdf + model.sdf + model_state.sdf + navsat.sdf + noise.sdf + particle_emitter.sdf + physics.sdf + plane_shape.sdf + plugin.sdf + polyline_shape.sdf + population.sdf + pose.sdf + projector.sdf + ray.sdf + rfidtag.sdf + rfid.sdf + road.sdf + root.sdf + scene.sdf + sensor.sdf + spherical_coordinates.sdf + sphere_shape.sdf + sonar.sdf + state.sdf + surface.sdf + transceiver.sdf + visual.sdf + world.sdf +) + +set (SDF_SCHEMA) + +foreach(FIL ${sdfs}) + get_filename_component(ABS_FIL ${FIL} ABSOLUTE) + get_filename_component(FIL_WE ${FIL} NAME_WE) + + list(APPEND SDF_SCHEMA "${CMAKE_CURRENT_BINARY_DIR}/${FIL_WE}.xsd") + + add_custom_command( + OUTPUT "${CMAKE_CURRENT_BINARY_DIR}/${FIL_WE}.xsd" + COMMAND ${RUBY} ${CMAKE_SOURCE_DIR}/tools/xmlschema.rb + ARGS -s ${CMAKE_CURRENT_SOURCE_DIR} -i ${ABS_FIL} -o ${CMAKE_CURRENT_BINARY_DIR} + DEPENDS ${ABS_FIL} + COMMENT "Running xml schema compiler on ${FIL}" + VERBATIM) +endforeach() + +add_custom_target(schema1_10 ALL DEPENDS ${SDF_SCHEMA}) + +set_source_files_properties(${SDF_SCHEMA} PROPERTIES GENERATED TRUE) + +install(FILES 1_9.convert ${sdfs} DESTINATION ${CMAKE_INSTALL_FULL_DATAROOTDIR}/sdformat${PROJECT_VERSION_MAJOR}/1.10) +install(FILES ${SDF_SCHEMA} DESTINATION ${CMAKE_INSTALL_FULL_DATAROOTDIR}/sdformat${PROJECT_VERSION_MAJOR}/1.10) diff --git a/sdf/1.11/actor.sdf b/sdf/1.11/actor.sdf new file mode 100644 index 000000000..4ff1c4cc6 --- /dev/null +++ b/sdf/1.11/actor.sdf @@ -0,0 +1,86 @@ + + + A special kind of model which can have a scripted motion. This includes both global waypoint type animations and skeleton animations. + + + A unique name for the actor. + + + + + + Skin file which defines a visual and the underlying skeleton which moves it. + + + Path to skin file, accepted formats: COLLADA, BVH. + + + + Scale the skin's size. + + + + + Animation file defines an animation for the skeleton in the skin. The skeleton must be compatible with the skin skeleton. + + + Unique name for animation. + + + + Path to animation file. Accepted formats: COLLADA, BVH. + + + Scale for the animation skeleton. + + + Set to true so the animation is interpolated on X. + + + + + Adds scripted trajectories to the actor. + + + Set this to true for the script to be repeated in a loop. For a fluid continuous motion, make sure the last waypoint matches the first one. + + + + This is the time to wait before starting the script. If running in a loop, this time will be waited before starting each cycle. + + + + Set to true if the animation should start as soon as the simulation starts playing. It is useful to set this to false if the animation should only start playing only when triggered by a plugin, for example. + + + + The trajectory contains a series of keyframes to be followed. + + Unique id for a trajectory. + + + + If it matches the type of an animation, they will be played at the same time. + + + + The tension of the trajectory spline. The default value of zero equates to a Catmull-Rom spline, which may also cause the animation to overshoot keyframes. A value of one will cause the animation to stick to the keyframes. + + + + Each point in the trajectory. + + The time in seconds, counted from the beginning of the script, when the pose should be reached. + + + The pose which should be reached at the given time. + + + + + + + + + + diff --git a/sdf/1.11/air_pressure.sdf b/sdf/1.11/air_pressure.sdf new file mode 100644 index 000000000..07fe5b564 --- /dev/null +++ b/sdf/1.11/air_pressure.sdf @@ -0,0 +1,15 @@ + + These elements are specific to an air pressure sensor. + + + The initial altitude in meters. This value can be used by a sensor implementation to augment the altitude of the sensor. For example, if you are using simulation instead of creating a 1000 m mountain model on which to place your sensor, you could instead set this value to 1000 and place your model on a ground plane with a Z height of zero. + + + + + Noise parameters for the pressure data. + + + + + diff --git a/sdf/1.11/air_speed.sdf b/sdf/1.11/air_speed.sdf new file mode 100644 index 000000000..1313f705a --- /dev/null +++ b/sdf/1.11/air_speed.sdf @@ -0,0 +1,11 @@ + + These elements are specific to an air speed sensor. This sensor determines speed based on the differential between static and dynamic pressure. + + + + Noise parameters for the pressure data. + + + + + diff --git a/sdf/1.11/altimeter.sdf b/sdf/1.11/altimeter.sdf new file mode 100644 index 000000000..66ddee579 --- /dev/null +++ b/sdf/1.11/altimeter.sdf @@ -0,0 +1,18 @@ + + These elements are specific to an altimeter sensor. + + + + Noise parameters for vertical position + + + + + + + Noise parameters for vertical velocity + + + + + diff --git a/sdf/1.11/atmosphere.sdf b/sdf/1.11/atmosphere.sdf new file mode 100644 index 000000000..641faccb9 --- /dev/null +++ b/sdf/1.11/atmosphere.sdf @@ -0,0 +1,21 @@ + + + The atmosphere tag specifies the type and properties of the atmosphere model. + + + The type of the atmosphere engine. Current options are adiabatic. Defaults to adiabatic if left unspecified. + + + + Temperature at sea level in kelvins. + + + + Pressure at sea level in pascals. + + + + Temperature gradient with respect to increasing altitude at sea level in units of K/m. + + + diff --git a/sdf/1.11/audio_sink.sdf b/sdf/1.11/audio_sink.sdf new file mode 100644 index 000000000..d3bd0711d --- /dev/null +++ b/sdf/1.11/audio_sink.sdf @@ -0,0 +1,4 @@ + + + An audio sink. + diff --git a/sdf/1.11/audio_source.sdf b/sdf/1.11/audio_source.sdf new file mode 100644 index 000000000..b6ee6069a --- /dev/null +++ b/sdf/1.11/audio_source.sdf @@ -0,0 +1,30 @@ + + + An audio source. + + + URI of the audio media. + + + + Pitch for the audio media, in Hz + + + + Gain for the audio media, in dB. + + + + List of collision objects that will trigger audio playback. + + Name of child collision element that will trigger audio playback. + + + + + True to make the audio source loop playback. + + + + + diff --git a/sdf/1.11/battery.sdf b/sdf/1.11/battery.sdf new file mode 100644 index 000000000..dc6f8deb6 --- /dev/null +++ b/sdf/1.11/battery.sdf @@ -0,0 +1,12 @@ + + + Description of a battery. + + + Unique name for the battery. + + + + Initial voltage in volts. + + diff --git a/sdf/1.11/box_shape.sdf b/sdf/1.11/box_shape.sdf new file mode 100644 index 000000000..826ab37a2 --- /dev/null +++ b/sdf/1.11/box_shape.sdf @@ -0,0 +1,6 @@ + + Box shape + + The three side lengths of the box. The origin of the box is in its geometric center (inside the center of the box). + + diff --git a/sdf/1.11/camera.sdf b/sdf/1.11/camera.sdf new file mode 100644 index 000000000..08f406218 --- /dev/null +++ b/sdf/1.11/camera.sdf @@ -0,0 +1,230 @@ + + These elements are specific to camera sensors. + + + An optional name for the camera. + + + + If the camera will be triggered by a topic + + + + Name of the camera info + + + + Name of the topic that will trigger the camera if enabled + + + + Horizontal field of view + + + + The image size in pixels and format. + + Width in pixels + + + Height in pixels + + + (L8|L16|R_FLOAT16|R_FLOAT32|R8G8B8|B8G8R8|BAYER_RGGB8|BAYER_BGGR8|BAYER_GBRG8|BAYER_GRBG8) + + + Value used for anti-aliasing + + + + + The near and far clip planes. Objects closer or farther than these planes are not rendered. + + + Near clipping plane + + + + Far clipping plane + + + + + Enable or disable saving of camera frames. + + True = saving enabled + + + The path name which will hold the frame data. If path name is relative, then directory is relative to current working directory. + + + + + Depth camera parameters + + Type of output + + + The near and far clip planes. Objects closer or farther than these planes are not detected by the depth camera. + + + Near clipping plane for depth camera + + + + Far clipping plane for depth camera + + + + + + + The segmentation type of the segmentation camera. Valid options are: + - semantic: Semantic segmentation, which provides 2 images: + 1. A grayscale image, with the pixel values representing the label of an object + 2. A colored image, with the pixel values being a unique color for each label + + - panoptic | instance: Panoptic segmentation, which provides an image where each pixel + has 1 channel for label value of the object and 2 channels for the + number of the instances of that label, and a colored image which its + pixels have a unique color for each instance. + + + + + + The boundingbox type of the boundingbox camera. Valid options are: + - 2d | visible_2d | visible_box_2d: a visible 2d box mode which provides axis aligned 2d boxes + on the visible parts of the objects + + - full_2d | full_box_2d: a full 2d box mode which provides axis aligned 2d boxes that fills the + object dimentions, even if it has an occluded part + + - 3d: a 3d mode which provides oriented 3d boxes + + + + + The properties of the noise model that should be applied to generated images + + The type of noise. Currently supported types are: "gaussian" (draw additive noise values independently for each pixel from a Gaussian distribution). + + + For type "gaussian," the mean of the Gaussian distribution from which noise values are drawn. + + + For type "gaussian," the standard deviation of the Gaussian distribution from which noise values are drawn. + + + + + Lens distortion to be applied to camera images. See http://en.wikipedia.org/wiki/Distortion_(optics)#Software_correction + + The radial distortion coefficient k1 + + + The radial distortion coefficient k2 + + + The radial distortion coefficient k3 + + + The tangential distortion coefficient p1 + + + The tangential distortion coefficient p2 + + + The distortion center or principal point + + + + + Lens projection description + + + Type of the lens mapping. Supported values are gnomonical, stereographic, equidistant, equisolid_angle, orthographic, custom. For gnomonical (perspective) projection, it is recommended to specify a horizontal_fov of less than or equal to 90° + + + If true the image will be scaled to fit horizontal FOV, otherwise it will be shown according to projection type parameters + + + + Definition of custom mapping function in a form of r=c1*f*fun(theta/c2 + c3). See https://en.wikipedia.org/wiki/Fisheye_lens#Mapping_function + + Linear scaling constant + + + Angle scaling constant + + + Angle offset constant + + + Focal length of the optical system. Note: It's not a focal length of the lens in a common sense! This value is ignored if 'scale_to_fov' is set to true + + + Possible values are 'sin', 'tan' and 'id' + + + + + Everything outside of the specified angle will be hidden, 90° by default + + + + Resolution of the environment cube map used to draw the world + + + + Camera intrinsic parameters for setting a custom perspective projection matrix (cannot be used with WideAngleCamera since this class uses image stitching from 6 different cameras for achieving a wide field of view). The focal lengths can be computed using focal_length_in_pixels = (image_width_in_pixels * 0.5) / tan(field_of_view_in_degrees * 0.5 * PI/180) + + X focal length (in pixels, overrides horizontal_fov) + + + Y focal length (in pixels, overrides horizontal_fov) + + + X principal point (in pixels) + + + Y principal point (in pixels) + + + XY axis skew + + + + + Camera projection matrix P for overriding camera intrinsic matrix K values so that users can configure P independently of K. This is useful when working with stereo cameras where P may be different from K due to the transform between the two cameras. + + X focal length for projection matrix(in pixels, overrides fx) + + + Y focal length for projection matrix(in pixels, overrides fy) + + + X principal point for projection matrix(in pixels, overrides cx) + + + Y principal point for projection matrix(in pixels, overrides cy) + + + X translation for projection matrix (in pixels) + + + Y translation for projection matrix (in pixels) + + + + + + + + + + An optional frame id name to be used in the camera_info message header. + + + + diff --git a/sdf/1.11/capsule_shape.sdf b/sdf/1.11/capsule_shape.sdf new file mode 100644 index 000000000..2831099ba --- /dev/null +++ b/sdf/1.11/capsule_shape.sdf @@ -0,0 +1,9 @@ + + Capsule shape + + Radius of the capsule + + + Length of the cylindrical portion of the capsule along the z axis + + diff --git a/sdf/1.11/collision.sdf b/sdf/1.11/collision.sdf new file mode 100644 index 000000000..e83406df1 --- /dev/null +++ b/sdf/1.11/collision.sdf @@ -0,0 +1,22 @@ + + + The collision properties of a link. Note that this can be different from the visual properties of a link, for example, simpler collision models are often used to reduce computation time. + + + Unique name for the collision element within the scope of the parent link. + + + + intensity value returned by laser sensor. + + + + Maximum number of contacts allowed between two entities. This value overrides the max_contacts element defined in physics. + + + + + + + + diff --git a/sdf/1.11/contact.sdf b/sdf/1.11/contact.sdf new file mode 100644 index 000000000..d1cde50ac --- /dev/null +++ b/sdf/1.11/contact.sdf @@ -0,0 +1,12 @@ + + These elements are specific to the contact sensor. + + + name of the collision element within a link that acts as the contact sensor. + + + + Topic on which contact data is published. + + + diff --git a/sdf/1.11/cylinder_shape.sdf b/sdf/1.11/cylinder_shape.sdf new file mode 100644 index 000000000..771596ba4 --- /dev/null +++ b/sdf/1.11/cylinder_shape.sdf @@ -0,0 +1,9 @@ + + Cylinder shape + + Radius of the cylinder + + + Length of the cylinder along the z axis + + diff --git a/sdf/1.11/ellipsoid_shape.sdf b/sdf/1.11/ellipsoid_shape.sdf new file mode 100644 index 000000000..821aadf12 --- /dev/null +++ b/sdf/1.11/ellipsoid_shape.sdf @@ -0,0 +1,6 @@ + + Ellipsoid shape + + The three radii of the ellipsoid. The origin of the ellipsoid is in its geometric center (inside the center of the ellipsoid). + + diff --git a/sdf/1.11/forcetorque.sdf b/sdf/1.11/forcetorque.sdf new file mode 100644 index 000000000..5a1846951 --- /dev/null +++ b/sdf/1.11/forcetorque.sdf @@ -0,0 +1,54 @@ + + These elements are specific to the force torque sensor. + + + Frame in which to report the wrench values. Currently supported frames are: + "parent" report the wrench expressed in the orientation of the parent link frame, + "child" report the wrench expressed in the orientation of the child link frame, + "sensor" report the wrench expressed in the orientation of the joint sensor frame. + Note that for each option the point with respect to which the + torque component of the wrench is expressed is the joint origin. + + + + + Direction of the wrench measured by the sensor. The supported options are: + "parent_to_child" if the measured wrench is the one applied by the parent link on the child link, + "child_to_parent" if the measured wrench is the one applied by the child link on the parent link. + + + + + These elements are specific to measurement-frame force, + which is expressed in Newtons + + Force along the X axis + + + + Force along the Y axis + + + + Force along the Z axis + + + + + + These elements are specific to measurement-frame torque, + which is expressed in Newton-meters + + Torque about the X axis + + + + Force about the Y axis + + + + Torque about the Z axis + + + + diff --git a/sdf/1.11/frame.sdf b/sdf/1.11/frame.sdf new file mode 100644 index 000000000..74802fd7e --- /dev/null +++ b/sdf/1.11/frame.sdf @@ -0,0 +1,35 @@ + + + A frame of reference in which poses may be expressed. + + + + Name of the frame. It must be unique whithin its scope (model/world), + i.e., it must not match the name of another frame, link, joint, or model + within the same scope. + + + + + + If specified, this frame is attached to the specified frame. The specified + frame must be within the same scope and may be defined implicitly, i.e., + the name of any //frame, //model, //joint, or //link within the same scope + may be used. + + If missing, this frame is attached to the containing scope's frame. Within + a //world scope this is the implicit world frame, and within a //model + scope this is the implicit model frame. + + A frame moves jointly with the frame it is @attached_to. This is different + from //pose/@relative_to. @attached_to defines how the frame is attached + to a //link, //model, or //world frame, while //pose/@relative_to defines + how the frame's pose is represented numerically. As a result, following + the chain of @attached_to attributes must always lead to a //link, + //model, //world, or //joint (implicitly attached_to its child //link). + + + + + + diff --git a/sdf/1.11/geometry.sdf b/sdf/1.11/geometry.sdf new file mode 100644 index 000000000..884902afb --- /dev/null +++ b/sdf/1.11/geometry.sdf @@ -0,0 +1,20 @@ + + + The shape of the visual or collision object. + + + You can use the empty tag to make empty geometries. + + + + + + + + + + + + + + diff --git a/sdf/1.11/gps.sdf b/sdf/1.11/gps.sdf new file mode 100644 index 000000000..95e004925 --- /dev/null +++ b/sdf/1.11/gps.sdf @@ -0,0 +1,40 @@ + + These elements are specific to the GPS sensor. + + + + Parameters related to GPS position measurement. + + + + Noise parameters for horizontal position measurement, in units of meters. + + + + + + Noise parameters for vertical position measurement, in units of meters. + + + + + + + + Parameters related to GPS position measurement. + + + + Noise parameters for horizontal velocity measurement, in units of meters/second. + + + + + + Noise parameters for vertical velocity measurement, in units of meters/second. + + + + + + diff --git a/sdf/1.11/gripper.sdf b/sdf/1.11/gripper.sdf new file mode 100644 index 000000000..12fd0b341 --- /dev/null +++ b/sdf/1.11/gripper.sdf @@ -0,0 +1,30 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sdf/1.11/gui.sdf b/sdf/1.11/gui.sdf new file mode 100644 index 000000000..3a87763a7 --- /dev/null +++ b/sdf/1.11/gui.sdf @@ -0,0 +1,60 @@ + + + + + + + + + + + + + + + + + + + + + Set the type of projection for the camera. Valid values are "perspective" and "orthographic". + + + + + + + Name of the tracked visual. If no name is provided, the remaining settings will be applied whenever tracking is triggered in the GUI. + + + + Minimum distance between the camera and the tracked visual. This parameter is only used if static is set to false. + + + + Maximum distance between the camera and the tracked visual. This parameter is only used if static is set to false. + + + + If set to true, the position of the camera is fixed relatively to the model or to the world, depending on the value of the use_model_frame element. Otherwise, the position of the camera may vary but the distance between the camera and the model will depend on the value of the min_dist and max_dist elements. In any case, the camera will always follow the model by changing its orientation. + + + + If set to true, the position of the camera is relative to the model reference frame, which means that its position relative to the model will not change. Otherwise, the position of the camera is relative to the world reference frame, which means that its position relative to the world will not change. This parameter is only used if static is set to true. + + + + The position of the camera's reference frame. This parameter is only used if static is set to true. If use_model_frame is set to true, the position is relative to the model reference frame, otherwise it represents world coordinates. + + + + If set to true, the camera will inherit the yaw rotation of the tracked model. This parameter is only used if static and use_model_frame are set to true. + + + + + + + + diff --git a/sdf/1.11/heightmap_shape.sdf b/sdf/1.11/heightmap_shape.sdf new file mode 100644 index 000000000..7a7462298 --- /dev/null +++ b/sdf/1.11/heightmap_shape.sdf @@ -0,0 +1,44 @@ + + A heightmap based on a 2d grayscale image. + + URI to a grayscale image file + + + The size of the heightmap in world units. + When loading an image: "size" is used if present, otherwise defaults to 1x1x1. + When loading a DEM: "size" is used if present, otherwise defaults to true size of DEM. + + + + A position offset. + + + + The heightmap can contain multiple textures. The order of the texture matters. The first texture will appear at the lowest height, and the last texture at the highest height. Use blend to control the height thresholds and fade between textures. + + Size of the applied texture in meters. + + + Diffuse texture image filename + + + Normalmap texture image filename + + + + The blend tag controls how two adjacent textures are mixed. The number of blend elements should equal one less than the number of textures. + + Min height of a blend layer + + + Distance over which the blend occurs + + + + Set if the rendering engine will use terrain paging + + + Samples per heightmap datum. For rasterized heightmaps, this indicates the number of samples to take per pixel. Using a higher value, e.g. 2, will generally improve the quality of the heightmap but lower performance. + + + diff --git a/sdf/1.11/image_shape.sdf b/sdf/1.11/image_shape.sdf new file mode 100644 index 000000000..369de7c5e --- /dev/null +++ b/sdf/1.11/image_shape.sdf @@ -0,0 +1,18 @@ + + Extrude a set of boxes from a grayscale image. + + URI of the grayscale image file + + + Scaling factor applied to the image + + + Grayscale threshold + + + Height of the extruded boxes + + + The amount of error in the model + + diff --git a/sdf/1.11/imu.sdf b/sdf/1.11/imu.sdf new file mode 100644 index 000000000..25690c8f1 --- /dev/null +++ b/sdf/1.11/imu.sdf @@ -0,0 +1,120 @@ + + These elements are specific to the IMU sensor. + + + + + + This string represents special hardcoded use cases that are commonly seen with typical robot IMU's: + - CUSTOM: use Euler angle custom_rpy orientation specification. + The orientation of the IMU's reference frame is defined by adding the custom_rpy rotation + to the parent_frame. + - NED: The IMU XYZ aligns with NED, where NED orientation relative to Gazebo world + is defined by the SphericalCoordinates class. + - ENU: The IMU XYZ aligns with ENU, where ENU orientation relative to Gazebo world + is defined by the SphericalCoordinates class. + - NWU: The IMU XYZ aligns with NWU, where NWU orientation relative to Gazebo world + is defined by the SphericalCoordinates class. + - GRAV_UP: where direction of gravity maps to IMU reference frame Z-axis with Z-axis pointing in + the opposite direction of gravity. IMU reference frame X-axis direction is defined by grav_dir_x. + Note if grav_dir_x is parallel to gravity direction, this configuration fails. + Otherwise, IMU reference frame X-axis is defined by projection of grav_dir_x onto a plane + normal to the gravity vector. IMU reference frame Y-axis is a vector orthogonal to both + X and Z axis following the right hand rule. + - GRAV_DOWN: where direction of gravity maps to IMU reference frame Z-axis with Z-axis pointing in + the direction of gravity. IMU reference frame X-axis direction is defined by grav_dir_x. + Note if grav_dir_x is parallel to gravity direction, this configuration fails. + Otherwise, IMU reference frame X-axis is defined by projection of grav_dir_x onto a plane + normal to the gravity vector. IMU reference frame Y-axis is a vector orthogonal to both + X and Z axis following the right hand rule. + + + + + This field and parent_frame are used when localization is set to CUSTOM. + Orientation (fixed axis roll, pitch yaw) transform from parent_frame to this IMU's reference frame. + Some common examples are: + - IMU reports in its local frame on boot. IMU sensor frame is the reference frame. + Example: parent_frame="", custom_rpy="0 0 0" + - IMU reports in Gazebo world frame. + Example sdf: parent_frame="world", custom_rpy="0 0 0" + - IMU reports in NWU frame. + Uses SphericalCoordinates class to determine world frame in relation to magnetic north and gravity; + i.e. rotation between North-West-Up and world (+X,+Y,+Z) frame is defined by SphericalCoordinates class. + Example sdf given world is NWU: parent_frame="world", custom_rpy="0 0 0" + - IMU reports in NED frame. + Uses SphericalCoordinates class to determine world frame in relation to magnetic north and gravity; + i.e. rotation between North-East-Down and world (+X,+Y,+Z) frame is defined by SphericalCoordinates class. + Example sdf given world is NWU: parent_frame="world", custom_rpy="M_PI 0 0" + - IMU reports in ENU frame. + Uses SphericalCoordinates class to determine world frame in relation to magnetic north and gravity; + i.e. rotation between East-North-Up and world (+X,+Y,+Z) frame is defined by SphericalCoordinates class. + Example sdf given world is NWU: parent_frame="world", custom_rpy="0 0 -0.5*M_PI" + - IMU reports in ROS optical frame as described in http://www.ros.org/reps/rep-0103.html#suffix-frames, which is + (z-forward, x-left to right when facing +z, y-top to bottom when facing +z). + (default gazebo camera is +x:view direction, +y:left, +z:up). + Example sdf: parent_frame="local", custom_rpy="-0.5*M_PI 0 -0.5*M_PI" + + + + Name of parent frame which the custom_rpy transform is defined relative to. + It can be any valid fully scoped Gazebo Link name or the special reserved "world" frame. + If left empty, use the sensor's own local frame. + + + + + + Used when localization is set to GRAV_UP or GRAV_DOWN, a projection of this vector + into a plane that is orthogonal to the gravity vector + defines the direction of the IMU reference frame's X-axis. + grav_dir_x is defined in the coordinate frame as defined by the parent_frame element. + + + + Name of parent frame in which the grav_dir_x vector is defined. + It can be any valid fully scoped Gazebo Link name or the special reserved "world" frame. + If left empty, use the sensor's own local frame. + + + + + + + These elements are specific to body-frame angular velocity, + which is expressed in radians per second + + Angular velocity about the X axis + + + + Angular velocity about the Y axis + + + + Angular velocity about the Z axis + + + + + + These elements are specific to body-frame linear acceleration, + which is expressed in meters per second squared + + Linear acceleration about the X axis + + + + Linear acceleration about the Y axis + + + + Linear acceleration about the Z axis + + + + + + Some IMU sensors rely on external filters to produce orientation estimates. True to generate and output orientation data, false to disable orientation data generation. + + diff --git a/sdf/1.11/inertial.sdf b/sdf/1.11/inertial.sdf new file mode 100644 index 000000000..f4d31ef59 --- /dev/null +++ b/sdf/1.11/inertial.sdf @@ -0,0 +1,209 @@ + + + + The link's mass, position of its center of mass, its central inertia + properties, and optionally its fluid added mass. + + + + The mass of the link. + + + + + This pose (translation, rotation) describes the position and orientation + of the link's center-of-mass-frame C relative to the link-frame L. + The first three components (x y z) specify the position vector from Lo + (the link-frame origin) to Co (the link's center of mass) as + `x L̂x + y L̂y + z L̂ᴢ`, where L̂x, L̂y, L̂ᴢ are link-frame L's orthogonal unit + vectors. The subsequent values characterize C's orientation relative to + link-frame L as a sequence of Euler rotations + (r p y) documented in http://sdformat.org/tutorials?tut=specify_pose, + or as a quaternion (x y z w), where w is the scalar component. + + + + 'euler_rpy' by default. Supported rotation formats are + 'euler_rpy', Euler angles representation in roll, pitch, yaw. The pose is expected to have 6 values. + 'quat_xyzw', Quaternion representation in x, y, z, w. The pose is expected to have 7 values. + + + + + + Whether or not the euler angles are in degrees, otherwise they will be interpreted as radians by default. + + + + + + + + This link's moments of inertia ixx, iyy, izz and products of inertia + ixy, ixz, iyz about Co (the link's center of mass) for the unit vectors + Ĉx, Ĉy, Ĉᴢ fixed in the center-of-mass-frame C. + Note: the orientation of Ĉx, Ĉy, Ĉᴢ relative to L̂x, L̂y, L̂ᴢ is specified + by the `pose` tag. + To avoid compatibility issues associated with the negative sign + convention for product of inertia, align Ĉx, Ĉy, Ĉᴢ with principal + inertia directions so that all the products of inertia are zero. + For more information about this sign convention, see the following + MathWorks documentation for working with CAD tools: + https://www.mathworks.com/help/releases/R2021b/physmod/sm/ug/specify-custom-inertia.html#mw_b043ec69-835b-4ca9-8769-af2e6f1b190c + + + + The link's moment of inertia about Co (the link's center of mass) for Ĉx. + + + + + The link's product of inertia about Co (the link's center of mass) for + Ĉx and Ĉy, where the product of inertia convention -m x y (not +m x y) + is used. If Ĉx or Ĉy is a principal inertia direction, ixy = 0. + + + + + The link's product of inertia about Co (the link's center of mass) for + Ĉx and Ĉz, where the product of inertia convention -m x z (not +m x z) + is used. If Ĉx or Ĉz is a principal inertia direction, ixz = 0. + + + + + The link's moment of inertia about Co (the link's center of mass) for Ĉy. + + + + + The link's product of inertia about Co (the link's center of mass) for + Ĉy and Ĉz, where the product of inertia convention -m y z (not +m y z) + is used. If Ĉy or Ĉz is a principal inertia direction, iyz = 0. + + + + + The link's moment of inertia about Co (the link's center of mass) for Ĉz. + + + + + + + This link's fluid added mass matrix about the link's origin. + This matrix represents the inertia of the fluid that is dislocated when the + body moves. Added mass should be zero if the density of the surrounding + fluid is negligible with respect to the body's density. + The 6x6 matrix is symmetric, therefore only 21 unique elements can be set. + The elements of the matrix follow the [x, y, z, p, q, r] notation, where + [x, y, z] correspond to translation and [p, q, r] to rotation + (i.e. roll, pitch, yaw). + + + + Added mass in the X axis due to linear acceleration in the X axis, in kg. + + + + + Added mass in the X axis due to linear acceleration in the Y axis, and vice-versa, in kg. + + + + + Added mass in the X axis due to linear acceleration in the Z axis, and vice-versa, in kg. + + + + + Added mass in the X axis due to angular acceleration about the X axis, and vice-versa, in kg * m. + + + + + Added mass in the X axis due to angular acceleration about the Y axis, and vice-versa, in kg * m. + + + + + Added mass in the X axis due to angular acceleration about the Z axis, and vice-versa, in kg * m. + + + + + Added mass in the Y axis due to linear acceleration in the Y axis, in kg. + + + + + Added mass in the Y axis due to linear acceleration in the Z axis, and vice-versa, in kg. + + + + + Added mass in the Y axis due to angular acceleration about the X axis, and vice-versa, in kg * m. + + + + + Added mass in the Y axis due to angular acceleration about the Y axis, and vice-versa, in kg * m. + + + + + Added mass in the Y axis due to angular acceleration about the Z axis, and vice-versa, in kg * m. + + + + + Added mass in the Z axis due to linear acceleration in the Z axis, in kg. + + + + + Added mass in the Z axis due to angular acceleration about the X axis, and vice-versa, in kg * m. + + + + + Added mass in the Z axis due to angular acceleration about the Y axis, and vice-versa, in kg * m. + + + + + Added mass in the Z axis due to angular acceleration about the Z axis, and vice-versa, in kg * m. + + + + + Added mass moment about the X axis due to angular acceleration about the X axis, in kg * m^2. + + + + + Added mass moment about the X axis due to angular acceleration about the Y axis, and vice-versa, in kg * m^2. + + + + + Added mass moment about the X axis due to angular acceleration about the Z axis, and vice-versa, in kg * m^2. + + + + + Added mass moment about the Y axis due to angular acceleration about the Y axis, in kg * m^2. + + + + + Added mass moment about the Y axis due to angular acceleration about the Z axis, and vice-versa, in kg * m^2. + + + + + Added mass moment about the Z axis due to angular acceleration about the Z axis, in kg * m^2. + + + + diff --git a/sdf/1.11/joint.sdf b/sdf/1.11/joint.sdf new file mode 100644 index 000000000..b757614e9 --- /dev/null +++ b/sdf/1.11/joint.sdf @@ -0,0 +1,240 @@ + + + A joint connects two links with kinematic and dynamic properties. By default, the pose of a joint is expressed in the child link frame. + + + A unique name for the joint within its scope. + + + + The type of joint, which must be one of the following: + (continuous) a hinge joint that rotates on a single axis with a continuous range of motion, + (revolute) a hinge joint that rotates on a single axis with a fixed range of motion, + (gearbox) geared revolute joints, + (revolute2) same as two revolute joints connected in series, + (prismatic) a sliding joint that slides along an axis with a limited range specified by upper and lower limits, + (ball) a ball and socket joint, + (screw) a single degree of freedom joint with coupled sliding and rotational motion, + (universal) like a ball joint, but constrains one degree of freedom, + (fixed) a joint with zero degrees of freedom that rigidly connects two links. + + + + + Name of the parent frame or "world". + + + + Name of the child frame. The value "world" may not be specified. + + + + Parameter for gearbox joints. Given theta_1 and theta_2 defined in description for gearbox_reference_body, theta_2 = -gearbox_ratio * theta_1. + + + + Parameter for gearbox joints. Gearbox ratio is enforced over two joint angles. First joint angle (theta_1) is the angle from the gearbox_reference_body to the parent link in the direction of the axis element and the second joint angle (theta_2) is the angle from the gearbox_reference_body to the child link in the direction of the axis2 element. + + + + + Parameter for screw joints representing the ratio between rotation + and translation of the joint. This parameter has been interpreted by + gazebo-classic as having units of radians / meter with a positive value + corresponding to a left-handed thread. + The parameter is now deprecated in favor of `screw_thread_pitch`. + + + + + + A parameter for screw joint kinematics, representing the + axial distance traveled for each revolution of the joint, + with units of meters / revolution with a positive value corresponding + to a right-handed thread. + This parameter supersedes `thread_pitch`. + + + + + + Parameters related to the axis of rotation for revolute joints, + the axis of translation for prismatic joints. + + + + Represents the x,y,z components of the axis unit vector. The axis is + expressed in the joint frame unless a different frame is expressed in + the expressed_in attribute. The vector should be normalized. + + + + Name of frame in whose coordinates the xyz unit vector is expressed. + + + + + An element specifying physical properties of the joint. These values are used to specify modeling properties of the joint, particularly useful for simulation. + + The physical velocity dependent viscous damping coefficient of the joint. + + + The physical static friction value of the joint. + + + The spring reference position for this joint axis. + + + The spring stiffness for this joint axis. + + + + specifies the limits of this joint + + Specifies the lower joint limit (radians for revolute joints, meters for prismatic joints). Omit if joint is continuous. + + + Specifies the upper joint limit (radians for revolute joints, meters for prismatic joints). Omit if joint is continuous. + + + A value for enforcing the maximum joint effort applied. Limit is not enforced if value is negative. + + + A value for enforcing the maximum joint velocity. + + + + Joint stop stiffness. + + + + Joint stop dissipation. + + + + + + + + Parameters related to the second axis of rotation for revolute2 joints and universal joints. + + + + Represents the x,y,z components of the axis unit vector. The axis is + expressed in the joint frame unless a different frame is expressed in + the expressed_in attribute. The vector should be normalized. + + + + Name of frame in whose coordinates the xyz unit vector is expressed. + + + + + An element specifying physical properties of the joint. These values are used to specify modeling properties of the joint, particularly useful for simulation. + + The physical velocity dependent viscous damping coefficient of the joint. EXPERIMENTAL: if damping coefficient is negative and implicit_spring_damper is true, adaptive damping is used. + + + The physical static friction value of the joint. + + + The spring reference position for this joint axis. + + + The spring stiffness for this joint axis. + + + + + + + An attribute specifying the lower joint limit (radians for revolute joints, meters for prismatic joints). Omit if joint is continuous. + + + An attribute specifying the upper joint limit (radians for revolute joints, meters for prismatic joints). Omit if joint is continuous. + + + An attribute for enforcing the maximum joint effort applied by Joint::SetForce. Limit is not enforced if value is negative. + + + (not implemented) An attribute for enforcing the maximum joint velocity. + + + + Joint stop stiffness. Supported physics engines: SimBody. + + + + Joint stop dissipation. Supported physics engines: SimBody. + + + + + + + Parameters that are specific to a certain physics engine. + + Simbody specific parameters + + Force cut in the multibody graph at this joint. + + + + ODE specific parameters + + If cfm damping is set to true, ODE will use CFM to simulate damping, allows for infinite damping, and one additional constraint row (previously used for joint limit) is always active. + + + + If implicit_spring_damper is set to true, ODE will use CFM, ERP to simulate stiffness and damping, allows for infinite damping, and one additional constraint row (previously used for joint limit) is always active. This replaces cfm_damping parameter in SDFormat 1.4. + + + + Scale the excess for in a joint motor at joint limits. Should be between zero and one. + + + Constraint force mixing for constrained directions + + + Error reduction parameter for constrained directions + + + Bounciness of the limits + + + Maximum force or torque used to reach the desired velocity. + + + The desired velocity of the joint. Should only be set if you want the joint to move on load. + + + + + + Constraint force mixing parameter used by the joint stop + + + Error reduction parameter used by the joint stop + + + + + + + Suspension constraint force mixing parameter + + + Suspension error reduction parameter + + + + + + If provide feedback is set to true, physics engine will compute the constraint forces at this joint. + + + + + + diff --git a/sdf/1.11/lidar.sdf b/sdf/1.11/lidar.sdf new file mode 100644 index 000000000..f0100a9a3 --- /dev/null +++ b/sdf/1.11/lidar.sdf @@ -0,0 +1,78 @@ + + These elements are specific to the lidar sensor. + + + + + + + + The number of simulated lidar rays to generate per complete laser sweep cycle. + + + + This number is multiplied by samples to determine the number of range data points returned. If resolution is not equal to one, range data is interpolated. + + + + + + + + Must be greater or equal to min_angle + + + + + + + + The number of simulated lidar rays to generate per complete laser sweep cycle. + + + + This number is multiplied by samples to determine the number of range data points returned. If resolution is not equal to one, range data is interpolated. + + + + + + + + Must be greater or equal to min_angle + + + + + + + specifies range properties of each simulated lidar + + The minimum distance for each lidar ray. + + + The maximum distance for each lidar ray. + + + Linear resolution of each lidar ray. + + + + + The properties of the noise model that should be applied to generated scans + + The type of noise. Currently supported types are: "gaussian" (draw noise values independently for each beam from a Gaussian distribution). + + + For type "gaussian," the mean of the Gaussian distribution from which noise values are drawn. + + + For type "gaussian," the standard deviation of the Gaussian distribution from which noise values are drawn. + + + + + + + + diff --git a/sdf/1.11/light.sdf b/sdf/1.11/light.sdf new file mode 100644 index 000000000..a6939ac48 --- /dev/null +++ b/sdf/1.11/light.sdf @@ -0,0 +1,67 @@ + + + The light element describes a light source. + + + A unique name for the light. + + + + The light type: point, directional, spot. + + + + When true, the light will cast shadows. + + + + When true, the light is on. + + + + Scale factor to set the relative power of a light. + + + + + + Diffuse light color + + + Specular light color + + + + Light attenuation + + Range of the light + + + The linear attenuation factor: 1 means attenuate evenly over the distance. + + + The constant attenuation factor: 1.0 means never attenuate, 0.0 is complete attenutation. + + + The quadratic attenuation factor: adds a curvature to the attenuation. + + + + + Direction of the light, only applicable for spot and directional lights. + + + + Spot light parameters + + Angle covered by the bright inner cone + + + Angle covered by the outer cone + + + The rate of falloff between the inner and outer cones. 1.0 means a linear falloff, less means slower falloff, higher means faster falloff. + + + + diff --git a/sdf/1.11/light_state.sdf b/sdf/1.11/light_state.sdf new file mode 100644 index 000000000..673efcdb4 --- /dev/null +++ b/sdf/1.11/light_state.sdf @@ -0,0 +1,10 @@ + + + Light state + + + Name of the light + + + + diff --git a/sdf/1.11/link.sdf b/sdf/1.11/link.sdf new file mode 100644 index 000000000..b518ff3c5 --- /dev/null +++ b/sdf/1.11/link.sdf @@ -0,0 +1,51 @@ + + + A physical link with inertia, collision, and visual properties. A link must be a child of a model, and any number of links may exist in a model. + + + A unique name for the link within the scope of the model. + + + + If true, the link is affected by gravity. + + + + If true, the link is affected by the wind. + + + + If true, the link can collide with other links in the model. Two links within a model will collide if link1.self_collide OR link2.self_collide. Links connected by a joint will never collide. + + + + If true, the link is kinematic only + + + + If true, the link will have 6DOF and be a direct child of world. + + + + Exponential damping of the link's velocity. + + Linear damping + + + Angular damping + + + + + + + + + + + + + + + + diff --git a/sdf/1.11/link_state.sdf b/sdf/1.11/link_state.sdf new file mode 100644 index 000000000..28fff465d --- /dev/null +++ b/sdf/1.11/link_state.sdf @@ -0,0 +1,40 @@ + + + Link state + + + Name of the link + + + + Velocity of the link. The x, y, z components of the pose + correspond to the linear velocity of the link, and the roll, pitch, yaw + components correspond to the angular velocity of the link + + + + + Acceleration of the link. The x, y, z components of the pose + correspond to the linear acceleration of the link, and the roll, + pitch, yaw components correspond to the angular acceleration of the link + + + + + Force and torque applied to the link. The x, y, z components + of the pose correspond to the force applied to the link, and the roll, + pitch, yaw components correspond to the torque applied to the link + + + + + Collision state + + + Name of the collision + + + + + + diff --git a/sdf/1.11/logical_camera.sdf b/sdf/1.11/logical_camera.sdf new file mode 100644 index 000000000..de47703c0 --- /dev/null +++ b/sdf/1.11/logical_camera.sdf @@ -0,0 +1,19 @@ + + These elements are specific to logical camera sensors. A logical camera reports objects that fall within a frustum. Computation should be performed on the CPU. + + + Near clipping distance of the view frustum + + + + Far clipping distance of the view frustum + + + + Aspect ratio of the near and far planes. This is the width divided by the height of the near or far planes. + + + + Horizontal field of view of the frustum, in radians. This is the angle between the frustum's vertex and the edges of the near or far plane. + + diff --git a/sdf/1.11/magnetometer.sdf b/sdf/1.11/magnetometer.sdf new file mode 100644 index 000000000..2f3bfeeee --- /dev/null +++ b/sdf/1.11/magnetometer.sdf @@ -0,0 +1,21 @@ + + These elements are specific to a Magnetometer sensor. + + + Parameters related to the body-frame X axis of the magnetometer + + + + + + Parameters related to the body-frame Y axis of the magnetometer + + + + + + Parameters related to the body-frame Z axis of the magnetometer + + + + \ No newline at end of file diff --git a/sdf/1.11/material.sdf b/sdf/1.11/material.sdf new file mode 100644 index 000000000..3459654e4 --- /dev/null +++ b/sdf/1.11/material.sdf @@ -0,0 +1,166 @@ + + + The material of the visual element. + + + Name of material from an installed script file. This will override the color element if the script exists. + + + URI of the material script file + + + + Name of the script within the script file + + + + + + + vertex, pixel, normal_map_object_space, normal_map_tangent_space + + + + filename of the normal map + + + + + Set render order for coplanar polygons. The higher value will be rendered on top of the other coplanar polygons + + + + If false, dynamic lighting will be disabled + + + + The ambient color of a material specified by set of four numbers representing red/green/blue, each in the range of [0,1]. + + + + The diffuse color of a material specified by set of four numbers representing red/green/blue/alpha, each in the range of [0,1]. + + + + The specular color of a material specified by set of four numbers representing red/green/blue/alpha, each in the range of [0,1]. + + + + The specular exponent of a material + + + + The emissive color of a material specified by set of four numbers representing red/green/blue, each in the range of [0,1]. + + + + If true, the mesh that this material is applied to will be rendered as double sided + + + + + Physically Based Rendering (PBR) material. There are two PBR workflows: metal and specular. While both workflows and their parameters can be specified at the same time, typically only one of them will be used (depending on the underlying renderer capability). It is also recommended to use the same workflow for all materials in the world. + + + PBR using the Metallic/Roughness workflow. + + + Filename of the diffuse/albedo map. + + + + Filename of the roughness map. + + + + Material roughness in the range of [0,1], where 0 represents a smooth surface and 1 represents a rough surface. This is the inverse of a specular map in a PBR specular workflow. + + + + Filename of the metalness map. + + + + Material metalness in the range of [0,1], where 0 represents non-metal and 1 represents raw metal + + + + Filename of the environment / reflection map, typically in the form of a cubemap + + + + Filename of the ambient occlusion map. The map defines the amount of ambient lighting on the surface. + + + + + The space that the normals are in. Values are: 'object' or 'tangent' + + + Filename of the normal map. The normals can be in the object space or tangent space as specified in the 'type' attribute + + + + Filename of the emissive map. + + + + + Index of the texture coordinate set to use. + + Filename of the light map. The light map is a prebaked light texture that is applied over the albedo map + + + + + + PBR using the Specular/Glossiness workflow. + + + Filename of the diffuse/albedo map. + + + + Filename of the specular map. + + + + Filename of the glossiness map. + + + + Material glossiness in the range of [0-1], where 0 represents a rough surface and 1 represents a smooth surface. This is the inverse of a roughness map in a PBR metal workflow. + + + + Filename of the environment / reflection map, typically in the form of a cubemap + + + + Filename of the ambient occlusion map. The map defines the amount of ambient lighting on the surface. + + + + + The space that the normals are in. Values are: 'object' or 'tangent' + + + Filename of the normal map. The normals can be in the object space or tangent space as specified in the 'type' attribute + + + + Filename of the emissive map. + + + + + Index of the texture coordinate set to use. + + Filename of the light map. The light map is a prebaked light texture that is applied over the albedo map + + + + + + + diff --git a/sdf/1.11/mesh_shape.sdf b/sdf/1.11/mesh_shape.sdf new file mode 100644 index 000000000..61bce76dd --- /dev/null +++ b/sdf/1.11/mesh_shape.sdf @@ -0,0 +1,20 @@ + + Mesh shape + + Mesh uri + + + + Use a named submesh. The submesh must exist in the mesh specified by the uri + + Name of the submesh within the parent mesh + + + Set to true to center the vertices of the submesh at 0,0,0. This will effectively remove any transformations on the submesh before the poses from parent links and models are applied. + + + + + Scaling factor applied to the mesh + + diff --git a/sdf/1.11/model.sdf b/sdf/1.11/model.sdf new file mode 100644 index 000000000..57dc83a01 --- /dev/null +++ b/sdf/1.11/model.sdf @@ -0,0 +1,92 @@ + + + The model element defines a complete robot or any other physical object. + + + + The name of the model and its implicit frame. This name must be unique + among all elements defining frames within the same scope, i.e., it must + not match another //model, //frame, //joint, or //link within the same + scope. + + + + + + The name of the model's canonical link, to which the model's implicit + coordinate frame is attached. If unset or set to an empty string, the + first `/link` listed as a direct child of this model is chosen as the + canonical link. If the model has no direct `/link` children, it will + instead be attached to the first nested (or included) model's implicit + frame. + + + + The frame inside this model whose pose will be set by the pose element of the model. i.e, the pose element specifies the pose of this frame instead of the model frame. + + + + + If set to true, the model is immovable; i.e., a dynamics engine will not + update its position. This will also overwrite this model's `@canonical_link` + and instead attach the model's implicit frame to the world's implicit frame. + This holds even if this model is nested (or included) by another model + instead of being a direct child of `//world`. + + + + + If set to true, all links in the model will collide with each other (except those connected by a joint). Can be overridden by the link or collision element self_collide property. Two links within a model will collide if link1.self_collide OR link2.self_collide. Links connected by a joint will never collide. + + + + Allows a model to auto-disable, which is means the physics engine can skip updating the model when the model is at rest. This parameter is only used by models with no joints. + + + + + + + + + + + + Include resources from a URI. This can be used to nest models. The included resource can only contain one 'model' element. The URI can point to a directory or a file. If the URI is a directory, it must conform to the model database structure (see /tutorials?tut=composition&cat=specification&#defining-models-in-separate-files). + + + Merge the included nested model into the top model + + + + URI to a resource, such as a model + + + + + + + Override the name of the included model. + + + + Override the static value of the included model. + + + + The frame inside the included model whose pose will be set by the specified pose element. If this element is specified, the pose must be specified. + + + + + A nested model element + + A unique name for the model. This name must not match another nested model in the same level as this model. + + + + + If set to true, all links in the model will be affected by the wind. Can be overriden by the link wind property. + + + diff --git a/sdf/1.11/model_state.sdf b/sdf/1.11/model_state.sdf new file mode 100644 index 000000000..3fd296ff2 --- /dev/null +++ b/sdf/1.11/model_state.sdf @@ -0,0 +1,41 @@ + + + Model state + + + Name of the model + + + + Joint angle + + + Name of the joint + + + + + Index of the axis. + + + Angle of an axis + + + + + A nested model state element + + Name of the model. + + + + + + + + Scale for the 3 dimensions of the model. + + + + + diff --git a/sdf/1.11/navsat.sdf b/sdf/1.11/navsat.sdf new file mode 100644 index 000000000..8a8f1719b --- /dev/null +++ b/sdf/1.11/navsat.sdf @@ -0,0 +1,40 @@ + + These elements are specific to the NAVSAT sensor. + + + + Parameters related to NAVSAT position measurement. + + + + Noise parameters for horizontal position measurement, in units of meters. + + + + + + Noise parameters for vertical position measurement, in units of meters. + + + + + + + + Parameters related to NAVSAT position measurement. + + + + Noise parameters for horizontal velocity measurement, in units of meters/second. + + + + + + Noise parameters for vertical velocity measurement, in units of meters/second. + + + + + + diff --git a/sdf/1.11/noise.sdf b/sdf/1.11/noise.sdf new file mode 100644 index 000000000..ceeac5037 --- /dev/null +++ b/sdf/1.11/noise.sdf @@ -0,0 +1,43 @@ + + The properties of a sensor noise model. + + + + The type of noise. Currently supported types are: + "none" (no noise). + "gaussian" (draw noise values independently for each measurement from a Gaussian distribution). + "gaussian_quantized" ("gaussian" plus quantization of outputs (ie. rounding)) + + + + + For type "gaussian*", the mean of the Gaussian distribution from which + noise values are drawn. + + + + For type "gaussian*", the standard deviation of the Gaussian distribution from which noise values are drawn. + + + For type "gaussian*", the mean of the Gaussian distribution from which bias values are drawn. + + + For type "gaussian*", the standard deviation of the Gaussian distribution from which bias values are drawn. + + + + For type "gaussian*", the standard deviation of the noise used to drive a process to model slow variations in a sensor bias. + + + + For type "gaussian*", the correlation time in seconds of the noise used to drive a process to model slow variations in a sensor bias. A typical value, when used, would be on the order of 3600 seconds (1 hour). + + + + + For type "gaussian_quantized", the precision of output signals. A value + of zero implies infinite precision / no quantization. + + + + diff --git a/sdf/1.11/particle_emitter.sdf b/sdf/1.11/particle_emitter.sdf new file mode 100755 index 000000000..d6a2fdf7e --- /dev/null +++ b/sdf/1.11/particle_emitter.sdf @@ -0,0 +1,120 @@ + + + A particle emitter that can be used to describe fog, smoke, and dust. + + + A unique name for the particle emitter. + + + + The type of a particle emitter. One of "box", "cylinder", "ellipsoid", or "point". + + + + True indicates that the particle emitter should generate particles when loaded + + + + The number of seconds the emitter is active. A value less than or equal to zero means infinite duration. + + + + + The size of the emitter where the particles are sampled. + Default value is (1, 1, 1). + Note that the interpretation of the emitter area varies + depending on the emmiter type: + - point: The area is ignored. + - box: The area is interpreted as width X height X depth. + - cylinder: The area is interpreted as the bounding box of the + cylinder. The cylinder is oriented along the Z-axis. + - ellipsoid: The area is interpreted as the bounding box of an + ellipsoid shaped area, i.e. a sphere or + squashed-sphere area. The parameters are again + identical to EM_BOX, except that the dimensions + describe the widest points along each of the axes. + + + + + The particle dimensions (width, height, depth). + + + + The number of seconds each particle will ’live’ for before being destroyed. This value must be greater than zero. + + + + The number of particles per second that should be emitted. + + + + Sets a minimum velocity for each particle (m/s). + + + + Sets a maximum velocity for each particle (m/s). + + + + Sets the amount by which to scale the particles in both x and y direction per second. + + + + + Sets the starting color for all particles emitted. + The actual color will be interpolated between this color + and the one set under color_end. + Color::White is the default color for the particles + unless a specific function is used. + To specify a color, RGB values should be passed in. + For example, to specify red, a user should enter: + 1 0 0 + Note that this function overrides the particle colors set + with color_range_image. + + + + + + Sets the end color for all particles emitted. + The actual color will be interpolated between this color + and the one set under color_start. + Color::White is the default color for the particles + unless a specific function is used (see color_start for + more information about defining custom colors with RGB + values). + Note that this function overrides the particle colors set + with color_range_image. + + + + + + Sets the path to the color image used as an affector. This affector modifies the color of particles in flight. The colors are taken from a specified image file. The range of color values begins from the left side of the image and moves to the right over the lifetime of the particle, therefore only the horizontal dimension of the image is used. Note that this function overrides the particle colors set with color_start and color_end. + + + + + + Topic used to update particle emitter properties at runtime. + The default topic is + /model/{model_name}/particle_emitter/{emitter_name} + Note that the emitter id and name may not be changed. + + + + + + This is used to determine the ratio of particles that will be detected + by sensors. Increasing the ratio means there is a higher chance of + particles reflecting and interfering with depth sensing, making the + emitter appear more dense. Decreasing the ratio decreases the chance + of particles reflecting and interfering with depth sensing, making it + appear less dense. + + + + + + diff --git a/sdf/1.11/physics.sdf b/sdf/1.11/physics.sdf new file mode 100644 index 000000000..f644ae9fd --- /dev/null +++ b/sdf/1.11/physics.sdf @@ -0,0 +1,238 @@ + + + The physics tag specifies the type and properties of the dynamics engine. + + + The name of this set of physics parameters. + + + + If true, this physics element is set as the default physics profile for the world. If multiple default physics elements exist, the first element marked as default is chosen. If no default physics element exists, the first physics element is chosen. + + + + The type of the dynamics engine. Current options are ode, bullet, simbody and dart. Defaults to ode if left unspecified. + + + + Maximum time step size at which every system in simulation can interact with the states of the world. (was physics.sdf's dt). + + + + + target simulation speedup factor, defined by ratio of simulation time to real-time. + + + + + Rate at which to update the physics engine (UpdatePhysics calls per real-time second). (was physics.sdf's update_rate). + + + + Maximum number of contacts allowed between two entities. This value can be over ridden by a max_contacts element in a collision element. + + + + DART specific physics properties + + + + One of the following types: pgs, dantzig. PGS stands for Projected Gauss-Seidel. + + + + Specify collision detector for DART to use. Can be dart, fcl, bullet or ode. + + + + + Simbody specific physics properties + + (Currently not used in simbody) The time duration which advances with each iteration of the dynamics engine, this has to be no bigger than max_step_size under physics block. If left unspecified, min_step_size defaults to max_step_size. + + + Roughly the relative error of the system. + -LOG(accuracy) is roughly the number of significant digits. + + + Tolerable "slip" velocity allowed by the solver when static + friction is supposed to hold object in place. + + + vc + Assume real COR=1 when v=0. + e_min = given minimum COR, at v >= vp (a.k.a. plastic_coef_restitution) + d = slope = (1-e_min)/vp + OR, e_min = 1 - d*vp + e_max = maximum COR = 1-d*vc, reached at v=vc + e = 0, v <= vc + = 1 - d*v, vc < v < vp + = e_min, v >= vp + + dissipation factor = d*min(v,vp) [compliant] + cor = e [rigid] + + Combining rule e = 0, e1==e2==0 + = 2*e1*e2/(e1+e2), otherwise]]> + + + + Default contact material stiffness + (force/dist or torque/radian). + + + dissipation coefficient to be used in compliant contact; + if not given it is (1-min_cor)/plastic_impact_velocity + + + + this is the COR to be used at high velocities for rigid + impacts; if not given it is 1 - dissipation*plastic_impact_velocity + + + + + smallest impact velocity at which min COR is reached; set + to zero if you want the min COR always to be used + + + + static friction (mu_s) as described by this plot: http://gazebosim.org/wiki/File:Stribeck_friction.png + + + dynamic friction (mu_d) as described by this plot: http://gazebosim.org/wiki/File:Stribeck_friction.png + + + viscous friction (mu_v) with units of (1/velocity) as described by this plot: http://gazebosim.org/wiki/File:Stribeck_friction.png + + + + for rigid impacts only, impact velocity at which + COR is set to zero; normally inherited from global default but can + be overridden here. Combining rule: use larger velocity + + + + This is the largest slip velocity at which + we'll consider a transition to stiction. Normally inherited + from a global default setting. For a continuous friction model + this is the velocity at which the max static friction force + is reached. Combining rule: use larger velocity + + + + + + + Bullet specific physics properties + + + + One of the following types: sequential_impulse only. + + + The time duration which advances with each iteration of the dynamics engine, this has to be no bigger than max_step_size under physics block. If left unspecified, min_step_size defaults to max_step_size. + + + Number of iterations for each step. A higher number produces greater accuracy at a performance cost. + + + Set the successive over-relaxation parameter. + + + + + Bullet constraint parameters. + + Constraint force mixing parameter. See the ODE page for more information. + + + Error reduction parameter. See the ODE page for more information. + + + The depth of the surface layer around all geometry objects. Contacts are allowed to sink into the surface layer up to the given depth before coming to rest. The default value is zero. Increasing this to some small value (e.g. 0.001) can help prevent jittering problems due to contacts being repeatedly made and broken. + + + Similar to ODE's max_vel implementation. See http://web.archive.org/web/20120430155635/http://bulletphysics.org/mediawiki-1.5.8/index.php/BtContactSolverInfo#Split_Impulse for more information. + + + Similar to ODE's max_vel implementation. See http://web.archive.org/web/20120430155635/http://bulletphysics.org/mediawiki-1.5.8/index.php/BtContactSolverInfo#Split_Impulse for more information. + + + + + + ODE specific physics properties + + + + One of the following types: world, quick + + + The time duration which advances with each iteration of the dynamics engine, this has to be no bigger than max_step_size under physics block. If left unspecified, min_step_size defaults to max_step_size. + + + Number of threads to use for "islands" of disconnected models. + + + Number of iterations for each step. A higher number produces greater accuracy at a performance cost. + + + Experimental parameter. + + + Set the successive over-relaxation parameter. + + + Flag to use threading to speed up position correction computation. + + + + Flag to enable dynamic rescaling of moment of inertia in constrained directions. + See gazebo pull request 1114 for the implementation of this feature. + https://osrf-migration.github.io/gazebo-gh-pages/#!/osrf/gazebo/pull-request/1114 + + + + + Name of ODE friction model to use. Valid values include: + + pyramid_model: (default) friction forces limited in two directions + in proportion to normal force. + box_model: friction forces limited to constant in two directions. + cone_model: friction force magnitude limited in proportion to normal force. + + See gazebo pull request 1522 for the implementation of this feature. + https://osrf-migration.github.io/gazebo-gh-pages/#!/osrf/gazebo/pull-request/1522 + https://github.com/osrf/gazebo/commit/968dccafdfbfca09c9b3326f855612076fed7e6f + + + + + + ODE constraint parameters. + + Constraint force mixing parameter. See the ODE page for more information. + + + Error reduction parameter. See the ODE page for more information. + + + The maximum correcting velocities allowed when resolving contacts. + + + The depth of the surface layer around all geometry objects. Contacts are allowed to sink into the surface layer up to the given depth before coming to rest. The default value is zero. Increasing this to some small value (e.g. 0.001) can help prevent jittering problems due to contacts being repeatedly made and broken. + + + + diff --git a/sdf/1.11/plane_shape.sdf b/sdf/1.11/plane_shape.sdf new file mode 100644 index 000000000..2a82be152 --- /dev/null +++ b/sdf/1.11/plane_shape.sdf @@ -0,0 +1,9 @@ + + Plane shape + + Normal direction for the plane. When a Plane is used as a geometry for a Visual or Collision object, then the normal is specified in the Visual or Collision frame, respectively. + + + Length of each side of the plane. Note that this property is meaningful only for visualizing the Plane, i.e., when the Plane is used as a geometry for a Visual object. The Plane has infinite size when used as a geometry for a Collision object. + + diff --git a/sdf/1.11/plugin.sdf b/sdf/1.11/plugin.sdf new file mode 100644 index 000000000..4564fe4fb --- /dev/null +++ b/sdf/1.11/plugin.sdf @@ -0,0 +1,13 @@ + + + A plugin is a dynamically loaded chunk of code. It can exist as a child of world, model, and sensor. + + A name for the plugin. + + + Name of the shared library to load. If the filename is not a full path name, the file will be searched for in the configuration paths. + + + This is a special element that should not be specified in an SDFormat file. It automatically copies child elements into the SDFormat element so that a plugin can access the data. + + diff --git a/sdf/1.11/polyline_shape.sdf b/sdf/1.11/polyline_shape.sdf new file mode 100644 index 000000000..665884354 --- /dev/null +++ b/sdf/1.11/polyline_shape.sdf @@ -0,0 +1,14 @@ + + Defines an extruded polyline shape + + + + A series of points that define the path of the polyline. + + + + + Height of the polyline + + + diff --git a/sdf/1.11/population.sdf b/sdf/1.11/population.sdf new file mode 100644 index 000000000..b14ad739c --- /dev/null +++ b/sdf/1.11/population.sdf @@ -0,0 +1,58 @@ + + + + The population element defines how and where a set of models will + be automatically populated in Gazebo. + + + + + A unique name for the population. This name must not match + another population in the world. + + + + + + + + The number of models to place. + + + + + Specifies the type of object distribution and its optional parameters. + + + + + Define how the objects will be placed in the specified region. + - random: Models placed at random. + - uniform: Models approximately placed in a 2D grid pattern with control + over the number of objects. + - grid: Models evenly placed in a 2D grid pattern. The number of objects + is not explicitly specified, it is based on the number of rows and + columns of the grid. + - linear-x: Models evently placed in a row along the global x-axis. + - linear-y: Models evently placed in a row along the global y-axis. + - linear-z: Models evently placed in a row along the global z-axis. + + + + + Number of rows in the grid. + + + Number of columns in the grid. + + + Distance between elements of the grid. + + + + + + + + + diff --git a/sdf/1.11/pose.sdf b/sdf/1.11/pose.sdf new file mode 100644 index 000000000..81bc62c3e --- /dev/null +++ b/sdf/1.11/pose.sdf @@ -0,0 +1,43 @@ + + + A pose (translation, rotation) expressed in the frame named by + @relative_to. The first three components (x, y, z) represent the position of + the element's origin (in the @relative_to frame). The rotation component + represents the orientation of the element as either a sequence of Euler + rotations (r, p, y), see http://sdformat.org/tutorials?tut=specify_pose, + or as a quaternion (x, y, z, w), where w is the real component. + + + + If specified, this pose is expressed in the named frame. The named frame + must be declared within the same scope (world/model) as the element that + has its pose specified by this tag. + + If missing, the pose is expressed in the frame of the parent XML element + of the element that contains the pose. For exceptions to this rule and + more details on the default behavior, see + http://sdformat.org/tutorials?tut=pose_frame_semantics. + + Note that @relative_to merely affects an element's initial pose and + does not affect the element's dynamic movement thereafter. + + New in v1.8: @relative_to may use frames of nested scopes. In this case, + the frame is specified using `::` as delimiter to define the scope of the + frame, e.g. `nested_model_A::nested_model_B::awesome_frame`. + + + + + 'euler_rpy' by default. Supported rotation formats are + 'euler_rpy', Euler angles representation in roll, pitch, yaw. The pose is expected to have 6 values. + 'quat_xyzw', Quaternion representation in x, y, z, w. The pose is expected to have 7 values. + + + + + + Whether or not the euler angles are in degrees, otherwise they will be interpreted as radians by default. + + + + diff --git a/sdf/1.11/projector.sdf b/sdf/1.11/projector.sdf new file mode 100644 index 000000000..a502cdfe2 --- /dev/null +++ b/sdf/1.11/projector.sdf @@ -0,0 +1,28 @@ + + + + Name of the projector + + + + Texture name + + + + Field of view + + + + + Near clip distance + + + + + far clip distance + + + + + + diff --git a/sdf/1.11/ray.sdf b/sdf/1.11/ray.sdf new file mode 100644 index 000000000..58cf68b39 --- /dev/null +++ b/sdf/1.11/ray.sdf @@ -0,0 +1,78 @@ + + These elements are specific to the ray (laser) sensor. + + + + + + + + The number of simulated rays to generate per complete laser sweep cycle. + + + + This number is multiplied by samples to determine the number of range data points returned. If resolution is less than one, range data is interpolated. If resolution is greater than one, range data is averaged. + + + + + + + + Must be greater or equal to min_angle + + + + + + + + The number of simulated rays to generate per complete laser sweep cycle. + + + + This number is multiplied by samples to determine the number of range data points returned. If resolution is less than one, range data is interpolated. If resolution is greater than one, range data is averaged. + + + + + + + + Must be greater or equal to min_angle + + + + + + + specifies range properties of each simulated ray + + The minimum distance for each ray. + + + The maximum distance for each ray. + + + Linear resolution of each ray. + + + + + The properties of the noise model that should be applied to generated scans + + The type of noise. Currently supported types are: "gaussian" (draw noise values independently for each beam from a Gaussian distribution). + + + For type "gaussian," the mean of the Gaussian distribution from which noise values are drawn. + + + For type "gaussian," the standard deviation of the Gaussian distribution from which noise values are drawn. + + + + + + + + diff --git a/sdf/1.11/rfid.sdf b/sdf/1.11/rfid.sdf new file mode 100644 index 000000000..61351dd8a --- /dev/null +++ b/sdf/1.11/rfid.sdf @@ -0,0 +1,2 @@ + + diff --git a/sdf/1.11/rfidtag.sdf b/sdf/1.11/rfidtag.sdf new file mode 100644 index 000000000..55699dc08 --- /dev/null +++ b/sdf/1.11/rfidtag.sdf @@ -0,0 +1,2 @@ + + diff --git a/sdf/1.11/road.sdf b/sdf/1.11/road.sdf new file mode 100644 index 000000000..172e48045 --- /dev/null +++ b/sdf/1.11/road.sdf @@ -0,0 +1,16 @@ + + + + Name of the road + + + + Width of the road + + + + A series of points that define the path of the road. + + + + diff --git a/sdf/1.11/root.sdf b/sdf/1.11/root.sdf new file mode 100644 index 000000000..94ecc8731 --- /dev/null +++ b/sdf/1.11/root.sdf @@ -0,0 +1,21 @@ + + SDFormat base element that can include one model, actor, light, or worlds. A user of multiple worlds could run parallel instances of simulation, or offer selection of a world at runtime. + + + + Version number of the SDFormat specification, consisting of major + and minor versions delimited by a `.` character. + A major version bump is required if older versions cannot be + automatically converted to this version. + A minor version bump is required when there are breaking changes that + can be handled by the automatic conversion functionality encoded in the + `*.convert` files. + + + + + + + + + diff --git a/sdf/1.11/scene.sdf b/sdf/1.11/scene.sdf new file mode 100644 index 000000000..1428d2c92 --- /dev/null +++ b/sdf/1.11/scene.sdf @@ -0,0 +1,86 @@ + + + Specifies the look of the environment. + + + Color of the ambient light. + + + + Color of the background. + + + + Properties for the sky + + Time of day [0..24] + + + Sunrise time [0..24] + + + Sunset time [0..24] + + + + Information about clouds in the sky. + + Speed of the clouds + + + + Direction of the cloud movement + + + Density of clouds + + + + Average size of the clouds + + + + Ambient cloud color + + + + + The URI to a cubemap texture for a skybox. A .dds file is typically used for the cubemap. + + + + + Enable/disable shadows + + + + Controls fog + + Fog color + + + Fog type: constant, linear, quadratic + + + Distance to start of fog + + + Distance to end of fog + + + Density of fog + + + + + Enable/disable the grid + + + + Show/hide world origin indicator + + + diff --git a/sdf/1.11/schema/types.xsd b/sdf/1.11/schema/types.xsd new file mode 100644 index 000000000..3d1215ace --- /dev/null +++ b/sdf/1.11/schema/types.xsd @@ -0,0 +1,46 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sdf/1.11/sensor.sdf b/sdf/1.11/sensor.sdf new file mode 100644 index 000000000..78954738b --- /dev/null +++ b/sdf/1.11/sensor.sdf @@ -0,0 +1,82 @@ + + + The sensor tag describes the type and properties of a sensor. + + + A unique name for the sensor. This name must not match another model in the model. + + + + The type name of the sensor. By default, SDFormat supports types + air_pressure, + air_speed, + altimeter, + camera, + contact, + boundingbox_camera, boundingbox, + custom, + depth_camera, depth, + force_torque, + gps, + gpu_lidar, + gpu_ray, + imu, + lidar, + logical_camera, + magnetometer, + multicamera, + navsat, + ray, + rfid, + rfidtag, + rgbd_camera, rgbd, + segmentation_camera, segmentation, + sonar, + thermal_camera, thermal, + wireless_receiver, and + wireless_transmitter. + The "ray", "gpu_ray", and "gps" types are equivalent to "lidar", "gpu_lidar", and "navsat", respectively. It is preferred to use "lidar", "gpu_lidar", and "navsat" since "ray", "gpu_ray", and "gps" will be deprecated. The "ray", "gpu_ray", and "gps" types are maintained for legacy support. + + + + + If true the sensor will always be updated according to the update rate. + + + + The frequency at which the sensor data is generated. If left unspecified, the sensor will generate data every cycle. + + + + If true, the sensor is visualized in the GUI + + + + Name of the topic on which data is published. This is necessary for visualization + + + + If true, the sensor will publish performance metrics + + + + + + + + + + + + + + + + + + + + + + + diff --git a/sdf/1.11/sonar.sdf b/sdf/1.11/sonar.sdf new file mode 100644 index 000000000..0a048cda8 --- /dev/null +++ b/sdf/1.11/sonar.sdf @@ -0,0 +1,16 @@ + + These elements are specific to the sonar sensor. + + The sonar collision shape. Currently supported geometries are: "cone" and "sphere". + + + Minimum range + + + Max range + + + + Radius of the sonar cone at max range. This parameter is only used if geometry is "cone". + + diff --git a/sdf/1.11/sphere_shape.sdf b/sdf/1.11/sphere_shape.sdf new file mode 100644 index 000000000..73ad03e5d --- /dev/null +++ b/sdf/1.11/sphere_shape.sdf @@ -0,0 +1,6 @@ + + Sphere shape + + radius of the sphere + + diff --git a/sdf/1.11/spherical_coordinates.sdf b/sdf/1.11/spherical_coordinates.sdf new file mode 100644 index 000000000..978b15fbd --- /dev/null +++ b/sdf/1.11/spherical_coordinates.sdf @@ -0,0 +1,65 @@ + + + + Name of planetary surface model, used to determine the surface altitude + at a given latitude and longitude. The default is an ellipsoid model of + the earth based on the WGS-84 standard. It is used in Gazebo's GPS sensor + implementation. + + + + + + This field identifies how Gazebo world frame is aligned in Geographical + sense. The final Gazebo world frame orientation is obtained by rotating + a frame aligned with following notation by the field heading_deg. + Options are: + - ENU (East-North-Up) + + + + + Geodetic latitude at origin of gazebo reference frame, specified + in units of degrees. + + + + + + Longitude at origin of gazebo reference frame, specified in units + of degrees. + + + + + + Elevation of origin of gazebo reference frame, specified in meters. + + + + + + Equatorial axis of a custom surface type, specified in meters. + This is only required for custom surfaces. + + + + + + Polar axis of a custom surface type, specified in meters. + This is only required for custom surfaces. + + + + + + Heading offset of gazebo reference frame, measured as angle between + Gazebo world frame and the world_frame_orientation type. + The direction of rotation follows the right-hand rule, so a positive + angle indicates clockwise rotation (from east to north) when viewed from top-down. Note + that this is not consistent with compass heading convention. + The angle is specified in degrees. + + + + diff --git a/sdf/1.11/state.sdf b/sdf/1.11/state.sdf new file mode 100644 index 000000000..6a13e28bf --- /dev/null +++ b/sdf/1.11/state.sdf @@ -0,0 +1,41 @@ + + + + + Name of the world this state applies to + + + + Simulation time stamp of the state [seconds nanoseconds] + + + + Wall time stamp of the state [seconds nanoseconds] + + + + Real time stamp of the state [seconds nanoseconds] + + + + Number of simulation iterations. + + + + A list containing the entire description of entities inserted. + + + + + + A list of names of deleted entities/ + + The name of a deleted entity. + + + + + + + + diff --git a/sdf/1.11/surface.sdf b/sdf/1.11/surface.sdf new file mode 100644 index 000000000..a1631368d --- /dev/null +++ b/sdf/1.11/surface.sdf @@ -0,0 +1,245 @@ + + The surface parameters + + + + Bounciness coefficient of restitution, from [0...1], where 0=no bounciness. + + + Bounce capture velocity, below which effective coefficient of restitution is 0. + + + + + + + + Parameters for torsional friction + + + Torsional friction coefficient, unitless maximum ratio of + tangential stress to normal stress. + + + + + If this flag is true, + torsional friction is calculated using the "patch_radius" parameter. + If this flag is set to false, + "surface_radius" (R) and contact depth (d) + are used to compute the patch radius as sqrt(R*d). + + + + Radius of contact patch surface. + + + Surface radius on the point of contact. + + + Torsional friction parameters for ODE + + + Force dependent slip for torsional friction, + equivalent to inverse of viscous damping coefficient + with units of rad/s/(Nm). + A slip value of 0 is infinitely viscous. + + + + + + + ODE friction parameters + + + Coefficient of friction in first friction pyramid direction, + the unitless maximum ratio of force in first friction pyramid + direction to normal force. + + + + + Coefficient of friction in second friction pyramid direction, + the unitless maximum ratio of force in second friction pyramid + direction to normal force. + + + + + Unit vector specifying first friction pyramid direction in + collision-fixed reference frame. + If the friction pyramid model is in use, + and this value is set to a unit vector for one of the + colliding surfaces, + the ODE Collide callback function will align the friction pyramid directions + with a reference frame fixed to that collision surface. + If both surfaces have this value set to a vector of zeros, + the friction pyramid directions will be aligned with the world frame. + If this value is set for both surfaces, the behavior is undefined. + + + + + Force dependent slip in first friction pyramid direction, + equivalent to inverse of viscous damping coefficient + with units of m/s/N. + A slip value of 0 is infinitely viscous. + + + + + Force dependent slip in second friction pyramid direction, + equivalent to inverse of viscous damping coefficient + with units of m/s/N. + A slip value of 0 is infinitely viscous. + + + + + + + Coefficient of friction in first friction pyramid direction, + the unitless maximum ratio of force in first friction pyramid + direction to normal force. + + + + + Coefficient of friction in second friction pyramid direction, + the unitless maximum ratio of force in second friction pyramid + direction to normal force. + + + + + Unit vector specifying first friction pyramid direction in + collision-fixed reference frame. + If the friction pyramid model is in use, + and this value is set to a unit vector for one of the + colliding surfaces, + the friction pyramid directions will be aligned + with a reference frame fixed to that collision surface. + If both surfaces have this value set to a vector of zeros, + the friction pyramid directions will be aligned with the world frame. + If this value is set for both surfaces, the behavior is undefined. + + + + Coefficient of rolling friction + + + + + + + + Flag to disable contact force generation, while still allowing collision checks and contact visualization to occur. + + + Bitmask for collision filtering when collide_without_contact is on + + + + Bitmask for collision filtering. This will override collide_without_contact. Parsed as 16-bit unsigned integer. + + + + + + + + + Poisson's ratio is the unitless ratio between transverse and axial strain. + This value must lie between (-1, 0.5). Defaults to 0.3 for typical steel. + Note typical silicone elastomers have Poisson's ratio near 0.49 ~ 0.50. + + For reference, approximate values for Material:(Young's Modulus, Poisson's Ratio) + for some of the typical materials are: + Plastic: (1e8 ~ 3e9 Pa, 0.35 ~ 0.41), + Wood: (4e9 ~ 1e10 Pa, 0.22 ~ 0.50), + Aluminum: (7e10 Pa, 0.32 ~ 0.35), + Steel: (2e11 Pa, 0.26 ~ 0.31). + + + + + Young's Modulus in SI derived unit Pascal. + Defaults to -1. If value is less or equal to zero, + contact using elastic modulus (with Poisson's Ratio) is disabled. + + For reference, approximate values for Material:(Young's Modulus, Poisson's Ratio) + for some of the typical materials are: + Plastic: (1e8 ~ 3e9 Pa, 0.35 ~ 0.41), + Wood: (4e9 ~ 1e10 Pa, 0.22 ~ 0.50), + Aluminum: (7e10 Pa, 0.32 ~ 0.35), + Steel: (2e11 Pa, 0.26 ~ 0.31). + + + + + ODE contact parameters + + Soft constraint force mixing. + + + Soft error reduction parameter + + + dynamically "stiffness"-equivalent coefficient for contact joints + + + dynamically "damping"-equivalent coefficient for contact joints + + + maximum contact correction velocity truncation term. + + + minimum allowable depth before contact correction impulse is applied + + + + Bullet contact parameters + + Soft constraint force mixing. + + + Soft error reduction parameter + + + dynamically "stiffness"-equivalent coefficient for contact joints + + + dynamically "damping"-equivalent coefficient for contact joints + + + Similar to ODE's max_vel implementation. See http://bulletphysics.org/mediawiki-1.5.8/index.php/BtContactSolverInfo#Split_Impulse for more information. + + + Similar to ODE's max_vel implementation. See http://bulletphysics.org/mediawiki-1.5.8/index.php/BtContactSolverInfo#Split_Impulse for more information. + + + + + + + + soft contact pamameters based on paper: + http://www.cc.gatech.edu/graphics/projects/Sumit/homepage/papers/sigasia11/jain_softcontacts_siga11.pdf + + + This is variable k_v in the soft contacts paper. Its unit is N/m. + + + This is variable k_e in the soft contacts paper. Its unit is N/m. + + + Viscous damping of point velocity in body frame. Its unit is N/m/s. + + + Fraction of mass to be distributed among deformable nodes. + + + + + diff --git a/sdf/1.11/transceiver.sdf b/sdf/1.11/transceiver.sdf new file mode 100644 index 000000000..9f05b069d --- /dev/null +++ b/sdf/1.11/transceiver.sdf @@ -0,0 +1,34 @@ + + These elements are specific to a wireless transceiver. + + + Service set identifier (network name) + + + + Specifies the frequency of transmission in MHz + + + + Only a frequency range is filtered. Here we set the lower bound (MHz). + + + + + Only a frequency range is filtered. Here we set the upper bound (MHz). + + + + + Specifies the antenna gain in dBi + + + + Specifies the transmission power in dBm + + + + Mininum received signal power in dBm + + + diff --git a/sdf/1.11/visual.sdf b/sdf/1.11/visual.sdf new file mode 100644 index 000000000..b07f45025 --- /dev/null +++ b/sdf/1.11/visual.sdf @@ -0,0 +1,38 @@ + + + The visual properties of the link. This element specifies the shape of the object (box, cylinder, etc.) for visualization purposes. + + + Unique name for the visual element within the scope of the parent link. + + + + If true the visual will cast shadows. + + + + will be implemented in the future release. + + + + The amount of transparency( 0=opaque, 1 = fully transparent) + + + + + + + + Optional meta information for the visual. The information contained within this element should be used to provide additional feedback to an end user. + + + The layer in which this visual is displayed. The layer number is useful for programs, such as Gazebo, that put visuals in different layers for enhanced visualization. + + + + + + + + + diff --git a/sdf/1.11/world.sdf b/sdf/1.11/world.sdf new file mode 100644 index 000000000..07dafc08e --- /dev/null +++ b/sdf/1.11/world.sdf @@ -0,0 +1,72 @@ + + The world element encapsulates an entire world description including: models, scene, physics, and plugins. + + + Unique name of the world + + + + Global audio properties. + + + Device to use for audio playback. A value of "default" will use the system's default audio device. Otherwise, specify a an audio device file" + + + + + The wind tag specifies the type and properties of the wind. + + + Linear velocity of the wind. + + + + + + Include resources from a URI. Included resources can only contain one 'model', 'light' or 'actor' element. The URI can point to a directory or a file. If the URI is a directory, it must conform to the model database structure (see /tutorials?tut=composition&cat=specification&#defining-models-in-separate-files). + + + URI to a resource, such as a model + + + + Override the name of the included entity. + + + + Override the static value of the included entity. + + + + + + + The frame inside the included entity whose pose will be set by the specified pose element. If this element is specified, the pose must be specified. + + + + + The gravity vector in m/s^2, expressed in a coordinate frame defined by the spherical_coordinates tag. + + + + The magnetic vector in Tesla, expressed in a coordinate frame defined by the spherical_coordinates tag. + + + + + + + + + + + + + + + + + + + From bf33df29f933588c4ff92e4a8214ced54a1bdb12 Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Mon, 19 Jun 2023 15:49:49 +0530 Subject: [PATCH 02/88] Bumped sdf 1.10 to sdf 1.11 Signed-off-by: Jasmeet Singh --- CMakeLists.txt | 2 +- sdf/1.11/{1_9.convert => 1_10.convert} | 0 sdf/1.11/CMakeLists.txt | 6 +++--- sdf/CMakeLists.txt | 3 ++- sdf/embedSdf.py | 4 ++-- 5 files changed, 8 insertions(+), 7 deletions(-) rename sdf/1.11/{1_9.convert => 1_10.convert} (100%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 328992316..af09e1629 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -9,7 +9,7 @@ project (sdformat14 VERSION 14.0.0) # The protocol version has nothing to do with the package version. # It represents the current version of SDFormat implemented by the software -set (SDF_PROTOCOL_VERSION 1.10) +set (SDF_PROTOCOL_VERSION 1.11) OPTION(SDFORMAT_DISABLE_CONSOLE_LOGFILE "Disable the sdformat console logfile" OFF) diff --git a/sdf/1.11/1_9.convert b/sdf/1.11/1_10.convert similarity index 100% rename from sdf/1.11/1_9.convert rename to sdf/1.11/1_10.convert diff --git a/sdf/1.11/CMakeLists.txt b/sdf/1.11/CMakeLists.txt index 531202e38..7c06ab947 100644 --- a/sdf/1.11/CMakeLists.txt +++ b/sdf/1.11/CMakeLists.txt @@ -80,9 +80,9 @@ foreach(FIL ${sdfs}) VERBATIM) endforeach() -add_custom_target(schema1_10 ALL DEPENDS ${SDF_SCHEMA}) +add_custom_target(schema1_11 ALL DEPENDS ${SDF_SCHEMA}) set_source_files_properties(${SDF_SCHEMA} PROPERTIES GENERATED TRUE) -install(FILES 1_9.convert ${sdfs} DESTINATION ${CMAKE_INSTALL_FULL_DATAROOTDIR}/sdformat${PROJECT_VERSION_MAJOR}/1.10) -install(FILES ${SDF_SCHEMA} DESTINATION ${CMAKE_INSTALL_FULL_DATAROOTDIR}/sdformat${PROJECT_VERSION_MAJOR}/1.10) +install(FILES 1_10.convert ${sdfs} DESTINATION ${CMAKE_INSTALL_FULL_DATAROOTDIR}/sdformat${PROJECT_VERSION_MAJOR}/1.11) +install(FILES ${SDF_SCHEMA} DESTINATION ${CMAKE_INSTALL_FULL_DATAROOTDIR}/sdformat${PROJECT_VERSION_MAJOR}/1.11) diff --git a/sdf/CMakeLists.txt b/sdf/CMakeLists.txt index 4691138ec..d658ba0e8 100644 --- a/sdf/CMakeLists.txt +++ b/sdf/CMakeLists.txt @@ -8,6 +8,7 @@ add_subdirectory(1.7) add_subdirectory(1.8) add_subdirectory(1.9) add_subdirectory(1.10) +add_subdirectory(1.11) add_custom_target(schema) add_dependencies(schema schema1_10) @@ -30,7 +31,7 @@ execute_process( if (GZ_PROGRAM) # Update this list as new sdformat spec versions are added. - set(sdf_desc_versions 1.4 1.5 1.6 1.7 1.8 1.9 1.10) + set(sdf_desc_versions 1.4 1.5 1.6 1.7 1.8 1.9 1.10 1.11) set(description_targets) foreach(desc_ver ${sdf_desc_versions}) diff --git a/sdf/embedSdf.py b/sdf/embedSdf.py index b89607fa6..72a4f7b6e 100644 --- a/sdf/embedSdf.py +++ b/sdf/embedSdf.py @@ -6,11 +6,11 @@ # The list of supported SDF specification versions. This will let us drop # versions without removing the directories. -SUPPORTED_SDF_VERSIONS = ['1.10', '1.9', '1.8', '1.7', '1.6', '1.5', '1.4', '1.3', '1.2'] +SUPPORTED_SDF_VERSIONS = ['1.11', '1.10', '1.9', '1.8', '1.7', '1.6', '1.5', '1.4', '1.3', '1.2'] # The list of supported SDF conversions. This list includes versions that # a user can convert an existing SDF version to. -SUPPORTED_SDF_CONVERSIONS = ['1.10', '1.9', '1.8', '1.7', '1.6', '1.5', '1.4', '1.3'] +SUPPORTED_SDF_CONVERSIONS = ['1.11', '1.10', '1.9', '1.8', '1.7', '1.6', '1.5', '1.4', '1.3'] # whitespace indentation for C++ code INDENTATION = ' ' From 94505745cb5ebe173dd1d2bec5e7d501f64ab8e1 Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Tue, 27 Jun 2023 21:26:42 +0530 Subject: [PATCH 03/88] Update SDF schema version in CMakeLists.txt Signed-off-by: Jasmeet Singh --- sdf/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sdf/CMakeLists.txt b/sdf/CMakeLists.txt index d658ba0e8..d0b1c21ae 100644 --- a/sdf/CMakeLists.txt +++ b/sdf/CMakeLists.txt @@ -11,7 +11,7 @@ add_subdirectory(1.10) add_subdirectory(1.11) add_custom_target(schema) -add_dependencies(schema schema1_10) +add_dependencies(schema schema1_11) if (NOT Python3_Interpreter_FOUND) gz_build_error("Python is required to generate the C++ file with the SDF content") From 0841705eb47d67b1ee298c427a4ba34dec2bc200 Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Mon, 19 Jun 2023 15:50:38 +0530 Subject: [PATCH 04/88] Added element to --- sdf/1.11/collision.sdf | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/sdf/1.11/collision.sdf b/sdf/1.11/collision.sdf index e83406df1..daf4122db 100644 --- a/sdf/1.11/collision.sdf +++ b/sdf/1.11/collision.sdf @@ -14,6 +14,10 @@ Maximum number of contacts allowed between two entities. This value overrides the max_contacts element defined in physics. + + Density of the collision geometry. Default is the density of water 1.0 kg/m^3 + + From 1c27bfc918589cb4584f7b627766275ac58cc785 Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Mon, 19 Jun 2023 15:50:59 +0530 Subject: [PATCH 05/88] Added auto attribute to tag Signed-off-by: Jasmeet Singh --- sdf/1.11/inertial.sdf | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/sdf/1.11/inertial.sdf b/sdf/1.11/inertial.sdf index f4d31ef59..fd3cda20b 100644 --- a/sdf/1.11/inertial.sdf +++ b/sdf/1.11/inertial.sdf @@ -51,6 +51,12 @@ MathWorks documentation for working with CAD tools: https://www.mathworks.com/help/releases/R2021b/physmod/sm/ug/specify-custom-inertia.html#mw_b043ec69-835b-4ca9-8769-af2e6f1b190c + + + Set to true if you want automatic computation for the moments of inertia (ixx, iyy, izz) + and products of inertia(ixy, iyz, ixz). Defaul value is false. + + The link's moment of inertia about Co (the link's center of mass) for Ĉx. From 8a1f8a7755acbfd04453e90e954c52a2f89bf0e5 Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Mon, 26 Jun 2023 19:03:17 +0530 Subject: [PATCH 06/88] Added gz::math::Material member to Collision Signed-off-by: Jasmeet Singh --- src/Collision.cc | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/src/Collision.cc b/src/Collision.cc index d45b47877..9aba91e21 100644 --- a/src/Collision.cc +++ b/src/Collision.cc @@ -17,6 +17,7 @@ #include #include #include +#include #include "sdf/Collision.hh" #include "sdf/Error.hh" #include "sdf/Geometry.hh" @@ -46,6 +47,9 @@ class sdf::Collision::Implementation /// \brief The collision's surface parameters. public: sdf::Surface surface; + /// \brief The collision's material parameters. + public: gz::math::Material material; + /// \brief The SDF element pointer used during load. public: sdf::ElementPtr sdf; @@ -108,6 +112,16 @@ Errors Collision::Load(ElementPtr _sdf, const ParserConfig &_config) _sdf->GetElement("geometry"), _config); errors.insert(errors.end(), geomErr.begin(), geomErr.end()); + if (_sdf->HasElement("material_density")) + { + double density = _sdf->Get("material_density"); + this->dataPtr->material.SetDensity(density); + } + else + { + this->dataPtr->material.SetDensity(1.0); + } + // Load the surface parameters if they are given if (_sdf->HasElement("surface")) { From e3fc2a3941b401391d77d132afaf242ff50ec69b Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Mon, 3 Jul 2023 17:28:08 +0530 Subject: [PATCH 07/88] Added MassMatrix() functions to Box, Capsule, Cylinder, Ellipsoid & Sphere Signed-off-by: Jasmeet Singh --- include/sdf/Box.hh | 6 ++++++ include/sdf/Capsule.hh | 6 ++++++ include/sdf/Cylinder.hh | 6 ++++++ include/sdf/Ellipsoid.hh | 6 ++++++ include/sdf/Sphere.hh | 6 ++++++ src/Box.cc | 12 ++++++++++++ src/Capsule.cc | 12 ++++++++++++ src/Cylinder.cc | 7 +++++++ src/Ellipsoid.cc | 8 ++++++++ src/Sphere.cc | 8 ++++++++ 10 files changed, 77 insertions(+) diff --git a/include/sdf/Box.hh b/include/sdf/Box.hh index 44e4963c9..4bf5bfa15 100644 --- a/include/sdf/Box.hh +++ b/include/sdf/Box.hh @@ -17,8 +17,11 @@ #ifndef SDF_BOX_HH_ #define SDF_BOX_HH_ +#include + #include #include +#include #include #include #include @@ -65,6 +68,9 @@ namespace sdf /// \return A reference to a gz::math::Boxd object. public: gz::math::Boxd &Shape(); + /// \brief Calculate an return the Mass Matrix values for the Box + public: std::optional< gz::math::MassMatrix3d > MassMatrix(const double density); + /// \brief Create and return an SDF element filled with data from this /// box. /// Note that parameter passing functionality is not captured with this diff --git a/include/sdf/Capsule.hh b/include/sdf/Capsule.hh index 472fc0f89..d26662fa0 100644 --- a/include/sdf/Capsule.hh +++ b/include/sdf/Capsule.hh @@ -17,7 +17,10 @@ #ifndef SDF_CAPSULE_HH_ #define SDF_CAPSULE_HH_ +#include + #include +#include #include #include #include @@ -72,6 +75,9 @@ namespace sdf /// \return A reference to a gz::math::Capsuled object. public: gz::math::Capsuled &Shape(); + /// \brief Calculate an return the Mass Matrix values for the Capsule + public: std::optional< gz::math::MassMatrix3d > MassMatrix(const double density); + /// \brief Create and return an SDF element filled with data from this /// capsule. /// Note that parameter passing functionality is not captured with this diff --git a/include/sdf/Cylinder.hh b/include/sdf/Cylinder.hh index 65b5ee517..1395ce5db 100644 --- a/include/sdf/Cylinder.hh +++ b/include/sdf/Cylinder.hh @@ -17,7 +17,10 @@ #ifndef SDF_CYLINDER_HH_ #define SDF_CYLINDER_HH_ +#include + #include +#include #include #include #include @@ -72,6 +75,9 @@ namespace sdf /// \return A reference to a gz::math::Cylinderd object. public: gz::math::Cylinderd &Shape(); + /// \brief Calculate an return the Mass Matrix values for the Cylinder + public: std::optional< gz::math::MassMatrix3d > MassMatrix(const double density); + /// \brief Create and return an SDF element filled with data from this /// cylinder. /// Note that parameter passing functionality is not captured with this diff --git a/include/sdf/Ellipsoid.hh b/include/sdf/Ellipsoid.hh index a4cecef32..44f85c696 100644 --- a/include/sdf/Ellipsoid.hh +++ b/include/sdf/Ellipsoid.hh @@ -17,6 +17,9 @@ #ifndef SDF_ELLIPSOID_HH_ #define SDF_ELLIPSOID_HH_ +#include + +#include #include #include #include @@ -64,6 +67,9 @@ namespace sdf /// \return A reference to a gz::math::Ellipsoidd object. public: gz::math::Ellipsoidd &Shape(); + /// \brief Calculate an return the Mass Matrix values for the Cylinder + public: std::optional< gz::math::MassMatrix3d > MassMatrix(const double density); + /// \brief Create and return an SDF element filled with data from this /// ellipsoid. /// Note that parameter passing functionality is not captured with this diff --git a/include/sdf/Sphere.hh b/include/sdf/Sphere.hh index 3a103834d..28e9d1ef4 100644 --- a/include/sdf/Sphere.hh +++ b/include/sdf/Sphere.hh @@ -17,6 +17,9 @@ #ifndef SDF_SPHERE_HH_ #define SDF_SPHERE_HH_ +#include + +#include #include #include @@ -65,6 +68,9 @@ namespace sdf /// not been called. public: sdf::ElementPtr Element() const; + /// \brief Calculate an return the Mass Matrix values for the Cylinder + public: std::optional< gz::math::MassMatrix3d > MassMatrix(const double density); + /// \brief Create and return an SDF element filled with data from this /// sphere. /// Note that parameter passing functionality is not captured with this diff --git a/src/Box.cc b/src/Box.cc index 432127e48..74b196840 100644 --- a/src/Box.cc +++ b/src/Box.cc @@ -14,7 +14,11 @@ * limitations under the License. * */ +#include + #include +#include +#include #include "sdf/Box.hh" #include "sdf/parser.hh" @@ -114,6 +118,14 @@ gz::math::Boxd &Box::Shape() return this->dataPtr->box; } +///////////////////////////////////////////////// +std::optional< gz::math::MassMatrix3d > Box::MassMatrix(const double _density) +{ + gz::math::Material material = gz::math::Material(_density); + this->dataPtr->box.SetMaterial(material); + return this->dataPtr->box.MassMatrix(); +} + ///////////////////////////////////////////////// sdf::ElementPtr Box::ToElement() const { diff --git a/src/Capsule.cc b/src/Capsule.cc index a05a8b180..84189c054 100644 --- a/src/Capsule.cc +++ b/src/Capsule.cc @@ -14,7 +14,11 @@ * limitations under the License. * */ +#include #include + +#include +#include #include "sdf/Capsule.hh" #include "sdf/parser.hh" @@ -136,6 +140,14 @@ gz::math::Capsuled &Capsule::Shape() return this->dataPtr->capsule; } +///////////////////////////////////////////////// +std::optional< gz::math::MassMatrix3d > Capsule::MassMatrix(const double _density) +{ + gz::math::Material material = gz::math::Material(_density); + this->dataPtr->capsule.SetMat(material); + return this->dataPtr->capsule.MassMatrix(); +} + ///////////////////////////////////////////////// sdf::ElementPtr Capsule::ToElement() const { diff --git a/src/Cylinder.cc b/src/Cylinder.cc index 092ebfe45..9fd66553c 100644 --- a/src/Cylinder.cc +++ b/src/Cylinder.cc @@ -136,6 +136,13 @@ gz::math::Cylinderd &Cylinder::Shape() return this->dataPtr->cylinder; } +std::optional< gz::math::MassMatrix3d > Cylinder::MassMatrix(const double _density) +{ + gz::math::Material material = gz::math::Material(_density); + this->dataPtr->cylinder.SetMat(material); + return this->dataPtr->cylinder.MassMatrix(); +} + ///////////////////////////////////////////////// sdf::ElementPtr Cylinder::ToElement() const { diff --git a/src/Ellipsoid.cc b/src/Ellipsoid.cc index 9b75b8ace..2139b9cdf 100644 --- a/src/Ellipsoid.cc +++ b/src/Ellipsoid.cc @@ -115,6 +115,14 @@ gz::math::Ellipsoidd &Ellipsoid::Shape() return this->dataPtr->ellipsoid; } +///////////////////////////////////////////////// +std::optional< gz::math::MassMatrix3d > Ellipsoid::MassMatrix(const double _density) +{ + gz::math::Material material = gz::math::Material(_density); + this->dataPtr->ellipsoid.SetMat(material); + return this->dataPtr->ellipsoid.MassMatrix(); +} + ///////////////////////////////////////////////// sdf::ElementPtr Ellipsoid::ToElement() const { diff --git a/src/Sphere.cc b/src/Sphere.cc index c918742c4..9baee6606 100644 --- a/src/Sphere.cc +++ b/src/Sphere.cc @@ -113,6 +113,14 @@ sdf::ElementPtr Sphere::Element() const return this->dataPtr->sdf; } +///////////////////////////////////////////////// +std::optional< gz::math::MassMatrix3d > Sphere::MassMatrix(const double _density) +{ + gz::math::Material material = gz::math::Material(_density); + this->dataPtr->sphere.SetMaterial(material); + return this->dataPtr->sphere.MassMatrix(); +} + ///////////////////////////////////////////////// sdf::ElementPtr Sphere::ToElement() const { From dbe7d3e81d5b094a9e6e0a81893ddc2503cd11cc Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Mon, 3 Jul 2023 17:29:24 +0530 Subject: [PATCH 08/88] Added MassMatrix() functions to sdf::Geometry class Signed-off-by: Jasmeet Singh --- include/sdf/Geometry.hh | 11 +++++++++++ src/Geometry.cc | 29 +++++++++++++++++++++++++++++ 2 files changed, 40 insertions(+) diff --git a/include/sdf/Geometry.hh b/include/sdf/Geometry.hh index 5ff016f97..fa45e752e 100644 --- a/include/sdf/Geometry.hh +++ b/include/sdf/Geometry.hh @@ -18,8 +18,10 @@ #define SDF_GEOMETRY_HH_ #include +#include #include +#include #include #include #include @@ -209,6 +211,15 @@ namespace sdf /// \param[in] _heightmap The heightmap shape. public: void SetHeightmapShape(const Heightmap &_heightmap); + /// \brief Calculate and return the Mass Matrix values for the Geometry + /// \param[in] _xxyyzz A vector 3d representing the diagonal elements + /// of the mass matrix + /// \param[in] _xyxzyx A vector 3d representing the off-diagonal elements + /// of the mass matrix + /// \param[in] _density The density of the geometry element. + /// \return Boolean that indicates whether the calculation was successfull + public: std::optional< gz::math::MassMatrix3d > MassMatrix(const double _density); + /// \brief Get a pointer to the SDF element that was used during /// load. /// \return SDF element pointer. The value will be nullptr if Load has diff --git a/src/Geometry.cc b/src/Geometry.cc index e31a5e69b..a82c48395 100644 --- a/src/Geometry.cc +++ b/src/Geometry.cc @@ -17,6 +17,7 @@ #include +#include #include "sdf/Geometry.hh" #include "sdf/Box.hh" #include "sdf/Capsule.hh" @@ -303,6 +304,34 @@ void Geometry::SetPolylineShape(const std::vector &_polylines) this->dataPtr->polylines = _polylines; } +std::optional< gz::math::MassMatrix3d > Geometry::MassMatrix(const double _density) +{ + std::optional< gz::math::MassMatrix3d > massMat; + + switch (this->dataPtr->type) + { + case GeometryType::BOX: + massMat = this->dataPtr->box->MassMatrix(_density); + break; + case GeometryType::CYLINDER: + massMat = this->dataPtr->cylinder->MassMatrix(_density); + break; + case GeometryType::SPHERE: + massMat = this->dataPtr->sphere->MassMatrix(_density); + break; + case GeometryType::CAPSULE: + massMat = this->dataPtr->capsule->MassMatrix(_density); + break; + case GeometryType::ELLIPSOID: + massMat = this->dataPtr->ellipsoid->MassMatrix(_density); + break; + default: + break; + } + + return massMat; +} + ///////////////////////////////////////////////// sdf::ElementPtr Geometry::Element() const { From 45a67bd7939271376aa0b45ea96e75b126cb3907 Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Mon, 3 Jul 2023 17:34:36 +0530 Subject: [PATCH 09/88] Replaced gz::math::Material member of sdf::Collision with double density Signed-off-by: Jasmeet Singh --- src/Collision.cc | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/src/Collision.cc b/src/Collision.cc index 9aba91e21..da773322a 100644 --- a/src/Collision.cc +++ b/src/Collision.cc @@ -17,7 +17,6 @@ #include #include #include -#include #include "sdf/Collision.hh" #include "sdf/Error.hh" #include "sdf/Geometry.hh" @@ -47,8 +46,8 @@ class sdf::Collision::Implementation /// \brief The collision's surface parameters. public: sdf::Surface surface; - /// \brief The collision's material parameters. - public: gz::math::Material material; + /// \brief Density of the collision. Default is 1.0 + public: double density; /// \brief The SDF element pointer used during load. public: sdf::ElementPtr sdf; @@ -112,14 +111,17 @@ Errors Collision::Load(ElementPtr _sdf, const ParserConfig &_config) _sdf->GetElement("geometry"), _config); errors.insert(errors.end(), geomErr.begin(), geomErr.end()); + // Set the density value for the collision material if (_sdf->HasElement("material_density")) { - double density = _sdf->Get("material_density"); - this->dataPtr->material.SetDensity(density); + this->dataPtr->density = _sdf->Get("material_density"); } else { - this->dataPtr->material.SetDensity(1.0); + this->dataPtr->density = 1.0; + errors.push_back({ErrorCode::ELEMENT_MISSING, + "Collision is missing a child " + "element. Using a density of 1.0"}); } // Load the surface parameters if they are given From db76c5f32565c1ea3e812b65957f8f28dee535c0 Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Mon, 3 Jul 2023 17:35:36 +0530 Subject: [PATCH 10/88] Added getter setter functions for density in sdf::Collision Signed-off-by: Jasmeet Singh --- include/sdf/Collision.hh | 10 ++++++++++ src/Collision.cc | 12 ++++++++++++ 2 files changed, 22 insertions(+) diff --git a/include/sdf/Collision.hh b/include/sdf/Collision.hh index 26b5dd843..bdf4ac096 100644 --- a/include/sdf/Collision.hh +++ b/include/sdf/Collision.hh @@ -75,6 +75,16 @@ namespace sdf /// \param[in] _name Name of the collision. public: void SetName(const std::string &_name); + /// \brief Get the density of the collision. + /// The density of the collision must be unique within the scope of a Link. + /// \return Density of the collision. + public: double Density() const; + + /// \brief Set the density of the collision. + /// The density of the collision must be unique within the scope of a Link. + /// \param[in] _name Density of the collision. + public: void SetDensity(const double density); + /// \brief Get a pointer to the collisions's geometry. /// \return The collision's geometry. public: const Geometry *Geom() const; diff --git a/src/Collision.cc b/src/Collision.cc index da773322a..8f002af1b 100644 --- a/src/Collision.cc +++ b/src/Collision.cc @@ -145,6 +145,18 @@ void Collision::SetName(const std::string &_name) this->dataPtr->name = _name; } +///////////////////////////////////////////////// +double Collision::Density() const +{ + return this->dataPtr->density; +} + +///////////////////////////////////////////////// +void Collision::SetDensity(const double _density) +{ + this->dataPtr->density = _density; +} + ///////////////////////////////////////////////// const Geometry *Collision::Geom() const { From 729048382567afffd23a2fe2b4a06ccc7575b4b6 Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Mon, 3 Jul 2023 17:36:22 +0530 Subject: [PATCH 11/88] Added MassMatrix() function to sdf::Collision class Signed-off-by: Jasmeet Singh --- include/sdf/Collision.hh | 11 +++++++++++ src/Collision.cc | 30 ++++++++++++++++++++++++++++++ 2 files changed, 41 insertions(+) diff --git a/include/sdf/Collision.hh b/include/sdf/Collision.hh index bdf4ac096..349743e39 100644 --- a/include/sdf/Collision.hh +++ b/include/sdf/Collision.hh @@ -20,6 +20,7 @@ #include #include #include +#include #include #include "sdf/Element.hh" #include "sdf/SemanticPose.hh" @@ -129,6 +130,16 @@ namespace sdf /// \return SemanticPose object for this link. public: sdf::SemanticPose SemanticPose() const; + /// \brief Calculate and return the MassMatrix for the collision + /// \param[in] _xxyyzz A vector 3d representing the diagonal elements + /// of the mass matrix + /// \param[in] _xyxzyx A vector 3d representing the off-diagonal elements + /// of the mass matrix + /// \param[in] _mass A double representing the mass of the collision + /// \return Errors, which is a vector of Error objects. Each Error includes + /// an error code and message. An empty vector indicates no error + public: Errors MassMatrix(gz::math::Vector3d &_xxyyzz, gz::math::Vector3d &_xyxzyz, double &_mass); + /// \brief Get a pointer to the SDF element that was used during /// load. /// \return SDF element pointer. The value will be nullptr if Load has diff --git a/src/Collision.cc b/src/Collision.cc index 8f002af1b..6b40eea58 100644 --- a/src/Collision.cc +++ b/src/Collision.cc @@ -228,6 +228,36 @@ sdf::SemanticPose Collision::SemanticPose() const this->dataPtr->poseRelativeToGraph); } +///////////////////////////////////////////////// +Errors Collision::MassMatrix(gz::math::Vector3d &_xxyyzz, + gz::math::Vector3d &_xyxzyx, double &_mass) +{ + Errors errors; + + auto massMat = this->dataPtr->geom.MassMatrix(this->dataPtr->density); + + if (!massMat) + { + errors.push_back({ErrorCode::LINK_INERTIA_INVALID, + "Inertia Calculated for collision: " + + this->dataPtr->name + " seems invalid."}); + } + else + { + _xxyyzz.X(massMat.value().Ixx()); + _xxyyzz.Y(massMat.value().Iyy()); + _xxyyzz.Z(massMat.value().Izz()); + + _xyxzyx.X(massMat.value().Ixy()); + _xyxzyx.Y(massMat.value().Ixz()); + _xyxzyx.Z(massMat.value().Iyz()); + + _mass = massMat.value().Mass(); + } + + return errors; +} + ///////////////////////////////////////////////// sdf::ElementPtr Collision::Element() const { From c8120ae193b25ca2f68b511c86fb421cc93601e7 Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Mon, 3 Jul 2023 17:37:49 +0530 Subject: [PATCH 12/88] Update Link::Load() to check for inertia auto attr & call MassMatrix for each collision Signed-off-by: Jasmeet Singh --- src/Link.cc | 45 +++++++++++++++++++++++++++++++++++---------- 1 file changed, 35 insertions(+), 10 deletions(-) diff --git a/src/Link.cc b/src/Link.cc index 859848c4e..585ce84e6 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -164,20 +164,39 @@ Errors Link::Load(ElementPtr _sdf, const ParserConfig &_config) if (inertialElem->HasElement("pose")) loadPose(inertialElem->GetElement("pose"), inertiaPose, inertiaFrame); - // Get the mass. - mass = inertialElem->Get("mass", 1.0).first; - if (inertialElem->HasElement("inertia")) { sdf::ElementPtr inertiaElem = inertialElem->GetElement("inertia"); - xxyyzz.X(inertiaElem->Get("ixx", 1.0).first); - xxyyzz.Y(inertiaElem->Get("iyy", 1.0).first); - xxyyzz.Z(inertiaElem->Get("izz", 1.0).first); - - xyxzyz.X(inertiaElem->Get("ixy", 0.0).first); - xyxzyz.Y(inertiaElem->Get("ixz", 0.0).first); - xyxzyz.Z(inertiaElem->Get("iyz", 0.0).first); + if (inertiaElem->Get("auto")) + { + std::cout << "Inertia is set to automatic" << std::endl; + for(auto collision : this->dataPtr->collisions) + { + std::cout << "Density of the collision is: " << collision.Density(); + std::cout << std::endl; + Errors inertiaErrors = collision.MassMatrix(xxyyzz, xyxzyz, mass); + std::cout << "Inertia of " << this->dataPtr->name << std::endl; + std::cout << xxyyzz.X() << " , " << xxyyzz.Y() << " , " << xxyyzz.Z() << std::endl; + std::cout << xyxzyz.X() << " , " << xyxzyz.Y() << " , " << xyxzyz.Z() << std::endl; + std::cout << "Mass of " << this->dataPtr->name << " is " << mass << std::endl; + errors.insert(errors.end(), inertiaErrors.begin(), + inertiaErrors.end()); + } + } + else + { + std::cout << "Inertia is not set to automatic" << std::endl; + // Get the mass. + mass = inertialElem->Get("mass", 1.0).first; + xxyyzz.X(inertiaElem->Get("ixx", 1.0).first); + xxyyzz.Y(inertiaElem->Get("iyy", 1.0).first); + xxyyzz.Z(inertiaElem->Get("izz", 1.0).first); + + xyxzyz.X(inertiaElem->Get("ixy", 0.0).first); + xyxzyz.Y(inertiaElem->Get("ixz", 0.0).first); + xyxzyz.Z(inertiaElem->Get("iyz", 0.0).first); + } } if (inertialElem->HasElement("fluid_added_mass")) @@ -236,6 +255,12 @@ Errors Link::Load(ElementPtr _sdf, const ParserConfig &_config) } } } + std::cout << "--------------------------------------" << std::endl; + std::cout << "Inertia of " << this->dataPtr->name << std::endl; + std::cout << xxyyzz.X() << " , " << xxyyzz.Y() << " , " << xxyyzz.Z() << std::endl; + std::cout << xyxzyz.X() << " , " << xyxzyz.Y() << " , " << xyxzyz.Z() << std::endl; + std::cout << "Mass of " << this->dataPtr->name << " is " << mass << std::endl; + if (!this->dataPtr->inertial.SetMassMatrix( gz::math::MassMatrix3d(mass, xxyyzz, xyxzyz))) { From 42a4707a9afae5300f08dfd9173eaf3026aa29ab Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Thu, 13 Jul 2023 21:46:54 +0530 Subject: [PATCH 13/88] Cleaned up the Code Signed-off-by: Jasmeet Singh --- src/Link.cc | 14 +------------- 1 file changed, 1 insertion(+), 13 deletions(-) diff --git a/src/Link.cc b/src/Link.cc index 585ce84e6..f43ed5bea 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -170,23 +170,16 @@ Errors Link::Load(ElementPtr _sdf, const ParserConfig &_config) if (inertiaElem->Get("auto")) { - std::cout << "Inertia is set to automatic" << std::endl; for(auto collision : this->dataPtr->collisions) { - std::cout << "Density of the collision is: " << collision.Density(); - std::cout << std::endl; + Errors inertiaErrors = collision.MassMatrix(xxyyzz, xyxzyz, mass); - std::cout << "Inertia of " << this->dataPtr->name << std::endl; - std::cout << xxyyzz.X() << " , " << xxyyzz.Y() << " , " << xxyyzz.Z() << std::endl; - std::cout << xyxzyz.X() << " , " << xyxzyz.Y() << " , " << xyxzyz.Z() << std::endl; - std::cout << "Mass of " << this->dataPtr->name << " is " << mass << std::endl; errors.insert(errors.end(), inertiaErrors.begin(), inertiaErrors.end()); } } else { - std::cout << "Inertia is not set to automatic" << std::endl; // Get the mass. mass = inertialElem->Get("mass", 1.0).first; xxyyzz.X(inertiaElem->Get("ixx", 1.0).first); @@ -255,11 +248,6 @@ Errors Link::Load(ElementPtr _sdf, const ParserConfig &_config) } } } - std::cout << "--------------------------------------" << std::endl; - std::cout << "Inertia of " << this->dataPtr->name << std::endl; - std::cout << xxyyzz.X() << " , " << xxyyzz.Y() << " , " << xxyyzz.Z() << std::endl; - std::cout << xyxzyz.X() << " , " << xyxzyz.Y() << " , " << xyxzyz.Z() << std::endl; - std::cout << "Mass of " << this->dataPtr->name << " is " << mass << std::endl; if (!this->dataPtr->inertial.SetMassMatrix( gz::math::MassMatrix3d(mass, xxyyzz, xyxzyz))) From bbacbe964780a6895d7c1cb3e74b2ee9747094a1 Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Mon, 17 Jul 2023 19:50:41 +0530 Subject: [PATCH 14/88] Corrected default density value Signed-off-by: Jasmeet Singh --- sdf/1.11/collision.sdf | 4 ++-- src/Collision.cc | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/sdf/1.11/collision.sdf b/sdf/1.11/collision.sdf index daf4122db..93b5bf425 100644 --- a/sdf/1.11/collision.sdf +++ b/sdf/1.11/collision.sdf @@ -14,8 +14,8 @@ Maximum number of contacts allowed between two entities. This value overrides the max_contacts element defined in physics. - - Density of the collision geometry. Default is the density of water 1.0 kg/m^3 + + Density of the collision geometry. Default is the density of water 1000 kg/m^3 diff --git a/src/Collision.cc b/src/Collision.cc index 6b40eea58..a383f2ed5 100644 --- a/src/Collision.cc +++ b/src/Collision.cc @@ -46,7 +46,7 @@ class sdf::Collision::Implementation /// \brief The collision's surface parameters. public: sdf::Surface surface; - /// \brief Density of the collision. Default is 1.0 + /// \brief Density of the collision. Default is 1000.0 public: double density; /// \brief The SDF element pointer used during load. @@ -118,10 +118,10 @@ Errors Collision::Load(ElementPtr _sdf, const ParserConfig &_config) } else { - this->dataPtr->density = 1.0; + this->dataPtr->density = 1000.0; errors.push_back({ErrorCode::ELEMENT_MISSING, "Collision is missing a child " - "element. Using a density of 1.0"}); + "element. Using a density of 1000.0"}); } // Load the surface parameters if they are given From b092904db4ef5c491672619076e75f4509b3369b Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Mon, 17 Jul 2023 19:56:03 +0530 Subject: [PATCH 15/88] Corrected function comments Signed-off-by: Jasmeet Singh --- include/sdf/Box.hh | 6 ++++-- include/sdf/Capsule.hh | 6 ++++-- include/sdf/Collision.hh | 4 ++-- include/sdf/Cylinder.hh | 6 ++++-- include/sdf/Ellipsoid.hh | 6 ++++-- include/sdf/Geometry.hh | 6 +----- include/sdf/Sphere.hh | 6 ++++-- 7 files changed, 23 insertions(+), 17 deletions(-) diff --git a/include/sdf/Box.hh b/include/sdf/Box.hh index 4bf5bfa15..228dd5eac 100644 --- a/include/sdf/Box.hh +++ b/include/sdf/Box.hh @@ -68,8 +68,10 @@ namespace sdf /// \return A reference to a gz::math::Boxd object. public: gz::math::Boxd &Shape(); - /// \brief Calculate an return the Mass Matrix values for the Box - public: std::optional< gz::math::MassMatrix3d > MassMatrix(const double density); + /// \brief Calculate and return the Mass Matrix values for the Box + /// \param[in] density Density of the box in kg/m^3 + /// \return A std::optional with gz::math::MassMatrix3d object or std::nullopt + public: std::optional< gz::math::MassMatrix3d > MassMatrix(const double _density); /// \brief Create and return an SDF element filled with data from this /// box. diff --git a/include/sdf/Capsule.hh b/include/sdf/Capsule.hh index d26662fa0..3348c017d 100644 --- a/include/sdf/Capsule.hh +++ b/include/sdf/Capsule.hh @@ -75,8 +75,10 @@ namespace sdf /// \return A reference to a gz::math::Capsuled object. public: gz::math::Capsuled &Shape(); - /// \brief Calculate an return the Mass Matrix values for the Capsule - public: std::optional< gz::math::MassMatrix3d > MassMatrix(const double density); + /// \brief Calculate and return the Mass Matrix values for the Capsule + /// \param[in] density Density of the capsule in kg/m^3 + /// \return A std::optional with gz::math::MassMatrix3d object or std::nullopt + public: std::optional< gz::math::MassMatrix3d > MassMatrix(const double _density); /// \brief Create and return an SDF element filled with data from this /// capsule. diff --git a/include/sdf/Collision.hh b/include/sdf/Collision.hh index 349743e39..eaf6da338 100644 --- a/include/sdf/Collision.hh +++ b/include/sdf/Collision.hh @@ -83,8 +83,8 @@ namespace sdf /// \brief Set the density of the collision. /// The density of the collision must be unique within the scope of a Link. - /// \param[in] _name Density of the collision. - public: void SetDensity(const double density); + /// \param[in] _density Density of the collision. + public: void SetDensity(const double _density); /// \brief Get a pointer to the collisions's geometry. /// \return The collision's geometry. diff --git a/include/sdf/Cylinder.hh b/include/sdf/Cylinder.hh index 1395ce5db..37fcbe35f 100644 --- a/include/sdf/Cylinder.hh +++ b/include/sdf/Cylinder.hh @@ -75,8 +75,10 @@ namespace sdf /// \return A reference to a gz::math::Cylinderd object. public: gz::math::Cylinderd &Shape(); - /// \brief Calculate an return the Mass Matrix values for the Cylinder - public: std::optional< gz::math::MassMatrix3d > MassMatrix(const double density); + /// \brief Calculate and return the Mass Matrix values for the Cylinder + /// \param[in] density Density of the cylinder in kg/m^3 + /// \return A std::optional with gz::math::MassMatrix3d object or std::nullopt + public: std::optional< gz::math::MassMatrix3d > MassMatrix(const double _density); /// \brief Create and return an SDF element filled with data from this /// cylinder. diff --git a/include/sdf/Ellipsoid.hh b/include/sdf/Ellipsoid.hh index 44f85c696..1c945d2bf 100644 --- a/include/sdf/Ellipsoid.hh +++ b/include/sdf/Ellipsoid.hh @@ -67,8 +67,10 @@ namespace sdf /// \return A reference to a gz::math::Ellipsoidd object. public: gz::math::Ellipsoidd &Shape(); - /// \brief Calculate an return the Mass Matrix values for the Cylinder - public: std::optional< gz::math::MassMatrix3d > MassMatrix(const double density); + /// \brief Calculate and return the Mass Matrix values for the Ellipsoid + /// \param[in] density Density of the ellipsoid in kg/m^3 + /// \return A std::optional with gz::math::MassMatrix3d object or std::nullopt + public: std::optional< gz::math::MassMatrix3d > MassMatrix(const double _density); /// \brief Create and return an SDF element filled with data from this /// ellipsoid. diff --git a/include/sdf/Geometry.hh b/include/sdf/Geometry.hh index fa45e752e..a097250cf 100644 --- a/include/sdf/Geometry.hh +++ b/include/sdf/Geometry.hh @@ -212,12 +212,8 @@ namespace sdf public: void SetHeightmapShape(const Heightmap &_heightmap); /// \brief Calculate and return the Mass Matrix values for the Geometry - /// \param[in] _xxyyzz A vector 3d representing the diagonal elements - /// of the mass matrix - /// \param[in] _xyxzyx A vector 3d representing the off-diagonal elements - /// of the mass matrix /// \param[in] _density The density of the geometry element. - /// \return Boolean that indicates whether the calculation was successfull + /// \return A std::optional with gz::math::MassMatrix3d object or std::nullopt public: std::optional< gz::math::MassMatrix3d > MassMatrix(const double _density); /// \brief Get a pointer to the SDF element that was used during diff --git a/include/sdf/Sphere.hh b/include/sdf/Sphere.hh index 28e9d1ef4..375517c36 100644 --- a/include/sdf/Sphere.hh +++ b/include/sdf/Sphere.hh @@ -68,8 +68,10 @@ namespace sdf /// not been called. public: sdf::ElementPtr Element() const; - /// \brief Calculate an return the Mass Matrix values for the Cylinder - public: std::optional< gz::math::MassMatrix3d > MassMatrix(const double density); + /// \brief Calculate and return the Mass Matrix values for the Sphere + /// \param[in] density Density of the sphere in kg/m^3 + /// \return A std::optional with gz::math::MassMatrix3d object or std::nullopt + public: std::optional< gz::math::MassMatrix3d > MassMatrix(const double _density); /// \brief Create and return an SDF element filled with data from this /// sphere. From c3c52aa223b69e6c406b327ab2c3cd1d8c2cb152 Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Tue, 1 Aug 2023 02:31:54 +0530 Subject: [PATCH 16/88] Changed tag to Signed-off-by: Jasmeet Singh --- sdf/1.11/collision.sdf | 8 ++++++-- src/Collision.cc | 9 ++++----- 2 files changed, 10 insertions(+), 7 deletions(-) diff --git a/sdf/1.11/collision.sdf b/sdf/1.11/collision.sdf index 93b5bf425..496d69a97 100644 --- a/sdf/1.11/collision.sdf +++ b/sdf/1.11/collision.sdf @@ -14,8 +14,12 @@ Maximum number of contacts allowed between two entities. This value overrides the max_contacts element defined in physics. - - Density of the collision geometry. Default is the density of water 1000 kg/m^3 + + + Mass Density of the collision geometry. + This is used to determine mass and inertia values during automatic calculation. + Default is the density of water 1000 kg/m^3. + diff --git a/src/Collision.cc b/src/Collision.cc index a383f2ed5..5e17ffdff 100644 --- a/src/Collision.cc +++ b/src/Collision.cc @@ -47,7 +47,7 @@ class sdf::Collision::Implementation public: sdf::Surface surface; /// \brief Density of the collision. Default is 1000.0 - public: double density; + public: double density{1000.0}; /// \brief The SDF element pointer used during load. public: sdf::ElementPtr sdf; @@ -112,15 +112,14 @@ Errors Collision::Load(ElementPtr _sdf, const ParserConfig &_config) errors.insert(errors.end(), geomErr.begin(), geomErr.end()); // Set the density value for the collision material - if (_sdf->HasElement("material_density")) + if (_sdf->HasElement("density")) { - this->dataPtr->density = _sdf->Get("material_density"); + this->dataPtr->density = _sdf->Get("density"); } else { - this->dataPtr->density = 1000.0; errors.push_back({ErrorCode::ELEMENT_MISSING, - "Collision is missing a child " + "Collision is missing a child " "element. Using a density of 1000.0"}); } From 8ff8726b1181e964a9384092179cf1406fe1f3f8 Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Thu, 13 Jul 2023 20:57:58 +0530 Subject: [PATCH 17/88] Updated Collision:MassMatrix() to use gz::math::Inertial object as parameter Signed-off-by: Jasmeet Singh --- include/sdf/Collision.hh | 3 ++- src/Collision.cc | 19 ++++++++----------- 2 files changed, 10 insertions(+), 12 deletions(-) diff --git a/include/sdf/Collision.hh b/include/sdf/Collision.hh index eaf6da338..804f0a68c 100644 --- a/include/sdf/Collision.hh +++ b/include/sdf/Collision.hh @@ -21,6 +21,7 @@ #include #include #include +#include #include #include "sdf/Element.hh" #include "sdf/SemanticPose.hh" @@ -138,7 +139,7 @@ namespace sdf /// \param[in] _mass A double representing the mass of the collision /// \return Errors, which is a vector of Error objects. Each Error includes /// an error code and message. An empty vector indicates no error - public: Errors MassMatrix(gz::math::Vector3d &_xxyyzz, gz::math::Vector3d &_xyxzyz, double &_mass); + public: Errors MassMatrix(gz::math::Inertiald &_inertial); /// \brief Get a pointer to the SDF element that was used during /// load. diff --git a/src/Collision.cc b/src/Collision.cc index 5e17ffdff..c233df4f6 100644 --- a/src/Collision.cc +++ b/src/Collision.cc @@ -17,6 +17,7 @@ #include #include #include +#include #include "sdf/Collision.hh" #include "sdf/Error.hh" #include "sdf/Geometry.hh" @@ -228,8 +229,7 @@ sdf::SemanticPose Collision::SemanticPose() const } ///////////////////////////////////////////////// -Errors Collision::MassMatrix(gz::math::Vector3d &_xxyyzz, - gz::math::Vector3d &_xyxzyx, double &_mass) +Errors Collision::MassMatrix(gz::math::Inertiald &_inertial) { Errors errors; @@ -243,15 +243,12 @@ Errors Collision::MassMatrix(gz::math::Vector3d &_xxyyzz, } else { - _xxyyzz.X(massMat.value().Ixx()); - _xxyyzz.Y(massMat.value().Iyy()); - _xxyyzz.Z(massMat.value().Izz()); - - _xyxzyx.X(massMat.value().Ixy()); - _xyxzyx.Y(massMat.value().Ixz()); - _xyxzyx.Z(massMat.value().Iyz()); - - _mass = massMat.value().Mass(); + if(!_inertial.SetMassMatrix(massMat.value())) + { + errors.push_back({ErrorCode::LINK_INERTIA_INVALID, + "Inertia Calculated for collision: " + + this->dataPtr->name + " seems invalid"}); + } } return errors; From 0cd267b6064b56121f3ab5623033e45ec2a5e9f7 Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Thu, 13 Jul 2023 20:58:38 +0530 Subject: [PATCH 18/88] Updated Link::Load() to add inertials from multiple collisions Signed-off-by: Jasmeet Singh --- src/Link.cc | 46 +++++++++++++++++++++++++++++++++++++++++++--- 1 file changed, 43 insertions(+), 3 deletions(-) diff --git a/src/Link.cc b/src/Link.cc index f43ed5bea..3d7529525 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -170,13 +170,51 @@ Errors Link::Load(ElementPtr _sdf, const ParserConfig &_config) if (inertiaElem->Get("auto")) { + std::cout << "Inertia is set to automatic" << std::endl; + gz::math::Inertiald totalInertia; + for(auto collision : this->dataPtr->collisions) { - - Errors inertiaErrors = collision.MassMatrix(xxyyzz, xyxzyz, mass); + gz::math::Inertiald collisionInertia; + std::cout << "Density of the collision is: " << collision.Density(); + std::cout << std::endl; + Errors inertiaErrors = collision.MassMatrix(collisionInertia); + + // Get pose of collision w.r.t inertial frame + collisionInertia.SetPose(inertiaPose); + std::string collisionName = collision.Name(); + Errors resolvePoseErrors = this->ResolveInertial(collisionInertia, collisionName); + std::cout << "Pose of Inertia w.r.t Collision: "; + std::cout << collisionInertia.Pose() << std::endl; + std::cout << "Pose of collision w.r.t Inertial: "; + std::cout << collisionInertia.Pose().Inverse() << std::endl; + collisionInertia.SetPose(collisionInertia.Pose().Inverse()); + errors.insert(errors.end(), resolvePoseErrors.begin(), resolvePoseErrors.end()); + + std::cout << "Inertia of " << collision.Name() << std::endl; + std::cout << collisionInertia.MassMatrix().Ixx() << " , " << collisionInertia.MassMatrix().Iyy() << " , " << collisionInertia.MassMatrix().Izz() << std::endl; + std::cout << xyxzyz.X() << " , " << xyxzyz.Y() << " , " << xyxzyz.Z() << std::endl; + std::cout << "Mass of " << collision.Name() << " is " << collisionInertia.MassMatrix().Mass() << std::endl; errors.insert(errors.end(), inertiaErrors.begin(), inertiaErrors.end()); + + totalInertia = totalInertia + collisionInertia; } + + std::cout << "--------------------------------------" << std::endl; + mass = totalInertia.MassMatrix().Mass(); + xxyyzz.X() = totalInertia.MassMatrix().Ixx(); + xxyyzz.Y() = totalInertia.MassMatrix().Iyy(); + xxyyzz.Z() = totalInertia.MassMatrix().Izz(); + + xyxzyz.X() = totalInertia.MassMatrix().Ixy(); + xyxzyz.Y() = totalInertia.MassMatrix().Ixz(); + xyxzyz.Z() = totalInertia.MassMatrix().Iyz(); + + std::cout << "Total Inertia of " << this->dataPtr->name << std::endl; + std::cout << xxyyzz.X() << " , " << xxyyzz.Y() << " , " << xxyyzz.Z() << std::endl; + std::cout << xyxzyz.X() << " , " << xyxzyz.Y() << " , " << xyxzyz.Z() << std::endl; + std::cout << "Total Mass of " << this->dataPtr->name << " is " << mass << std::endl; } else { @@ -505,7 +543,9 @@ Errors Link::ResolveInertial( auto errors = this->SemanticPose().Resolve(linkPose, _resolveTo); if (errors.empty()) { - _inertial = this->dataPtr->inertial; + //_inertial = this->dataPtr->inertial; + std::cout << "linkPose: " << linkPose << std::endl; + std::cout << "inertial.Pose(): " << _inertial.Pose() << std::endl; _inertial.SetPose(linkPose * _inertial.Pose()); } return errors; From 6881c2d5aa28aff8feb60803f27cf5bcbdac8279 Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Fri, 4 Aug 2023 21:18:33 +0530 Subject: [PATCH 19/88] Shifted auto attribute from to Signed-off-by: Jasmeet Singh --- sdf/1.11/inertial.sdf | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/sdf/1.11/inertial.sdf b/sdf/1.11/inertial.sdf index fd3cda20b..9e1d4a739 100644 --- a/sdf/1.11/inertial.sdf +++ b/sdf/1.11/inertial.sdf @@ -5,6 +5,13 @@ properties, and optionally its fluid added mass. + + + Set to true if you want automatic computation for the moments of inertia (ixx, iyy, izz) + and products of inertia(ixy, iyz, ixz). Default value is false. + + + The mass of the link. @@ -51,12 +58,6 @@ MathWorks documentation for working with CAD tools: https://www.mathworks.com/help/releases/R2021b/physmod/sm/ug/specify-custom-inertia.html#mw_b043ec69-835b-4ca9-8769-af2e6f1b190c - - - Set to true if you want automatic computation for the moments of inertia (ixx, iyy, izz) - and products of inertia(ixy, iyz, ixz). Defaul value is false. - - The link's moment of inertia about Co (the link's center of mass) for Ĉx. From dd0551abbc1a73278e7bbd6296ab183d3b0cf2e1 Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Tue, 8 Aug 2023 03:09:58 +0530 Subject: [PATCH 20/88] Added CalculateInertial() function to sdf::Root Signed-off-by: Jasmeet Singh --- include/sdf/Root.hh | 8 ++++++++ src/Root.cc | 22 ++++++++++++++++++++++ 2 files changed, 30 insertions(+) diff --git a/include/sdf/Root.hh b/include/sdf/Root.hh index 1d88defcc..600371769 100644 --- a/include/sdf/Root.hh +++ b/include/sdf/Root.hh @@ -226,6 +226,14 @@ namespace sdf /// an error code and message. An empty vector indicates no error. public: Errors UpdateGraphs(); + /// \brief Calculate and set the inertial properties (mass, mass matrix + /// and inertial pose) for all the worlds and models that are children of this + /// Root object. This function can be called after calling UpdateGraphs() since + /// it uses frame graphs to resolve inertial pose for links. + /// \return Errors, which is a vector of Error objects. Each Error includes + /// an error code and message. An empty vector indicates no error. + public: Errors CalculateInertials(); + /// \brief Create and return an SDF element filled with data from this /// root. /// Note that parameter passing functionality is not captured with this diff --git a/src/Root.cc b/src/Root.cc index 7e57e5673..c2df4eca1 100644 --- a/src/Root.cc +++ b/src/Root.cc @@ -326,6 +326,7 @@ Errors Root::Load(SDFPtr _sdf, const ParserConfig &_config) this->dataPtr->modelLightOrActor = std::move(models.front()); sdf::Model &model = std::get(this->dataPtr->modelLightOrActor); this->dataPtr->UpdateGraphs(model, errors); + model.CalculateInertials(errors); } // Load all the lights. @@ -587,6 +588,27 @@ void Root::Implementation::UpdateGraphs(sdf::Model &_model, _model.SetPoseRelativeToGraph(this->modelPoseRelativeToGraph); } +///////////////////////////////////////////////// +Errors Root::CalculateInertials() +{ + sdf::Errors errors; + + // Calculate and set Inertials for all the worlds + for (sdf::World &world : this->dataPtr->worlds) + { + world.CalculateInertials(errors); + } + + // Calculate and set Inertials for the model, if it is present + if (std::holds_alternative(this->dataPtr->modelLightOrActor)) + { + sdf::Model &model = std::get(this->dataPtr->modelLightOrActor); + model.CalculateInertials(errors); + } + + return errors; +} + ///////////////////////////////////////////////// sdf::ElementPtr Root::ToElement(const OutputConfig &_config) const { From 8d0c08422217734fac9fbec54654b4612b5357a6 Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Tue, 8 Aug 2023 03:10:29 +0530 Subject: [PATCH 21/88] Added CalculateInertial() function to sdf::Model Signed-off-by: Jasmeet Singh --- include/sdf/Model.hh | 6 ++++++ src/Model.cc | 21 +++++++++++++++++++++ 2 files changed, 27 insertions(+) diff --git a/include/sdf/Model.hh b/include/sdf/Model.hh index bb747d7be..3101ee521 100644 --- a/include/sdf/Model.hh +++ b/include/sdf/Model.hh @@ -508,6 +508,12 @@ namespace sdf /// \param[in] _plugin Plugin to add. public: void AddPlugin(const Plugin &_plugin); + /// \brief Calculate and set the inertials for all the links belonging + /// to the model object + /// \param[out] _errrors A vector of Errors objects. Each errors contains an + /// Error code and a message. An empty errors vector indicates no errors + public: void CalculateInertials(sdf::Errors &_errors); + /// \brief Give the scoped PoseRelativeToGraph to be used for resolving /// poses. This is private and is intended to be called by Root::Load or /// World::SetPoseRelativeToGraph if this is a standalone model and diff --git a/src/Model.cc b/src/Model.cc index 4f4f7b0f3..9ae4bc248 100644 --- a/src/Model.cc +++ b/src/Model.cc @@ -836,6 +836,27 @@ void Model::SetPoseRelativeTo(const std::string &_frame) this->dataPtr->poseRelativeTo = _frame; } +///////////////////////////////////////////////// +void Model::CalculateInertials(sdf::Errors &_errors) +{ + // Check if there are nested models. If yes then call + // CalculateInertials() for each model + if (!this->dataPtr->models.empty()) + { + for (sdf::Model &model : this->dataPtr->models) + { + model.CalculateInertials(_errors); + } + } + + // Calculate and set inertials for all the links in the model + for (sdf::Link &link : this->dataPtr->links) + { + link.CalculateInertials(_errors); + } + +} + ///////////////////////////////////////////////// void Model::SetPoseRelativeToGraph(sdf::ScopedGraph _graph) { From 942bf78fbf243d217818f7b77261237124e3a6f1 Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Tue, 8 Aug 2023 03:10:57 +0530 Subject: [PATCH 22/88] Added CalculateInertial() function to sdf::World Signed-off-by: Jasmeet Singh --- include/sdf/World.hh | 6 ++++++ src/World.cc | 10 ++++++++++ 2 files changed, 16 insertions(+) diff --git a/include/sdf/World.hh b/include/sdf/World.hh index 0afc85472..d83b9512b 100644 --- a/include/sdf/World.hh +++ b/include/sdf/World.hh @@ -516,6 +516,12 @@ namespace sdf /// \param[in] _plugin Plugin to add. public: void AddPlugin(const Plugin &_plugin); + /// \brief Calculate and set the inertials for all the models in the world + /// object + /// \param[out] _errrors A vector of Errors objects. Each errors contains an + /// Error code and a message. An empty errors vector indicates no errors + public: void CalculateInertials(sdf::Errors &_errors); + /// \brief Give the Scoped PoseRelativeToGraph to be passed on to child /// entities for resolving poses. This is private and is intended to be /// called by Root::Load. diff --git a/src/World.cc b/src/World.cc index aacb571c9..15205782b 100644 --- a/src/World.cc +++ b/src/World.cc @@ -780,6 +780,16 @@ const NestedInclude *World::InterfaceModelNestedIncludeByIndex( return nullptr; } +///////////////////////////////////////////////// +void World::CalculateInertials(sdf::Errors &_errors) +{ + // Call CalculateInertials() function for all the models + for (sdf::Model &model : this->dataPtr->models) + { + model.CalculateInertials(_errors); + } +} + ///////////////////////////////////////////////// void World::SetPoseRelativeToGraph(sdf::ScopedGraph _graph) { From 1eeed1e1240745f6624dceea2d56fe55cbc16602 Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Tue, 8 Aug 2023 03:11:45 +0530 Subject: [PATCH 23/88] Added CalculateInertial() function to sdf::Link to check for auto and set inertials Signed-off-by: Jasmeet Singh --- include/sdf/Link.hh | 7 ++ src/Link.cc | 210 +++++++++++++++++++++++++++++++------------- 2 files changed, 156 insertions(+), 61 deletions(-) diff --git a/include/sdf/Link.hh b/include/sdf/Link.hh index a5b3c4de7..b8a29c7e0 100644 --- a/include/sdf/Link.hh +++ b/include/sdf/Link.hh @@ -284,6 +284,13 @@ namespace sdf public: Errors ResolveInertial(gz::math::Inertiald &_inertial, const std::string &_resolveTo = "") const; + /// \brief Calculate and set inertial values (mass, mass matrix & inertial pose) + /// for the link. Inertial values can be provided by the user through the SDF + /// or can be calculated automatically by setting the auto attribute to true. + /// \param[out] _errors A vector of Errors object. Each object would contain an + /// error code and an error message. + public: void CalculateInertials(sdf::Errors &_errors); + /// \brief Get the pose of the link. This is the pose of the link /// as specified in SDF ( ... ). /// \return The pose of the link. diff --git a/src/Link.cc b/src/Link.cc index 3d7529525..44af87b1f 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -114,6 +114,8 @@ Errors Link::Load(ElementPtr _sdf, const ParserConfig &_config) "A link name is required, but the name is not set."}); } + std::cout << "Load called for link: " << this->dataPtr->name << std::endl; + // Check that the link's name is valid if (isReservedName(this->dataPtr->name)) { @@ -161,74 +163,74 @@ Errors Link::Load(ElementPtr _sdf, const ParserConfig &_config) { sdf::ElementPtr inertialElem = _sdf->GetElement("inertial"); - if (inertialElem->HasElement("pose")) - loadPose(inertialElem->GetElement("pose"), inertiaPose, inertiaFrame); + // if (inertialElem->HasElement("pose")) + // loadPose(inertialElem->GetElement("pose"), inertiaPose, inertiaFrame); - if (inertialElem->HasElement("inertia")) - { - sdf::ElementPtr inertiaElem = inertialElem->GetElement("inertia"); + // if (inertialElem->HasElement("inertia")) + // { + // sdf::ElementPtr inertiaElem = inertialElem->GetElement("inertia"); - if (inertiaElem->Get("auto")) - { - std::cout << "Inertia is set to automatic" << std::endl; - gz::math::Inertiald totalInertia; + // if (inertiaElem->Get("auto")) + // { + // std::cout << "Inertia is set to automatic" << std::endl; + // gz::math::Inertiald totalInertia; - for(auto collision : this->dataPtr->collisions) - { - gz::math::Inertiald collisionInertia; - std::cout << "Density of the collision is: " << collision.Density(); - std::cout << std::endl; - Errors inertiaErrors = collision.MassMatrix(collisionInertia); + // for(auto collision : this->dataPtr->collisions) + // { + // gz::math::Inertiald collisionInertia; + // std::cout << "Density of the collision is: " << collision.Density(); + // std::cout << std::endl; + // Errors inertiaErrors = collision.MassMatrix(collisionInertia); - // Get pose of collision w.r.t inertial frame - collisionInertia.SetPose(inertiaPose); - std::string collisionName = collision.Name(); - Errors resolvePoseErrors = this->ResolveInertial(collisionInertia, collisionName); - std::cout << "Pose of Inertia w.r.t Collision: "; - std::cout << collisionInertia.Pose() << std::endl; - std::cout << "Pose of collision w.r.t Inertial: "; - std::cout << collisionInertia.Pose().Inverse() << std::endl; - collisionInertia.SetPose(collisionInertia.Pose().Inverse()); - errors.insert(errors.end(), resolvePoseErrors.begin(), resolvePoseErrors.end()); + // // Get pose of collision w.r.t inertial frame + // collisionInertia.SetPose(inertiaPose); + // std::string collisionName = collision.Name(); + // Errors resolvePoseErrors = this->ResolveInertial(collisionInertia, collisionName); + // std::cout << "Pose of Inertia w.r.t Collision: "; + // std::cout << collisionInertia.Pose() << std::endl; + // std::cout << "Pose of collision w.r.t Inertial: "; + // std::cout << collisionInertia.Pose().Inverse() << std::endl; + // collisionInertia.SetPose(collisionInertia.Pose().Inverse()); + // errors.insert(errors.end(), resolvePoseErrors.begin(), resolvePoseErrors.end()); - std::cout << "Inertia of " << collision.Name() << std::endl; - std::cout << collisionInertia.MassMatrix().Ixx() << " , " << collisionInertia.MassMatrix().Iyy() << " , " << collisionInertia.MassMatrix().Izz() << std::endl; - std::cout << xyxzyz.X() << " , " << xyxzyz.Y() << " , " << xyxzyz.Z() << std::endl; - std::cout << "Mass of " << collision.Name() << " is " << collisionInertia.MassMatrix().Mass() << std::endl; - errors.insert(errors.end(), inertiaErrors.begin(), - inertiaErrors.end()); + // std::cout << "Inertia of " << collision.Name() << std::endl; + // std::cout << collisionInertia.MassMatrix().Ixx() << " , " << collisionInertia.MassMatrix().Iyy() << " , " << collisionInertia.MassMatrix().Izz() << std::endl; + // std::cout << xyxzyz.X() << " , " << xyxzyz.Y() << " , " << xyxzyz.Z() << std::endl; + // std::cout << "Mass of " << collision.Name() << " is " << collisionInertia.MassMatrix().Mass() << std::endl; + // errors.insert(errors.end(), inertiaErrors.begin(), + // inertiaErrors.end()); - totalInertia = totalInertia + collisionInertia; - } + // totalInertia = totalInertia + collisionInertia; + // } - std::cout << "--------------------------------------" << std::endl; - mass = totalInertia.MassMatrix().Mass(); - xxyyzz.X() = totalInertia.MassMatrix().Ixx(); - xxyyzz.Y() = totalInertia.MassMatrix().Iyy(); - xxyyzz.Z() = totalInertia.MassMatrix().Izz(); - - xyxzyz.X() = totalInertia.MassMatrix().Ixy(); - xyxzyz.Y() = totalInertia.MassMatrix().Ixz(); - xyxzyz.Z() = totalInertia.MassMatrix().Iyz(); - - std::cout << "Total Inertia of " << this->dataPtr->name << std::endl; - std::cout << xxyyzz.X() << " , " << xxyyzz.Y() << " , " << xxyyzz.Z() << std::endl; - std::cout << xyxzyz.X() << " , " << xyxzyz.Y() << " , " << xyxzyz.Z() << std::endl; - std::cout << "Total Mass of " << this->dataPtr->name << " is " << mass << std::endl; - } - else - { - // Get the mass. - mass = inertialElem->Get("mass", 1.0).first; - xxyyzz.X(inertiaElem->Get("ixx", 1.0).first); - xxyyzz.Y(inertiaElem->Get("iyy", 1.0).first); - xxyyzz.Z(inertiaElem->Get("izz", 1.0).first); - - xyxzyz.X(inertiaElem->Get("ixy", 0.0).first); - xyxzyz.Y(inertiaElem->Get("ixz", 0.0).first); - xyxzyz.Z(inertiaElem->Get("iyz", 0.0).first); - } - } + // std::cout << "--------------------------------------" << std::endl; + // mass = totalInertia.MassMatrix().Mass(); + // xxyyzz.X() = totalInertia.MassMatrix().Ixx(); + // xxyyzz.Y() = totalInertia.MassMatrix().Iyy(); + // xxyyzz.Z() = totalInertia.MassMatrix().Izz(); + + // xyxzyz.X() = totalInertia.MassMatrix().Ixy(); + // xyxzyz.Y() = totalInertia.MassMatrix().Ixz(); + // xyxzyz.Z() = totalInertia.MassMatrix().Iyz(); + + // std::cout << "Total Inertia of " << this->dataPtr->name << std::endl; + // std::cout << xxyyzz.X() << " , " << xxyyzz.Y() << " , " << xxyyzz.Z() << std::endl; + // std::cout << xyxzyz.X() << " , " << xyxzyz.Y() << " , " << xyxzyz.Z() << std::endl; + // std::cout << "Total Mass of " << this->dataPtr->name << " is " << mass << std::endl; + // } + // else + // { + // // Get the mass. + // mass = inertialElem->Get("mass", 1.0).first; + // xxyyzz.X(inertiaElem->Get("ixx", 1.0).first); + // xxyyzz.Y(inertiaElem->Get("iyy", 1.0).first); + // xxyyzz.Z(inertiaElem->Get("izz", 1.0).first); + + // xyxzyz.X(inertiaElem->Get("ixy", 0.0).first); + // xyxzyz.Y(inertiaElem->Get("ixz", 0.0).first); + // xyxzyz.Z(inertiaElem->Get("iyz", 0.0).first); + // } + // } if (inertialElem->HasElement("fluid_added_mass")) { @@ -551,6 +553,92 @@ Errors Link::ResolveInertial( return errors; } +///////////////////////////////////////////////// +void Link::CalculateInertials(sdf::Errors &_errors) +{ + std::cout << "CalculateInertial() called for " << this->dataPtr->name << std::endl; + if (this->dataPtr->sdf->HasElement("inertial")) + { + sdf::ElementPtr inertialElem = this->dataPtr->sdf->GetElement("inertial"); + + if (inertialElem->Get("auto")) + { + std::cout << "Inertia is set to automatic" << std::endl; + gz::math::Inertiald totalInertia; + + for(auto collision : this->dataPtr->collisions) + { + gz::math::Inertiald collisionInertia; + std::cout << "Density of the collision is: " << collision.Density(); + std::cout << std::endl; + Errors inertiaErrors = collision.MassMatrix(collisionInertia); + + // Get pose of collision w.r.t inertial frame + std::string collisionName = collision.Name(); + + // Errors resolvePoseErrors = this->ResolveInertial(collisionInertia, collisionName); + + // std::cout << "Pose of Inertia w.r.t Collision: "; + // std::cout << collisionInertia.Pose() << std::endl; + // std::cout << "Pose of collision w.r.t Inertial: "; + // std::cout << collisionInertia.Pose().Inverse() << std::endl; + + // collisionInertia.SetPose(collisionInertia.Pose().Inverse()); + + // _errors.insert(_errors.end(), resolvePoseErrors.begin(), resolvePoseErrors.end()); + _errors.insert(_errors.end(), inertiaErrors.begin(), inertiaErrors.end()); + + totalInertia = totalInertia + collisionInertia; + } + + std::cout << totalInertia.MassMatrix().Ixx() << ", " << totalInertia.MassMatrix().Iyy() << ", " << totalInertia.MassMatrix().Izz() << std::endl; + std::cout << "Total Mass is " << totalInertia.MassMatrix().Mass() << std::endl; + + this->dataPtr->inertial = totalInertia; + } + else + { + if (inertialElem->HasElement("inertia")) + { + sdf::ElementPtr inertiaElem = inertialElem->GetElement("inertia"); + + gz::math::Vector3d xxyyzz = gz::math::Vector3d::One; + gz::math::Vector3d xyxzyz = gz::math::Vector3d::Zero; + double mass = 1.0; + gz::math::Pose3d inertiaPose; + std::string inertiaFrame = ""; + + if (inertialElem->HasElement("pose")) + loadPose(inertialElem->GetElement("pose"), inertiaPose, inertiaFrame); + + mass = inertialElem->Get("mass", 1.0).first; + xxyyzz.X(inertiaElem->Get("ixx", 1.0).first); + xxyyzz.Y(inertiaElem->Get("iyy", 1.0).first); + xxyyzz.Z(inertiaElem->Get("izz", 1.0).first); + + xyxzyz.X(inertiaElem->Get("ixy", 0.0).first); + xyxzyz.Y(inertiaElem->Get("ixz", 0.0).first); + xyxzyz.Z(inertiaElem->Get("iyz", 0.0).first); + + if (this->dataPtr->inertial.SetMassMatrix( + gz::math::MassMatrix3d(mass, xxyyzz, xyxzyz))) + { + _errors.push_back({ErrorCode::LINK_INERTIA_INVALID, + "A link named " + + this->Name() + + " has invalid inertia."}); + } + } + } + } + else + { + _errors.push_back({ErrorCode::ELEMENT_MISSING, + " element is missing. " + "Using default inertial values for the link"}); + } +} + ///////////////////////////////////////////////// const gz::math::Pose3d &Link::RawPose() const { From b6a8a7b0866a766daa460634aace36394b6d33e8 Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Tue, 8 Aug 2023 03:12:19 +0530 Subject: [PATCH 24/88] Update Collision::MassMatrix() to set inertial pose using collision pose in link frame Signed-off-by: Jasmeet Singh --- src/Collision.cc | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/src/Collision.cc b/src/Collision.cc index c233df4f6..e2b89d1fb 100644 --- a/src/Collision.cc +++ b/src/Collision.cc @@ -249,6 +249,20 @@ Errors Collision::MassMatrix(gz::math::Inertiald &_inertial) "Inertia Calculated for collision: " + this->dataPtr->name + " seems invalid"}); } + + // If collision pose is in Link Frame then set that as inertial pose + // Else resolve collision pose in Link Frame and then set as inertial pose + if (this->dataPtr->poseRelativeTo.empty()) + { + _inertial.SetPose(this->dataPtr->pose); + } + else + { + gz::math::Pose3d collisionPoseLinkFrame; + Errors poseConvErrors = this->SemanticPose().Resolve(collisionPoseLinkFrame); + errors.insert(errors.end(), poseConvErrors.begin(), poseConvErrors.end()); + _inertial.SetPose(collisionPoseLinkFrame); + } } return errors; From 33d9979aa1de536c08920ceb6952680af4fc5d29 Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Tue, 8 Aug 2023 16:42:55 +0530 Subject: [PATCH 25/88] Called World::CalculateInertials() in Root::Load() function Signed-off-by: Jasmeet Singh --- src/Root.cc | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/Root.cc b/src/Root.cc index c2df4eca1..1e9392a4c 100644 --- a/src/Root.cc +++ b/src/Root.cc @@ -286,6 +286,8 @@ Errors Root::Load(SDFPtr _sdf, const ParserConfig &_config) this->dataPtr->UpdateGraphs(world, worldErrors); + world.CalculateInertials(worldErrors); + // Attempt to load the world if (worldErrors.empty()) { From 76842fe2c06096d474c44bde18cc8f170ffe7512 Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Tue, 8 Aug 2023 17:03:50 +0530 Subject: [PATCH 26/88] Added check for no collisions when auto is true & return an Element Missing Error Signed-off-by: Jasmeet Singh --- src/Link.cc | 68 ++++++++++++++++++++++++++--------------------------- 1 file changed, 34 insertions(+), 34 deletions(-) diff --git a/src/Link.cc b/src/Link.cc index 44af87b1f..28fb1bf4e 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -564,28 +564,26 @@ void Link::CalculateInertials(sdf::Errors &_errors) if (inertialElem->Get("auto")) { std::cout << "Inertia is set to automatic" << std::endl; + + // Return an error if auto is set to true but there are no + // collision elements in the link + if (this->dataPtr->collisions.empty()) + { + _errors.push_back({ErrorCode::ELEMENT_MISSING, + "Inertial is set to auto but there are no " + " elements for the link."}); + return; + } + gz::math::Inertiald totalInertia; - for(auto collision : this->dataPtr->collisions) + for(sdf::Collision &collision : this->dataPtr->collisions) { gz::math::Inertiald collisionInertia; std::cout << "Density of the collision is: " << collision.Density(); std::cout << std::endl; - Errors inertiaErrors = collision.MassMatrix(collisionInertia); - - // Get pose of collision w.r.t inertial frame - std::string collisionName = collision.Name(); - - // Errors resolvePoseErrors = this->ResolveInertial(collisionInertia, collisionName); - - // std::cout << "Pose of Inertia w.r.t Collision: "; - // std::cout << collisionInertia.Pose() << std::endl; - // std::cout << "Pose of collision w.r.t Inertial: "; - // std::cout << collisionInertia.Pose().Inverse() << std::endl; - // collisionInertia.SetPose(collisionInertia.Pose().Inverse()); - - // _errors.insert(_errors.end(), resolvePoseErrors.begin(), resolvePoseErrors.end()); + Errors inertiaErrors = collision.MassMatrix(collisionInertia); _errors.insert(_errors.end(), inertiaErrors.begin(), inertiaErrors.end()); totalInertia = totalInertia + collisionInertia; @@ -598,20 +596,21 @@ void Link::CalculateInertials(sdf::Errors &_errors) } else { + gz::math::Vector3d xxyyzz = gz::math::Vector3d::One; + gz::math::Vector3d xyxzyz = gz::math::Vector3d::Zero; + double mass = 1.0; + gz::math::Pose3d inertiaPose; + std::string inertiaFrame = ""; + + if (inertialElem->HasElement("pose")) + loadPose(inertialElem->GetElement("pose"), inertiaPose, inertiaFrame); + + mass = inertialElem->Get("mass", 1.0).first; + if (inertialElem->HasElement("inertia")) { sdf::ElementPtr inertiaElem = inertialElem->GetElement("inertia"); - gz::math::Vector3d xxyyzz = gz::math::Vector3d::One; - gz::math::Vector3d xyxzyz = gz::math::Vector3d::Zero; - double mass = 1.0; - gz::math::Pose3d inertiaPose; - std::string inertiaFrame = ""; - - if (inertialElem->HasElement("pose")) - loadPose(inertialElem->GetElement("pose"), inertiaPose, inertiaFrame); - - mass = inertialElem->Get("mass", 1.0).first; xxyyzz.X(inertiaElem->Get("ixx", 1.0).first); xxyyzz.Y(inertiaElem->Get("iyy", 1.0).first); xxyyzz.Z(inertiaElem->Get("izz", 1.0).first); @@ -619,15 +618,15 @@ void Link::CalculateInertials(sdf::Errors &_errors) xyxzyz.X(inertiaElem->Get("ixy", 0.0).first); xyxzyz.Y(inertiaElem->Get("ixz", 0.0).first); xyxzyz.Z(inertiaElem->Get("iyz", 0.0).first); + } - if (this->dataPtr->inertial.SetMassMatrix( - gz::math::MassMatrix3d(mass, xxyyzz, xyxzyz))) - { - _errors.push_back({ErrorCode::LINK_INERTIA_INVALID, - "A link named " + - this->Name() + - " has invalid inertia."}); - } + if (!this->dataPtr->inertial.SetMassMatrix( + gz::math::MassMatrix3d(mass, xxyyzz, xyxzyz))) + { + _errors.push_back({ErrorCode::LINK_INERTIA_INVALID, + "A link named " + + this->Name() + + " has invalid inertia."}); } } } @@ -635,7 +634,8 @@ void Link::CalculateInertials(sdf::Errors &_errors) { _errors.push_back({ErrorCode::ELEMENT_MISSING, " element is missing. " - "Using default inertial values for the link"}); + "Using default inertial values for the link: " + + this->dataPtr->name}); } } From f486a1b725670e50aa70ef4b51484be1540e2856 Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Thu, 10 Aug 2023 14:27:31 +0530 Subject: [PATCH 27/88] Used enforcePolicyCondition() to print element missing message in Collision::Load() Signed-off-by: Jasmeet Singh --- src/Collision.cc | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/src/Collision.cc b/src/Collision.cc index e2b89d1fb..a597892b3 100644 --- a/src/Collision.cc +++ b/src/Collision.cc @@ -119,9 +119,16 @@ Errors Collision::Load(ElementPtr _sdf, const ParserConfig &_config) } else { - errors.push_back({ErrorCode::ELEMENT_MISSING, - "Collision is missing a child " - "element. Using a density of 1000.0"}); + // If the density element is missing, let the user know that a default + // value would be used according to the policy + Error densityMissingErr( + ErrorCode::ELEMENT_MISSING, + "Collision is missing a child element. " + "Using a default density value of 1000.0 kg/m^3. " + ); + enforceConfigurablePolicyCondition( + _config.WarningsPolicy(), densityMissingErr, errors + ); } // Load the surface parameters if they are given From 429eb2fef4ef6f36061585b6f1e1ee866ce95f19 Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Thu, 10 Aug 2023 14:28:54 +0530 Subject: [PATCH 28/88] Restored original flow of Link::Load() to set inertial values & updated CalculateInertials() Signed-off-by: Jasmeet Singh --- src/Link.cc | 143 +++++++++++++--------------------------------------- 1 file changed, 36 insertions(+), 107 deletions(-) diff --git a/src/Link.cc b/src/Link.cc index 28fb1bf4e..26bc3cac2 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -163,74 +163,28 @@ Errors Link::Load(ElementPtr _sdf, const ParserConfig &_config) { sdf::ElementPtr inertialElem = _sdf->GetElement("inertial"); - // if (inertialElem->HasElement("pose")) - // loadPose(inertialElem->GetElement("pose"), inertiaPose, inertiaFrame); - - // if (inertialElem->HasElement("inertia")) - // { - // sdf::ElementPtr inertiaElem = inertialElem->GetElement("inertia"); - - // if (inertiaElem->Get("auto")) - // { - // std::cout << "Inertia is set to automatic" << std::endl; - // gz::math::Inertiald totalInertia; - - // for(auto collision : this->dataPtr->collisions) - // { - // gz::math::Inertiald collisionInertia; - // std::cout << "Density of the collision is: " << collision.Density(); - // std::cout << std::endl; - // Errors inertiaErrors = collision.MassMatrix(collisionInertia); - - // // Get pose of collision w.r.t inertial frame - // collisionInertia.SetPose(inertiaPose); - // std::string collisionName = collision.Name(); - // Errors resolvePoseErrors = this->ResolveInertial(collisionInertia, collisionName); - // std::cout << "Pose of Inertia w.r.t Collision: "; - // std::cout << collisionInertia.Pose() << std::endl; - // std::cout << "Pose of collision w.r.t Inertial: "; - // std::cout << collisionInertia.Pose().Inverse() << std::endl; - // collisionInertia.SetPose(collisionInertia.Pose().Inverse()); - // errors.insert(errors.end(), resolvePoseErrors.begin(), resolvePoseErrors.end()); - - // std::cout << "Inertia of " << collision.Name() << std::endl; - // std::cout << collisionInertia.MassMatrix().Ixx() << " , " << collisionInertia.MassMatrix().Iyy() << " , " << collisionInertia.MassMatrix().Izz() << std::endl; - // std::cout << xyxzyz.X() << " , " << xyxzyz.Y() << " , " << xyxzyz.Z() << std::endl; - // std::cout << "Mass of " << collision.Name() << " is " << collisionInertia.MassMatrix().Mass() << std::endl; - // errors.insert(errors.end(), inertiaErrors.begin(), - // inertiaErrors.end()); - - // totalInertia = totalInertia + collisionInertia; - // } - - // std::cout << "--------------------------------------" << std::endl; - // mass = totalInertia.MassMatrix().Mass(); - // xxyyzz.X() = totalInertia.MassMatrix().Ixx(); - // xxyyzz.Y() = totalInertia.MassMatrix().Iyy(); - // xxyyzz.Z() = totalInertia.MassMatrix().Izz(); - - // xyxzyz.X() = totalInertia.MassMatrix().Ixy(); - // xyxzyz.Y() = totalInertia.MassMatrix().Ixz(); - // xyxzyz.Z() = totalInertia.MassMatrix().Iyz(); - - // std::cout << "Total Inertia of " << this->dataPtr->name << std::endl; - // std::cout << xxyyzz.X() << " , " << xxyyzz.Y() << " , " << xxyyzz.Z() << std::endl; - // std::cout << xyxzyz.X() << " , " << xyxzyz.Y() << " , " << xyxzyz.Z() << std::endl; - // std::cout << "Total Mass of " << this->dataPtr->name << " is " << mass << std::endl; - // } - // else - // { - // // Get the mass. - // mass = inertialElem->Get("mass", 1.0).first; - // xxyyzz.X(inertiaElem->Get("ixx", 1.0).first); - // xxyyzz.Y(inertiaElem->Get("iyy", 1.0).first); - // xxyyzz.Z(inertiaElem->Get("izz", 1.0).first); - - // xyxzyz.X(inertiaElem->Get("ixy", 0.0).first); - // xyxzyz.Y(inertiaElem->Get("ixz", 0.0).first); - // xyxzyz.Z(inertiaElem->Get("iyz", 0.0).first); - // } - // } + // If auto is set to false or not specified + if (!inertialElem->Get("auto")) + { + if (inertialElem->HasElement("pose")) + loadPose(inertialElem->GetElement("pose"), inertiaPose, inertiaFrame); + + // Get the mass. + mass = inertialElem->Get("mass", 1.0).first; + + if (inertialElem->HasElement("inertia")) + { + sdf::ElementPtr inertiaElem = inertialElem->GetElement("inertia"); + + xxyyzz.X(inertiaElem->Get("ixx", 1.0).first); + xxyyzz.Y(inertiaElem->Get("iyy", 1.0).first); + xxyyzz.Z(inertiaElem->Get("izz", 1.0).first); + + xyxzyz.X(inertiaElem->Get("ixy", 0.0).first); + xyxzyz.Y(inertiaElem->Get("ixz", 0.0).first); + xyxzyz.Z(inertiaElem->Get("iyz", 0.0).first); + } + } if (inertialElem->HasElement("fluid_added_mass")) { @@ -288,6 +242,17 @@ Errors Link::Load(ElementPtr _sdf, const ParserConfig &_config) } } } + else + { + Error inertialMissingErr( + ErrorCode::ELEMENT_MISSING, + " element is missing. " + "Using default values for the inertial for the link: " + this->dataPtr->name + ); + enforceConfigurablePolicyCondition( + _config.WarningsPolicy(), inertialMissingErr, errors + ); + } if (!this->dataPtr->inertial.SetMassMatrix( gz::math::MassMatrix3d(mass, xxyyzz, xyxzyz))) @@ -594,49 +559,13 @@ void Link::CalculateInertials(sdf::Errors &_errors) this->dataPtr->inertial = totalInertia; } + // If auto is false, this means inertial values were set from user given values + // in Link::Load(), therefore we can return else { - gz::math::Vector3d xxyyzz = gz::math::Vector3d::One; - gz::math::Vector3d xyxzyz = gz::math::Vector3d::Zero; - double mass = 1.0; - gz::math::Pose3d inertiaPose; - std::string inertiaFrame = ""; - - if (inertialElem->HasElement("pose")) - loadPose(inertialElem->GetElement("pose"), inertiaPose, inertiaFrame); - - mass = inertialElem->Get("mass", 1.0).first; - - if (inertialElem->HasElement("inertia")) - { - sdf::ElementPtr inertiaElem = inertialElem->GetElement("inertia"); - - xxyyzz.X(inertiaElem->Get("ixx", 1.0).first); - xxyyzz.Y(inertiaElem->Get("iyy", 1.0).first); - xxyyzz.Z(inertiaElem->Get("izz", 1.0).first); - - xyxzyz.X(inertiaElem->Get("ixy", 0.0).first); - xyxzyz.Y(inertiaElem->Get("ixz", 0.0).first); - xyxzyz.Z(inertiaElem->Get("iyz", 0.0).first); - } - - if (!this->dataPtr->inertial.SetMassMatrix( - gz::math::MassMatrix3d(mass, xxyyzz, xyxzyz))) - { - _errors.push_back({ErrorCode::LINK_INERTIA_INVALID, - "A link named " + - this->Name() + - " has invalid inertia."}); - } + return; } } - else - { - _errors.push_back({ErrorCode::ELEMENT_MISSING, - " element is missing. " - "Using default inertial values for the link: " + - this->dataPtr->name}); - } } ///////////////////////////////////////////////// From b349c0db65fa7dd21bb46971e185b3390a604d57 Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Thu, 10 Aug 2023 15:42:19 +0530 Subject: [PATCH 29/88] Renamed & changed MassMatrix() functions for all shapes to return gz::math::Inertial objects Signed-off-by: Jasmeet Singh --- include/sdf/Box.hh | 6 +++--- include/sdf/Capsule.hh | 6 +++--- include/sdf/Cylinder.hh | 6 +++--- include/sdf/Ellipsoid.hh | 6 +++--- include/sdf/Sphere.hh | 6 +++--- src/Box.cc | 17 +++++++++++++++-- src/Capsule.cc | 18 +++++++++++++++--- src/Cylinder.cc | 17 +++++++++++++++-- src/Ellipsoid.cc | 17 +++++++++++++++-- src/Sphere.cc | 18 ++++++++++++++++-- 10 files changed, 91 insertions(+), 26 deletions(-) diff --git a/include/sdf/Box.hh b/include/sdf/Box.hh index 228dd5eac..e2d44a48b 100644 --- a/include/sdf/Box.hh +++ b/include/sdf/Box.hh @@ -21,7 +21,7 @@ #include #include -#include +#include #include #include #include @@ -70,8 +70,8 @@ namespace sdf /// \brief Calculate and return the Mass Matrix values for the Box /// \param[in] density Density of the box in kg/m^3 - /// \return A std::optional with gz::math::MassMatrix3d object or std::nullopt - public: std::optional< gz::math::MassMatrix3d > MassMatrix(const double _density); + /// \return A std::optional with gz::math::Inertiald object or std::nullopt + public: std::optional< gz::math::Inertiald > CalculateInertial(const double _density); /// \brief Create and return an SDF element filled with data from this /// box. diff --git a/include/sdf/Capsule.hh b/include/sdf/Capsule.hh index 3348c017d..af2471a84 100644 --- a/include/sdf/Capsule.hh +++ b/include/sdf/Capsule.hh @@ -20,7 +20,7 @@ #include #include -#include +#include #include #include #include @@ -77,8 +77,8 @@ namespace sdf /// \brief Calculate and return the Mass Matrix values for the Capsule /// \param[in] density Density of the capsule in kg/m^3 - /// \return A std::optional with gz::math::MassMatrix3d object or std::nullopt - public: std::optional< gz::math::MassMatrix3d > MassMatrix(const double _density); + /// \return A std::optional with gz::math::Inertiald object or std::nullopt + public: std::optional< gz::math::Inertiald > CalculateInertial(const double _density); /// \brief Create and return an SDF element filled with data from this /// capsule. diff --git a/include/sdf/Cylinder.hh b/include/sdf/Cylinder.hh index 37fcbe35f..3ab4df788 100644 --- a/include/sdf/Cylinder.hh +++ b/include/sdf/Cylinder.hh @@ -20,7 +20,7 @@ #include #include -#include +#include #include #include #include @@ -77,8 +77,8 @@ namespace sdf /// \brief Calculate and return the Mass Matrix values for the Cylinder /// \param[in] density Density of the cylinder in kg/m^3 - /// \return A std::optional with gz::math::MassMatrix3d object or std::nullopt - public: std::optional< gz::math::MassMatrix3d > MassMatrix(const double _density); + /// \return A std::optional with gz::math::Inertiald object or std::nullopt + public: std::optional< gz::math::Inertiald > CalculateInertial(const double _density); /// \brief Create and return an SDF element filled with data from this /// cylinder. diff --git a/include/sdf/Ellipsoid.hh b/include/sdf/Ellipsoid.hh index 1c945d2bf..da2077980 100644 --- a/include/sdf/Ellipsoid.hh +++ b/include/sdf/Ellipsoid.hh @@ -19,7 +19,7 @@ #include -#include +#include #include #include #include @@ -69,8 +69,8 @@ namespace sdf /// \brief Calculate and return the Mass Matrix values for the Ellipsoid /// \param[in] density Density of the ellipsoid in kg/m^3 - /// \return A std::optional with gz::math::MassMatrix3d object or std::nullopt - public: std::optional< gz::math::MassMatrix3d > MassMatrix(const double _density); + /// \return A std::optional with gz::math::Inertiald object or std::nullopt + public: std::optional< gz::math::Inertiald > CalculateInertial(const double _density); /// \brief Create and return an SDF element filled with data from this /// ellipsoid. diff --git a/include/sdf/Sphere.hh b/include/sdf/Sphere.hh index 375517c36..d4a242fd0 100644 --- a/include/sdf/Sphere.hh +++ b/include/sdf/Sphere.hh @@ -19,7 +19,7 @@ #include -#include +#include #include #include @@ -70,8 +70,8 @@ namespace sdf /// \brief Calculate and return the Mass Matrix values for the Sphere /// \param[in] density Density of the sphere in kg/m^3 - /// \return A std::optional with gz::math::MassMatrix3d object or std::nullopt - public: std::optional< gz::math::MassMatrix3d > MassMatrix(const double _density); + /// \return A std::optional with gz::math::Inertiald object or std::nullopt + public: std::optional< gz::math::Inertiald > CalculateInertial(const double _density); /// \brief Create and return an SDF element filled with data from this /// sphere. diff --git a/src/Box.cc b/src/Box.cc index 74b196840..2fe78c6a5 100644 --- a/src/Box.cc +++ b/src/Box.cc @@ -19,6 +19,7 @@ #include #include #include +#include #include "sdf/Box.hh" #include "sdf/parser.hh" @@ -119,11 +120,23 @@ gz::math::Boxd &Box::Shape() } ///////////////////////////////////////////////// -std::optional< gz::math::MassMatrix3d > Box::MassMatrix(const double _density) +std::optional< gz::math::Inertiald > Box::CalculateInertial(const double _density) { gz::math::Material material = gz::math::Material(_density); this->dataPtr->box.SetMaterial(material); - return this->dataPtr->box.MassMatrix(); + + auto boxMassMatrix = this->dataPtr->box.MassMatrix(); + + if (!boxMassMatrix) + { + return std::nullopt; + } + else + { + gz::math::Inertiald boxInertial; + boxInertial.SetMassMatrix(boxMassMatrix.value()); + return std::make_optional(boxInertial); + } } ///////////////////////////////////////////////// diff --git a/src/Capsule.cc b/src/Capsule.cc index 84189c054..550cfafb0 100644 --- a/src/Capsule.cc +++ b/src/Capsule.cc @@ -18,7 +18,7 @@ #include #include -#include +#include #include "sdf/Capsule.hh" #include "sdf/parser.hh" @@ -141,11 +141,23 @@ gz::math::Capsuled &Capsule::Shape() } ///////////////////////////////////////////////// -std::optional< gz::math::MassMatrix3d > Capsule::MassMatrix(const double _density) +std::optional< gz::math::Inertiald > Capsule::CalculateInertial(const double _density) { gz::math::Material material = gz::math::Material(_density); this->dataPtr->capsule.SetMat(material); - return this->dataPtr->capsule.MassMatrix(); + + auto capsuleMassMatrix = this->dataPtr->capsule.MassMatrix(); + + if(!capsuleMassMatrix) + { + return std::nullopt; + } + else + { + gz::math::Inertiald capsuleInertial; + capsuleInertial.SetMassMatrix(capsuleMassMatrix.value()); + return std::make_optional(capsuleInertial); + } } ///////////////////////////////////////////////// diff --git a/src/Cylinder.cc b/src/Cylinder.cc index 9fd66553c..ab803f27d 100644 --- a/src/Cylinder.cc +++ b/src/Cylinder.cc @@ -15,6 +15,7 @@ * */ #include +#include #include "sdf/Cylinder.hh" #include "sdf/parser.hh" @@ -136,11 +137,23 @@ gz::math::Cylinderd &Cylinder::Shape() return this->dataPtr->cylinder; } -std::optional< gz::math::MassMatrix3d > Cylinder::MassMatrix(const double _density) +std::optional< gz::math::Inertiald > Cylinder::CalculateInertial(const double _density) { gz::math::Material material = gz::math::Material(_density); this->dataPtr->cylinder.SetMat(material); - return this->dataPtr->cylinder.MassMatrix(); + + auto cylinderMassMatrix = this->dataPtr->cylinder.MassMatrix(); + + if (!cylinderMassMatrix) + { + return std::nullopt; + } + else + { + gz::math::Inertiald cylinderInertial; + cylinderInertial.SetMassMatrix(cylinderMassMatrix.value()); + return std::make_optional(cylinderInertial); + } } ///////////////////////////////////////////////// diff --git a/src/Ellipsoid.cc b/src/Ellipsoid.cc index 2139b9cdf..f4bf87d22 100644 --- a/src/Ellipsoid.cc +++ b/src/Ellipsoid.cc @@ -15,6 +15,7 @@ * */ #include +#include #include "sdf/Ellipsoid.hh" #include "sdf/parser.hh" @@ -116,11 +117,23 @@ gz::math::Ellipsoidd &Ellipsoid::Shape() } ///////////////////////////////////////////////// -std::optional< gz::math::MassMatrix3d > Ellipsoid::MassMatrix(const double _density) +std::optional< gz::math::Inertiald > Ellipsoid::CalculateInertial(const double _density) { gz::math::Material material = gz::math::Material(_density); this->dataPtr->ellipsoid.SetMat(material); - return this->dataPtr->ellipsoid.MassMatrix(); + + auto ellipsoidMassMatrix = this->dataPtr->ellipsoid.MassMatrix(); + + if(!ellipsoidMassMatrix) + { + return std::nullopt; + } + else + { + gz::math::Inertiald ellipsoidInertial; + ellipsoidInertial.SetMassMatrix(ellipsoidMassMatrix.value()); + return std::make_optional(ellipsoidInertial); + } } ///////////////////////////////////////////////// diff --git a/src/Sphere.cc b/src/Sphere.cc index 9baee6606..416cca1b5 100644 --- a/src/Sphere.cc +++ b/src/Sphere.cc @@ -14,6 +14,8 @@ * limitations under the License. * */ +#include + #include "sdf/parser.hh" #include "sdf/Sphere.hh" @@ -114,11 +116,23 @@ sdf::ElementPtr Sphere::Element() const } ///////////////////////////////////////////////// -std::optional< gz::math::MassMatrix3d > Sphere::MassMatrix(const double _density) +std::optional< gz::math::Inertiald > Sphere::CalculateInertial(const double _density) { gz::math::Material material = gz::math::Material(_density); this->dataPtr->sphere.SetMaterial(material); - return this->dataPtr->sphere.MassMatrix(); + + auto sphereMassMatrix = this->dataPtr->sphere.MassMatrix(); + + if (!sphereMassMatrix) + { + return std::nullopt; + } + else + { + gz::math::Inertiald sphereInertial; + sphereInertial.SetMassMatrix(sphereMassMatrix.value()); + return std::make_optional(sphereInertial); + } } ///////////////////////////////////////////////// From 5f118634b1694545971b96036244cd5840e84242 Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Thu, 10 Aug 2023 15:43:33 +0530 Subject: [PATCH 30/88] Renamed & updated MassMatrix() function for sdf::Geometry to return opitonal gz::math::Inertiald object Signed-off-by: Jasmeet Singh --- include/sdf/Geometry.hh | 6 +++--- src/Geometry.cc | 26 +++++++++++++------------- 2 files changed, 16 insertions(+), 16 deletions(-) diff --git a/include/sdf/Geometry.hh b/include/sdf/Geometry.hh index a097250cf..dc3e80991 100644 --- a/include/sdf/Geometry.hh +++ b/include/sdf/Geometry.hh @@ -21,7 +21,7 @@ #include #include -#include +#include #include #include #include @@ -213,8 +213,8 @@ namespace sdf /// \brief Calculate and return the Mass Matrix values for the Geometry /// \param[in] _density The density of the geometry element. - /// \return A std::optional with gz::math::MassMatrix3d object or std::nullopt - public: std::optional< gz::math::MassMatrix3d > MassMatrix(const double _density); + /// \return A std::optional with gz::math::Inertiald object or std::nullopt + public: std::optional< gz::math::Inertiald > CalculateInertial(const double _density); /// \brief Get a pointer to the SDF element that was used during /// load. diff --git a/src/Geometry.cc b/src/Geometry.cc index a82c48395..b2c087ed1 100644 --- a/src/Geometry.cc +++ b/src/Geometry.cc @@ -17,7 +17,7 @@ #include -#include +#include #include "sdf/Geometry.hh" #include "sdf/Box.hh" #include "sdf/Capsule.hh" @@ -304,32 +304,32 @@ void Geometry::SetPolylineShape(const std::vector &_polylines) this->dataPtr->polylines = _polylines; } -std::optional< gz::math::MassMatrix3d > Geometry::MassMatrix(const double _density) +std::optional< gz::math::Inertiald > Geometry::CalculateInertial(const double _density) { - std::optional< gz::math::MassMatrix3d > massMat; + std::optional< gz::math::Inertiald > geomInertial; switch (this->dataPtr->type) { case GeometryType::BOX: - massMat = this->dataPtr->box->MassMatrix(_density); - break; - case GeometryType::CYLINDER: - massMat = this->dataPtr->cylinder->MassMatrix(_density); - break; - case GeometryType::SPHERE: - massMat = this->dataPtr->sphere->MassMatrix(_density); + geomInertial = this->dataPtr->box->CalculateInertial(_density); break; case GeometryType::CAPSULE: - massMat = this->dataPtr->capsule->MassMatrix(_density); + geomInertial = this->dataPtr->capsule->CalculateInertial(_density); + break; + case GeometryType::CYLINDER: + geomInertial = this->dataPtr->cylinder->CalculateInertial(_density); break; case GeometryType::ELLIPSOID: - massMat = this->dataPtr->ellipsoid->MassMatrix(_density); + geomInertial = this->dataPtr->ellipsoid->CalculateInertial(_density); + break; + case GeometryType::SPHERE: + geomInertial = this->dataPtr->sphere->CalculateInertial(_density); break; default: break; } - return massMat; + return geomInertial; } ///////////////////////////////////////////////// From 8214ae6074af70851164d836d6f20f5d029eb450 Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Thu, 10 Aug 2023 15:44:41 +0530 Subject: [PATCH 31/88] Renamed & Updated MassMatrix() function of sdf::Collision to use Inertial object Added check for geometry type before setting inertial pose Signed-off-by: Jasmeet Singh --- include/sdf/Collision.hh | 9 +++----- src/Collision.cc | 44 +++++++++++++++++++++------------------- 2 files changed, 26 insertions(+), 27 deletions(-) diff --git a/include/sdf/Collision.hh b/include/sdf/Collision.hh index 804f0a68c..3d98f3a0a 100644 --- a/include/sdf/Collision.hh +++ b/include/sdf/Collision.hh @@ -132,14 +132,11 @@ namespace sdf public: sdf::SemanticPose SemanticPose() const; /// \brief Calculate and return the MassMatrix for the collision - /// \param[in] _xxyyzz A vector 3d representing the diagonal elements - /// of the mass matrix - /// \param[in] _xyxzyx A vector 3d representing the off-diagonal elements - /// of the mass matrix - /// \param[in] _mass A double representing the mass of the collision + /// \param[out] _inertial An inertial object which will be set with the + /// calculated inertial values /// \return Errors, which is a vector of Error objects. Each Error includes /// an error code and message. An empty vector indicates no error - public: Errors MassMatrix(gz::math::Inertiald &_inertial); + public: Errors CalculateInertial(gz::math::Inertiald &_inertial); /// \brief Get a pointer to the SDF element that was used during /// load. diff --git a/src/Collision.cc b/src/Collision.cc index a597892b3..3535151f0 100644 --- a/src/Collision.cc +++ b/src/Collision.cc @@ -236,13 +236,13 @@ sdf::SemanticPose Collision::SemanticPose() const } ///////////////////////////////////////////////// -Errors Collision::MassMatrix(gz::math::Inertiald &_inertial) +Errors Collision::CalculateInertial(gz::math::Inertiald &_inertial) { Errors errors; - auto massMat = this->dataPtr->geom.MassMatrix(this->dataPtr->density); + auto geomInertial = this->dataPtr->geom.CalculateInertial(this->dataPtr->density); - if (!massMat) + if (!geomInertial) { errors.push_back({ErrorCode::LINK_INERTIA_INVALID, "Inertia Calculated for collision: " + @@ -250,25 +250,27 @@ Errors Collision::MassMatrix(gz::math::Inertiald &_inertial) } else { - if(!_inertial.SetMassMatrix(massMat.value())) - { - errors.push_back({ErrorCode::LINK_INERTIA_INVALID, - "Inertia Calculated for collision: " + - this->dataPtr->name + " seems invalid"}); - } + _inertial = geomInertial.value(); - // If collision pose is in Link Frame then set that as inertial pose - // Else resolve collision pose in Link Frame and then set as inertial pose - if (this->dataPtr->poseRelativeTo.empty()) - { - _inertial.SetPose(this->dataPtr->pose); - } - else - { - gz::math::Pose3d collisionPoseLinkFrame; - Errors poseConvErrors = this->SemanticPose().Resolve(collisionPoseLinkFrame); - errors.insert(errors.end(), poseConvErrors.begin(), poseConvErrors.end()); - _inertial.SetPose(collisionPoseLinkFrame); + // If geometry type is not mesh than calculate inertial pose in Link frame + // considering collision frame to be same as inertial frame + // In case of mesh the custom inertia calculator should return the inertial object + // with the pose already set + if (this->dataPtr->geom.Type() != GeometryType::MESH) + { + // If collision pose is in Link Frame then set that as inertial pose + // Else resolve collision pose in Link Frame and then set as inertial pose + if (this->dataPtr->poseRelativeTo.empty()) + { + _inertial.SetPose(this->dataPtr->pose); + } + else + { + gz::math::Pose3d collisionPoseLinkFrame; + Errors poseConvErrors = this->SemanticPose().Resolve(collisionPoseLinkFrame); + errors.insert(errors.end(), poseConvErrors.begin(), poseConvErrors.end()); + _inertial.SetPose(collisionPoseLinkFrame); + } } } From 0d1e1bca32f25d41bdcbde946c3182d52c8e90fe Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Thu, 10 Aug 2023 15:46:03 +0530 Subject: [PATCH 32/88] Updated call to collision inertia calculation function with the new name Signed-off-by: Jasmeet Singh --- src/Link.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Link.cc b/src/Link.cc index 26bc3cac2..1b539aab5 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -548,7 +548,7 @@ void Link::CalculateInertials(sdf::Errors &_errors) std::cout << "Density of the collision is: " << collision.Density(); std::cout << std::endl; - Errors inertiaErrors = collision.MassMatrix(collisionInertia); + Errors inertiaErrors = collision.CalculateInertial(collisionInertia); _errors.insert(_errors.end(), inertiaErrors.begin(), inertiaErrors.end()); totalInertia = totalInertia + collisionInertia; From b1f7a51602f9da7910fb25093775919b03b376df Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Thu, 10 Aug 2023 15:50:12 +0530 Subject: [PATCH 33/88] Removed print statements Signed-off-by: Jasmeet Singh --- src/Link.cc | 18 ++---------------- 1 file changed, 2 insertions(+), 16 deletions(-) diff --git a/src/Link.cc b/src/Link.cc index 1b539aab5..ead9b18f6 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -114,8 +114,6 @@ Errors Link::Load(ElementPtr _sdf, const ParserConfig &_config) "A link name is required, but the name is not set."}); } - std::cout << "Load called for link: " << this->dataPtr->name << std::endl; - // Check that the link's name is valid if (isReservedName(this->dataPtr->name)) { @@ -510,9 +508,7 @@ Errors Link::ResolveInertial( auto errors = this->SemanticPose().Resolve(linkPose, _resolveTo); if (errors.empty()) { - //_inertial = this->dataPtr->inertial; - std::cout << "linkPose: " << linkPose << std::endl; - std::cout << "inertial.Pose(): " << _inertial.Pose() << std::endl; + _inertial = this->dataPtr->inertial; _inertial.SetPose(linkPose * _inertial.Pose()); } return errors; @@ -521,15 +517,12 @@ Errors Link::ResolveInertial( ///////////////////////////////////////////////// void Link::CalculateInertials(sdf::Errors &_errors) { - std::cout << "CalculateInertial() called for " << this->dataPtr->name << std::endl; if (this->dataPtr->sdf->HasElement("inertial")) { sdf::ElementPtr inertialElem = this->dataPtr->sdf->GetElement("inertial"); if (inertialElem->Get("auto")) { - std::cout << "Inertia is set to automatic" << std::endl; - // Return an error if auto is set to true but there are no // collision elements in the link if (this->dataPtr->collisions.empty()) @@ -542,21 +535,14 @@ void Link::CalculateInertials(sdf::Errors &_errors) gz::math::Inertiald totalInertia; - for(sdf::Collision &collision : this->dataPtr->collisions) + for (sdf::Collision &collision : this->dataPtr->collisions) { gz::math::Inertiald collisionInertia; - std::cout << "Density of the collision is: " << collision.Density(); - std::cout << std::endl; - Errors inertiaErrors = collision.CalculateInertial(collisionInertia); _errors.insert(_errors.end(), inertiaErrors.begin(), inertiaErrors.end()); - totalInertia = totalInertia + collisionInertia; } - std::cout << totalInertia.MassMatrix().Ixx() << ", " << totalInertia.MassMatrix().Iyy() << ", " << totalInertia.MassMatrix().Izz() << std::endl; - std::cout << "Total Mass is " << totalInertia.MassMatrix().Mass() << std::endl; - this->dataPtr->inertial = totalInertia; } // If auto is false, this means inertial values were set from user given values From 702dd9fcd2058ff358cb7eb5be7662d522f5c855 Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Thu, 10 Aug 2023 16:18:30 +0530 Subject: [PATCH 34/88] Completed codecheck Signed-off-by: Jasmeet Singh --- include/sdf/Box.hh | 3 ++- include/sdf/Capsule.hh | 3 ++- include/sdf/Collision.hh | 2 +- include/sdf/Cylinder.hh | 3 ++- include/sdf/Ellipsoid.hh | 3 ++- include/sdf/Geometry.hh | 3 ++- include/sdf/Link.hh | 11 ++++++----- include/sdf/Model.hh | 2 +- include/sdf/Root.hh | 14 ++++++++------ include/sdf/Sphere.hh | 3 ++- src/Box.cc | 3 ++- src/Capsule.cc | 3 ++- src/Collision.cc | 20 ++++++++++++-------- src/Cylinder.cc | 3 ++- src/Ellipsoid.cc | 3 ++- src/Geometry.cc | 5 +++-- src/Link.cc | 15 +++++++++------ src/Sphere.cc | 3 ++- 18 files changed, 62 insertions(+), 40 deletions(-) diff --git a/include/sdf/Box.hh b/include/sdf/Box.hh index e2d44a48b..5327e0f06 100644 --- a/include/sdf/Box.hh +++ b/include/sdf/Box.hh @@ -71,7 +71,8 @@ namespace sdf /// \brief Calculate and return the Mass Matrix values for the Box /// \param[in] density Density of the box in kg/m^3 /// \return A std::optional with gz::math::Inertiald object or std::nullopt - public: std::optional< gz::math::Inertiald > CalculateInertial(const double _density); + public: std::optional CalculateInertial( + const double _density); /// \brief Create and return an SDF element filled with data from this /// box. diff --git a/include/sdf/Capsule.hh b/include/sdf/Capsule.hh index af2471a84..7150b188c 100644 --- a/include/sdf/Capsule.hh +++ b/include/sdf/Capsule.hh @@ -78,7 +78,8 @@ namespace sdf /// \brief Calculate and return the Mass Matrix values for the Capsule /// \param[in] density Density of the capsule in kg/m^3 /// \return A std::optional with gz::math::Inertiald object or std::nullopt - public: std::optional< gz::math::Inertiald > CalculateInertial(const double _density); + public: std::optional CalculateInertial( + const double _density); /// \brief Create and return an SDF element filled with data from this /// capsule. diff --git a/include/sdf/Collision.hh b/include/sdf/Collision.hh index 3d98f3a0a..15f364d81 100644 --- a/include/sdf/Collision.hh +++ b/include/sdf/Collision.hh @@ -132,7 +132,7 @@ namespace sdf public: sdf::SemanticPose SemanticPose() const; /// \brief Calculate and return the MassMatrix for the collision - /// \param[out] _inertial An inertial object which will be set with the + /// \param[out] _inertial An inertial object which will be set with the /// calculated inertial values /// \return Errors, which is a vector of Error objects. Each Error includes /// an error code and message. An empty vector indicates no error diff --git a/include/sdf/Cylinder.hh b/include/sdf/Cylinder.hh index 3ab4df788..4d2fe7e19 100644 --- a/include/sdf/Cylinder.hh +++ b/include/sdf/Cylinder.hh @@ -78,7 +78,8 @@ namespace sdf /// \brief Calculate and return the Mass Matrix values for the Cylinder /// \param[in] density Density of the cylinder in kg/m^3 /// \return A std::optional with gz::math::Inertiald object or std::nullopt - public: std::optional< gz::math::Inertiald > CalculateInertial(const double _density); + public: std::optional CalculateInertial( + const double _density); /// \brief Create and return an SDF element filled with data from this /// cylinder. diff --git a/include/sdf/Ellipsoid.hh b/include/sdf/Ellipsoid.hh index da2077980..07e37b613 100644 --- a/include/sdf/Ellipsoid.hh +++ b/include/sdf/Ellipsoid.hh @@ -70,7 +70,8 @@ namespace sdf /// \brief Calculate and return the Mass Matrix values for the Ellipsoid /// \param[in] density Density of the ellipsoid in kg/m^3 /// \return A std::optional with gz::math::Inertiald object or std::nullopt - public: std::optional< gz::math::Inertiald > CalculateInertial(const double _density); + public: std::optional CalculateInertial( + const double _density); /// \brief Create and return an SDF element filled with data from this /// ellipsoid. diff --git a/include/sdf/Geometry.hh b/include/sdf/Geometry.hh index dc3e80991..4cf99eca1 100644 --- a/include/sdf/Geometry.hh +++ b/include/sdf/Geometry.hh @@ -214,7 +214,8 @@ namespace sdf /// \brief Calculate and return the Mass Matrix values for the Geometry /// \param[in] _density The density of the geometry element. /// \return A std::optional with gz::math::Inertiald object or std::nullopt - public: std::optional< gz::math::Inertiald > CalculateInertial(const double _density); + public: std::optional CalculateInertial( + const double _density); /// \brief Get a pointer to the SDF element that was used during /// load. diff --git a/include/sdf/Link.hh b/include/sdf/Link.hh index b8a29c7e0..fc34e89c9 100644 --- a/include/sdf/Link.hh +++ b/include/sdf/Link.hh @@ -284,11 +284,12 @@ namespace sdf public: Errors ResolveInertial(gz::math::Inertiald &_inertial, const std::string &_resolveTo = "") const; - /// \brief Calculate and set inertial values (mass, mass matrix & inertial pose) - /// for the link. Inertial values can be provided by the user through the SDF - /// or can be calculated automatically by setting the auto attribute to true. - /// \param[out] _errors A vector of Errors object. Each object would contain an - /// error code and an error message. + /// \brief Calculate & set inertial values(mass, mass matrix + /// & inertial pose) for the link. Inertial values can be provided + /// by the user through the SDF or can be calculated automatically + /// by setting the auto attribute to true. + /// \param[out] _errors A vector of Errors object. Each object + /// would contain an error code and an error message. public: void CalculateInertials(sdf::Errors &_errors); /// \brief Get the pose of the link. This is the pose of the link diff --git a/include/sdf/Model.hh b/include/sdf/Model.hh index 3101ee521..32c5d4ee1 100644 --- a/include/sdf/Model.hh +++ b/include/sdf/Model.hh @@ -508,7 +508,7 @@ namespace sdf /// \param[in] _plugin Plugin to add. public: void AddPlugin(const Plugin &_plugin); - /// \brief Calculate and set the inertials for all the links belonging + /// \brief Calculate and set the inertials for all the links belonging /// to the model object /// \param[out] _errrors A vector of Errors objects. Each errors contains an /// Error code and a message. An empty errors vector indicates no errors diff --git a/include/sdf/Root.hh b/include/sdf/Root.hh index 600371769..53564a4c6 100644 --- a/include/sdf/Root.hh +++ b/include/sdf/Root.hh @@ -226,12 +226,14 @@ namespace sdf /// an error code and message. An empty vector indicates no error. public: Errors UpdateGraphs(); - /// \brief Calculate and set the inertial properties (mass, mass matrix - /// and inertial pose) for all the worlds and models that are children of this - /// Root object. This function can be called after calling UpdateGraphs() since - /// it uses frame graphs to resolve inertial pose for links. - /// \return Errors, which is a vector of Error objects. Each Error includes - /// an error code and message. An empty vector indicates no error. + /// \brief Calculate & set the inertial properties (mass, mass matrix + /// and inertial pose) for all the worlds & models that are children + /// of this Root object. This function can be called after calling + /// UpdateGraphs() since it uses frame graphs to resolve inertial pose + /// for links. + /// \return Errors, which is a vector of Error objects. + /// Each Error includes an error code and message. An empty vector + /// indicates no error. public: Errors CalculateInertials(); /// \brief Create and return an SDF element filled with data from this diff --git a/include/sdf/Sphere.hh b/include/sdf/Sphere.hh index d4a242fd0..a2acc1be5 100644 --- a/include/sdf/Sphere.hh +++ b/include/sdf/Sphere.hh @@ -71,7 +71,8 @@ namespace sdf /// \brief Calculate and return the Mass Matrix values for the Sphere /// \param[in] density Density of the sphere in kg/m^3 /// \return A std::optional with gz::math::Inertiald object or std::nullopt - public: std::optional< gz::math::Inertiald > CalculateInertial(const double _density); + public: std::optional CalculateInertial( + const double _density); /// \brief Create and return an SDF element filled with data from this /// sphere. diff --git a/src/Box.cc b/src/Box.cc index 2fe78c6a5..1c30fc798 100644 --- a/src/Box.cc +++ b/src/Box.cc @@ -120,7 +120,8 @@ gz::math::Boxd &Box::Shape() } ///////////////////////////////////////////////// -std::optional< gz::math::Inertiald > Box::CalculateInertial(const double _density) +std::optional Box::CalculateInertial( + const double _density) { gz::math::Material material = gz::math::Material(_density); this->dataPtr->box.SetMaterial(material); diff --git a/src/Capsule.cc b/src/Capsule.cc index 550cfafb0..0976169e7 100644 --- a/src/Capsule.cc +++ b/src/Capsule.cc @@ -141,7 +141,8 @@ gz::math::Capsuled &Capsule::Shape() } ///////////////////////////////////////////////// -std::optional< gz::math::Inertiald > Capsule::CalculateInertial(const double _density) +std::optional Capsule::CalculateInertial( + const double _density) { gz::math::Material material = gz::math::Material(_density); this->dataPtr->capsule.SetMat(material); diff --git a/src/Collision.cc b/src/Collision.cc index 3535151f0..dd691f5fc 100644 --- a/src/Collision.cc +++ b/src/Collision.cc @@ -120,7 +120,7 @@ Errors Collision::Load(ElementPtr _sdf, const ParserConfig &_config) else { // If the density element is missing, let the user know that a default - // value would be used according to the policy + // value would be used according to the policy Error densityMissingErr( ErrorCode::ELEMENT_MISSING, "Collision is missing a child element. " @@ -240,7 +240,8 @@ Errors Collision::CalculateInertial(gz::math::Inertiald &_inertial) { Errors errors; - auto geomInertial = this->dataPtr->geom.CalculateInertial(this->dataPtr->density); + auto geomInertial = + this->dataPtr->geom.CalculateInertial(this->dataPtr->density); if (!geomInertial) { @@ -251,13 +252,13 @@ Errors Collision::CalculateInertial(gz::math::Inertiald &_inertial) else { _inertial = geomInertial.value(); - + // If geometry type is not mesh than calculate inertial pose in Link frame // considering collision frame to be same as inertial frame - // In case of mesh the custom inertia calculator should return the inertial object - // with the pose already set + // In case of mesh the custom inertia calculator should return + // the inertial object with the pose already set if (this->dataPtr->geom.Type() != GeometryType::MESH) - { + { // If collision pose is in Link Frame then set that as inertial pose // Else resolve collision pose in Link Frame and then set as inertial pose if (this->dataPtr->poseRelativeTo.empty()) @@ -267,8 +268,11 @@ Errors Collision::CalculateInertial(gz::math::Inertiald &_inertial) else { gz::math::Pose3d collisionPoseLinkFrame; - Errors poseConvErrors = this->SemanticPose().Resolve(collisionPoseLinkFrame); - errors.insert(errors.end(), poseConvErrors.begin(), poseConvErrors.end()); + Errors poseConvErrors = + this->SemanticPose().Resolve(collisionPoseLinkFrame); + errors.insert(errors.end(), + poseConvErrors.begin(), + poseConvErrors.end()); _inertial.SetPose(collisionPoseLinkFrame); } } diff --git a/src/Cylinder.cc b/src/Cylinder.cc index ab803f27d..b54293e02 100644 --- a/src/Cylinder.cc +++ b/src/Cylinder.cc @@ -137,7 +137,8 @@ gz::math::Cylinderd &Cylinder::Shape() return this->dataPtr->cylinder; } -std::optional< gz::math::Inertiald > Cylinder::CalculateInertial(const double _density) +std::optional Cylinder::CalculateInertial( + const double _density) { gz::math::Material material = gz::math::Material(_density); this->dataPtr->cylinder.SetMat(material); diff --git a/src/Ellipsoid.cc b/src/Ellipsoid.cc index f4bf87d22..25c9a2b11 100644 --- a/src/Ellipsoid.cc +++ b/src/Ellipsoid.cc @@ -117,7 +117,8 @@ gz::math::Ellipsoidd &Ellipsoid::Shape() } ///////////////////////////////////////////////// -std::optional< gz::math::Inertiald > Ellipsoid::CalculateInertial(const double _density) +std::optional Ellipsoid::CalculateInertial( + const double _density) { gz::math::Material material = gz::math::Material(_density); this->dataPtr->ellipsoid.SetMat(material); diff --git a/src/Geometry.cc b/src/Geometry.cc index b2c087ed1..b72ed9595 100644 --- a/src/Geometry.cc +++ b/src/Geometry.cc @@ -304,7 +304,8 @@ void Geometry::SetPolylineShape(const std::vector &_polylines) this->dataPtr->polylines = _polylines; } -std::optional< gz::math::Inertiald > Geometry::CalculateInertial(const double _density) +std::optional Geometry::CalculateInertial( + const double _density) { std::optional< gz::math::Inertiald > geomInertial; @@ -328,7 +329,7 @@ std::optional< gz::math::Inertiald > Geometry::CalculateInertial(const double _d default: break; } - + return geomInertial; } diff --git a/src/Link.cc b/src/Link.cc index ead9b18f6..aea225197 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -245,7 +245,8 @@ Errors Link::Load(ElementPtr _sdf, const ParserConfig &_config) Error inertialMissingErr( ErrorCode::ELEMENT_MISSING, " element is missing. " - "Using default values for the inertial for the link: " + this->dataPtr->name + "Using default values for the inertial " + "for the link: " + this->dataPtr->name ); enforceConfigurablePolicyCondition( _config.WarningsPolicy(), inertialMissingErr, errors @@ -527,8 +528,8 @@ void Link::CalculateInertials(sdf::Errors &_errors) // collision elements in the link if (this->dataPtr->collisions.empty()) { - _errors.push_back({ErrorCode::ELEMENT_MISSING, - "Inertial is set to auto but there are no " + _errors.push_back({ErrorCode::ELEMENT_MISSING, + "Inertial is set to auto but there are no " " elements for the link."}); return; } @@ -539,14 +540,16 @@ void Link::CalculateInertials(sdf::Errors &_errors) { gz::math::Inertiald collisionInertia; Errors inertiaErrors = collision.CalculateInertial(collisionInertia); - _errors.insert(_errors.end(), inertiaErrors.begin(), inertiaErrors.end()); + _errors.insert(_errors.end(), + inertiaErrors.begin(), + inertiaErrors.end()); totalInertia = totalInertia + collisionInertia; } this->dataPtr->inertial = totalInertia; } - // If auto is false, this means inertial values were set from user given values - // in Link::Load(), therefore we can return + // If auto is false, this means inertial values were set + // from user given values in Link::Load(), therefore we can return else { return; diff --git a/src/Sphere.cc b/src/Sphere.cc index 416cca1b5..cf1bc2fe8 100644 --- a/src/Sphere.cc +++ b/src/Sphere.cc @@ -116,7 +116,8 @@ sdf::ElementPtr Sphere::Element() const } ///////////////////////////////////////////////// -std::optional< gz::math::Inertiald > Sphere::CalculateInertial(const double _density) +std::optional Sphere::CalculateInertial( + const double _density) { gz::math::Material material = gz::math::Material(_density); this->dataPtr->sphere.SetMaterial(material); From 0a6a528bfa0a709c917932f996bd2ce24bb62334 Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Fri, 11 Aug 2023 12:53:34 +0530 Subject: [PATCH 35/88] Removed const type qualifier from double density param for CalculateInertial() functions Signed-off-by: Jasmeet Singh --- include/sdf/Box.hh | 3 +-- include/sdf/Capsule.hh | 3 +-- include/sdf/Cylinder.hh | 3 +-- include/sdf/Ellipsoid.hh | 3 +-- include/sdf/Sphere.hh | 3 +-- src/Box.cc | 3 +-- src/Capsule.cc | 3 +-- src/Cylinder.cc | 3 +-- src/Ellipsoid.cc | 3 +-- src/Sphere.cc | 3 +-- 10 files changed, 10 insertions(+), 20 deletions(-) diff --git a/include/sdf/Box.hh b/include/sdf/Box.hh index 2aed09696..6af007dff 100644 --- a/include/sdf/Box.hh +++ b/include/sdf/Box.hh @@ -71,8 +71,7 @@ namespace sdf /// \brief Calculate and return the Mass Matrix values for the Box /// \param[in] density Density of the box in kg/m^3 /// \return A std::optional with gz::math::Inertiald object or std::nullopt - public: std::optional CalculateInertial( - const double _density); + public: std::optional CalculateInertial(double _density); /// \brief Create and return an SDF element filled with data from this /// box. diff --git a/include/sdf/Capsule.hh b/include/sdf/Capsule.hh index e5b4f8363..faa12da90 100644 --- a/include/sdf/Capsule.hh +++ b/include/sdf/Capsule.hh @@ -78,8 +78,7 @@ namespace sdf /// \brief Calculate and return the Mass Matrix values for the Capsule /// \param[in] density Density of the capsule in kg/m^3 /// \return A std::optional with gz::math::Inertiald object or std::nullopt - public: std::optional CalculateInertial( - const double _density); + public: std::optional CalculateInertial(double _density); /// \brief Create and return an SDF element filled with data from this /// capsule. diff --git a/include/sdf/Cylinder.hh b/include/sdf/Cylinder.hh index 6061720c6..4a09cb993 100644 --- a/include/sdf/Cylinder.hh +++ b/include/sdf/Cylinder.hh @@ -78,8 +78,7 @@ namespace sdf /// \brief Calculate and return the Mass Matrix values for the Cylinder /// \param[in] density Density of the cylinder in kg/m^3 /// \return A std::optional with gz::math::Inertiald object or std::nullopt - public: std::optional CalculateInertial( - const double _density); + public: std::optional CalculateInertial(double _density); /// \brief Create and return an SDF element filled with data from this /// cylinder. diff --git a/include/sdf/Ellipsoid.hh b/include/sdf/Ellipsoid.hh index d284021ad..e90972ad4 100644 --- a/include/sdf/Ellipsoid.hh +++ b/include/sdf/Ellipsoid.hh @@ -70,8 +70,7 @@ namespace sdf /// \brief Calculate and return the Mass Matrix values for the Ellipsoid /// \param[in] density Density of the ellipsoid in kg/m^3 /// \return A std::optional with gz::math::Inertiald object or std::nullopt - public: std::optional CalculateInertial( - const double _density); + public: std::optional CalculateInertial(double _density); /// \brief Create and return an SDF element filled with data from this /// ellipsoid. diff --git a/include/sdf/Sphere.hh b/include/sdf/Sphere.hh index eb1bb4eb4..0f556644f 100644 --- a/include/sdf/Sphere.hh +++ b/include/sdf/Sphere.hh @@ -71,8 +71,7 @@ namespace sdf /// \brief Calculate and return the Mass Matrix values for the Sphere /// \param[in] density Density of the sphere in kg/m^3 /// \return A std::optional with gz::math::Inertiald object or std::nullopt - public: std::optional CalculateInertial( - const double _density); + public: std::optional CalculateInertial(double _density); /// \brief Create and return an SDF element filled with data from this /// sphere. diff --git a/src/Box.cc b/src/Box.cc index 10b70fb79..a2338c1fe 100644 --- a/src/Box.cc +++ b/src/Box.cc @@ -121,8 +121,7 @@ gz::math::Boxd &Box::Shape() } ///////////////////////////////////////////////// -std::optional Box::CalculateInertial( - const double _density) +std::optional Box::CalculateInertial(double _density) { gz::math::Material material = gz::math::Material(_density); this->dataPtr->box.SetMaterial(material); diff --git a/src/Capsule.cc b/src/Capsule.cc index 996fe0975..1f8e2bd00 100644 --- a/src/Capsule.cc +++ b/src/Capsule.cc @@ -142,8 +142,7 @@ gz::math::Capsuled &Capsule::Shape() } ///////////////////////////////////////////////// -std::optional Capsule::CalculateInertial( - const double _density) +std::optional Capsule::CalculateInertial(double _density) { gz::math::Material material = gz::math::Material(_density); this->dataPtr->capsule.SetMat(material); diff --git a/src/Cylinder.cc b/src/Cylinder.cc index 4a934cfe4..365037172 100644 --- a/src/Cylinder.cc +++ b/src/Cylinder.cc @@ -138,8 +138,7 @@ gz::math::Cylinderd &Cylinder::Shape() return this->dataPtr->cylinder; } -std::optional Cylinder::CalculateInertial( - const double _density) +std::optional Cylinder::CalculateInertial(double _density) { gz::math::Material material = gz::math::Material(_density); this->dataPtr->cylinder.SetMat(material); diff --git a/src/Ellipsoid.cc b/src/Ellipsoid.cc index a22cc20e0..6d04757e7 100644 --- a/src/Ellipsoid.cc +++ b/src/Ellipsoid.cc @@ -118,8 +118,7 @@ gz::math::Ellipsoidd &Ellipsoid::Shape() } ///////////////////////////////////////////////// -std::optional Ellipsoid::CalculateInertial( - const double _density) +std::optional Ellipsoid::CalculateInertial(double _density) { gz::math::Material material = gz::math::Material(_density); this->dataPtr->ellipsoid.SetMat(material); diff --git a/src/Sphere.cc b/src/Sphere.cc index b0723b794..55bb052d8 100644 --- a/src/Sphere.cc +++ b/src/Sphere.cc @@ -117,8 +117,7 @@ sdf::ElementPtr Sphere::Element() const } ///////////////////////////////////////////////// -std::optional Sphere::CalculateInertial( - const double _density) +std::optional Sphere::CalculateInertial(double _density) { gz::math::Material material = gz::math::Material(_density); this->dataPtr->sphere.SetMaterial(material); From 138bdb45d76c00dc1383c7529134cde1f6e40cd6 Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Fri, 11 Aug 2023 12:54:22 +0530 Subject: [PATCH 36/88] Shifted check for density element from Collision::Load() to Collision::CalculateInertials() Signed-off-by: Jasmeet Singh --- include/sdf/Collision.hh | 2 +- src/Collision.cc | 40 ++++++++++++++++++++-------------------- 2 files changed, 21 insertions(+), 21 deletions(-) diff --git a/include/sdf/Collision.hh b/include/sdf/Collision.hh index 6b098cfc2..e49d34e79 100644 --- a/include/sdf/Collision.hh +++ b/include/sdf/Collision.hh @@ -136,7 +136,7 @@ namespace sdf /// calculated inertial values /// \return Errors, which is a vector of Error objects. Each Error includes /// an error code and message. An empty vector indicates no error - public: Errors CalculateInertial(gz::math::Inertiald &_inertial); + public: Errors CalculateInertial(gz::math::Inertiald &_inertial, const ParserConfig &_config); /// \brief Get a pointer to the SDF element that was used during /// load. diff --git a/src/Collision.cc b/src/Collision.cc index 019333cb0..3058b4b0a 100644 --- a/src/Collision.cc +++ b/src/Collision.cc @@ -112,25 +112,6 @@ Errors Collision::Load(ElementPtr _sdf, const ParserConfig &_config) _sdf->GetElement("geometry", errors), _config); errors.insert(errors.end(), geomErr.begin(), geomErr.end()); - // Set the density value for the collision material - if (_sdf->HasElement("density")) - { - this->dataPtr->density = _sdf->Get("density"); - } - else - { - // If the density element is missing, let the user know that a default - // value would be used according to the policy - Error densityMissingErr( - ErrorCode::ELEMENT_MISSING, - "Collision is missing a child element. " - "Using a default density value of 1000.0 kg/m^3. " - ); - enforceConfigurablePolicyCondition( - _config.WarningsPolicy(), densityMissingErr, errors - ); - } - // Load the surface parameters if they are given if (_sdf->HasElement("surface")) { @@ -236,10 +217,29 @@ sdf::SemanticPose Collision::SemanticPose() const } ///////////////////////////////////////////////// -Errors Collision::CalculateInertial(gz::math::Inertiald &_inertial) +Errors Collision::CalculateInertial(gz::math::Inertiald &_inertial, const ParserConfig &_config) { Errors errors; + // Set the density value for the collision material + if (this->dataPtr->sdf->HasElement("density")) + { + this->dataPtr->density = this->dataPtr->sdf->Get("density"); + } + else + { + // If the density element is missing, let the user know that a default + // value would be used according to the policy + Error densityMissingErr( + ErrorCode::ELEMENT_MISSING, + "Collision is missing a child element. " + "Using a default density value of 1000.0 kg/m^3. " + ); + enforceConfigurablePolicyCondition( + _config.WarningsPolicy(), densityMissingErr, errors + ); + } + auto geomInertial = this->dataPtr->geom.CalculateInertial(this->dataPtr->density); From fa1da0b40f3755878cd13529a3c50433c8728627 Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Fri, 11 Aug 2023 12:56:22 +0530 Subject: [PATCH 37/88] Shifted CalculateInertials() call to end of Root:Load() Signed-off-by: Jasmeet Singh --- src/Root.cc | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/Root.cc b/src/Root.cc index 1e9392a4c..2338073c5 100644 --- a/src/Root.cc +++ b/src/Root.cc @@ -286,8 +286,6 @@ Errors Root::Load(SDFPtr _sdf, const ParserConfig &_config) this->dataPtr->UpdateGraphs(world, worldErrors); - world.CalculateInertials(worldErrors); - // Attempt to load the world if (worldErrors.empty()) { @@ -328,7 +326,6 @@ Errors Root::Load(SDFPtr _sdf, const ParserConfig &_config) this->dataPtr->modelLightOrActor = std::move(models.front()); sdf::Model &model = std::get(this->dataPtr->modelLightOrActor); this->dataPtr->UpdateGraphs(model, errors); - model.CalculateInertials(errors); } // Load all the lights. @@ -392,6 +389,10 @@ Errors Root::Load(SDFPtr _sdf, const ParserConfig &_config) // Check that //axis*/xyz/@expressed_in values specify valid frames. checkJointAxisExpressedInValues(this, errors); + // Calculate Inertials for all worlds and models belonging to this root object + Errors inertialErrors = CalculateInertials(_config); + errors.insert(errors.end(), inertialErrors.begin(), inertialErrors.end()); + return errors; } From 9c6f8c5e8e9821622f953e7d6b721e6e57084eef Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Fri, 11 Aug 2023 13:01:44 +0530 Subject: [PATCH 38/88] Update CalculateInertials() function in Root, World, Model, Link & Collision to take ParserConfig as param Signed-off-by: Jasmeet Singh --- include/sdf/Collision.hh | 1 + include/sdf/Link.hh | 3 ++- include/sdf/Model.hh | 3 ++- include/sdf/Root.hh | 3 ++- include/sdf/World.hh | 3 ++- src/Link.cc | 4 ++-- src/Model.cc | 15 +++++---------- src/Root.cc | 6 +++--- src/World.cc | 4 ++-- 9 files changed, 21 insertions(+), 21 deletions(-) diff --git a/include/sdf/Collision.hh b/include/sdf/Collision.hh index e49d34e79..c91dbbeb3 100644 --- a/include/sdf/Collision.hh +++ b/include/sdf/Collision.hh @@ -132,6 +132,7 @@ namespace sdf public: sdf::SemanticPose SemanticPose() const; /// \brief Calculate and return the MassMatrix for the collision + /// \param[in] _config Custom parser configuration /// \param[out] _inertial An inertial object which will be set with the /// calculated inertial values /// \return Errors, which is a vector of Error objects. Each Error includes diff --git a/include/sdf/Link.hh b/include/sdf/Link.hh index c5c3e7b3f..fc0b5b522 100644 --- a/include/sdf/Link.hh +++ b/include/sdf/Link.hh @@ -330,9 +330,10 @@ namespace sdf /// & inertial pose) for the link. Inertial values can be provided /// by the user through the SDF or can be calculated automatically /// by setting the auto attribute to true. + /// \param[in] _config Custom parser configuration /// \param[out] _errors A vector of Errors object. Each object /// would contain an error code and an error message. - public: void CalculateInertials(sdf::Errors &_errors); + public: void CalculateInertials(sdf::Errors &_errors, const ParserConfig &_config); /// \brief Get the pose of the link. This is the pose of the link /// as specified in SDF ( ... ). diff --git a/include/sdf/Model.hh b/include/sdf/Model.hh index 32c5d4ee1..b79827b90 100644 --- a/include/sdf/Model.hh +++ b/include/sdf/Model.hh @@ -510,9 +510,10 @@ namespace sdf /// \brief Calculate and set the inertials for all the links belonging /// to the model object + /// \param[in] _config Custom parser configuration /// \param[out] _errrors A vector of Errors objects. Each errors contains an /// Error code and a message. An empty errors vector indicates no errors - public: void CalculateInertials(sdf::Errors &_errors); + public: void CalculateInertials(sdf::Errors &_errors, const ParserConfig &_config); /// \brief Give the scoped PoseRelativeToGraph to be used for resolving /// poses. This is private and is intended to be called by Root::Load or diff --git a/include/sdf/Root.hh b/include/sdf/Root.hh index 53564a4c6..116e97b24 100644 --- a/include/sdf/Root.hh +++ b/include/sdf/Root.hh @@ -231,10 +231,11 @@ namespace sdf /// of this Root object. This function can be called after calling /// UpdateGraphs() since it uses frame graphs to resolve inertial pose /// for links. + /// \param[in] _config Custom parser configuration /// \return Errors, which is a vector of Error objects. /// Each Error includes an error code and message. An empty vector /// indicates no error. - public: Errors CalculateInertials(); + public: Errors CalculateInertials(const ParserConfig &_config); /// \brief Create and return an SDF element filled with data from this /// root. diff --git a/include/sdf/World.hh b/include/sdf/World.hh index d83b9512b..46527bc3a 100644 --- a/include/sdf/World.hh +++ b/include/sdf/World.hh @@ -518,9 +518,10 @@ namespace sdf /// \brief Calculate and set the inertials for all the models in the world /// object + /// \param[in] _config Custom parser configuration /// \param[out] _errrors A vector of Errors objects. Each errors contains an /// Error code and a message. An empty errors vector indicates no errors - public: void CalculateInertials(sdf::Errors &_errors); + public: void CalculateInertials(sdf::Errors &_errors, const ParserConfig &_config); /// \brief Give the Scoped PoseRelativeToGraph to be passed on to child /// entities for resolving poses. This is private and is intended to be diff --git a/src/Link.cc b/src/Link.cc index 2d1bfd2b9..d2dabea4c 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -581,7 +581,7 @@ Errors Link::ResolveInertial( } ///////////////////////////////////////////////// -void Link::CalculateInertials(sdf::Errors &_errors) +void Link::CalculateInertials(sdf::Errors &_errors, const ParserConfig &_config) { if (this->dataPtr->sdf->HasElement("inertial")) { @@ -604,7 +604,7 @@ void Link::CalculateInertials(sdf::Errors &_errors) for (sdf::Collision &collision : this->dataPtr->collisions) { gz::math::Inertiald collisionInertia; - Errors inertiaErrors = collision.CalculateInertial(collisionInertia); + Errors inertiaErrors = collision.CalculateInertial(collisionInertia, _config); _errors.insert(_errors.end(), inertiaErrors.begin(), inertiaErrors.end()); diff --git a/src/Model.cc b/src/Model.cc index 9ae4bc248..7f6c758b8 100644 --- a/src/Model.cc +++ b/src/Model.cc @@ -837,24 +837,19 @@ void Model::SetPoseRelativeTo(const std::string &_frame) } ///////////////////////////////////////////////// -void Model::CalculateInertials(sdf::Errors &_errors) +void Model::CalculateInertials(sdf::Errors &_errors, const ParserConfig &_config) { - // Check if there are nested models. If yes then call - // CalculateInertials() for each model - if (!this->dataPtr->models.empty()) + // Loop through all the nested models, if there are any + for (sdf::Model &model : this->dataPtr->models) { - for (sdf::Model &model : this->dataPtr->models) - { - model.CalculateInertials(_errors); - } + model.CalculateInertials(_errors, _config); } // Calculate and set inertials for all the links in the model for (sdf::Link &link : this->dataPtr->links) { - link.CalculateInertials(_errors); + link.CalculateInertials(_errors, _config); } - } ///////////////////////////////////////////////// diff --git a/src/Root.cc b/src/Root.cc index 2338073c5..cb84a9f64 100644 --- a/src/Root.cc +++ b/src/Root.cc @@ -592,21 +592,21 @@ void Root::Implementation::UpdateGraphs(sdf::Model &_model, } ///////////////////////////////////////////////// -Errors Root::CalculateInertials() +Errors Root::CalculateInertials(const ParserConfig &_config) { sdf::Errors errors; // Calculate and set Inertials for all the worlds for (sdf::World &world : this->dataPtr->worlds) { - world.CalculateInertials(errors); + world.CalculateInertials(errors, _config); } // Calculate and set Inertials for the model, if it is present if (std::holds_alternative(this->dataPtr->modelLightOrActor)) { sdf::Model &model = std::get(this->dataPtr->modelLightOrActor); - model.CalculateInertials(errors); + model.CalculateInertials(errors, _config); } return errors; diff --git a/src/World.cc b/src/World.cc index 15205782b..2296e8362 100644 --- a/src/World.cc +++ b/src/World.cc @@ -781,12 +781,12 @@ const NestedInclude *World::InterfaceModelNestedIncludeByIndex( } ///////////////////////////////////////////////// -void World::CalculateInertials(sdf::Errors &_errors) +void World::CalculateInertials(sdf::Errors &_errors, const ParserConfig &_config) { // Call CalculateInertials() function for all the models for (sdf::Model &model : this->dataPtr->models) { - model.CalculateInertials(_errors); + model.CalculateInertials(_errors, _config); } } From 1a483a24ef811dadc4bbb7b526956a042791a860 Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Fri, 11 Aug 2023 16:13:56 +0530 Subject: [PATCH 39/88] Completed codecheck Signed-off-by: Jasmeet Singh --- include/sdf/Box.hh | 3 ++- include/sdf/Capsule.hh | 3 ++- include/sdf/Collision.hh | 3 ++- include/sdf/Cylinder.hh | 3 ++- include/sdf/Ellipsoid.hh | 3 ++- include/sdf/Geometry.hh | 4 ++-- include/sdf/Link.hh | 3 ++- include/sdf/Model.hh | 3 ++- include/sdf/Sphere.hh | 3 ++- include/sdf/World.hh | 3 ++- src/Collision.cc | 3 ++- src/Link.cc | 3 ++- src/Model.cc | 3 ++- src/World.cc | 3 ++- 14 files changed, 28 insertions(+), 15 deletions(-) diff --git a/include/sdf/Box.hh b/include/sdf/Box.hh index 6af007dff..6df22ea3c 100644 --- a/include/sdf/Box.hh +++ b/include/sdf/Box.hh @@ -71,7 +71,8 @@ namespace sdf /// \brief Calculate and return the Mass Matrix values for the Box /// \param[in] density Density of the box in kg/m^3 /// \return A std::optional with gz::math::Inertiald object or std::nullopt - public: std::optional CalculateInertial(double _density); + public: std::optional + CalculateInertial(double _density); /// \brief Create and return an SDF element filled with data from this /// box. diff --git a/include/sdf/Capsule.hh b/include/sdf/Capsule.hh index faa12da90..672d6e868 100644 --- a/include/sdf/Capsule.hh +++ b/include/sdf/Capsule.hh @@ -78,7 +78,8 @@ namespace sdf /// \brief Calculate and return the Mass Matrix values for the Capsule /// \param[in] density Density of the capsule in kg/m^3 /// \return A std::optional with gz::math::Inertiald object or std::nullopt - public: std::optional CalculateInertial(double _density); + public: std::optional CalculateInertial( + double _density); /// \brief Create and return an SDF element filled with data from this /// capsule. diff --git a/include/sdf/Collision.hh b/include/sdf/Collision.hh index c91dbbeb3..3f35d515c 100644 --- a/include/sdf/Collision.hh +++ b/include/sdf/Collision.hh @@ -137,7 +137,8 @@ namespace sdf /// calculated inertial values /// \return Errors, which is a vector of Error objects. Each Error includes /// an error code and message. An empty vector indicates no error - public: Errors CalculateInertial(gz::math::Inertiald &_inertial, const ParserConfig &_config); + public: Errors CalculateInertial(gz::math::Inertiald &_inertial, + const ParserConfig &_config); /// \brief Get a pointer to the SDF element that was used during /// load. diff --git a/include/sdf/Cylinder.hh b/include/sdf/Cylinder.hh index 4a09cb993..4f3b74651 100644 --- a/include/sdf/Cylinder.hh +++ b/include/sdf/Cylinder.hh @@ -78,7 +78,8 @@ namespace sdf /// \brief Calculate and return the Mass Matrix values for the Cylinder /// \param[in] density Density of the cylinder in kg/m^3 /// \return A std::optional with gz::math::Inertiald object or std::nullopt - public: std::optional CalculateInertial(double _density); + public: std::optional + CalculateInertial(double _density); /// \brief Create and return an SDF element filled with data from this /// cylinder. diff --git a/include/sdf/Ellipsoid.hh b/include/sdf/Ellipsoid.hh index e90972ad4..1baa8e78e 100644 --- a/include/sdf/Ellipsoid.hh +++ b/include/sdf/Ellipsoid.hh @@ -70,7 +70,8 @@ namespace sdf /// \brief Calculate and return the Mass Matrix values for the Ellipsoid /// \param[in] density Density of the ellipsoid in kg/m^3 /// \return A std::optional with gz::math::Inertiald object or std::nullopt - public: std::optional CalculateInertial(double _density); + public: std::optional + CalculateInertial(double _density); /// \brief Create and return an SDF element filled with data from this /// ellipsoid. diff --git a/include/sdf/Geometry.hh b/include/sdf/Geometry.hh index 41c6ab002..949dcf896 100644 --- a/include/sdf/Geometry.hh +++ b/include/sdf/Geometry.hh @@ -214,8 +214,8 @@ namespace sdf /// \brief Calculate and return the Mass Matrix values for the Geometry /// \param[in] _density The density of the geometry element. /// \return A std::optional with gz::math::Inertiald object or std::nullopt - public: std::optional CalculateInertial( - const double _density); + public: std::optional + CalculateInertial(const double _density); /// \brief Get a pointer to the SDF element that was used during /// load. diff --git a/include/sdf/Link.hh b/include/sdf/Link.hh index fc0b5b522..b6de9ef34 100644 --- a/include/sdf/Link.hh +++ b/include/sdf/Link.hh @@ -333,7 +333,8 @@ namespace sdf /// \param[in] _config Custom parser configuration /// \param[out] _errors A vector of Errors object. Each object /// would contain an error code and an error message. - public: void CalculateInertials(sdf::Errors &_errors, const ParserConfig &_config); + public: void CalculateInertials(sdf::Errors &_errors, + const ParserConfig &_config); /// \brief Get the pose of the link. This is the pose of the link /// as specified in SDF ( ... ). diff --git a/include/sdf/Model.hh b/include/sdf/Model.hh index b79827b90..273dc763a 100644 --- a/include/sdf/Model.hh +++ b/include/sdf/Model.hh @@ -513,7 +513,8 @@ namespace sdf /// \param[in] _config Custom parser configuration /// \param[out] _errrors A vector of Errors objects. Each errors contains an /// Error code and a message. An empty errors vector indicates no errors - public: void CalculateInertials(sdf::Errors &_errors, const ParserConfig &_config); + public: void CalculateInertials(sdf::Errors &_errors, + const ParserConfig &_config); /// \brief Give the scoped PoseRelativeToGraph to be used for resolving /// poses. This is private and is intended to be called by Root::Load or diff --git a/include/sdf/Sphere.hh b/include/sdf/Sphere.hh index 0f556644f..bafd20c91 100644 --- a/include/sdf/Sphere.hh +++ b/include/sdf/Sphere.hh @@ -71,7 +71,8 @@ namespace sdf /// \brief Calculate and return the Mass Matrix values for the Sphere /// \param[in] density Density of the sphere in kg/m^3 /// \return A std::optional with gz::math::Inertiald object or std::nullopt - public: std::optional CalculateInertial(double _density); + public: std::optional + CalculateInertial(double _density); /// \brief Create and return an SDF element filled with data from this /// sphere. diff --git a/include/sdf/World.hh b/include/sdf/World.hh index 46527bc3a..fc0467bad 100644 --- a/include/sdf/World.hh +++ b/include/sdf/World.hh @@ -521,7 +521,8 @@ namespace sdf /// \param[in] _config Custom parser configuration /// \param[out] _errrors A vector of Errors objects. Each errors contains an /// Error code and a message. An empty errors vector indicates no errors - public: void CalculateInertials(sdf::Errors &_errors, const ParserConfig &_config); + public: void CalculateInertials(sdf::Errors &_errors, + const ParserConfig &_config); /// \brief Give the Scoped PoseRelativeToGraph to be passed on to child /// entities for resolving poses. This is private and is intended to be diff --git a/src/Collision.cc b/src/Collision.cc index 3058b4b0a..cf96812fa 100644 --- a/src/Collision.cc +++ b/src/Collision.cc @@ -217,7 +217,8 @@ sdf::SemanticPose Collision::SemanticPose() const } ///////////////////////////////////////////////// -Errors Collision::CalculateInertial(gz::math::Inertiald &_inertial, const ParserConfig &_config) +Errors Collision::CalculateInertial(gz::math::Inertiald &_inertial, + const ParserConfig &_config) { Errors errors; diff --git a/src/Link.cc b/src/Link.cc index d2dabea4c..3a38ad5a6 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -604,7 +604,8 @@ void Link::CalculateInertials(sdf::Errors &_errors, const ParserConfig &_config) for (sdf::Collision &collision : this->dataPtr->collisions) { gz::math::Inertiald collisionInertia; - Errors inertiaErrors = collision.CalculateInertial(collisionInertia, _config); + Errors inertiaErrors = + collision.CalculateInertial(collisionInertia, _config); _errors.insert(_errors.end(), inertiaErrors.begin(), inertiaErrors.end()); diff --git a/src/Model.cc b/src/Model.cc index 7f6c758b8..d8c5eb1c3 100644 --- a/src/Model.cc +++ b/src/Model.cc @@ -837,7 +837,8 @@ void Model::SetPoseRelativeTo(const std::string &_frame) } ///////////////////////////////////////////////// -void Model::CalculateInertials(sdf::Errors &_errors, const ParserConfig &_config) +void Model::CalculateInertials(sdf::Errors &_errors, + const ParserConfig &_config) { // Loop through all the nested models, if there are any for (sdf::Model &model : this->dataPtr->models) diff --git a/src/World.cc b/src/World.cc index 2296e8362..a2191096d 100644 --- a/src/World.cc +++ b/src/World.cc @@ -781,7 +781,8 @@ const NestedInclude *World::InterfaceModelNestedIncludeByIndex( } ///////////////////////////////////////////////// -void World::CalculateInertials(sdf::Errors &_errors, const ParserConfig &_config) +void World::CalculateInertials(sdf::Errors &_errors, + const ParserConfig &_config) { // Call CalculateInertials() function for all the models for (sdf::Model &model : this->dataPtr->models) From 2f0c8405676e31e272d7f7b495466ba78366be41 Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Fri, 11 Aug 2023 16:36:14 +0530 Subject: [PATCH 40/88] Removed const specifier for double density from Geometry & Collision Signed-off-by: Jasmeet Singh --- include/sdf/Collision.hh | 2 +- include/sdf/Geometry.hh | 2 +- src/Collision.cc | 2 +- src/Geometry.cc | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/include/sdf/Collision.hh b/include/sdf/Collision.hh index 3f35d515c..22c760e03 100644 --- a/include/sdf/Collision.hh +++ b/include/sdf/Collision.hh @@ -85,7 +85,7 @@ namespace sdf /// \brief Set the density of the collision. /// The density of the collision must be unique within the scope of a Link. /// \param[in] _density Density of the collision. - public: void SetDensity(const double _density); + public: void SetDensity(double _density); /// \brief Get a pointer to the collisions's geometry. /// \return The collision's geometry. diff --git a/include/sdf/Geometry.hh b/include/sdf/Geometry.hh index 949dcf896..05aec3244 100644 --- a/include/sdf/Geometry.hh +++ b/include/sdf/Geometry.hh @@ -215,7 +215,7 @@ namespace sdf /// \param[in] _density The density of the geometry element. /// \return A std::optional with gz::math::Inertiald object or std::nullopt public: std::optional - CalculateInertial(const double _density); + CalculateInertial(double _density); /// \brief Get a pointer to the SDF element that was used during /// load. diff --git a/src/Collision.cc b/src/Collision.cc index cf96812fa..0b1787cf8 100644 --- a/src/Collision.cc +++ b/src/Collision.cc @@ -140,7 +140,7 @@ double Collision::Density() const } ///////////////////////////////////////////////// -void Collision::SetDensity(const double _density) +void Collision::SetDensity(double _density) { this->dataPtr->density = _density; } diff --git a/src/Geometry.cc b/src/Geometry.cc index b7332d90a..70fa0df94 100644 --- a/src/Geometry.cc +++ b/src/Geometry.cc @@ -310,7 +310,7 @@ void Geometry::SetPolylineShape(const std::vector &_polylines) } std::optional Geometry::CalculateInertial( - const double _density) + double _density) { std::optional< gz::math::Inertiald > geomInertial; From ac943c652f84ff23acc602fd13f00f9d9a32f2a7 Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Mon, 14 Aug 2023 18:49:34 +0530 Subject: [PATCH 41/88] Corrected function comments Signed-off-by: Jasmeet Singh --- include/sdf/Box.hh | 2 +- include/sdf/Capsule.hh | 2 +- include/sdf/Collision.hh | 2 +- include/sdf/Cylinder.hh | 2 +- include/sdf/Ellipsoid.hh | 2 +- include/sdf/Link.hh | 2 +- include/sdf/Model.hh | 2 +- include/sdf/Sphere.hh | 2 +- include/sdf/World.hh | 2 +- 9 files changed, 9 insertions(+), 9 deletions(-) diff --git a/include/sdf/Box.hh b/include/sdf/Box.hh index 6df22ea3c..1cf4ab7fd 100644 --- a/include/sdf/Box.hh +++ b/include/sdf/Box.hh @@ -69,7 +69,7 @@ namespace sdf public: gz::math::Boxd &Shape(); /// \brief Calculate and return the Mass Matrix values for the Box - /// \param[in] density Density of the box in kg/m^3 + /// \param[in] _density Density of the box in kg/m^3 /// \return A std::optional with gz::math::Inertiald object or std::nullopt public: std::optional CalculateInertial(double _density); diff --git a/include/sdf/Capsule.hh b/include/sdf/Capsule.hh index 672d6e868..5c91fa3ea 100644 --- a/include/sdf/Capsule.hh +++ b/include/sdf/Capsule.hh @@ -76,7 +76,7 @@ namespace sdf public: gz::math::Capsuled &Shape(); /// \brief Calculate and return the Mass Matrix values for the Capsule - /// \param[in] density Density of the capsule in kg/m^3 + /// \param[in] _density Density of the capsule in kg/m^3 /// \return A std::optional with gz::math::Inertiald object or std::nullopt public: std::optional CalculateInertial( double _density); diff --git a/include/sdf/Collision.hh b/include/sdf/Collision.hh index 22c760e03..e713c3b34 100644 --- a/include/sdf/Collision.hh +++ b/include/sdf/Collision.hh @@ -132,9 +132,9 @@ namespace sdf public: sdf::SemanticPose SemanticPose() const; /// \brief Calculate and return the MassMatrix for the collision - /// \param[in] _config Custom parser configuration /// \param[out] _inertial An inertial object which will be set with the /// calculated inertial values + /// \param[in] _config Custom parser configuration /// \return Errors, which is a vector of Error objects. Each Error includes /// an error code and message. An empty vector indicates no error public: Errors CalculateInertial(gz::math::Inertiald &_inertial, diff --git a/include/sdf/Cylinder.hh b/include/sdf/Cylinder.hh index 4f3b74651..91b0d1fcc 100644 --- a/include/sdf/Cylinder.hh +++ b/include/sdf/Cylinder.hh @@ -76,7 +76,7 @@ namespace sdf public: gz::math::Cylinderd &Shape(); /// \brief Calculate and return the Mass Matrix values for the Cylinder - /// \param[in] density Density of the cylinder in kg/m^3 + /// \param[in] _density Density of the cylinder in kg/m^3 /// \return A std::optional with gz::math::Inertiald object or std::nullopt public: std::optional CalculateInertial(double _density); diff --git a/include/sdf/Ellipsoid.hh b/include/sdf/Ellipsoid.hh index 1baa8e78e..168331cb5 100644 --- a/include/sdf/Ellipsoid.hh +++ b/include/sdf/Ellipsoid.hh @@ -68,7 +68,7 @@ namespace sdf public: gz::math::Ellipsoidd &Shape(); /// \brief Calculate and return the Mass Matrix values for the Ellipsoid - /// \param[in] density Density of the ellipsoid in kg/m^3 + /// \param[in] _density Density of the ellipsoid in kg/m^3 /// \return A std::optional with gz::math::Inertiald object or std::nullopt public: std::optional CalculateInertial(double _density); diff --git a/include/sdf/Link.hh b/include/sdf/Link.hh index b6de9ef34..a34a10914 100644 --- a/include/sdf/Link.hh +++ b/include/sdf/Link.hh @@ -330,9 +330,9 @@ namespace sdf /// & inertial pose) for the link. Inertial values can be provided /// by the user through the SDF or can be calculated automatically /// by setting the auto attribute to true. - /// \param[in] _config Custom parser configuration /// \param[out] _errors A vector of Errors object. Each object /// would contain an error code and an error message. + /// \param[in] _config Custom parser configuration public: void CalculateInertials(sdf::Errors &_errors, const ParserConfig &_config); diff --git a/include/sdf/Model.hh b/include/sdf/Model.hh index 273dc763a..d3581c24a 100644 --- a/include/sdf/Model.hh +++ b/include/sdf/Model.hh @@ -510,9 +510,9 @@ namespace sdf /// \brief Calculate and set the inertials for all the links belonging /// to the model object - /// \param[in] _config Custom parser configuration /// \param[out] _errrors A vector of Errors objects. Each errors contains an /// Error code and a message. An empty errors vector indicates no errors + /// \param[in] _config Custom parser configuration public: void CalculateInertials(sdf::Errors &_errors, const ParserConfig &_config); diff --git a/include/sdf/Sphere.hh b/include/sdf/Sphere.hh index bafd20c91..f82306c18 100644 --- a/include/sdf/Sphere.hh +++ b/include/sdf/Sphere.hh @@ -69,7 +69,7 @@ namespace sdf public: sdf::ElementPtr Element() const; /// \brief Calculate and return the Mass Matrix values for the Sphere - /// \param[in] density Density of the sphere in kg/m^3 + /// \param[in] _density Density of the sphere in kg/m^3 /// \return A std::optional with gz::math::Inertiald object or std::nullopt public: std::optional CalculateInertial(double _density); diff --git a/include/sdf/World.hh b/include/sdf/World.hh index fc0467bad..29747bcad 100644 --- a/include/sdf/World.hh +++ b/include/sdf/World.hh @@ -518,9 +518,9 @@ namespace sdf /// \brief Calculate and set the inertials for all the models in the world /// object - /// \param[in] _config Custom parser configuration /// \param[out] _errrors A vector of Errors objects. Each errors contains an /// Error code and a message. An empty errors vector indicates no errors + /// \param[in] _config Custom parser configuration public: void CalculateInertials(sdf::Errors &_errors, const ParserConfig &_config); From 8fef7400ad028ca9015c8454e52e713a01f0129c Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Mon, 14 Aug 2023 19:21:30 +0530 Subject: [PATCH 42/88] Included header Signed-off-by: Jasmeet Singh --- src/Cylinder.cc | 2 ++ src/Ellipsoid.cc | 2 ++ src/Geometry.cc | 2 +- src/Sphere.cc | 1 + 4 files changed, 6 insertions(+), 1 deletion(-) diff --git a/src/Cylinder.cc b/src/Cylinder.cc index 365037172..8c7779d00 100644 --- a/src/Cylinder.cc +++ b/src/Cylinder.cc @@ -15,6 +15,8 @@ * */ #include +#include + #include #include "sdf/Cylinder.hh" #include "sdf/parser.hh" diff --git a/src/Ellipsoid.cc b/src/Ellipsoid.cc index 6d04757e7..5300df90b 100644 --- a/src/Ellipsoid.cc +++ b/src/Ellipsoid.cc @@ -15,6 +15,8 @@ * */ #include +#include + #include #include "sdf/Ellipsoid.hh" #include "sdf/parser.hh" diff --git a/src/Geometry.cc b/src/Geometry.cc index 70fa0df94..341c614df 100644 --- a/src/Geometry.cc +++ b/src/Geometry.cc @@ -312,7 +312,7 @@ void Geometry::SetPolylineShape(const std::vector &_polylines) std::optional Geometry::CalculateInertial( double _density) { - std::optional< gz::math::Inertiald > geomInertial; + std::optional geomInertial; switch (this->dataPtr->type) { diff --git a/src/Sphere.cc b/src/Sphere.cc index 55bb052d8..6eb851b14 100644 --- a/src/Sphere.cc +++ b/src/Sphere.cc @@ -14,6 +14,7 @@ * limitations under the License. * */ +#include #include #include "sdf/parser.hh" From 80e321cf2952dd458f5ab0b2ce7995849550df67 Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Thu, 17 Aug 2023 17:57:09 +0530 Subject: [PATCH 43/88] Removed warning for missing element Signed-off-by: Jasmeet Singh --- src/Link.cc | 12 ------------ 1 file changed, 12 deletions(-) diff --git a/src/Link.cc b/src/Link.cc index 3a38ad5a6..14bef876f 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -250,18 +250,6 @@ Errors Link::Load(ElementPtr _sdf, const ParserConfig &_config) } } } - else - { - Error inertialMissingErr( - ErrorCode::ELEMENT_MISSING, - " element is missing. " - "Using default values for the inertial " - "for the link: " + this->dataPtr->name - ); - enforceConfigurablePolicyCondition( - _config.WarningsPolicy(), inertialMissingErr, errors - ); - } if (!this->dataPtr->inertial.SetMassMatrix( gz::math::MassMatrix3d(mass, xxyyzz, xyxzyz))) From e3041e43b8527a5e27b9dca221e628b3397a0b5a Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Thu, 17 Aug 2023 20:41:44 +0530 Subject: [PATCH 44/88] Added warning for overwritting of user given inertial value sif auto is true Signed-off-by: Jasmeet Singh --- src/Link.cc | 20 +++++++++++++++++++- 1 file changed, 19 insertions(+), 1 deletion(-) diff --git a/src/Link.cc b/src/Link.cc index 14bef876f..46bb48cc8 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -171,8 +171,26 @@ Errors Link::Load(ElementPtr _sdf, const ParserConfig &_config) { sdf::ElementPtr inertialElem = _sdf->GetElement("inertial"); + if (inertialElem->Get("auto")) + { + // If auto is to true but user has still provided + // inertial values + if (inertialElem->HasElement("pose") || + inertialElem->HasElement("mass") || + inertialElem->HasElement("inertia")) + { + Error err( + ErrorCode::WARNING, + "Inertial was used with auto=true for link, " + + this->dataPtr->name + " but user defined inertial values were found. " + "They would be overwritten by computed ones." + ); + enforceConfigurablePolicyCondition( + _config.WarningsPolicy(), err, errors); + } + } // If auto is set to false or not specified - if (!inertialElem->Get("auto")) + else { if (inertialElem->HasElement("pose")) loadPose(inertialElem->GetElement("pose"), inertiaPose, inertiaFrame); From cc9849cd91cf471742e2652ba8b4af338269dad7 Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Thu, 17 Aug 2023 22:13:40 +0530 Subject: [PATCH 45/88] Removed call to CalculateInertial() from Root::Load() Signed-off-by: Jasmeet Singh --- src/Root.cc | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/Root.cc b/src/Root.cc index cb84a9f64..baa1ffa55 100644 --- a/src/Root.cc +++ b/src/Root.cc @@ -389,10 +389,6 @@ Errors Root::Load(SDFPtr _sdf, const ParserConfig &_config) // Check that //axis*/xyz/@expressed_in values specify valid frames. checkJointAxisExpressedInValues(this, errors); - // Calculate Inertials for all worlds and models belonging to this root object - Errors inertialErrors = CalculateInertials(_config); - errors.insert(errors.end(), inertialErrors.begin(), inertialErrors.end()); - return errors; } From 0e41116cee7053559ffc43c25ac714aa6f480f25 Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Tue, 22 Aug 2023 00:15:19 +0530 Subject: [PATCH 46/88] Corrected codecheck Signed-off-by: Jasmeet Singh --- src/Link.cc | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/Link.cc b/src/Link.cc index 46bb48cc8..15a49a807 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -173,7 +173,7 @@ Errors Link::Load(ElementPtr _sdf, const ParserConfig &_config) if (inertialElem->Get("auto")) { - // If auto is to true but user has still provided + // If auto is to true but user has still provided // inertial values if (inertialElem->HasElement("pose") || inertialElem->HasElement("mass") || @@ -181,9 +181,9 @@ Errors Link::Load(ElementPtr _sdf, const ParserConfig &_config) { Error err( ErrorCode::WARNING, - "Inertial was used with auto=true for link, " + - this->dataPtr->name + " but user defined inertial values were found. " - "They would be overwritten by computed ones." + "Inertial was used with auto=true for link, " + + this->dataPtr->name + " but user defined inertial values " + "were found. They would be overwritten by computed ones." ); enforceConfigurablePolicyCondition( _config.WarningsPolicy(), err, errors); From 4731e211116fbf6bb86ae2fa7b9ca167c1d60b8b Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Tue, 22 Aug 2023 18:38:28 +0530 Subject: [PATCH 47/88] Added unit tests for CalculateInertial() function in Box, Capsule, Cylinder, Ellipsoid & Sphere Signed-off-by: Jasmeet Singh --- src/Box_TEST.cc | 39 +++++++++++++++++++++++++++++++++++++ src/Capsule_TEST.cc | 45 +++++++++++++++++++++++++++++++++++++++++++ src/Cylinder_TEST.cc | 38 ++++++++++++++++++++++++++++++++++++ src/Ellipsoid_TEST.cc | 39 +++++++++++++++++++++++++++++++++++++ src/Sphere_TEST.cc | 35 +++++++++++++++++++++++++++++++++ 5 files changed, 196 insertions(+) diff --git a/src/Box_TEST.cc b/src/Box_TEST.cc index c386710b5..2b8f34c7b 100644 --- a/src/Box_TEST.cc +++ b/src/Box_TEST.cc @@ -145,6 +145,45 @@ TEST(DOMBox, Shape) EXPECT_EQ(gz::math::Vector3d(1, 2, 3), box.Size()); } +///////////////////////////////////////////////// +TEST(DOMBox, CalculateInertial) +{ + sdf::Box box; + + // density of aluminium + double density = 2710; + double l = 2; + double w = 2; + double h = 2; + + box.SetSize(gz::math::Vector3d(l, w, h)); + + double expectedMass = box.Shape().Volume() * density; + double ixx = (1.0/12.0) * expectedMass * (w*w + h*h); + double iyy = (1.0/12.0) * expectedMass * (l*l + h*h); + double izz = (1.0/12.0) * expectedMass * (l*l + w*w); + + gz::math::MassMatrix3d expectedMassMat( + expectedMass, + gz::math::Vector3d(ixx, iyy, izz), + gz::math::Vector3d::Zero + ); + + gz::math::Inertiald expectedInertial; + expectedInertial.SetMassMatrix(expectedMassMat); + expectedInertial.SetPose(gz::math::Pose3d::Zero); + + auto boxInertial = box.CalculateInertial(density); + EXPECT_EQ(box.Shape().Material().Density(), density); + ASSERT_NE(std::nullopt, boxInertial); + EXPECT_EQ(expectedInertial, *boxInertial); + EXPECT_EQ(expectedInertial.MassMatrix().DiagonalMoments(), + boxInertial->MassMatrix().DiagonalMoments()); + EXPECT_EQ(expectedInertial.MassMatrix().Mass(), + boxInertial->MassMatrix().Mass()); + EXPECT_EQ(expectedInertial.Pose(), boxInertial->Pose()); +} + ///////////////////////////////////////////////// TEST(DOMBox, ToElement) { diff --git a/src/Capsule_TEST.cc b/src/Capsule_TEST.cc index b24495966..64e8c2d06 100644 --- a/src/Capsule_TEST.cc +++ b/src/Capsule_TEST.cc @@ -181,6 +181,51 @@ TEST(DOMCapsule, Shape) EXPECT_DOUBLE_EQ(0.456, capsule.Length()); } +///////////////////////////////////////////////// +TEST(DOMCapsule, CalculateInertial) +{ + sdf::Capsule capsule; + + // density of aluminium + double density = 2710; + double l = 2.0; + double r = 0.1; + + capsule.SetLength(l); + capsule.SetRadius(r); + + double expectedMass = capsule.Shape().Volume() * density; + const double cylinderVolume = GZ_PI * r*r * l; + const double sphereVolume = GZ_PI * 4. / 3. * r*r*r; + const double volume = cylinderVolume + sphereVolume; + const double cylinderMass = expectedMass * cylinderVolume / volume; + const double sphereMass = expectedMass * sphereVolume / volume; + + double ixxIyy = (1/12.0) * cylinderMass * (3*r*r + l*l) + + sphereMass * (0.4*r*r + 0.375*r*l + 0.25*l*l); + double izz = r*r * (0.5 * cylinderMass + 0.4 * sphereMass); + + gz::math::MassMatrix3d expectedMassMat( + expectedMass, + gz::math::Vector3d(ixxIyy, ixxIyy, izz), + gz::math::Vector3d::Zero + ); + + gz::math::Inertiald expectedInertial; + expectedInertial.SetMassMatrix(expectedMassMat); + expectedInertial.SetPose(gz::math::Pose3d::Zero); + + auto capsuleInertial = capsule.CalculateInertial(density); + EXPECT_EQ(capsule.Shape().Mat().Density(), density); + ASSERT_NE(std::nullopt, capsuleInertial); + EXPECT_EQ(expectedInertial, *capsuleInertial); + EXPECT_EQ(expectedInertial.MassMatrix().DiagonalMoments(), + capsuleInertial->MassMatrix().DiagonalMoments()); + EXPECT_EQ(expectedInertial.MassMatrix().Mass(), + capsuleInertial->MassMatrix().Mass()); + EXPECT_EQ(expectedInertial.Pose(), capsuleInertial->Pose()); +} + ///////////////////////////////////////////////// TEST(DOMCapsule, ToElement) { diff --git a/src/Cylinder_TEST.cc b/src/Cylinder_TEST.cc index efc7f302b..698c0a735 100644 --- a/src/Cylinder_TEST.cc +++ b/src/Cylinder_TEST.cc @@ -177,6 +177,44 @@ TEST(DOMCylinder, Shape) EXPECT_DOUBLE_EQ(0.456, cylinder.Length()); } +///////////////////////////////////////////////// +TEST(DOMCylinder, CalculateInertial) +{ + sdf::Cylinder cylinder; + + // density of aluminium + double density = 2170; + double l = 2.0; + double r = 0.1; + + cylinder.SetLength(l); + cylinder.SetRadius(r); + + double expectedMass = cylinder.Shape().Volume() * density; + double ixxIyy = (1/12.0) * expectedMass * (3*r*r + l*l); + double izz = 0.5 * expectedMass * r * r; + + gz::math::MassMatrix3d expectedMassMat( + expectedMass, + gz::math::Vector3d(ixxIyy, ixxIyy, izz), + gz::math::Vector3d::Zero + ); + + gz::math::Inertiald expectedInertial; + expectedInertial.SetMassMatrix(expectedMassMat); + expectedInertial.SetPose(gz::math::Pose3d::Zero); + + auto cylinderInertial = cylinder.CalculateInertial(density); + EXPECT_EQ(cylinder.Shape().Mat().Density(), density); + ASSERT_NE(std::nullopt, cylinderInertial); + EXPECT_EQ(expectedInertial, *cylinderInertial); + EXPECT_EQ(expectedInertial.MassMatrix().DiagonalMoments(), + cylinderInertial->MassMatrix().DiagonalMoments()); + EXPECT_EQ(expectedInertial.MassMatrix().Mass(), + cylinderInertial->MassMatrix().Mass()); + EXPECT_EQ(expectedInertial.Pose(), cylinderInertial->Pose()); +} + ///////////////////////////////////////////////// TEST(DOMCylinder, ToElement) { diff --git a/src/Ellipsoid_TEST.cc b/src/Ellipsoid_TEST.cc index 9f3e33a8e..1186fd7fe 100644 --- a/src/Ellipsoid_TEST.cc +++ b/src/Ellipsoid_TEST.cc @@ -141,6 +141,45 @@ TEST(DOMEllipsoid, Shape) EXPECT_EQ(expectedRadii, ellipsoid.Radii()); } +///////////////////////////////////////////////// +TEST(DOMEllipsoid, CalculateInertial) +{ + sdf::Ellipsoid ellipsoid; + + // density of aluminium + double density = 2170; + double a = 1.0; + double b = 10.0; + double c = 100.0; + + ellipsoid.SetRadii(gz::math::Vector3d(a, b, c)); + + double expectedMass = ellipsoid.Shape().Volume() * density; + double ixx = (expectedMass / 5.0) * (b*b + c*c); + double iyy = (expectedMass / 5.0) * (a*a + c*c); + double izz = (expectedMass / 5.0) * (a*a + b*b); + + gz::math::MassMatrix3d expectedMassMat( + expectedMass, + gz::math::Vector3d(ixx, iyy, izz), + gz::math::Vector3d::Zero + ); + + gz::math::Inertiald expectedInertial; + expectedInertial.SetMassMatrix(expectedMassMat); + expectedInertial.SetPose(gz::math::Pose3d::Zero); + + auto ellipsoidInertial = ellipsoid.CalculateInertial(density); + EXPECT_EQ(ellipsoid.Shape().Mat().Density(), density); + ASSERT_NE(std::nullopt, ellipsoidInertial); + EXPECT_EQ(expectedInertial, *ellipsoidInertial); + EXPECT_EQ(expectedInertial.MassMatrix().DiagonalMoments(), + ellipsoidInertial->MassMatrix().DiagonalMoments()); + EXPECT_EQ(expectedInertial.MassMatrix().Mass(), + ellipsoidInertial->MassMatrix().Mass()); + EXPECT_EQ(expectedInertial.Pose(), ellipsoidInertial->Pose()); +} + ///////////////////////////////////////////////// TEST(DOMEllipsoid, ToElement) { diff --git a/src/Sphere_TEST.cc b/src/Sphere_TEST.cc index d4796ee4d..18b4b8001 100644 --- a/src/Sphere_TEST.cc +++ b/src/Sphere_TEST.cc @@ -135,6 +135,41 @@ TEST(DOMSphere, Shape) EXPECT_DOUBLE_EQ(0.123, sphere.Radius()); } +///////////////////////////////////////////////// +TEST(DOMSphere, CalculateInertial) +{ + sdf::Sphere sphere; + + // density of aluminium + double density = 2170; + double r = 0.1; + + sphere.SetRadius(r); + + double expectedMass = sphere.Shape().Volume() * density; + double ixxIyyIzz = 0.4 * expectedMass * r * r; + + gz::math::MassMatrix3d expectedMassMat( + expectedMass, + gz::math::Vector3d(ixxIyyIzz, ixxIyyIzz, ixxIyyIzz), + gz::math::Vector3d::Zero + ); + + gz::math::Inertiald expectedInertial; + expectedInertial.SetMassMatrix(expectedMassMat); + expectedInertial.SetPose(gz::math::Pose3d::Zero); + + auto sphereInertial = sphere.CalculateInertial(density); + EXPECT_EQ(sphere.Shape().Material().Density(), density); + ASSERT_NE(std::nullopt, sphereInertial); + EXPECT_EQ(expectedInertial, *sphereInertial); + EXPECT_EQ(expectedInertial.MassMatrix().DiagonalMoments(), + sphereInertial->MassMatrix().DiagonalMoments()); + EXPECT_EQ(expectedInertial.MassMatrix().Mass(), + sphereInertial->MassMatrix().Mass()); + EXPECT_EQ(expectedInertial.Pose(), sphereInertial->Pose()); +} + ///////////////////////////////////////////////// TEST(DOMSphere, ToElement) { From 3aefdc3d965729a6d8bed0a44d5dd71e76720088 Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Tue, 22 Aug 2023 20:46:22 +0530 Subject: [PATCH 48/88] Added unit tests for CalculateInertial() in Geometry Signed-off-by: Jasmeet Singh --- src/Geometry_TEST.cc | 167 +++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 167 insertions(+) diff --git a/src/Geometry_TEST.cc b/src/Geometry_TEST.cc index 3155c9870..2abf9f6ca 100644 --- a/src/Geometry_TEST.cc +++ b/src/Geometry_TEST.cc @@ -298,6 +298,173 @@ TEST(DOMGeometry, Polyline) EXPECT_EQ(gz::math::Vector2d(8.7, 0.9), polylineShape[1].Points()[1]); } +///////////////////////////////////////////////// +TEST(DOMGeometry, CalculateInertial) +{ + sdf::Geometry geom; + + // Density of Aluminimum + double density = 2170.0; + double expectedMass; + gz::math::MassMatrix3d expectedMassMat; + gz::math::Inertiald expectedInertial; + + // Box + { + sdf::Box box; + double l = 2; + double w = 2; + double h = 2; + box.SetSize(gz::math::Vector3d(l, w, h)); + + expectedMass = box.Shape().Volume() * density; + double ixx = (1.0/12.0) * expectedMass * (w*w + h*h); + double iyy = (1.0/12.0) * expectedMass * (l*l + h*h); + double izz = (1.0/12.0) * expectedMass * (l*l + w*w); + + expectedMassMat.SetMass(expectedMass); + expectedMassMat.SetDiagonalMoments(gz::math::Vector3d(ixx, iyy, izz)); + expectedMassMat.SetOffDiagonalMoments(gz::math::Vector3d::Zero); + + expectedInertial.SetMassMatrix(expectedMassMat); + expectedInertial.SetPose(gz::math::Pose3d::Zero); + + geom.SetType(sdf::GeometryType::BOX); + geom.SetBoxShape(box); + auto boxInertial = geom.CalculateInertial(density); + + ASSERT_NE(std::nullopt, boxInertial); + EXPECT_EQ(expectedInertial, *boxInertial); + EXPECT_EQ(expectedInertial.MassMatrix(), expectedMassMat); + EXPECT_EQ(expectedInertial.Pose(), boxInertial->Pose()); + } + + // Capsule + { + sdf::Capsule capsule; + double l = 2.0; + double r = 0.1; + capsule.SetLength(l); + capsule.SetRadius(r); + + expectedMass = capsule.Shape().Volume() * density; + const double cylinderVolume = GZ_PI * r*r * l; + const double sphereVolume = GZ_PI * 4. / 3. * r*r*r; + const double volume = cylinderVolume + sphereVolume; + const double cylinderMass = expectedMass * cylinderVolume / volume; + const double sphereMass = expectedMass * sphereVolume / volume; + double ixxIyy = (1/12.0) * cylinderMass * (3*r*r + l*l) + + sphereMass * (0.4*r*r + 0.375*r*l + 0.25*l*l); + double izz = r*r * (0.5 * cylinderMass + 0.4 * sphereMass); + + expectedMassMat.SetMass(expectedMass); + expectedMassMat.SetDiagonalMoments(gz::math::Vector3d(ixxIyy, ixxIyy, izz)); + expectedMassMat.SetOffDiagonalMoments(gz::math::Vector3d::Zero); + + expectedInertial.SetMassMatrix(expectedMassMat); + expectedInertial.SetPose(gz::math::Pose3d::Zero); + + geom.SetType(sdf::GeometryType::CAPSULE); + geom.SetCapsuleShape(capsule); + auto capsuleInertial = geom.CalculateInertial(density); + + ASSERT_NE(std::nullopt, capsuleInertial); + EXPECT_EQ(expectedInertial, *capsuleInertial); + EXPECT_EQ(expectedInertial.MassMatrix(), expectedMassMat); + EXPECT_EQ(expectedInertial.Pose(), capsuleInertial->Pose()); + } + + // Cylinder + { + sdf::Cylinder cylinder; + double l = 2.0; + double r = 0.1; + + cylinder.SetLength(l); + cylinder.SetRadius(r); + + expectedMass = cylinder.Shape().Volume() * density; + double ixxIyy = (1/12.0) * expectedMass * (3*r*r + l*l); + double izz = 0.5 * expectedMass * r * r; + + expectedMassMat.SetMass(expectedMass); + expectedMassMat.SetDiagonalMoments(gz::math::Vector3d(ixxIyy, ixxIyy, izz)); + expectedMassMat.SetOffDiagonalMoments(gz::math::Vector3d::Zero); + + expectedInertial.SetMassMatrix(expectedMassMat); + expectedInertial.SetPose(gz::math::Pose3d::Zero); + + geom.SetType(sdf::GeometryType::CYLINDER); + geom.SetCylinderShape(cylinder); + auto cylinderInertial = geom.CalculateInertial(density); + + ASSERT_NE(std::nullopt, cylinderInertial); + EXPECT_EQ(expectedInertial, *cylinderInertial); + EXPECT_EQ(expectedInertial.MassMatrix(), expectedMassMat); + EXPECT_EQ(expectedInertial.Pose(), cylinderInertial->Pose()); + } + + // Ellipsoid + { + sdf::Ellipsoid ellipsoid; + + double a = 1.0; + double b = 10.0; + double c = 100.0; + + ellipsoid.SetRadii(gz::math::Vector3d(a, b, c)); + + expectedMass = ellipsoid.Shape().Volume() * density; + double ixx = (expectedMass / 5.0) * (b*b + c*c); + double iyy = (expectedMass / 5.0) * (a*a + c*c); + double izz = (expectedMass / 5.0) * (a*a + b*b); + + expectedMassMat.SetMass(expectedMass); + expectedMassMat.SetDiagonalMoments(gz::math::Vector3d(ixx, iyy, izz)); + expectedMassMat.SetOffDiagonalMoments(gz::math::Vector3d::Zero); + + expectedInertial.SetMassMatrix(expectedMassMat); + expectedInertial.SetPose(gz::math::Pose3d::Zero); + + geom.SetType(sdf::GeometryType::ELLIPSOID); + geom.SetEllipsoidShape(ellipsoid); + auto ellipsoidInertial = geom.CalculateInertial(density); + + ASSERT_NE(std::nullopt, ellipsoidInertial); + EXPECT_EQ(expectedInertial, *ellipsoidInertial); + EXPECT_EQ(expectedInertial.MassMatrix(), expectedMassMat); + EXPECT_EQ(expectedInertial.Pose(), ellipsoidInertial->Pose()); + } + + // Sphere + { + sdf::Sphere sphere; + double r = 0.1; + + sphere.SetRadius(r); + + expectedMass = sphere.Shape().Volume() * density; + double ixxIyyIzz = 0.4 * expectedMass * r * r; + + expectedMassMat.SetMass(expectedMass); + expectedMassMat.SetDiagonalMoments( + gz::math::Vector3d(ixxIyyIzz, ixxIyyIzz, ixxIyyIzz)); + expectedMassMat.SetOffDiagonalMoments(gz::math::Vector3d::Zero); + + expectedInertial.SetMassMatrix(expectedMassMat); + expectedInertial.SetPose(gz::math::Pose3d::Zero); + + geom.SetType(sdf::GeometryType::SPHERE); + geom.SetSphereShape(sphere); + auto sphereInertial = geom.CalculateInertial(density); + + ASSERT_NE(std::nullopt, sphereInertial); + EXPECT_EQ(expectedInertial, *sphereInertial); + EXPECT_EQ(expectedInertial.MassMatrix(), expectedMassMat); + EXPECT_EQ(expectedInertial.Pose(), sphereInertial->Pose()); + } +} + ///////////////////////////////////////////////// TEST(DOMGeometry, ToElement) { From 2836be90d9520fba8c9601b2b6a26a550eaf84bb Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Tue, 22 Aug 2023 21:04:20 +0530 Subject: [PATCH 49/88] Updated construction unit test of Collision with checks for density value Signed-off-by: Jasmeet Singh --- src/Collision_TEST.cc | 4 ++++ src/Geometry_TEST.cc | 4 ++-- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/src/Collision_TEST.cc b/src/Collision_TEST.cc index 0129b25db..920735e4b 100644 --- a/src/Collision_TEST.cc +++ b/src/Collision_TEST.cc @@ -27,6 +27,7 @@ TEST(DOMcollision, Construction) sdf::Collision collision; EXPECT_EQ(nullptr, collision.Element()); EXPECT_TRUE(collision.Name().empty()); + EXPECT_EQ(collision.Density(), 1000.0); collision.SetName("test_collison"); EXPECT_EQ(collision.Name(), "test_collison"); @@ -42,6 +43,9 @@ TEST(DOMcollision, Construction) EXPECT_FALSE(semanticPose.Resolve(pose).empty()); } + collision.SetDensity(1240.0); + EXPECT_DOUBLE_EQ(collision.Density(), 1240.0); + collision.SetRawPose({-10, -20, -30, GZ_PI, GZ_PI, GZ_PI}); EXPECT_EQ(gz::math::Pose3d(-10, -20, -30, GZ_PI, GZ_PI, GZ_PI), collision.RawPose()); diff --git a/src/Geometry_TEST.cc b/src/Geometry_TEST.cc index 2abf9f6ca..529199fd9 100644 --- a/src/Geometry_TEST.cc +++ b/src/Geometry_TEST.cc @@ -356,7 +356,7 @@ TEST(DOMGeometry, CalculateInertial) double ixxIyy = (1/12.0) * cylinderMass * (3*r*r + l*l) + sphereMass * (0.4*r*r + 0.375*r*l + 0.25*l*l); double izz = r*r * (0.5 * cylinderMass + 0.4 * sphereMass); - + expectedMassMat.SetMass(expectedMass); expectedMassMat.SetDiagonalMoments(gz::math::Vector3d(ixxIyy, ixxIyy, izz)); expectedMassMat.SetOffDiagonalMoments(gz::math::Vector3d::Zero); @@ -367,7 +367,7 @@ TEST(DOMGeometry, CalculateInertial) geom.SetType(sdf::GeometryType::CAPSULE); geom.SetCapsuleShape(capsule); auto capsuleInertial = geom.CalculateInertial(density); - + ASSERT_NE(std::nullopt, capsuleInertial); EXPECT_EQ(expectedInertial, *capsuleInertial); EXPECT_EQ(expectedInertial.MassMatrix(), expectedMassMat); From dbe543fdb1c5cc2bfc0b505febeeb84ed4c21052 Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Tue, 22 Aug 2023 23:55:19 +0530 Subject: [PATCH 50/88] Updated CalculateInertial() unit test to test for std::nullopt return in case of invalid inertial Signed-off-by: Jasmeet Singh --- src/Box_TEST.cc | 8 +++++++- src/Cylinder_TEST.cc | 8 ++++++++ src/Ellipsoid_TEST.cc | 7 +++++++ src/Sphere_TEST.cc | 5 +++++ 4 files changed, 27 insertions(+), 1 deletion(-) diff --git a/src/Box_TEST.cc b/src/Box_TEST.cc index 2b8f34c7b..40baebbd2 100644 --- a/src/Box_TEST.cc +++ b/src/Box_TEST.cc @@ -152,10 +152,16 @@ TEST(DOMBox, CalculateInertial) // density of aluminium double density = 2710; + + // Invalid dimensions leading to std::nullopt return + // CalculateInertials() + box.SetSize(gz::math::Vector3d(-1, 1, 0)); + auto invalidBoxInertial = box.CalculateInertial(density); + ASSERT_EQ(std::nullopt, invalidBoxInertial); + double l = 2; double w = 2; double h = 2; - box.SetSize(gz::math::Vector3d(l, w, h)); double expectedMass = box.Shape().Volume() * density; diff --git a/src/Cylinder_TEST.cc b/src/Cylinder_TEST.cc index 698c0a735..b27444cb4 100644 --- a/src/Cylinder_TEST.cc +++ b/src/Cylinder_TEST.cc @@ -184,6 +184,14 @@ TEST(DOMCylinder, CalculateInertial) // density of aluminium double density = 2170; + + // Invalid dimensions leading to std::nullopt return + // CalculateInertials() + cylinder.SetLength(-1); + cylinder.SetRadius(0); + auto invalidCylinderInertial = cylinder.CalculateInertial(density); + ASSERT_EQ(std::nullopt, invalidCylinderInertial); + double l = 2.0; double r = 0.1; diff --git a/src/Ellipsoid_TEST.cc b/src/Ellipsoid_TEST.cc index 1186fd7fe..d482cab93 100644 --- a/src/Ellipsoid_TEST.cc +++ b/src/Ellipsoid_TEST.cc @@ -148,6 +148,13 @@ TEST(DOMEllipsoid, CalculateInertial) // density of aluminium double density = 2170; + + // Invalid dimensions leading to std::nullopt return + // CalculateInertials() + ellipsoid.SetRadii(gz::math::Vector3d(-1, 2, 0)); + auto invalidEllipsoidInertial = ellipsoid.CalculateInertial(density); + ASSERT_EQ(std::nullopt, invalidEllipsoidInertial); + double a = 1.0; double b = 10.0; double c = 100.0; diff --git a/src/Sphere_TEST.cc b/src/Sphere_TEST.cc index 18b4b8001..4570c0aef 100644 --- a/src/Sphere_TEST.cc +++ b/src/Sphere_TEST.cc @@ -142,6 +142,11 @@ TEST(DOMSphere, CalculateInertial) // density of aluminium double density = 2170; + + sphere.SetRadius(-2); + auto invalidSphereInertial = sphere.CalculateInertial(density); + ASSERT_EQ(std::nullopt, invalidSphereInertial); + double r = 0.1; sphere.SetRadius(r); From 8d459c7c1ed64fd4c6db3f3fa35a8bf42fc193dc Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Wed, 23 Aug 2023 01:45:52 +0530 Subject: [PATCH 51/88] Added unit tests for CalculateInertial() in sdf::Collision Signed-off-by: Jasmeet Singh --- include/sdf/Collision.hh | 2 +- src/Collision_TEST.cc | 69 ++++++++++++++++++++++++++++++++++++++++ 2 files changed, 70 insertions(+), 1 deletion(-) diff --git a/include/sdf/Collision.hh b/include/sdf/Collision.hh index e713c3b34..6efa482cf 100644 --- a/include/sdf/Collision.hh +++ b/include/sdf/Collision.hh @@ -138,7 +138,7 @@ namespace sdf /// \return Errors, which is a vector of Error objects. Each Error includes /// an error code and message. An empty vector indicates no error public: Errors CalculateInertial(gz::math::Inertiald &_inertial, - const ParserConfig &_config); + const ParserConfig &_config); /// \brief Get a pointer to the SDF element that was used during /// load. diff --git a/src/Collision_TEST.cc b/src/Collision_TEST.cc index 920735e4b..883617e3f 100644 --- a/src/Collision_TEST.cc +++ b/src/Collision_TEST.cc @@ -18,6 +18,7 @@ #include #include "sdf/Collision.hh" #include "sdf/Geometry.hh" +#include "sdf/Box.hh" #include "sdf/Surface.hh" #include "test_utils.hh" @@ -174,6 +175,74 @@ TEST(DOMcollision, SetSurface) EXPECT_EQ(collision.Surface()->Contact()->CollideBitmask(), 0x2); } +///////////////////////////////////////////////// +TEST(DOMCollision, IncorrectBoxCollisionCalculateInertial) +{ + sdf::Collision collision; + EXPECT_DOUBLE_EQ(1000.0, collision.Density()); + + sdf::ElementPtr sdf(new sdf::Element()); + collision.Load(sdf); + + gz::math::Inertiald collisionInertial; + const sdf::ParserConfig sdfParserConfig; + sdf::Geometry geom; + sdf::Box box; + + // Invalid Inertial test + box.SetSize(gz::math::Vector3d(-1, 1, 0)); + geom.SetType(sdf::GeometryType::BOX); + geom.SetBoxShape(box); + collision.SetGeom(geom); + + sdf::Errors errors = collision.CalculateInertial(collisionInertial, sdfParserConfig); + ASSERT_FALSE(errors.empty()); +} + +///////////////////////////////////////////////// +TEST(DOMCollision, CorrectBoxCollisionCalculateInertial) +{ + sdf::Collision collision; + const sdf::ParserConfig sdfParserConfig; + sdf::Geometry geom; + sdf::Box box; + sdf::ElementPtr sdf(new sdf::Element()); + + gz::math::Inertiald collisionInertial; + + sdf->AddElement("density"); + sdf::ElementPtr densityElem = sdf->GetElement("density"); + collision.Load(sdf); + + double l = 2; + double w = 2; + double h = 2; + box.SetSize(gz::math::Vector3d(l, w, h)); + geom.SetType(sdf::GeometryType::BOX); + geom.SetBoxShape(box); + collision.SetGeom(geom); + + double expectedMass = box.Shape().Volume() * collision.Density(); + double ixx = (1.0/12.0) * expectedMass * (w*w + h*h); + double iyy = (1.0/12.0) * expectedMass * (l*l + h*h); + double izz = (1.0/12.0) * expectedMass * (l*l + w*w); + + gz::math::MassMatrix3d expectedMassMat( + expectedMass, + gz::math::Vector3d(ixx, iyy, izz), + gz::math::Vector3d::Zero + ); + + gz::math::Inertiald expectedInertial; + expectedInertial.SetMassMatrix(expectedMassMat); + expectedInertial.SetPose(gz::math::Pose3d::Zero); + + sdf::Errors error2 = collision.CalculateInertial(collisionInertial, sdfParserConfig); + ASSERT_TRUE(error2.empty()); + EXPECT_EQ(expectedInertial.MassMatrix(), collisionInertial.MassMatrix()); + EXPECT_EQ(expectedInertial.Pose(), collisionInertial.Pose()); +} + ///////////////////////////////////////////////// TEST(DOMCollision, ToElement) { From 464f7de0299bdc64f2e38d774a62ca8ef3347c07 Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Wed, 23 Aug 2023 01:51:12 +0530 Subject: [PATCH 52/88] Completed codecheck Signed-off-by: Jasmeet Singh --- src/Collision_TEST.cc | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/src/Collision_TEST.cc b/src/Collision_TEST.cc index 883617e3f..adf75331e 100644 --- a/src/Collision_TEST.cc +++ b/src/Collision_TEST.cc @@ -189,13 +189,14 @@ TEST(DOMCollision, IncorrectBoxCollisionCalculateInertial) sdf::Geometry geom; sdf::Box box; - // Invalid Inertial test + // Invalid Inertial test box.SetSize(gz::math::Vector3d(-1, 1, 0)); geom.SetType(sdf::GeometryType::BOX); geom.SetBoxShape(box); collision.SetGeom(geom); - sdf::Errors errors = collision.CalculateInertial(collisionInertial, sdfParserConfig); + sdf::Errors errors = + collision.CalculateInertial(collisionInertial, sdfParserConfig); ASSERT_FALSE(errors.empty()); } @@ -237,7 +238,8 @@ TEST(DOMCollision, CorrectBoxCollisionCalculateInertial) expectedInertial.SetMassMatrix(expectedMassMat); expectedInertial.SetPose(gz::math::Pose3d::Zero); - sdf::Errors error2 = collision.CalculateInertial(collisionInertial, sdfParserConfig); + sdf::Errors error2 = + collision.CalculateInertial(collisionInertial, sdfParserConfig); ASSERT_TRUE(error2.empty()); EXPECT_EQ(expectedInertial.MassMatrix(), collisionInertial.MassMatrix()); EXPECT_EQ(expectedInertial.Pose(), collisionInertial.Pose()); From 485dd84b77fa4fcd69fe871606848063e06f3b6e Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Thu, 24 Aug 2023 12:38:05 +0530 Subject: [PATCH 53/88] Added enum class for CalculateInertial() configuration Signed-off-by: Jasmeet Singh --- include/sdf/ParserConfig.hh | 25 +++++++++++++++++++++++++ src/ParserConfig.cc | 19 +++++++++++++++++++ 2 files changed, 44 insertions(+) diff --git a/include/sdf/ParserConfig.hh b/include/sdf/ParserConfig.hh index ad8195fe5..5cb7c7098 100644 --- a/include/sdf/ParserConfig.hh +++ b/include/sdf/ParserConfig.hh @@ -48,6 +48,20 @@ enum class EnforcementPolicy LOG, }; +/// \enum ConfigureCalculateInertial +/// \brief Configuration options of how CalculateInertial() function +/// would be used +enum class ConfigureCalculateInertial +{ + /// \brief If this value is used, CalculateInertial() won't be + /// called from inside the Root::Load() function + SKIP_CALCULATION_IN_LOAD, + + /// \brief If this values is used, CalculateInertial() would be + /// called and the computed inertial values would be saved + CALCULATE_AND_SAVE +}; + // Forward declare private data class. class ParserConfigPrivate; @@ -160,6 +174,17 @@ class SDFORMAT_VISIBLE ParserConfig /// \return The deperacted elements policy enum value public: EnforcementPolicy DeprecatedElementsPolicy() const; + /// \brief Get the current configuration for the CalculateInertial() + /// function + /// \return Current set value of the ConfigureCalculateInertial enum + public: ConfigureCalculateInertial CalculateInertialConfiguration() const; + + /// \brief Set the configuration for the CalculateInertial() function + /// \param[in] _configuration The configuration to set for the CalculateInertial() + /// function + public: void SetCalculateInertialConfiguration( + ConfigureCalculateInertial _configuration); + /// \brief Registers a custom model parser. /// \param[in] _modelParser Callback as described in /// sdf/InterfaceElements.hh. diff --git a/src/ParserConfig.cc b/src/ParserConfig.cc index d6282383a..57b3c5e7f 100644 --- a/src/ParserConfig.cc +++ b/src/ParserConfig.cc @@ -43,6 +43,12 @@ class sdf::ParserConfig::Implementation /// to behave behave differently than the `warningsPolicy`. public: std::optional deprecatedElementsPolicy; + /// \brief Configuration that is set for the CalculateInertial() function + /// By default it is set to SKIP_CALCULATION_IN_LOAD which would cause + /// Root::Load() to not call CalculateInertial() + public: ConfigureCalculateInertial calculateInertialConfiguration = + ConfigureCalculateInertial::SKIP_CALCULATION_IN_LOAD; + /// \brief Collection of custom model parsers. public: std::vector customParsers; @@ -157,6 +163,19 @@ EnforcementPolicy ParserConfig::DeprecatedElementsPolicy() const this->dataPtr->warningsPolicy); } +///////////////////////////////////////////////// +ConfigureCalculateInertial ParserConfig::CalculateInertialConfiguration() const +{ + return this->dataPtr->calculateInertialConfiguration; +} + +///////////////////////////////////////////////// +void ParserConfig::SetCalculateInertialConfiguration( + ConfigureCalculateInertial _configuration) +{ + this->dataPtr->calculateInertialConfiguration = _configuration; +} + ///////////////////////////////////////////////// void ParserConfig::RegisterCustomModelParser(CustomModelParser _modelParser) { From ca78026ffbb8d8b1b4cb99ce7c595908573112b1 Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Thu, 24 Aug 2023 12:44:42 +0530 Subject: [PATCH 54/88] Added check for CalculateInertialConfiguration in Root::Load() Signed-off-by: Jasmeet Singh --- src/Root.cc | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/Root.cc b/src/Root.cc index baa1ffa55..ccbe18eea 100644 --- a/src/Root.cc +++ b/src/Root.cc @@ -389,6 +389,15 @@ Errors Root::Load(SDFPtr _sdf, const ParserConfig &_config) // Check that //axis*/xyz/@expressed_in values specify valid frames. checkJointAxisExpressedInValues(this, errors); + // Check if CalculateInertialConfiguration() is not set to skip in load + if (_config.CalculateInertialConfiguration() != + ConfigureCalculateInertial::SKIP_CALCULATION_IN_LOAD) + { + Errors inertialErrors = this->CalculateInertials(_config); + errors.insert(errors.end(), + inertialErrors.begin(), inertialErrors.end()); + } + return errors; } From 002c643af84895126ff86b876dca8c1015207854 Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Thu, 24 Aug 2023 13:54:22 +0530 Subject: [PATCH 55/88] Renamed CALCULATE_AND_SAVE value to SAVE_CALCULATION in COnfigureCalculateInertial enum Signed-off-by: Jasmeet Singh --- include/sdf/ParserConfig.hh | 2 +- src/Root.cc | 5 ++--- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/include/sdf/ParserConfig.hh b/include/sdf/ParserConfig.hh index 5cb7c7098..583a1738a 100644 --- a/include/sdf/ParserConfig.hh +++ b/include/sdf/ParserConfig.hh @@ -59,7 +59,7 @@ enum class ConfigureCalculateInertial /// \brief If this values is used, CalculateInertial() would be /// called and the computed inertial values would be saved - CALCULATE_AND_SAVE + SAVE_CALCULATION }; // Forward declare private data class. diff --git a/src/Root.cc b/src/Root.cc index ccbe18eea..e67844184 100644 --- a/src/Root.cc +++ b/src/Root.cc @@ -393,9 +393,8 @@ Errors Root::Load(SDFPtr _sdf, const ParserConfig &_config) if (_config.CalculateInertialConfiguration() != ConfigureCalculateInertial::SKIP_CALCULATION_IN_LOAD) { - Errors inertialErrors = this->CalculateInertials(_config); - errors.insert(errors.end(), - inertialErrors.begin(), inertialErrors.end()); + Errors inertialErr = this->CalculateInertials(_config); + errors.insert(errors.end(), inertialErr.begin(), inertialErr.end()); } return errors; From 1688522ae42c54b7c9bf81c22d9e263117699271 Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Thu, 24 Aug 2023 14:35:43 +0530 Subject: [PATCH 56/88] Added boolean autoInertiaSaved member to link Signed-off-by: Jasmeet Singh --- include/sdf/Link.hh | 13 +++++++++++++ src/Link.cc | 14 ++++++++++++++ 2 files changed, 27 insertions(+) diff --git a/include/sdf/Link.hh b/include/sdf/Link.hh index a34a10914..b62b40fec 100644 --- a/include/sdf/Link.hh +++ b/include/sdf/Link.hh @@ -390,6 +390,19 @@ namespace sdf /// \sa Model::SetEnableWind(bool) public: void SetEnableWind(bool _enableWind); + /// \brief Check if the inertial values for this link were saved. + /// If true, the inertial values for this link wont be calculated + /// when CalculateInertial() is called. This value is set to true + /// when CalculateInertial() is called with SAVE_CALCULATION + /// configuration. + /// \return True if CalculateInertial() was called with SAVE_CALCULATION + /// configuration, false otherwise. + public: bool AutoInertiaSaved() const; + + /// \brief Set the autoInertiaSaved() values + /// \param _autoInertiaSaved True or False + public: void SetAutoInertiaSaved(bool _autoInertiaSaved); + /// \brief Add a collision to the link. /// \param[in] _collision Collision to add. /// \return True if successful, false if a collision with the name already diff --git a/src/Link.cc b/src/Link.cc index 15a49a807..81f8f48d3 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -78,6 +78,8 @@ class sdf::Link::Implementation /// \brief True if this link should be subject to wind, false otherwise. public: bool enableWind = false; + public: bool autoInertiaSaved = false; + /// \brief Scoped Pose Relative-To graph at the parent model scope. public: sdf::ScopedGraph poseRelativeToGraph; }; @@ -783,6 +785,18 @@ void Link::SetEnableWind(const bool _enableWind) this->dataPtr->enableWind = _enableWind; } +///////////////////////////////////////////////// +bool Link::AutoInertiaSaved() const +{ + return this->dataPtr->autoInertiaSaved; +} + +///////////////////////////////////////////////// +void Link::SetAutoInertiaSaved(bool _autoInertiaSaved) +{ + this->dataPtr->autoInertiaSaved = _autoInertiaSaved; +} + ////////////////////////////////////////////////// bool Link::AddCollision(const Collision &_collision) { From 230bde2b645e2f86239359d9e7a877f1b251a83c Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Thu, 24 Aug 2023 14:37:04 +0530 Subject: [PATCH 57/88] Added check for autoInertiaSaved & SAVE_CALCULATION configuration in CalculateInertial() function Signed-off-by: Jasmeet Singh --- src/Link.cc | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/src/Link.cc b/src/Link.cc index 81f8f48d3..de6ef65ba 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -595,7 +595,7 @@ void Link::CalculateInertials(sdf::Errors &_errors, const ParserConfig &_config) { sdf::ElementPtr inertialElem = this->dataPtr->sdf->GetElement("inertial"); - if (inertialElem->Get("auto")) + if (inertialElem->Get("auto") && !this->dataPtr->autoInertiaSaved) { // Return an error if auto is set to true but there are no // collision elements in the link @@ -606,7 +606,7 @@ void Link::CalculateInertials(sdf::Errors &_errors, const ParserConfig &_config) " elements for the link."}); return; } - + gz::math::Inertiald totalInertia; for (sdf::Collision &collision : this->dataPtr->collisions) @@ -621,6 +621,14 @@ void Link::CalculateInertials(sdf::Errors &_errors, const ParserConfig &_config) } this->dataPtr->inertial = totalInertia; + + // If CalculateInertial() was called with SAVE_CALCULATION + // configuration then set autoInertiaSaved to true + if (_config.CalculateInertialConfiguration() == + ConfigureCalculateInertial::SAVE_CALCULATION) + { + this->dataPtr->autoInertiaSaved = true; + } } // If auto is false, this means inertial values were set // from user given values in Link::Load(), therefore we can return From 0df7eaeebaba6ab7b94b89f43888d97f8978f484 Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Thu, 24 Aug 2023 14:42:35 +0530 Subject: [PATCH 58/88] Completed codecheck Signed-off-by: Jasmeet Singh --- include/sdf/Link.hh | 4 ++-- include/sdf/ParserConfig.hh | 12 ++++++------ src/Link.cc | 2 +- 3 files changed, 9 insertions(+), 9 deletions(-) diff --git a/include/sdf/Link.hh b/include/sdf/Link.hh index b62b40fec..4ccb5606a 100644 --- a/include/sdf/Link.hh +++ b/include/sdf/Link.hh @@ -393,10 +393,10 @@ namespace sdf /// \brief Check if the inertial values for this link were saved. /// If true, the inertial values for this link wont be calculated /// when CalculateInertial() is called. This value is set to true - /// when CalculateInertial() is called with SAVE_CALCULATION + /// when CalculateInertial() is called with SAVE_CALCULATION /// configuration. /// \return True if CalculateInertial() was called with SAVE_CALCULATION - /// configuration, false otherwise. + /// configuration, false otherwise. public: bool AutoInertiaSaved() const; /// \brief Set the autoInertiaSaved() values diff --git a/include/sdf/ParserConfig.hh b/include/sdf/ParserConfig.hh index 583a1738a..810541736 100644 --- a/include/sdf/ParserConfig.hh +++ b/include/sdf/ParserConfig.hh @@ -53,12 +53,12 @@ enum class EnforcementPolicy /// would be used enum class ConfigureCalculateInertial { - /// \brief If this value is used, CalculateInertial() won't be + /// \brief If this value is used, CalculateInertial() won't be /// called from inside the Root::Load() function SKIP_CALCULATION_IN_LOAD, - /// \brief If this values is used, CalculateInertial() would be - /// called and the computed inertial values would be saved + /// \brief If this values is used, CalculateInertial() would be + /// called and the computed inertial values would be saved SAVE_CALCULATION }; @@ -174,14 +174,14 @@ class SDFORMAT_VISIBLE ParserConfig /// \return The deperacted elements policy enum value public: EnforcementPolicy DeprecatedElementsPolicy() const; - /// \brief Get the current configuration for the CalculateInertial() + /// \brief Get the current configuration for the CalculateInertial() /// function /// \return Current set value of the ConfigureCalculateInertial enum public: ConfigureCalculateInertial CalculateInertialConfiguration() const; /// \brief Set the configuration for the CalculateInertial() function - /// \param[in] _configuration The configuration to set for the CalculateInertial() - /// function + /// \param[in] _configuration The configuration to set for the + /// CalculateInertial() function public: void SetCalculateInertialConfiguration( ConfigureCalculateInertial _configuration); diff --git a/src/Link.cc b/src/Link.cc index de6ef65ba..e3021314a 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -606,7 +606,7 @@ void Link::CalculateInertials(sdf::Errors &_errors, const ParserConfig &_config) " elements for the link."}); return; } - + gz::math::Inertiald totalInertia; for (sdf::Collision &collision : this->dataPtr->collisions) From dd06f0893db2d5cbfc333c25b5e663235bfbb7b7 Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Thu, 24 Aug 2023 15:33:32 +0530 Subject: [PATCH 59/88] Added //inertial/density to the spec Signed-off-by: Jasmeet Singh --- sdf/1.11/inertial.sdf | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/sdf/1.11/inertial.sdf b/sdf/1.11/inertial.sdf index 9e1d4a739..dd7517ac7 100644 --- a/sdf/1.11/inertial.sdf +++ b/sdf/1.11/inertial.sdf @@ -16,6 +16,14 @@ The mass of the link. + + + Mass Density of the collision geometry. + This is used to determine mass and inertia values during automatic calculation. + Default is the density of water 1000 kg/m^3. + + + This pose (translation, rotation) describes the position and orientation From 325854cb188bef904659c6bfc6b5bea1338afd4f Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Thu, 24 Aug 2023 16:44:41 +0530 Subject: [PATCH 60/88] Updated Geometry::CalculateInertial() to accept density & parser config as parameter Signed-off-by: Jasmeet Singh --- include/sdf/Geometry.hh | 6 +++++- src/Collision.cc | 2 +- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/include/sdf/Geometry.hh b/include/sdf/Geometry.hh index 05aec3244..a94a8ebb8 100644 --- a/include/sdf/Geometry.hh +++ b/include/sdf/Geometry.hh @@ -213,9 +213,13 @@ namespace sdf /// \brief Calculate and return the Mass Matrix values for the Geometry /// \param[in] _density The density of the geometry element. + /// \param[in] _config Parser Config + /// \param[out] _errors A vector of Errors object. Each object + /// would contain an error code and an error message. /// \return A std::optional with gz::math::Inertiald object or std::nullopt public: std::optional - CalculateInertial(double _density); + CalculateInertial(double _density, const sdf::ParserConfig &_config, + sdf::Errors &_errors); /// \brief Get a pointer to the SDF element that was used during /// load. diff --git a/src/Collision.cc b/src/Collision.cc index 0b1787cf8..903ea2cbc 100644 --- a/src/Collision.cc +++ b/src/Collision.cc @@ -242,7 +242,7 @@ Errors Collision::CalculateInertial(gz::math::Inertiald &_inertial, } auto geomInertial = - this->dataPtr->geom.CalculateInertial(this->dataPtr->density); + this->dataPtr->geom.CalculateInertial(this->dataPtr->density, _config, errors); if (!geomInertial) { From fc01633adc27164b778e814b1e786bc86ca33473 Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Thu, 24 Aug 2023 16:45:19 +0530 Subject: [PATCH 61/88] Updated Geometry::CalculateInertial() to give warning when a not supported geometry type is used Signed-off-by: Jasmeet Singh --- src/Geometry.cc | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/src/Geometry.cc b/src/Geometry.cc index 341c614df..896c0ed8c 100644 --- a/src/Geometry.cc +++ b/src/Geometry.cc @@ -310,7 +310,7 @@ void Geometry::SetPolylineShape(const std::vector &_polylines) } std::optional Geometry::CalculateInertial( - double _density) + double _density, const sdf::ParserConfig &_config, sdf::Errors &_errors) { std::optional geomInertial; @@ -332,6 +332,15 @@ std::optional Geometry::CalculateInertial( geomInertial = this->dataPtr->sphere->CalculateInertial(_density); break; default: + Error invalidGeomTypeErr( + ErrorCode::WARNING, + "Automatic inertia calculations are not supported for the given" + " Geometry type. " + ); + enforceConfigurablePolicyCondition( + _config.WarningsPolicy(), invalidGeomTypeErr, _errors + ); + geomInertial = std::nullopt; break; } From d1e74842cf9480789368d2db11938c6a7873358d Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Thu, 24 Aug 2023 16:45:48 +0530 Subject: [PATCH 62/88] Updated Geometry unit test for not supported geom type Signed-off-by: Jasmeet Singh --- src/Geometry_TEST.cc | 25 ++++++++++++++++++++----- 1 file changed, 20 insertions(+), 5 deletions(-) diff --git a/src/Geometry_TEST.cc b/src/Geometry_TEST.cc index 529199fd9..98bccefd9 100644 --- a/src/Geometry_TEST.cc +++ b/src/Geometry_TEST.cc @@ -308,6 +308,16 @@ TEST(DOMGeometry, CalculateInertial) double expectedMass; gz::math::MassMatrix3d expectedMassMat; gz::math::Inertiald expectedInertial; + const sdf::ParserConfig sdfParserConfig; + sdf::Errors errors; + + // Not supported geom type + { + geom.SetType(sdf::GeometryType::EMPTY); + auto notSupportedInertial = geom.CalculateInertial(density, + sdfParserConfig, errors); + ASSERT_EQ(notSupportedInertial, std::nullopt); + } // Box { @@ -331,7 +341,8 @@ TEST(DOMGeometry, CalculateInertial) geom.SetType(sdf::GeometryType::BOX); geom.SetBoxShape(box); - auto boxInertial = geom.CalculateInertial(density); + auto boxInertial = geom.CalculateInertial(density, + sdfParserConfig, errors); ASSERT_NE(std::nullopt, boxInertial); EXPECT_EQ(expectedInertial, *boxInertial); @@ -366,7 +377,8 @@ TEST(DOMGeometry, CalculateInertial) geom.SetType(sdf::GeometryType::CAPSULE); geom.SetCapsuleShape(capsule); - auto capsuleInertial = geom.CalculateInertial(density); + auto capsuleInertial = geom.CalculateInertial(density, + sdfParserConfig, errors); ASSERT_NE(std::nullopt, capsuleInertial); EXPECT_EQ(expectedInertial, *capsuleInertial); @@ -396,7 +408,8 @@ TEST(DOMGeometry, CalculateInertial) geom.SetType(sdf::GeometryType::CYLINDER); geom.SetCylinderShape(cylinder); - auto cylinderInertial = geom.CalculateInertial(density); + auto cylinderInertial = geom.CalculateInertial(density, + sdfParserConfig, errors); ASSERT_NE(std::nullopt, cylinderInertial); EXPECT_EQ(expectedInertial, *cylinderInertial); @@ -428,7 +441,8 @@ TEST(DOMGeometry, CalculateInertial) geom.SetType(sdf::GeometryType::ELLIPSOID); geom.SetEllipsoidShape(ellipsoid); - auto ellipsoidInertial = geom.CalculateInertial(density); + auto ellipsoidInertial = geom.CalculateInertial(density, + sdfParserConfig, errors); ASSERT_NE(std::nullopt, ellipsoidInertial); EXPECT_EQ(expectedInertial, *ellipsoidInertial); @@ -456,7 +470,8 @@ TEST(DOMGeometry, CalculateInertial) geom.SetType(sdf::GeometryType::SPHERE); geom.SetSphereShape(sphere); - auto sphereInertial = geom.CalculateInertial(density); + auto sphereInertial = geom.CalculateInertial(density, + sdfParserConfig, errors); ASSERT_NE(std::nullopt, sphereInertial); EXPECT_EQ(expectedInertial, *sphereInertial); From 88d5eebe8af974f6be2427b261b883dfd2ec8f9d Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Thu, 24 Aug 2023 20:30:32 +0530 Subject: [PATCH 63/88] Updated Collision::ToElement() to set density Signed-off-by: Jasmeet Singh --- src/Collision.cc | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/Collision.cc b/src/Collision.cc index 903ea2cbc..c45132827 100644 --- a/src/Collision.cc +++ b/src/Collision.cc @@ -313,6 +313,10 @@ sdf::ElementPtr Collision::ToElement(sdf::Errors &_errors) const } poseElem->Set(_errors, this->RawPose()); + // Set the density + sdf::ElementPtr densityElem = elem->GetElement("density", _errors); + densityElem->Set(this->Density()); + // Set the geometry elem->InsertElement(this->dataPtr->geom.ToElement(_errors), true); From 0ec1aac6fadaad1a4d4844fb4b9be0ece8612828 Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Thu, 24 Aug 2023 20:31:34 +0530 Subject: [PATCH 64/88] Added unit tests for collision pose relative to some other frame test case Signed-off-by: Jasmeet Singh --- src/Collision_TEST.cc | 112 +++++++++++++++++++++++++++++++++++------- 1 file changed, 94 insertions(+), 18 deletions(-) diff --git a/src/Collision_TEST.cc b/src/Collision_TEST.cc index adf75331e..cb377dfe8 100644 --- a/src/Collision_TEST.cc +++ b/src/Collision_TEST.cc @@ -19,6 +19,8 @@ #include "sdf/Collision.hh" #include "sdf/Geometry.hh" #include "sdf/Box.hh" +#include "sdf/Model.hh" +#include "sdf/Link.hh" #include "sdf/Surface.hh" #include "test_utils.hh" @@ -203,27 +205,40 @@ TEST(DOMCollision, IncorrectBoxCollisionCalculateInertial) ///////////////////////////////////////////////// TEST(DOMCollision, CorrectBoxCollisionCalculateInertial) { - sdf::Collision collision; + std::string sdf = "" + " " + " " + " " + " " + " " + " 1240.0" + " " + " " + " 2 2 2" + " " + " " + " " + " " + " " + " "; + + sdf::Root root; const sdf::ParserConfig sdfParserConfig; - sdf::Geometry geom; - sdf::Box box; - sdf::ElementPtr sdf(new sdf::Element()); + sdf::Errors errors = root.LoadSdfString(sdf, sdfParserConfig); + EXPECT_TRUE(errors.empty()); + EXPECT_NE(nullptr, root.Element()); - gz::math::Inertiald collisionInertial; + const sdf::Model *model = root.Model(); + const sdf::Link *link = model->LinkByIndex(0); + const sdf::Collision *collision = link->CollisionByIndex(0); - sdf->AddElement("density"); - sdf::ElementPtr densityElem = sdf->GetElement("density"); - collision.Load(sdf); + sdf::Errors inertialErr = root.CalculateInertials(sdfParserConfig); double l = 2; double w = 2; double h = 2; - box.SetSize(gz::math::Vector3d(l, w, h)); - geom.SetType(sdf::GeometryType::BOX); - geom.SetBoxShape(box); - collision.SetGeom(geom); - double expectedMass = box.Shape().Volume() * collision.Density(); + double expectedMass = l*w*h * collision->Density(); double ixx = (1.0/12.0) * expectedMass * (w*w + h*h); double iyy = (1.0/12.0) * expectedMass * (l*l + h*h); double izz = (1.0/12.0) * expectedMass * (l*l + w*w); @@ -237,12 +252,73 @@ TEST(DOMCollision, CorrectBoxCollisionCalculateInertial) gz::math::Inertiald expectedInertial; expectedInertial.SetMassMatrix(expectedMassMat); expectedInertial.SetPose(gz::math::Pose3d::Zero); + + ASSERT_TRUE(inertialErr.empty()); + EXPECT_DOUBLE_EQ(1240.0, collision->Density()); + EXPECT_DOUBLE_EQ(expectedMass, link->Inertial().MassMatrix().Mass()); + EXPECT_EQ(expectedInertial.MassMatrix(), link->Inertial().MassMatrix()); + EXPECT_EQ(expectedInertial.Pose(), link->Inertial().Pose()); +} - sdf::Errors error2 = - collision.CalculateInertial(collisionInertial, sdfParserConfig); - ASSERT_TRUE(error2.empty()); - EXPECT_EQ(expectedInertial.MassMatrix(), collisionInertial.MassMatrix()); - EXPECT_EQ(expectedInertial.Pose(), collisionInertial.Pose()); +TEST(DOMCollision, CalculateInertialPoseNotRelativeToLink) +{ + std::string sdf = "" + " " + " " + " " + " 0 0 1 0 0 0" + " " + " " + " " + " " + " 0 0 -1 0 0 0" + " 1240.0" + " " + " " + " 2 2 2" + " " + " " + " " + " " + " " + " "; + + sdf::Root root; + const sdf::ParserConfig sdfParserConfig; + sdf::Errors errors = root.LoadSdfString(sdf, sdfParserConfig); + EXPECT_TRUE(errors.empty()); + EXPECT_NE(nullptr, root.Element()); + + const sdf::Model *model = root.Model(); + const sdf::Link *link = model->LinkByIndex(0); + const sdf::Collision *collision = link->CollisionByIndex(0); + + sdf::Errors inertialErr = root.CalculateInertials(sdfParserConfig); + + double l = 2; + double w = 2; + double h = 2; + + double expectedMass = l*w*h * collision->Density(); + double ixx = (1.0/12.0) * expectedMass * (w*w + h*h); + double iyy = (1.0/12.0) * expectedMass * (l*l + h*h); + double izz = (1.0/12.0) * expectedMass * (l*l + w*w); + + gz::math::MassMatrix3d expectedMassMat( + expectedMass, + gz::math::Vector3d(ixx, iyy, izz), + gz::math::Vector3d::Zero + ); + + gz::math::Inertiald expectedInertial; + expectedInertial.SetMassMatrix(expectedMassMat); + expectedInertial.SetPose(gz::math::Pose3d::Zero); + + ASSERT_TRUE(inertialErr.empty()); + EXPECT_DOUBLE_EQ(1240.0, collision->Density()); + EXPECT_DOUBLE_EQ(expectedMass, link->Inertial().MassMatrix().Mass()); + EXPECT_EQ(expectedInertial.MassMatrix(), link->Inertial().MassMatrix()); + EXPECT_EQ(expectedInertial.Pose(), link->Inertial().Pose()); } ///////////////////////////////////////////////// From dc30cf98239a7de2bb1cb32789dba215a4dc91d9 Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Thu, 24 Aug 2023 20:47:18 +0530 Subject: [PATCH 65/88] Completed codecheck Signed-off-by: Jasmeet Singh --- include/sdf/Geometry.hh | 7 ++++--- src/Collision.cc | 3 ++- src/Collision_TEST.cc | 4 ++-- 3 files changed, 8 insertions(+), 6 deletions(-) diff --git a/include/sdf/Geometry.hh b/include/sdf/Geometry.hh index a94a8ebb8..81cb64405 100644 --- a/include/sdf/Geometry.hh +++ b/include/sdf/Geometry.hh @@ -217,9 +217,10 @@ namespace sdf /// \param[out] _errors A vector of Errors object. Each object /// would contain an error code and an error message. /// \return A std::optional with gz::math::Inertiald object or std::nullopt - public: std::optional - CalculateInertial(double _density, const sdf::ParserConfig &_config, - sdf::Errors &_errors); + public: std::optional CalculateInertial( + double _density, + const sdf::ParserConfig &_config, + sdf::Errors &_errors); /// \brief Get a pointer to the SDF element that was used during /// load. diff --git a/src/Collision.cc b/src/Collision.cc index c45132827..924ac0ce4 100644 --- a/src/Collision.cc +++ b/src/Collision.cc @@ -242,7 +242,8 @@ Errors Collision::CalculateInertial(gz::math::Inertiald &_inertial, } auto geomInertial = - this->dataPtr->geom.CalculateInertial(this->dataPtr->density, _config, errors); + this->dataPtr->geom.CalculateInertial(this->dataPtr->density, + _config, errors); if (!geomInertial) { diff --git a/src/Collision_TEST.cc b/src/Collision_TEST.cc index cb377dfe8..4226626fc 100644 --- a/src/Collision_TEST.cc +++ b/src/Collision_TEST.cc @@ -252,7 +252,7 @@ TEST(DOMCollision, CorrectBoxCollisionCalculateInertial) gz::math::Inertiald expectedInertial; expectedInertial.SetMassMatrix(expectedMassMat); expectedInertial.SetPose(gz::math::Pose3d::Zero); - + ASSERT_TRUE(inertialErr.empty()); EXPECT_DOUBLE_EQ(1240.0, collision->Density()); EXPECT_DOUBLE_EQ(expectedMass, link->Inertial().MassMatrix().Mass()); @@ -313,7 +313,7 @@ TEST(DOMCollision, CalculateInertialPoseNotRelativeToLink) gz::math::Inertiald expectedInertial; expectedInertial.SetMassMatrix(expectedMassMat); expectedInertial.SetPose(gz::math::Pose3d::Zero); - + ASSERT_TRUE(inertialErr.empty()); EXPECT_DOUBLE_EQ(1240.0, collision->Density()); EXPECT_DOUBLE_EQ(expectedMass, link->Inertial().MassMatrix().Mass()); From 40811cf90d7b4b537767ca0e8755809f667bc71b Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Fri, 25 Aug 2023 01:19:01 +0530 Subject: [PATCH 66/88] Added unit tests for ParserConfig Signed-off-by: Jasmeet Singh --- src/Collision_TEST.cc | 1 + src/ParserConfig_TEST.cc | 2 ++ 2 files changed, 3 insertions(+) diff --git a/src/Collision_TEST.cc b/src/Collision_TEST.cc index 4226626fc..e9336b165 100644 --- a/src/Collision_TEST.cc +++ b/src/Collision_TEST.cc @@ -260,6 +260,7 @@ TEST(DOMCollision, CorrectBoxCollisionCalculateInertial) EXPECT_EQ(expectedInertial.Pose(), link->Inertial().Pose()); } +///////////////////////////////////////////////// TEST(DOMCollision, CalculateInertialPoseNotRelativeToLink) { std::string sdf = "" diff --git a/src/ParserConfig_TEST.cc b/src/ParserConfig_TEST.cc index 6d139817c..4450e95eb 100644 --- a/src/ParserConfig_TEST.cc +++ b/src/ParserConfig_TEST.cc @@ -56,6 +56,8 @@ TEST(ParserConfig, Construction) EXPECT_EQ(sdf::EnforcementPolicy::WARN, config.WarningsPolicy()); EXPECT_EQ(sdf::EnforcementPolicy::WARN, config.UnrecognizedElementsPolicy()); EXPECT_EQ(sdf::EnforcementPolicy::WARN, config.DeprecatedElementsPolicy()); + EXPECT_EQ(sdf::ConfigureCalculateInertial::SKIP_CALCULATION_IN_LOAD, + config.CalculateInertialConfiguration()); EXPECT_FALSE(config.URDFPreserveFixedJoint()); EXPECT_FALSE(config.StoreResolvedURIs()); } From 01c49c474137a7eb7e82835e73d3dd2390a6fffc Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Fri, 25 Aug 2023 01:19:16 +0530 Subject: [PATCH 67/88] Added unit tests for Link Signed-off-by: Jasmeet Singh --- src/Link_TEST.cc | 131 +++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 131 insertions(+) diff --git a/src/Link_TEST.cc b/src/Link_TEST.cc index bf53c118a..28d58fa2b 100644 --- a/src/Link_TEST.cc +++ b/src/Link_TEST.cc @@ -22,6 +22,8 @@ #include "sdf/Collision.hh" #include "sdf/Light.hh" #include "sdf/Link.hh" +#include "sdf/Root.hh" +#include "sdf/Model.hh" #include "sdf/ParticleEmitter.hh" #include "sdf/Projector.hh" #include "sdf/Sensor.hh" @@ -68,6 +70,10 @@ TEST(DOMLink, Construction) link.SetEnableWind(true); EXPECT_TRUE(link.EnableWind()); + EXPECT_FALSE(link.AutoInertiaSaved()); + link.SetAutoInertiaSaved(true); + EXPECT_TRUE(link.AutoInertiaSaved()); + EXPECT_EQ(0u, link.SensorCount()); EXPECT_EQ(nullptr, link.SensorByIndex(0)); EXPECT_EQ(nullptr, link.SensorByIndex(1)); @@ -266,6 +272,131 @@ TEST(DOMLink, InvalidInertia) EXPECT_FALSE(link.Inertial().MassMatrix().IsValid()); } +///////////////////////////////////////////////// +TEST(DOMLink, CalculateInertialWithNoCollisionsInLink) +{ + std::string sdf = "" + " " + " " + " " + " " + " " + " " + " "; + + sdf::Root root; + const sdf::ParserConfig sdfParserConfig; + sdf::Errors errors = root.LoadSdfString(sdf, sdfParserConfig); + EXPECT_TRUE(errors.empty()); + EXPECT_NE(nullptr, root.Element()); + + const sdf::Model *model = root.Model(); + const sdf::Link *link = model->LinkByIndex(0); + sdf::Errors inertialErr = root.CalculateInertials(sdfParserConfig); + ASSERT_FALSE(inertialErr.empty()); + + // Default Inertial values set during load + EXPECT_EQ(1.0, link->Inertial().MassMatrix().Mass()); + EXPECT_EQ(gz::math::Vector3d::One, + link->Inertial().MassMatrix().DiagonalMoments()); +} + +///////////////////////////////////////////////// +TEST(DOMLink, CalculateInertialWithMultipleCollisions) +{ + std::string sdf = "" + "" + " " + " 0 0 1.0 0 0 0" + " " + " " + " " + " 0 0 -0.5 0 0 0" + " 2.0" + " " + " " + " 1 1 1" + " " + " " + " " + " " + " 0 0 0.5 0 0 0" + " 4" + " " + " " + " 0.5" + " 1.0" + " " + " " + " " + " " + " " + " "; + + sdf::Root root; + const sdf::ParserConfig sdfParserConfig; + sdf::Errors errors = root.LoadSdfString(sdf, sdfParserConfig); + EXPECT_TRUE(errors.empty()); + EXPECT_NE(nullptr, root.Element()); + + const sdf::Model *model = root.Model(); + const sdf::Link *link = model->LinkByIndex(0); + sdf::Errors inertialErr = root.CalculateInertials(sdfParserConfig); + EXPECT_TRUE(inertialErr.empty()); + + // Mass of cube(volume * density) + mass of cylinder(volume * density) + double expectedMass = 1.0 * 2.0 + GZ_PI * 0.5 * 0.5 * 1 * 4.0; + + EXPECT_DOUBLE_EQ(expectedMass, link->Inertial().MassMatrix().Mass()); + EXPECT_EQ(gz::math::Vector3d(2.013513, 2.013513, 0.72603), + link->Inertial().MassMatrix().DiagonalMoments()); +} + +///////////////////////////////////////////////// +TEST(DOMLink, InertialValuesGivenWithAutoSetToTrue) +{ + std::string sdf = "" + "" + " " + " " + " " + " 4.0" + " 1 1 1 2 2 2" + " " + " 1" + " 1" + " 1" + " " + " " + " " + " 2.0" + " " + " " + " 1 1 1" + " " + " " + " " + " " + " " + " "; + + sdf::Root root; + const sdf::ParserConfig sdfParserConfig; + sdf::Errors errors = root.LoadSdfString(sdf, sdfParserConfig); + EXPECT_TRUE(errors.empty()); + EXPECT_NE(nullptr, root.Element()); + + const sdf::Model *model = root.Model(); + const sdf::Link *link = model->LinkByIndex(0); + sdf::Errors inertialErr = root.CalculateInertials(sdfParserConfig); + EXPECT_TRUE(inertialErr.empty()); + + EXPECT_NE(4.0, link->Inertial().MassMatrix().Mass()); + EXPECT_EQ(gz::math::Pose3d::Zero, link->Inertial().Pose()); + EXPECT_EQ(gz::math::Vector3d(0.33333, 0.33333, 0.33333), + link->Inertial().MassMatrix().DiagonalMoments()); +} + ///////////////////////////////////////////////// TEST(DOMLink, AddCollision) { From b5648ff408e1a47170f6b98d4336ae6ffca710ea Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Fri, 25 Aug 2023 01:22:41 +0530 Subject: [PATCH 68/88] Replaced tabs with spaces in sdf string in Link_Test Signed-off-by: Jasmeet Singh --- src/Link_TEST.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/Link_TEST.cc b/src/Link_TEST.cc index 28d58fa2b..661970e8f 100644 --- a/src/Link_TEST.cc +++ b/src/Link_TEST.cc @@ -312,7 +312,7 @@ TEST(DOMLink, CalculateInertialWithMultipleCollisions) " " " " " 0 0 -0.5 0 0 0" - " 2.0" + " 2.0" " " " " " 1 1 1" @@ -369,7 +369,7 @@ TEST(DOMLink, InertialValuesGivenWithAutoSetToTrue) " " " " " " - " 2.0" + " 2.0" " " " " " 1 1 1" From 4a2357adf785ed76065282ee118a1553ed9f3f90 Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Fri, 25 Aug 2023 02:54:38 +0530 Subject: [PATCH 69/88] Updated ParserConfig Unit test to test SetCalculateInertialConfiguration() Signed-off-by: Jasmeet Singh --- src/ParserConfig_TEST.cc | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/ParserConfig_TEST.cc b/src/ParserConfig_TEST.cc index 4450e95eb..71a4b8ef3 100644 --- a/src/ParserConfig_TEST.cc +++ b/src/ParserConfig_TEST.cc @@ -56,8 +56,14 @@ TEST(ParserConfig, Construction) EXPECT_EQ(sdf::EnforcementPolicy::WARN, config.WarningsPolicy()); EXPECT_EQ(sdf::EnforcementPolicy::WARN, config.UnrecognizedElementsPolicy()); EXPECT_EQ(sdf::EnforcementPolicy::WARN, config.DeprecatedElementsPolicy()); + EXPECT_EQ(sdf::ConfigureCalculateInertial::SKIP_CALCULATION_IN_LOAD, config.CalculateInertialConfiguration()); + config.SetCalculateInertialConfiguration( + sdf::ConfigureCalculateInertial::SAVE_CALCULATION); + EXPECT_EQ(sdf::ConfigureCalculateInertial::SAVE_CALCULATION, + config.CalculateInertialConfiguration()); + EXPECT_FALSE(config.URDFPreserveFixedJoint()); EXPECT_FALSE(config.StoreResolvedURIs()); } From 60131ddca76629e3f1da131db71465696d443f97 Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Fri, 25 Aug 2023 02:56:54 +0530 Subject: [PATCH 70/88] Added test for CalculateInertial() called with auto set to false Signed-off-by: Jasmeet Singh --- src/Link_TEST.cc | 29 +++++++++++++++++++++++++++++ src/ParserConfig_TEST.cc | 2 +- 2 files changed, 30 insertions(+), 1 deletion(-) diff --git a/src/Link_TEST.cc b/src/Link_TEST.cc index 661970e8f..c8be863ff 100644 --- a/src/Link_TEST.cc +++ b/src/Link_TEST.cc @@ -397,6 +397,35 @@ TEST(DOMLink, InertialValuesGivenWithAutoSetToTrue) link->Inertial().MassMatrix().DiagonalMoments()); } +///////////////////////////////////////////////// +TEST(DOMLink, CalculateInertialCalledWithAutoFalse) +{ + std::string sdf = "" + " " + " " + " " + " " + " " + " " + " "; + + sdf::Root root; + const sdf::ParserConfig sdfParserConfig; + sdf::Errors errors = root.LoadSdfString(sdf, sdfParserConfig); + EXPECT_TRUE(errors.empty()); + EXPECT_NE(nullptr, root.Element()); + + const sdf::Model *model = root.Model(); + const sdf::Link *link = model->LinkByIndex(0); + sdf::Errors inertialErr = root.CalculateInertials(sdfParserConfig); + EXPECT_TRUE(inertialErr.empty()); + + // Default Inertial values set during load + EXPECT_EQ(1.0, link->Inertial().MassMatrix().Mass()); + EXPECT_EQ(gz::math::Vector3d::One, + link->Inertial().MassMatrix().DiagonalMoments()); +} + ///////////////////////////////////////////////// TEST(DOMLink, AddCollision) { diff --git a/src/ParserConfig_TEST.cc b/src/ParserConfig_TEST.cc index 71a4b8ef3..5c90041d8 100644 --- a/src/ParserConfig_TEST.cc +++ b/src/ParserConfig_TEST.cc @@ -63,7 +63,7 @@ TEST(ParserConfig, Construction) sdf::ConfigureCalculateInertial::SAVE_CALCULATION); EXPECT_EQ(sdf::ConfigureCalculateInertial::SAVE_CALCULATION, config.CalculateInertialConfiguration()); - + EXPECT_FALSE(config.URDFPreserveFixedJoint()); EXPECT_FALSE(config.StoreResolvedURIs()); } From a3cb221424e81fb037da385a00c1261137aaa55d Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Fri, 25 Aug 2023 02:57:30 +0530 Subject: [PATCH 71/88] Added unit test for World::CalculateInertial() Signed-off-by: Jasmeet Singh --- src/World_TEST.cc | 58 +++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 58 insertions(+) diff --git a/src/World_TEST.cc b/src/World_TEST.cc index b1f93b92e..76b968488 100644 --- a/src/World_TEST.cc +++ b/src/World_TEST.cc @@ -26,6 +26,7 @@ #include "sdf/Physics.hh" #include "sdf/Root.hh" #include "sdf/World.hh" +#include "sdf/Collision.hh" ///////////////////////////////////////////////// TEST(DOMWorld, Construction) @@ -509,6 +510,63 @@ TEST(DOMWorld, AddLight) EXPECT_EQ(lightFromWorld->Name(), light.Name()); } +///////////////////////////////////////////////// +TEST(DOMWorld, CalculateInertial) +{ + std::string sdf = "" + " " + " " + " " + " " + " " + " " + " 1240.0" + " " + " " + " 2 2 2" + " " + " " + " " + " " + " " + " " + " "; + + sdf::Root root; + const sdf::ParserConfig sdfParserConfig; + sdf::Errors errors = root.LoadSdfString(sdf, sdfParserConfig); + EXPECT_TRUE(errors.empty()); + EXPECT_NE(nullptr, root.Element()); + + const sdf::World *world = root.WorldByIndex(0); + const sdf::Model *model = world->ModelByIndex(0); + const sdf::Link *link = model->LinkByIndex(0); + + sdf::Errors inertialErr = root.CalculateInertials(sdfParserConfig); + + double l = 2; + double w = 2; + double h = 2; + + double expectedMass = l*w*h * 1240.0; + double ixx = (1.0/12.0) * expectedMass * (w*w + h*h); + double iyy = (1.0/12.0) * expectedMass * (l*l + h*h); + double izz = (1.0/12.0) * expectedMass * (l*l + w*w); + + gz::math::MassMatrix3d expectedMassMat( + expectedMass, + gz::math::Vector3d(ixx, iyy, izz), + gz::math::Vector3d::Zero + ); + + gz::math::Inertiald expectedInertial; + expectedInertial.SetMassMatrix(expectedMassMat); + expectedInertial.SetPose(gz::math::Pose3d::Zero); + + ASSERT_TRUE(inertialErr.empty()); + EXPECT_EQ(expectedInertial, link->Inertial()); +} + ///////////////////////////////////////////////// TEST(DOMWorld, ToElement) { From 7483b89c14fb7e612450dfa0c77505a36abef42e Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Fri, 25 Aug 2023 02:58:29 +0530 Subject: [PATCH 72/88] Added unit test for CalculateInertial() call with SAVE_CALCULATION configuration Signed-off-by: Jasmeet Singh --- src/Root_TEST.cc | 37 +++++++++++++++++++++++++++++++++++++ 1 file changed, 37 insertions(+) diff --git a/src/Root_TEST.cc b/src/Root_TEST.cc index f7a643743..4c5c7e574 100644 --- a/src/Root_TEST.cc +++ b/src/Root_TEST.cc @@ -322,6 +322,43 @@ TEST(DOMRoot, FrameSemanticsOnMove) } } +///////////////////////////////////////////////// +TEST(DOMRoot, CalculateInertialWithSaveCalculationConfiguration) +{ + std::string sdf = "" + " " + " " + " " + " " + " " + " 1240.0" + " " + " " + " 2 2 2" + " " + " " + " " + " " + " " + " "; + + sdf::Root root; + sdf::ParserConfig sdfParserConfig; + sdf::Errors errors = root.LoadSdfString(sdf, sdfParserConfig); + EXPECT_TRUE(errors.empty()); + EXPECT_NE(nullptr, root.Element()); + + const sdf::Model *model = root.Model(); + const sdf::Link *link = model->LinkByIndex(0); + + sdfParserConfig.SetCalculateInertialConfiguration( + sdf::ConfigureCalculateInertial::SAVE_CALCULATION); + + sdf::Errors inertialErr = root.CalculateInertials(sdfParserConfig); + EXPECT_TRUE(inertialErr.empty()); + ASSERT_TRUE(link->AutoInertiaSaved()); +} + ///////////////////////////////////////////////// TEST(DOMRoot, AddWorld) { From e772c244f39bb971fc1d3f7524bc67bfb9bf5488 Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Fri, 25 Aug 2023 14:40:04 +0530 Subject: [PATCH 73/88] Included missing header files Signed-off-by: Jasmeet Singh --- include/sdf/Collision.hh | 1 + include/sdf/Geometry.hh | 2 ++ include/sdf/Link.hh | 2 ++ src/Box_TEST.cc | 5 +++++ src/Capsule_TEST.cc | 5 +++++ src/Collision.cc | 2 ++ src/Collision_TEST.cc | 8 ++++++++ src/Cylinder_TEST.cc | 5 +++++ src/Ellipsoid.cc | 1 + src/Ellipsoid_TEST.cc | 5 +++++ src/Geometry.cc | 3 +++ src/Geometry_TEST.cc | 9 ++++++++- src/Link.cc | 2 ++ src/Link_TEST.cc | 2 ++ src/Root_TEST.cc | 2 ++ src/Sphere.cc | 1 + src/Sphere_TEST.cc | 6 +++++- src/World_TEST.cc | 5 +++++ 18 files changed, 64 insertions(+), 2 deletions(-) diff --git a/include/sdf/Collision.hh b/include/sdf/Collision.hh index 6efa482cf..72ddec399 100644 --- a/include/sdf/Collision.hh +++ b/include/sdf/Collision.hh @@ -28,6 +28,7 @@ #include "sdf/Types.hh" #include "sdf/sdf_config.h" #include "sdf/system_util.hh" +#include "sdf/ParserConfig.hh" namespace sdf { diff --git a/include/sdf/Geometry.hh b/include/sdf/Geometry.hh index 81cb64405..bdb274378 100644 --- a/include/sdf/Geometry.hh +++ b/include/sdf/Geometry.hh @@ -25,6 +25,8 @@ #include #include #include +#include +#include namespace sdf { diff --git a/include/sdf/Link.hh b/include/sdf/Link.hh index 4ccb5606a..9dccfe4e2 100644 --- a/include/sdf/Link.hh +++ b/include/sdf/Link.hh @@ -27,6 +27,8 @@ #include "sdf/Types.hh" #include "sdf/sdf_config.h" #include "sdf/system_util.hh" +#include "sdf/ParserConfig.hh" +#include "sdf/Error.hh" namespace sdf { diff --git a/src/Box_TEST.cc b/src/Box_TEST.cc index 40baebbd2..b7b9e5ee9 100644 --- a/src/Box_TEST.cc +++ b/src/Box_TEST.cc @@ -14,11 +14,16 @@ * limitations under the License. * */ +#include #include #include "sdf/Box.hh" #include "sdf/Element.hh" #include "test_utils.hh" +#include +#include +#include +#include ///////////////////////////////////////////////// TEST(DOMBox, Construction) diff --git a/src/Capsule_TEST.cc b/src/Capsule_TEST.cc index 64e8c2d06..18de9e553 100644 --- a/src/Capsule_TEST.cc +++ b/src/Capsule_TEST.cc @@ -15,9 +15,14 @@ * */ +#include #include #include "sdf/Capsule.hh" #include "test_utils.hh" +#include +#include +#include +#include ///////////////////////////////////////////////// TEST(DOMCapsule, Construction) diff --git a/src/Collision.cc b/src/Collision.cc index 924ac0ce4..da82d801a 100644 --- a/src/Collision.cc +++ b/src/Collision.cc @@ -24,6 +24,8 @@ #include "sdf/parser.hh" #include "sdf/Surface.hh" #include "sdf/Types.hh" +#include "sdf/ParserConfig.hh" +#include "sdf/Element.hh" #include "FrameSemantics.hh" #include "ScopedGraph.hh" #include "Utils.hh" diff --git a/src/Collision_TEST.cc b/src/Collision_TEST.cc index e9336b165..05991279b 100644 --- a/src/Collision_TEST.cc +++ b/src/Collision_TEST.cc @@ -22,7 +22,15 @@ #include "sdf/Model.hh" #include "sdf/Link.hh" #include "sdf/Surface.hh" +#include "sdf/ParserConfig.hh" +#include "sdf/Types.hh" +#include "sdf/Root.hh" +#include "sdf/Model.hh" #include "test_utils.hh" +#include +#include +#include +#include ///////////////////////////////////////////////// TEST(DOMcollision, Construction) diff --git a/src/Cylinder_TEST.cc b/src/Cylinder_TEST.cc index b27444cb4..0a75fee39 100644 --- a/src/Cylinder_TEST.cc +++ b/src/Cylinder_TEST.cc @@ -15,9 +15,14 @@ * */ +#include #include #include "sdf/Cylinder.hh" #include "test_utils.hh" +#include +#include +#include +#include ///////////////////////////////////////////////// TEST(DOMCylinder, Construction) diff --git a/src/Ellipsoid.cc b/src/Ellipsoid.cc index 5300df90b..294a0b87b 100644 --- a/src/Ellipsoid.cc +++ b/src/Ellipsoid.cc @@ -18,6 +18,7 @@ #include #include +#include #include "sdf/Ellipsoid.hh" #include "sdf/parser.hh" #include "Utils.hh" diff --git a/src/Ellipsoid_TEST.cc b/src/Ellipsoid_TEST.cc index d482cab93..68664bc6e 100644 --- a/src/Ellipsoid_TEST.cc +++ b/src/Ellipsoid_TEST.cc @@ -15,9 +15,14 @@ * */ +#include #include #include "sdf/Ellipsoid.hh" #include "test_utils.hh" +#include +#include +#include +#include ///////////////////////////////////////////////// TEST(DOMEllipsoid, Construction) diff --git a/src/Geometry.cc b/src/Geometry.cc index 896c0ed8c..e6d35a3db 100644 --- a/src/Geometry.cc +++ b/src/Geometry.cc @@ -29,6 +29,9 @@ #include "sdf/Plane.hh" #include "sdf/Polyline.hh" #include "sdf/Sphere.hh" +#include "sdf/Element.hh" +#include "sdf/Types.hh" +#include "sdf/Error.hh" #include "Utils.hh" diff --git a/src/Geometry_TEST.cc b/src/Geometry_TEST.cc index 98bccefd9..1b92398ce 100644 --- a/src/Geometry_TEST.cc +++ b/src/Geometry_TEST.cc @@ -14,7 +14,7 @@ * limitations under the License. * */ - +#include #include #include "sdf/Box.hh" #include "sdf/Capsule.hh" @@ -26,7 +26,14 @@ #include "sdf/Plane.hh" #include "sdf/Polyline.hh" #include "sdf/Sphere.hh" +#include "sdf/ParserConfig.hh" +#include "sdf/Types.hh" +#include "sdf/Element.hh" #include "test_utils.hh" +#include +#include +#include +#include ///////////////////////////////////////////////// TEST(DOMGeometry, Construction) diff --git a/src/Link.cc b/src/Link.cc index e3021314a..faeef3f22 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -31,6 +31,8 @@ #include "sdf/Sensor.hh" #include "sdf/Types.hh" #include "sdf/Visual.hh" +#include "sdf/ParserConfig.hh" +#include "sdf/Element.hh" #include "FrameSemantics.hh" #include "ScopedGraph.hh" diff --git a/src/Link_TEST.cc b/src/Link_TEST.cc index c8be863ff..88d299d71 100644 --- a/src/Link_TEST.cc +++ b/src/Link_TEST.cc @@ -28,6 +28,8 @@ #include "sdf/Projector.hh" #include "sdf/Sensor.hh" #include "sdf/Visual.hh" +#include "sdf/ParserConfig.hh" +#include "sdf/Types.hh" ///////////////////////////////////////////////// TEST(DOMLink, Construction) diff --git a/src/Root_TEST.cc b/src/Root_TEST.cc index 4c5c7e574..52077886d 100644 --- a/src/Root_TEST.cc +++ b/src/Root_TEST.cc @@ -26,6 +26,8 @@ #include "sdf/World.hh" #include "sdf/Frame.hh" #include "sdf/Root.hh" +#include "sdf/ParserConfig.hh" +#include "sdf/Types.hh" #include "test_config.hh" ///////////////////////////////////////////////// diff --git a/src/Sphere.cc b/src/Sphere.cc index 6eb851b14..9d274530c 100644 --- a/src/Sphere.cc +++ b/src/Sphere.cc @@ -16,6 +16,7 @@ */ #include #include +#include #include "sdf/parser.hh" #include "sdf/Sphere.hh" diff --git a/src/Sphere_TEST.cc b/src/Sphere_TEST.cc index 4570c0aef..028dd645d 100644 --- a/src/Sphere_TEST.cc +++ b/src/Sphere_TEST.cc @@ -14,10 +14,14 @@ * limitations under the License. * */ - +#include #include #include "sdf/Sphere.hh" #include "test_utils.hh" +#include +#include +#include +#include ///////////////////////////////////////////////// TEST(DOMSphere, Construction) diff --git a/src/World_TEST.cc b/src/World_TEST.cc index 76b968488..f5e10e9c8 100644 --- a/src/World_TEST.cc +++ b/src/World_TEST.cc @@ -19,10 +19,15 @@ #include #include #include +#include +#include +#include #include "sdf/Frame.hh" #include "sdf/Light.hh" #include "sdf/Actor.hh" #include "sdf/Model.hh" +#include "sdf/Link.hh" +#include "sdf/Types.hh" #include "sdf/Physics.hh" #include "sdf/Root.hh" #include "sdf/World.hh" From f3cd3be5e94c99a80997429bd9c7ad7d890ef5c0 Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Sat, 26 Aug 2023 18:10:11 +0530 Subject: [PATCH 74/88] Removed redundant sdf/Model.hh include from Collision_TEST Signed-off-by: Jasmeet Singh --- src/Collision_TEST.cc | 1 - 1 file changed, 1 deletion(-) diff --git a/src/Collision_TEST.cc b/src/Collision_TEST.cc index 05991279b..967426f37 100644 --- a/src/Collision_TEST.cc +++ b/src/Collision_TEST.cc @@ -25,7 +25,6 @@ #include "sdf/ParserConfig.hh" #include "sdf/Types.hh" #include "sdf/Root.hh" -#include "sdf/Model.hh" #include "test_utils.hh" #include #include From 16ce852ad51294257cf3b836fe54a5f1ea79ae66 Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Mon, 28 Aug 2023 20:08:49 +0530 Subject: [PATCH 75/88] Updated CalculateInertial() documentation for Box, Capsule, Cylinder, Ellipsoid & Sphere Signed-off-by: Jasmeet Singh --- include/sdf/Box.hh | 4 +++- include/sdf/Capsule.hh | 4 +++- include/sdf/Cylinder.hh | 4 +++- include/sdf/Ellipsoid.hh | 4 +++- include/sdf/Sphere.hh | 4 +++- 5 files changed, 15 insertions(+), 5 deletions(-) diff --git a/include/sdf/Box.hh b/include/sdf/Box.hh index 1cf4ab7fd..c536011f1 100644 --- a/include/sdf/Box.hh +++ b/include/sdf/Box.hh @@ -68,7 +68,9 @@ namespace sdf /// \return A reference to a gz::math::Boxd object. public: gz::math::Boxd &Shape(); - /// \brief Calculate and return the Mass Matrix values for the Box + /// \brief Calculate and return the Inertial values for the Box. In order + /// to calculate the inertial properties, the function mutates the object + /// by updating its material properties. /// \param[in] _density Density of the box in kg/m^3 /// \return A std::optional with gz::math::Inertiald object or std::nullopt public: std::optional diff --git a/include/sdf/Capsule.hh b/include/sdf/Capsule.hh index 5c91fa3ea..b60e105c5 100644 --- a/include/sdf/Capsule.hh +++ b/include/sdf/Capsule.hh @@ -75,7 +75,9 @@ namespace sdf /// \return A reference to a gz::math::Capsuled object. public: gz::math::Capsuled &Shape(); - /// \brief Calculate and return the Mass Matrix values for the Capsule + /// \brief Calculate and return the Inertial values for the Capsule. In order + /// to calculate the inertial properties, the function mutates the object + /// by updating its material properties. /// \param[in] _density Density of the capsule in kg/m^3 /// \return A std::optional with gz::math::Inertiald object or std::nullopt public: std::optional CalculateInertial( diff --git a/include/sdf/Cylinder.hh b/include/sdf/Cylinder.hh index 91b0d1fcc..905166560 100644 --- a/include/sdf/Cylinder.hh +++ b/include/sdf/Cylinder.hh @@ -75,7 +75,9 @@ namespace sdf /// \return A reference to a gz::math::Cylinderd object. public: gz::math::Cylinderd &Shape(); - /// \brief Calculate and return the Mass Matrix values for the Cylinder + /// \brief Calculate and return the Inertial values for the Cylinder. In + /// order to calculate the inertial properties, the function mutates the + /// object by updating its material properties. /// \param[in] _density Density of the cylinder in kg/m^3 /// \return A std::optional with gz::math::Inertiald object or std::nullopt public: std::optional diff --git a/include/sdf/Ellipsoid.hh b/include/sdf/Ellipsoid.hh index 168331cb5..1bdd54b42 100644 --- a/include/sdf/Ellipsoid.hh +++ b/include/sdf/Ellipsoid.hh @@ -67,7 +67,9 @@ namespace sdf /// \return A reference to a gz::math::Ellipsoidd object. public: gz::math::Ellipsoidd &Shape(); - /// \brief Calculate and return the Mass Matrix values for the Ellipsoid + /// \brief Calculate and return the Inertial values for the Ellipsoid. In + /// order to calculate the inertial properties, the function mutates the + /// object by updating its material properties. /// \param[in] _density Density of the ellipsoid in kg/m^3 /// \return A std::optional with gz::math::Inertiald object or std::nullopt public: std::optional diff --git a/include/sdf/Sphere.hh b/include/sdf/Sphere.hh index f82306c18..a46ec3c41 100644 --- a/include/sdf/Sphere.hh +++ b/include/sdf/Sphere.hh @@ -68,7 +68,9 @@ namespace sdf /// not been called. public: sdf::ElementPtr Element() const; - /// \brief Calculate and return the Mass Matrix values for the Sphere + /// \brief Calculate and return the Inertial values for the Sphere. In + /// order to calculate the inertial properties, the function mutates the + /// object by updating its material properties. /// \param[in] _density Density of the sphere in kg/m^3 /// \return A std::optional with gz::math::Inertiald object or std::nullopt public: std::optional From 65c370311bbb9ed2974b5f437951650421f67b0f Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Mon, 28 Aug 2023 20:09:30 +0530 Subject: [PATCH 76/88] Updated Collision::Density() & Collision::SetDensity() docs Signed-off-by: Jasmeet Singh --- include/sdf/Collision.hh | 2 -- 1 file changed, 2 deletions(-) diff --git a/include/sdf/Collision.hh b/include/sdf/Collision.hh index 72ddec399..41b3be631 100644 --- a/include/sdf/Collision.hh +++ b/include/sdf/Collision.hh @@ -79,12 +79,10 @@ namespace sdf public: void SetName(const std::string &_name); /// \brief Get the density of the collision. - /// The density of the collision must be unique within the scope of a Link. /// \return Density of the collision. public: double Density() const; /// \brief Set the density of the collision. - /// The density of the collision must be unique within the scope of a Link. /// \param[in] _density Density of the collision. public: void SetDensity(double _density); From fee0d9067b14d6f1e849b89e512c1fbf21dc6d30 Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Mon, 28 Aug 2023 20:10:14 +0530 Subject: [PATCH 77/88] Updated Root::CalculateInertial() docs Signed-off-by: Jasmeet Singh --- include/sdf/Root.hh | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/include/sdf/Root.hh b/include/sdf/Root.hh index 116e97b24..0c9bd6cfa 100644 --- a/include/sdf/Root.hh +++ b/include/sdf/Root.hh @@ -229,8 +229,8 @@ namespace sdf /// \brief Calculate & set the inertial properties (mass, mass matrix /// and inertial pose) for all the worlds & models that are children /// of this Root object. This function can be called after calling - /// UpdateGraphs() since it uses frame graphs to resolve inertial pose - /// for links. + /// Root::Load() since it uses frame graphs to resolve inertial pose + /// for links and they would be automatically built /// \param[in] _config Custom parser configuration /// \return Errors, which is a vector of Error objects. /// Each Error includes an error code and message. An empty vector From ff887207bf68326e12fa75e525e97eb8cbe7a3f8 Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Mon, 28 Aug 2023 20:10:48 +0530 Subject: [PATCH 78/88] Updated description for element in Signed-off-by: Jasmeet Singh --- sdf/1.11/inertial.sdf | 1 + 1 file changed, 1 insertion(+) diff --git a/sdf/1.11/inertial.sdf b/sdf/1.11/inertial.sdf index dd7517ac7..b092401ff 100644 --- a/sdf/1.11/inertial.sdf +++ b/sdf/1.11/inertial.sdf @@ -20,6 +20,7 @@ Mass Density of the collision geometry. This is used to determine mass and inertia values during automatic calculation. + This density value would be overwritten by the density value in . Default is the density of water 1000 kg/m^3. From af8052ec8faf58206660a1fa508b58b7ccfbefe0 Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Mon, 28 Aug 2023 20:11:19 +0530 Subject: [PATCH 79/88] Added doc for autoInertiaSaved variable in sdf::Link Signed-off-by: Jasmeet Singh --- src/Link.cc | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/Link.cc b/src/Link.cc index faeef3f22..cc4e1a478 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -80,6 +80,8 @@ class sdf::Link::Implementation /// \brief True if this link should be subject to wind, false otherwise. public: bool enableWind = false; + /// \brief This variable is used to track whether the inertia values for + /// link was saved in CalculateInertial() public: bool autoInertiaSaved = false; /// \brief Scoped Pose Relative-To graph at the parent model scope. From 943b8865b2e2b427c656ef2f69720e8d190b1b85 Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Tue, 29 Aug 2023 00:03:29 +0530 Subject: [PATCH 80/88] Update function APIs - Updated all functions returning Errors object to take errors object as an output param - Updated function params to take errors as a first params Signed-off-by: Jasmeet Singh --- include/sdf/Collision.hh | 11 ++++++----- include/sdf/Geometry.hh | 10 +++++----- include/sdf/Root.hh | 8 ++++---- sdf/1.11/inertial.sdf | 2 +- src/Collision.cc | 22 ++++++++++------------ src/Collision_TEST.cc | 12 ++++++++---- src/Geometry.cc | 2 +- src/Geometry_TEST.cc | 24 ++++++++++++------------ src/Link.cc | 6 +----- src/Link_TEST.cc | 16 ++++++++-------- src/Root.cc | 13 ++++--------- src/Root_TEST.cc | 3 ++- src/World_TEST.cc | 4 ++-- 13 files changed, 64 insertions(+), 69 deletions(-) diff --git a/include/sdf/Collision.hh b/include/sdf/Collision.hh index 41b3be631..a29b62ac9 100644 --- a/include/sdf/Collision.hh +++ b/include/sdf/Collision.hh @@ -131,13 +131,14 @@ namespace sdf public: sdf::SemanticPose SemanticPose() const; /// \brief Calculate and return the MassMatrix for the collision + /// \param[out] _errors A vector of Errors objects. Each errors contains an + /// Error code and a message. An empty errors vector indicates no errors + /// \param[in] _config Custom parser configuration /// \param[out] _inertial An inertial object which will be set with the /// calculated inertial values - /// \param[in] _config Custom parser configuration - /// \return Errors, which is a vector of Error objects. Each Error includes - /// an error code and message. An empty vector indicates no error - public: Errors CalculateInertial(gz::math::Inertiald &_inertial, - const ParserConfig &_config); + public: void CalculateInertial(sdf::Errors &_errors, + const ParserConfig &_config, + gz::math::Inertiald &_inertial); /// \brief Get a pointer to the SDF element that was used during /// load. diff --git a/include/sdf/Geometry.hh b/include/sdf/Geometry.hh index bdb274378..ecb0d2cba 100644 --- a/include/sdf/Geometry.hh +++ b/include/sdf/Geometry.hh @@ -214,15 +214,15 @@ namespace sdf public: void SetHeightmapShape(const Heightmap &_heightmap); /// \brief Calculate and return the Mass Matrix values for the Geometry - /// \param[in] _density The density of the geometry element. - /// \param[in] _config Parser Config /// \param[out] _errors A vector of Errors object. Each object /// would contain an error code and an error message. + /// \param[in] _config Parser Config + /// \param[in] _density The density of the geometry element. /// \return A std::optional with gz::math::Inertiald object or std::nullopt public: std::optional CalculateInertial( - double _density, - const sdf::ParserConfig &_config, - sdf::Errors &_errors); + sdf::Errors &_errors, + const sdf::ParserConfig &_config, + double _density); /// \brief Get a pointer to the SDF element that was used during /// load. diff --git a/include/sdf/Root.hh b/include/sdf/Root.hh index 0c9bd6cfa..a0b51838c 100644 --- a/include/sdf/Root.hh +++ b/include/sdf/Root.hh @@ -231,11 +231,11 @@ namespace sdf /// of this Root object. This function can be called after calling /// Root::Load() since it uses frame graphs to resolve inertial pose /// for links and they would be automatically built + /// \param[out] _errors A vector of Errors objects. Each errors contains an + /// Error code and a message. An empty errors vector indicates no errors /// \param[in] _config Custom parser configuration - /// \return Errors, which is a vector of Error objects. - /// Each Error includes an error code and message. An empty vector - /// indicates no error. - public: Errors CalculateInertials(const ParserConfig &_config); + public: void CalculateInertials(sdf::Errors &_errors, + const ParserConfig &_config); /// \brief Create and return an SDF element filled with data from this /// root. diff --git a/sdf/1.11/inertial.sdf b/sdf/1.11/inertial.sdf index b092401ff..ce2c30f14 100644 --- a/sdf/1.11/inertial.sdf +++ b/sdf/1.11/inertial.sdf @@ -20,7 +20,7 @@ Mass Density of the collision geometry. This is used to determine mass and inertia values during automatic calculation. - This density value would be overwritten by the density value in . + This density value would be overwritten by the density value in collision. Default is the density of water 1000 kg/m^3. diff --git a/src/Collision.cc b/src/Collision.cc index da82d801a..13f07efbb 100644 --- a/src/Collision.cc +++ b/src/Collision.cc @@ -219,11 +219,11 @@ sdf::SemanticPose Collision::SemanticPose() const } ///////////////////////////////////////////////// -Errors Collision::CalculateInertial(gz::math::Inertiald &_inertial, - const ParserConfig &_config) +void Collision::CalculateInertial( + sdf::Errors &_errors, + const ParserConfig &_config, + gz::math::Inertiald &_inertial) { - Errors errors; - // Set the density value for the collision material if (this->dataPtr->sdf->HasElement("density")) { @@ -239,19 +239,19 @@ Errors Collision::CalculateInertial(gz::math::Inertiald &_inertial, "Using a default density value of 1000.0 kg/m^3. " ); enforceConfigurablePolicyCondition( - _config.WarningsPolicy(), densityMissingErr, errors + _config.WarningsPolicy(), densityMissingErr, _errors ); } auto geomInertial = - this->dataPtr->geom.CalculateInertial(this->dataPtr->density, - _config, errors); + this->dataPtr->geom.CalculateInertial(_errors, _config, + this->dataPtr->density); if (!geomInertial) { - errors.push_back({ErrorCode::LINK_INERTIA_INVALID, + _errors.push_back({ErrorCode::LINK_INERTIA_INVALID, "Inertia Calculated for collision: " + - this->dataPtr->name + " seems invalid."}); + this->dataPtr->name + " is invalid."}); } else { @@ -274,15 +274,13 @@ Errors Collision::CalculateInertial(gz::math::Inertiald &_inertial, gz::math::Pose3d collisionPoseLinkFrame; Errors poseConvErrors = this->SemanticPose().Resolve(collisionPoseLinkFrame); - errors.insert(errors.end(), + _errors.insert(_errors.end(), poseConvErrors.begin(), poseConvErrors.end()); _inertial.SetPose(collisionPoseLinkFrame); } } } - - return errors; } ///////////////////////////////////////////////// diff --git a/src/Collision_TEST.cc b/src/Collision_TEST.cc index 967426f37..a246ed2f8 100644 --- a/src/Collision_TEST.cc +++ b/src/Collision_TEST.cc @@ -204,8 +204,10 @@ TEST(DOMCollision, IncorrectBoxCollisionCalculateInertial) geom.SetBoxShape(box); collision.SetGeom(geom); - sdf::Errors errors = - collision.CalculateInertial(collisionInertial, sdfParserConfig); + sdf::Errors errors; + + collision.CalculateInertial(errors, sdfParserConfig, + collisionInertial); ASSERT_FALSE(errors.empty()); } @@ -239,7 +241,8 @@ TEST(DOMCollision, CorrectBoxCollisionCalculateInertial) const sdf::Link *link = model->LinkByIndex(0); const sdf::Collision *collision = link->CollisionByIndex(0); - sdf::Errors inertialErr = root.CalculateInertials(sdfParserConfig); + sdf::Errors inertialErr; + root.CalculateInertials(inertialErr, sdfParserConfig); double l = 2; double w = 2; @@ -301,7 +304,8 @@ TEST(DOMCollision, CalculateInertialPoseNotRelativeToLink) const sdf::Link *link = model->LinkByIndex(0); const sdf::Collision *collision = link->CollisionByIndex(0); - sdf::Errors inertialErr = root.CalculateInertials(sdfParserConfig); + sdf::Errors inertialErr; + root.CalculateInertials(inertialErr, sdfParserConfig); double l = 2; double w = 2; diff --git a/src/Geometry.cc b/src/Geometry.cc index e6d35a3db..fb3a56d52 100644 --- a/src/Geometry.cc +++ b/src/Geometry.cc @@ -313,7 +313,7 @@ void Geometry::SetPolylineShape(const std::vector &_polylines) } std::optional Geometry::CalculateInertial( - double _density, const sdf::ParserConfig &_config, sdf::Errors &_errors) + sdf::Errors &_errors, const sdf::ParserConfig &_config, double _density) { std::optional geomInertial; diff --git a/src/Geometry_TEST.cc b/src/Geometry_TEST.cc index 1b92398ce..c33f04737 100644 --- a/src/Geometry_TEST.cc +++ b/src/Geometry_TEST.cc @@ -321,8 +321,8 @@ TEST(DOMGeometry, CalculateInertial) // Not supported geom type { geom.SetType(sdf::GeometryType::EMPTY); - auto notSupportedInertial = geom.CalculateInertial(density, - sdfParserConfig, errors); + auto notSupportedInertial = geom.CalculateInertial(errors, + sdfParserConfig, density); ASSERT_EQ(notSupportedInertial, std::nullopt); } @@ -348,8 +348,8 @@ TEST(DOMGeometry, CalculateInertial) geom.SetType(sdf::GeometryType::BOX); geom.SetBoxShape(box); - auto boxInertial = geom.CalculateInertial(density, - sdfParserConfig, errors); + auto boxInertial = geom.CalculateInertial(errors, + sdfParserConfig, density); ASSERT_NE(std::nullopt, boxInertial); EXPECT_EQ(expectedInertial, *boxInertial); @@ -384,8 +384,8 @@ TEST(DOMGeometry, CalculateInertial) geom.SetType(sdf::GeometryType::CAPSULE); geom.SetCapsuleShape(capsule); - auto capsuleInertial = geom.CalculateInertial(density, - sdfParserConfig, errors); + auto capsuleInertial = geom.CalculateInertial(errors, + sdfParserConfig, density); ASSERT_NE(std::nullopt, capsuleInertial); EXPECT_EQ(expectedInertial, *capsuleInertial); @@ -415,8 +415,8 @@ TEST(DOMGeometry, CalculateInertial) geom.SetType(sdf::GeometryType::CYLINDER); geom.SetCylinderShape(cylinder); - auto cylinderInertial = geom.CalculateInertial(density, - sdfParserConfig, errors); + auto cylinderInertial = geom.CalculateInertial(errors, + sdfParserConfig, density); ASSERT_NE(std::nullopt, cylinderInertial); EXPECT_EQ(expectedInertial, *cylinderInertial); @@ -448,8 +448,8 @@ TEST(DOMGeometry, CalculateInertial) geom.SetType(sdf::GeometryType::ELLIPSOID); geom.SetEllipsoidShape(ellipsoid); - auto ellipsoidInertial = geom.CalculateInertial(density, - sdfParserConfig, errors); + auto ellipsoidInertial = geom.CalculateInertial(errors, + sdfParserConfig, density); ASSERT_NE(std::nullopt, ellipsoidInertial); EXPECT_EQ(expectedInertial, *ellipsoidInertial); @@ -477,8 +477,8 @@ TEST(DOMGeometry, CalculateInertial) geom.SetType(sdf::GeometryType::SPHERE); geom.SetSphereShape(sphere); - auto sphereInertial = geom.CalculateInertial(density, - sdfParserConfig, errors); + auto sphereInertial = geom.CalculateInertial(errors, + sdfParserConfig, density); ASSERT_NE(std::nullopt, sphereInertial); EXPECT_EQ(expectedInertial, *sphereInertial); diff --git a/src/Link.cc b/src/Link.cc index cc4e1a478..e1203d48b 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -616,11 +616,7 @@ void Link::CalculateInertials(sdf::Errors &_errors, const ParserConfig &_config) for (sdf::Collision &collision : this->dataPtr->collisions) { gz::math::Inertiald collisionInertia; - Errors inertiaErrors = - collision.CalculateInertial(collisionInertia, _config); - _errors.insert(_errors.end(), - inertiaErrors.begin(), - inertiaErrors.end()); + collision.CalculateInertial(_errors, _config, collisionInertia); totalInertia = totalInertia + collisionInertia; } diff --git a/src/Link_TEST.cc b/src/Link_TEST.cc index 88d299d71..c73dc9d7a 100644 --- a/src/Link_TEST.cc +++ b/src/Link_TEST.cc @@ -294,8 +294,8 @@ TEST(DOMLink, CalculateInertialWithNoCollisionsInLink) const sdf::Model *model = root.Model(); const sdf::Link *link = model->LinkByIndex(0); - sdf::Errors inertialErr = root.CalculateInertials(sdfParserConfig); - ASSERT_FALSE(inertialErr.empty()); + root.CalculateInertials(errors, sdfParserConfig); + ASSERT_FALSE(errors.empty()); // Default Inertial values set during load EXPECT_EQ(1.0, link->Inertial().MassMatrix().Mass()); @@ -343,8 +343,8 @@ TEST(DOMLink, CalculateInertialWithMultipleCollisions) const sdf::Model *model = root.Model(); const sdf::Link *link = model->LinkByIndex(0); - sdf::Errors inertialErr = root.CalculateInertials(sdfParserConfig); - EXPECT_TRUE(inertialErr.empty()); + root.CalculateInertials(errors, sdfParserConfig); + EXPECT_TRUE(errors.empty()); // Mass of cube(volume * density) + mass of cylinder(volume * density) double expectedMass = 1.0 * 2.0 + GZ_PI * 0.5 * 0.5 * 1 * 4.0; @@ -390,8 +390,8 @@ TEST(DOMLink, InertialValuesGivenWithAutoSetToTrue) const sdf::Model *model = root.Model(); const sdf::Link *link = model->LinkByIndex(0); - sdf::Errors inertialErr = root.CalculateInertials(sdfParserConfig); - EXPECT_TRUE(inertialErr.empty()); + root.CalculateInertials(errors, sdfParserConfig); + EXPECT_TRUE(errors.empty()); EXPECT_NE(4.0, link->Inertial().MassMatrix().Mass()); EXPECT_EQ(gz::math::Pose3d::Zero, link->Inertial().Pose()); @@ -419,8 +419,8 @@ TEST(DOMLink, CalculateInertialCalledWithAutoFalse) const sdf::Model *model = root.Model(); const sdf::Link *link = model->LinkByIndex(0); - sdf::Errors inertialErr = root.CalculateInertials(sdfParserConfig); - EXPECT_TRUE(inertialErr.empty()); + root.CalculateInertials(errors, sdfParserConfig); + EXPECT_TRUE(errors.empty()); // Default Inertial values set during load EXPECT_EQ(1.0, link->Inertial().MassMatrix().Mass()); diff --git a/src/Root.cc b/src/Root.cc index 41b2d3e74..960d62a2a 100644 --- a/src/Root.cc +++ b/src/Root.cc @@ -396,8 +396,7 @@ Errors Root::Load(SDFPtr _sdf, const ParserConfig &_config) if (_config.CalculateInertialConfiguration() != ConfigureCalculateInertial::SKIP_CALCULATION_IN_LOAD) { - Errors inertialErr = this->CalculateInertials(_config); - errors.insert(errors.end(), inertialErr.begin(), inertialErr.end()); + this->CalculateInertials(errors, _config); } return errors; @@ -599,24 +598,20 @@ void Root::Implementation::UpdateGraphs(sdf::Model &_model, } ///////////////////////////////////////////////// -Errors Root::CalculateInertials(const ParserConfig &_config) +void Root::CalculateInertials(sdf::Errors &_errors, const ParserConfig &_config) { - sdf::Errors errors; - // Calculate and set Inertials for all the worlds for (sdf::World &world : this->dataPtr->worlds) { - world.CalculateInertials(errors, _config); + world.CalculateInertials(_errors, _config); } // Calculate and set Inertials for the model, if it is present if (std::holds_alternative(this->dataPtr->modelLightOrActor)) { sdf::Model &model = std::get(this->dataPtr->modelLightOrActor); - model.CalculateInertials(errors, _config); + model.CalculateInertials(_errors, _config); } - - return errors; } ///////////////////////////////////////////////// diff --git a/src/Root_TEST.cc b/src/Root_TEST.cc index 52077886d..c7814316c 100644 --- a/src/Root_TEST.cc +++ b/src/Root_TEST.cc @@ -356,7 +356,8 @@ TEST(DOMRoot, CalculateInertialWithSaveCalculationConfiguration) sdfParserConfig.SetCalculateInertialConfiguration( sdf::ConfigureCalculateInertial::SAVE_CALCULATION); - sdf::Errors inertialErr = root.CalculateInertials(sdfParserConfig); + sdf::Errors inertialErr; + root.CalculateInertials(inertialErr, sdfParserConfig); EXPECT_TRUE(inertialErr.empty()); ASSERT_TRUE(link->AutoInertiaSaved()); } diff --git a/src/World_TEST.cc b/src/World_TEST.cc index f5e10e9c8..d1f202a2e 100644 --- a/src/World_TEST.cc +++ b/src/World_TEST.cc @@ -547,7 +547,7 @@ TEST(DOMWorld, CalculateInertial) const sdf::Model *model = world->ModelByIndex(0); const sdf::Link *link = model->LinkByIndex(0); - sdf::Errors inertialErr = root.CalculateInertials(sdfParserConfig); + root.CalculateInertials(errors, sdfParserConfig); double l = 2; double w = 2; @@ -568,7 +568,7 @@ TEST(DOMWorld, CalculateInertial) expectedInertial.SetMassMatrix(expectedMassMat); expectedInertial.SetPose(gz::math::Pose3d::Zero); - ASSERT_TRUE(inertialErr.empty()); + ASSERT_TRUE(errors.empty()); EXPECT_EQ(expectedInertial, link->Inertial()); } From a658c25f76cd67a881c2a6f6dcb1017a753ceba2 Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Tue, 29 Aug 2023 02:08:28 +0530 Subject: [PATCH 81/88] Added flag to track if density was set at load Signed-off-by: Jasmeet Singh --- src/Collision.cc | 21 +++++++++++++-------- 1 file changed, 13 insertions(+), 8 deletions(-) diff --git a/src/Collision.cc b/src/Collision.cc index 13f07efbb..7ce933fae 100644 --- a/src/Collision.cc +++ b/src/Collision.cc @@ -52,6 +52,9 @@ class sdf::Collision::Implementation /// \brief Density of the collision. Default is 1000.0 public: double density{1000.0}; + /// \brief True if density was set during load from sdf. + public: bool densitySetAtLoad = false; + /// \brief The SDF element pointer used during load. public: sdf::ElementPtr sdf; @@ -120,6 +123,13 @@ Errors Collision::Load(ElementPtr _sdf, const ParserConfig &_config) this->dataPtr->surface.Load(_sdf->GetElement("surface", errors)); } + // Load the density value if given + if (_sdf->HasElement("density")) + { + this->dataPtr->density = _sdf->Get("density"); + this->dataPtr->densitySetAtLoad = true; + } + return errors; } @@ -224,15 +234,10 @@ void Collision::CalculateInertial( const ParserConfig &_config, gz::math::Inertiald &_inertial) { - // Set the density value for the collision material - if (this->dataPtr->sdf->HasElement("density")) - { - this->dataPtr->density = this->dataPtr->sdf->Get("density"); - } - else + // Check if density was not set during load & send a warning + // about the default value being used + if (!this->dataPtr->densitySetAtLoad) { - // If the density element is missing, let the user know that a default - // value would be used according to the policy Error densityMissingErr( ErrorCode::ELEMENT_MISSING, "Collision is missing a child element. " From d6527176f3adac188fe34b972ce976306edb1f61 Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Tue, 29 Aug 2023 02:10:04 +0530 Subject: [PATCH 82/88] Added separate variable to track if auto inertia is enabled or not Signed-off-by: Jasmeet Singh --- include/sdf/Link.hh | 15 ++++++++- src/Link.cc | 79 ++++++++++++++++++++++++++------------------- src/Link_TEST.cc | 4 +++ 3 files changed, 64 insertions(+), 34 deletions(-) diff --git a/include/sdf/Link.hh b/include/sdf/Link.hh index 9dccfe4e2..e9ada0deb 100644 --- a/include/sdf/Link.hh +++ b/include/sdf/Link.hh @@ -392,6 +392,19 @@ namespace sdf /// \sa Model::SetEnableWind(bool) public: void SetEnableWind(bool _enableWind); + /// \brief Check if the automatic calculation for the link inertial + /// is enabled or not. + /// \return True if automatic calculation is enabled. This can be done + /// setting the auto attribute of the element of the link to + /// true or by setting the autoInertia member to true + /// using SetAutoInertia(). + public: bool AutoInertia() const; + + /// \brief Enable automatic inertial calculations by setting autoInertia + /// to true. + /// \param[in] _autoInertia True or False + public: void SetAutoInertia(bool _autoInertia); + /// \brief Check if the inertial values for this link were saved. /// If true, the inertial values for this link wont be calculated /// when CalculateInertial() is called. This value is set to true @@ -402,7 +415,7 @@ namespace sdf public: bool AutoInertiaSaved() const; /// \brief Set the autoInertiaSaved() values - /// \param _autoInertiaSaved True or False + /// \param[in] _autoInertiaSaved True or False public: void SetAutoInertiaSaved(bool _autoInertiaSaved); /// \brief Add a collision to the link. diff --git a/src/Link.cc b/src/Link.cc index e1203d48b..e0d2fd2dd 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -80,6 +80,9 @@ class sdf::Link::Implementation /// \brief True if this link should be subject to wind, false otherwise. public: bool enableWind = false; + /// \brief True if automatic caluclation for the link inertial is enabled + public: bool autoInertia = false; + /// \brief This variable is used to track whether the inertia values for /// link was saved in CalculateInertial() public: bool autoInertiaSaved = false; @@ -179,6 +182,7 @@ Errors Link::Load(ElementPtr _sdf, const ParserConfig &_config) if (inertialElem->Get("auto")) { + this->dataPtr->autoInertia = true; // If auto is to true but user has still provided // inertial values if (inertialElem->HasElement("pose") || @@ -595,48 +599,45 @@ Errors Link::ResolveInertial( ///////////////////////////////////////////////// void Link::CalculateInertials(sdf::Errors &_errors, const ParserConfig &_config) { - if (this->dataPtr->sdf->HasElement("inertial")) + // If inertial calculations is set to automatic & the inertial values for the + // link was not saved previously + if (this->dataPtr->autoInertia && !this->dataPtr->autoInertiaSaved) { - sdf::ElementPtr inertialElem = this->dataPtr->sdf->GetElement("inertial"); - - if (inertialElem->Get("auto") && !this->dataPtr->autoInertiaSaved) + // Return an error if auto is set to true but there are no + // collision elements in the link + if (this->dataPtr->collisions.empty()) { - // Return an error if auto is set to true but there are no - // collision elements in the link - if (this->dataPtr->collisions.empty()) - { - _errors.push_back({ErrorCode::ELEMENT_MISSING, - "Inertial is set to auto but there are no " - " elements for the link."}); - return; - } + _errors.push_back({ErrorCode::ELEMENT_MISSING, + "Inertial is set to auto but there are no " + " elements for the link."}); + return; + } - gz::math::Inertiald totalInertia; + gz::math::Inertiald totalInertia; - for (sdf::Collision &collision : this->dataPtr->collisions) - { - gz::math::Inertiald collisionInertia; - collision.CalculateInertial(_errors, _config, collisionInertia); - totalInertia = totalInertia + collisionInertia; - } + for (sdf::Collision &collision : this->dataPtr->collisions) + { + gz::math::Inertiald collisionInertia; + collision.CalculateInertial(_errors, _config, collisionInertia); + totalInertia = totalInertia + collisionInertia; + } - this->dataPtr->inertial = totalInertia; + this->dataPtr->inertial = totalInertia; - // If CalculateInertial() was called with SAVE_CALCULATION - // configuration then set autoInertiaSaved to true - if (_config.CalculateInertialConfiguration() == - ConfigureCalculateInertial::SAVE_CALCULATION) - { - this->dataPtr->autoInertiaSaved = true; - } - } - // If auto is false, this means inertial values were set - // from user given values in Link::Load(), therefore we can return - else + // If CalculateInertial() was called with SAVE_CALCULATION + // configuration then set autoInertiaSaved to true + if (_config.CalculateInertialConfiguration() == + ConfigureCalculateInertial::SAVE_CALCULATION) { - return; + this->dataPtr->autoInertiaSaved = true; } } + // If auto is false, this means inertial values were set + // from user given values in Link::Load(), therefore we can return + else + { + return; + } } ///////////////////////////////////////////////// @@ -793,6 +794,18 @@ void Link::SetEnableWind(const bool _enableWind) this->dataPtr->enableWind = _enableWind; } +///////////////////////////////////////////////// +bool Link::AutoInertia() const +{ + return this->dataPtr->autoInertia; +} + +///////////////////////////////////////////////// +void Link::SetAutoInertia(bool _autoInertia) +{ + this->dataPtr->autoInertia = _autoInertia; +} + ///////////////////////////////////////////////// bool Link::AutoInertiaSaved() const { diff --git a/src/Link_TEST.cc b/src/Link_TEST.cc index c73dc9d7a..0e0f23efe 100644 --- a/src/Link_TEST.cc +++ b/src/Link_TEST.cc @@ -76,6 +76,10 @@ TEST(DOMLink, Construction) link.SetAutoInertiaSaved(true); EXPECT_TRUE(link.AutoInertiaSaved()); + EXPECT_FALSE(link.AutoInertia()); + link.SetAutoInertia(true); + EXPECT_TRUE(link.AutoInertia()); + EXPECT_EQ(0u, link.SensorCount()); EXPECT_EQ(nullptr, link.SensorByIndex(0)); EXPECT_EQ(nullptr, link.SensorByIndex(1)); From ec627665250facb0448caf2914aa78a2f98f4775 Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Tue, 29 Aug 2023 02:23:46 +0530 Subject: [PATCH 83/88] Completed codecheck Signed-off-by: Jasmeet Singh --- include/sdf/Box.hh | 2 +- include/sdf/Capsule.hh | 6 +++--- include/sdf/Cylinder.hh | 4 ++-- include/sdf/Ellipsoid.hh | 4 ++-- include/sdf/Geometry.hh | 2 +- include/sdf/Link.hh | 2 +- include/sdf/Sphere.hh | 4 ++-- 7 files changed, 12 insertions(+), 12 deletions(-) diff --git a/include/sdf/Box.hh b/include/sdf/Box.hh index c536011f1..ae3515f39 100644 --- a/include/sdf/Box.hh +++ b/include/sdf/Box.hh @@ -69,7 +69,7 @@ namespace sdf public: gz::math::Boxd &Shape(); /// \brief Calculate and return the Inertial values for the Box. In order - /// to calculate the inertial properties, the function mutates the object + /// to calculate the inertial properties, the function mutates the object /// by updating its material properties. /// \param[in] _density Density of the box in kg/m^3 /// \return A std::optional with gz::math::Inertiald object or std::nullopt diff --git a/include/sdf/Capsule.hh b/include/sdf/Capsule.hh index b60e105c5..97f6dce0b 100644 --- a/include/sdf/Capsule.hh +++ b/include/sdf/Capsule.hh @@ -75,9 +75,9 @@ namespace sdf /// \return A reference to a gz::math::Capsuled object. public: gz::math::Capsuled &Shape(); - /// \brief Calculate and return the Inertial values for the Capsule. In order - /// to calculate the inertial properties, the function mutates the object - /// by updating its material properties. + /// \brief Calculate and return the Inertial values for the Capsule. + /// In order to calculate the inertial properties, the function + /// mutates the object by updating its material properties. /// \param[in] _density Density of the capsule in kg/m^3 /// \return A std::optional with gz::math::Inertiald object or std::nullopt public: std::optional CalculateInertial( diff --git a/include/sdf/Cylinder.hh b/include/sdf/Cylinder.hh index 905166560..bba37ba24 100644 --- a/include/sdf/Cylinder.hh +++ b/include/sdf/Cylinder.hh @@ -75,8 +75,8 @@ namespace sdf /// \return A reference to a gz::math::Cylinderd object. public: gz::math::Cylinderd &Shape(); - /// \brief Calculate and return the Inertial values for the Cylinder. In - /// order to calculate the inertial properties, the function mutates the + /// \brief Calculate and return the Inertial values for the Cylinder. In + /// order to calculate the inertial properties, the function mutates the /// object by updating its material properties. /// \param[in] _density Density of the cylinder in kg/m^3 /// \return A std::optional with gz::math::Inertiald object or std::nullopt diff --git a/include/sdf/Ellipsoid.hh b/include/sdf/Ellipsoid.hh index 1bdd54b42..33a046c2c 100644 --- a/include/sdf/Ellipsoid.hh +++ b/include/sdf/Ellipsoid.hh @@ -67,8 +67,8 @@ namespace sdf /// \return A reference to a gz::math::Ellipsoidd object. public: gz::math::Ellipsoidd &Shape(); - /// \brief Calculate and return the Inertial values for the Ellipsoid. In - /// order to calculate the inertial properties, the function mutates the + /// \brief Calculate and return the Inertial values for the Ellipsoid. In + /// order to calculate the inertial properties, the function mutates the /// object by updating its material properties. /// \param[in] _density Density of the ellipsoid in kg/m^3 /// \return A std::optional with gz::math::Inertiald object or std::nullopt diff --git a/include/sdf/Geometry.hh b/include/sdf/Geometry.hh index ecb0d2cba..7c6779c2e 100644 --- a/include/sdf/Geometry.hh +++ b/include/sdf/Geometry.hh @@ -221,7 +221,7 @@ namespace sdf /// \return A std::optional with gz::math::Inertiald object or std::nullopt public: std::optional CalculateInertial( sdf::Errors &_errors, - const sdf::ParserConfig &_config, + const sdf::ParserConfig &_config, double _density); /// \brief Get a pointer to the SDF element that was used during diff --git a/include/sdf/Link.hh b/include/sdf/Link.hh index e9ada0deb..37de66dc2 100644 --- a/include/sdf/Link.hh +++ b/include/sdf/Link.hh @@ -396,7 +396,7 @@ namespace sdf /// is enabled or not. /// \return True if automatic calculation is enabled. This can be done /// setting the auto attribute of the element of the link to - /// true or by setting the autoInertia member to true + /// true or by setting the autoInertia member to true /// using SetAutoInertia(). public: bool AutoInertia() const; diff --git a/include/sdf/Sphere.hh b/include/sdf/Sphere.hh index a46ec3c41..05eb072e3 100644 --- a/include/sdf/Sphere.hh +++ b/include/sdf/Sphere.hh @@ -68,8 +68,8 @@ namespace sdf /// not been called. public: sdf::ElementPtr Element() const; - /// \brief Calculate and return the Inertial values for the Sphere. In - /// order to calculate the inertial properties, the function mutates the + /// \brief Calculate and return the Inertial values for the Sphere. In + /// order to calculate the inertial properties, the function mutates the /// object by updating its material properties. /// \param[in] _density Density of the sphere in kg/m^3 /// \return A std::optional with gz::math::Inertiald object or std::nullopt From c8f9fc4828d23b3e13097e3d609bea7e6af9f483 Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Wed, 30 Aug 2023 00:31:38 +0530 Subject: [PATCH 84/88] Updated Collision::CalculateInertial() API Signed-off-by: Jasmeet Singh --- include/sdf/Collision.hh | 4 ++-- src/Collision.cc | 4 ++-- src/Collision_TEST.cc | 4 ++-- src/Link.cc | 2 +- 4 files changed, 7 insertions(+), 7 deletions(-) diff --git a/include/sdf/Collision.hh b/include/sdf/Collision.hh index a29b62ac9..5c2275b20 100644 --- a/include/sdf/Collision.hh +++ b/include/sdf/Collision.hh @@ -137,8 +137,8 @@ namespace sdf /// \param[out] _inertial An inertial object which will be set with the /// calculated inertial values public: void CalculateInertial(sdf::Errors &_errors, - const ParserConfig &_config, - gz::math::Inertiald &_inertial); + gz::math::Inertiald &_inertial, + const ParserConfig &_config); /// \brief Get a pointer to the SDF element that was used during /// load. diff --git a/src/Collision.cc b/src/Collision.cc index 7ce933fae..ed4303a4a 100644 --- a/src/Collision.cc +++ b/src/Collision.cc @@ -231,8 +231,8 @@ sdf::SemanticPose Collision::SemanticPose() const ///////////////////////////////////////////////// void Collision::CalculateInertial( sdf::Errors &_errors, - const ParserConfig &_config, - gz::math::Inertiald &_inertial) + gz::math::Inertiald &_inertial, + const ParserConfig &_config) { // Check if density was not set during load & send a warning // about the default value being used diff --git a/src/Collision_TEST.cc b/src/Collision_TEST.cc index a246ed2f8..bc71e8f8b 100644 --- a/src/Collision_TEST.cc +++ b/src/Collision_TEST.cc @@ -206,8 +206,8 @@ TEST(DOMCollision, IncorrectBoxCollisionCalculateInertial) sdf::Errors errors; - collision.CalculateInertial(errors, sdfParserConfig, - collisionInertial); + collision.CalculateInertial(errors, collisionInertial, + sdfParserConfig); ASSERT_FALSE(errors.empty()); } diff --git a/src/Link.cc b/src/Link.cc index e0d2fd2dd..883028ead 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -618,7 +618,7 @@ void Link::CalculateInertials(sdf::Errors &_errors, const ParserConfig &_config) for (sdf::Collision &collision : this->dataPtr->collisions) { gz::math::Inertiald collisionInertia; - collision.CalculateInertial(_errors, _config, collisionInertia); + collision.CalculateInertial(_errors, collisionInertia, _config); totalInertia = totalInertia + collisionInertia; } From b05e267fa77d19cda8f488f904b56e53a797beeb Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Wed, 30 Aug 2023 00:35:20 +0530 Subject: [PATCH 85/88] Updated check for geometry type before resolving inertial pose Signed-off-by: Jasmeet Singh --- src/Collision.cc | 37 +++++++++++++++---------------------- 1 file changed, 15 insertions(+), 22 deletions(-) diff --git a/src/Collision.cc b/src/Collision.cc index ed4303a4a..a77051d66 100644 --- a/src/Collision.cc +++ b/src/Collision.cc @@ -261,29 +261,22 @@ void Collision::CalculateInertial( else { _inertial = geomInertial.value(); - - // If geometry type is not mesh than calculate inertial pose in Link frame - // considering collision frame to be same as inertial frame - // In case of mesh the custom inertia calculator should return - // the inertial object with the pose already set - if (this->dataPtr->geom.Type() != GeometryType::MESH) + + // If collision pose is in Link Frame then set that as inertial pose + // Else resolve collision pose in Link Frame and then set as inertial pose + if (this->dataPtr->poseRelativeTo.empty()) + { + _inertial.SetPose(this->dataPtr->pose); + } + else { - // If collision pose is in Link Frame then set that as inertial pose - // Else resolve collision pose in Link Frame and then set as inertial pose - if (this->dataPtr->poseRelativeTo.empty()) - { - _inertial.SetPose(this->dataPtr->pose); - } - else - { - gz::math::Pose3d collisionPoseLinkFrame; - Errors poseConvErrors = - this->SemanticPose().Resolve(collisionPoseLinkFrame); - _errors.insert(_errors.end(), - poseConvErrors.begin(), - poseConvErrors.end()); - _inertial.SetPose(collisionPoseLinkFrame); - } + gz::math::Pose3d collisionPoseLinkFrame; + Errors poseConvErrors = + this->SemanticPose().Resolve(collisionPoseLinkFrame); + _errors.insert(_errors.end(), + poseConvErrors.begin(), + poseConvErrors.end()); + _inertial.SetPose(collisionPoseLinkFrame); } } } From 3a21b69150a87d81ce02477184199746e1d6c60b Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Wed, 30 Aug 2023 00:35:33 +0530 Subject: [PATCH 86/88] Updated Root::CalculateInertial() docs Signed-off-by: Jasmeet Singh --- include/sdf/Root.hh | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/include/sdf/Root.hh b/include/sdf/Root.hh index a0b51838c..84a73385a 100644 --- a/include/sdf/Root.hh +++ b/include/sdf/Root.hh @@ -228,9 +228,9 @@ namespace sdf /// \brief Calculate & set the inertial properties (mass, mass matrix /// and inertial pose) for all the worlds & models that are children - /// of this Root object. This function can be called after calling - /// Root::Load() since it uses frame graphs to resolve inertial pose - /// for links and they would be automatically built + /// of this Root object. This function must be called after calling + /// Root::Load() or UpdateGraphs() since it uses frame graphs to + /// resolve inertial pose for links and they would be automatically built /// \param[out] _errors A vector of Errors objects. Each errors contains an /// Error code and a message. An empty errors vector indicates no errors /// \param[in] _config Custom parser configuration From 91c828a1fbd6feb5cfb8b19d8c29ef8502fdf75d Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Wed, 30 Aug 2023 00:52:13 +0530 Subject: [PATCH 87/88] Added const specifier to variables where needed Signed-off-by: Jasmeet Singh --- include/sdf/Root.hh | 2 +- src/Box_TEST.cc | 6 +++--- src/Capsule_TEST.cc | 6 +++--- src/Collision.cc | 2 +- src/Collision_TEST.cc | 12 ++++++------ src/Cylinder_TEST.cc | 6 +++--- src/Ellipsoid_TEST.cc | 8 ++++---- src/Geometry_TEST.cc | 24 ++++++++++++------------ src/Sphere_TEST.cc | 4 ++-- src/World_TEST.cc | 6 +++--- 10 files changed, 38 insertions(+), 38 deletions(-) diff --git a/include/sdf/Root.hh b/include/sdf/Root.hh index 84a73385a..da4cdd35a 100644 --- a/include/sdf/Root.hh +++ b/include/sdf/Root.hh @@ -229,7 +229,7 @@ namespace sdf /// \brief Calculate & set the inertial properties (mass, mass matrix /// and inertial pose) for all the worlds & models that are children /// of this Root object. This function must be called after calling - /// Root::Load() or UpdateGraphs() since it uses frame graphs to + /// Root::Load() or UpdateGraphs() since it uses frame graphs to /// resolve inertial pose for links and they would be automatically built /// \param[out] _errors A vector of Errors objects. Each errors contains an /// Error code and a message. An empty errors vector indicates no errors diff --git a/src/Box_TEST.cc b/src/Box_TEST.cc index b7b9e5ee9..eb57aeaa5 100644 --- a/src/Box_TEST.cc +++ b/src/Box_TEST.cc @@ -164,9 +164,9 @@ TEST(DOMBox, CalculateInertial) auto invalidBoxInertial = box.CalculateInertial(density); ASSERT_EQ(std::nullopt, invalidBoxInertial); - double l = 2; - double w = 2; - double h = 2; + const double l = 2; + const double w = 2; + const double h = 2; box.SetSize(gz::math::Vector3d(l, w, h)); double expectedMass = box.Shape().Volume() * density; diff --git a/src/Capsule_TEST.cc b/src/Capsule_TEST.cc index 18de9e553..4cb39a851 100644 --- a/src/Capsule_TEST.cc +++ b/src/Capsule_TEST.cc @@ -192,9 +192,9 @@ TEST(DOMCapsule, CalculateInertial) sdf::Capsule capsule; // density of aluminium - double density = 2710; - double l = 2.0; - double r = 0.1; + const double density = 2710; + const double l = 2.0; + const double r = 0.1; capsule.SetLength(l); capsule.SetRadius(r); diff --git a/src/Collision.cc b/src/Collision.cc index a77051d66..89d15ffce 100644 --- a/src/Collision.cc +++ b/src/Collision.cc @@ -261,7 +261,7 @@ void Collision::CalculateInertial( else { _inertial = geomInertial.value(); - + // If collision pose is in Link Frame then set that as inertial pose // Else resolve collision pose in Link Frame and then set as inertial pose if (this->dataPtr->poseRelativeTo.empty()) diff --git a/src/Collision_TEST.cc b/src/Collision_TEST.cc index bc71e8f8b..996912f22 100644 --- a/src/Collision_TEST.cc +++ b/src/Collision_TEST.cc @@ -244,9 +244,9 @@ TEST(DOMCollision, CorrectBoxCollisionCalculateInertial) sdf::Errors inertialErr; root.CalculateInertials(inertialErr, sdfParserConfig); - double l = 2; - double w = 2; - double h = 2; + const double l = 2; + const double w = 2; + const double h = 2; double expectedMass = l*w*h * collision->Density(); double ixx = (1.0/12.0) * expectedMass * (w*w + h*h); @@ -307,9 +307,9 @@ TEST(DOMCollision, CalculateInertialPoseNotRelativeToLink) sdf::Errors inertialErr; root.CalculateInertials(inertialErr, sdfParserConfig); - double l = 2; - double w = 2; - double h = 2; + const double l = 2; + const double w = 2; + const double h = 2; double expectedMass = l*w*h * collision->Density(); double ixx = (1.0/12.0) * expectedMass * (w*w + h*h); diff --git a/src/Cylinder_TEST.cc b/src/Cylinder_TEST.cc index 0a75fee39..018c4afd7 100644 --- a/src/Cylinder_TEST.cc +++ b/src/Cylinder_TEST.cc @@ -188,7 +188,7 @@ TEST(DOMCylinder, CalculateInertial) sdf::Cylinder cylinder; // density of aluminium - double density = 2170; + const double density = 2170; // Invalid dimensions leading to std::nullopt return // CalculateInertials() @@ -197,8 +197,8 @@ TEST(DOMCylinder, CalculateInertial) auto invalidCylinderInertial = cylinder.CalculateInertial(density); ASSERT_EQ(std::nullopt, invalidCylinderInertial); - double l = 2.0; - double r = 0.1; + const double l = 2.0; + const double r = 0.1; cylinder.SetLength(l); cylinder.SetRadius(r); diff --git a/src/Ellipsoid_TEST.cc b/src/Ellipsoid_TEST.cc index 68664bc6e..2a6fec0cd 100644 --- a/src/Ellipsoid_TEST.cc +++ b/src/Ellipsoid_TEST.cc @@ -152,7 +152,7 @@ TEST(DOMEllipsoid, CalculateInertial) sdf::Ellipsoid ellipsoid; // density of aluminium - double density = 2170; + const double density = 2170; // Invalid dimensions leading to std::nullopt return // CalculateInertials() @@ -160,9 +160,9 @@ TEST(DOMEllipsoid, CalculateInertial) auto invalidEllipsoidInertial = ellipsoid.CalculateInertial(density); ASSERT_EQ(std::nullopt, invalidEllipsoidInertial); - double a = 1.0; - double b = 10.0; - double c = 100.0; + const double a = 1.0; + const double b = 10.0; + const double c = 100.0; ellipsoid.SetRadii(gz::math::Vector3d(a, b, c)); diff --git a/src/Geometry_TEST.cc b/src/Geometry_TEST.cc index c33f04737..6c3f724fe 100644 --- a/src/Geometry_TEST.cc +++ b/src/Geometry_TEST.cc @@ -311,7 +311,7 @@ TEST(DOMGeometry, CalculateInertial) sdf::Geometry geom; // Density of Aluminimum - double density = 2170.0; + const double density = 2170.0; double expectedMass; gz::math::MassMatrix3d expectedMassMat; gz::math::Inertiald expectedInertial; @@ -329,9 +329,9 @@ TEST(DOMGeometry, CalculateInertial) // Box { sdf::Box box; - double l = 2; - double w = 2; - double h = 2; + const double l = 2; + const double w = 2; + const double h = 2; box.SetSize(gz::math::Vector3d(l, w, h)); expectedMass = box.Shape().Volume() * density; @@ -360,8 +360,8 @@ TEST(DOMGeometry, CalculateInertial) // Capsule { sdf::Capsule capsule; - double l = 2.0; - double r = 0.1; + const double l = 2.0; + const double r = 0.1; capsule.SetLength(l); capsule.SetRadius(r); @@ -396,8 +396,8 @@ TEST(DOMGeometry, CalculateInertial) // Cylinder { sdf::Cylinder cylinder; - double l = 2.0; - double r = 0.1; + const double l = 2.0; + const double r = 0.1; cylinder.SetLength(l); cylinder.SetRadius(r); @@ -428,9 +428,9 @@ TEST(DOMGeometry, CalculateInertial) { sdf::Ellipsoid ellipsoid; - double a = 1.0; - double b = 10.0; - double c = 100.0; + const double a = 1.0; + const double b = 10.0; + const double c = 100.0; ellipsoid.SetRadii(gz::math::Vector3d(a, b, c)); @@ -460,7 +460,7 @@ TEST(DOMGeometry, CalculateInertial) // Sphere { sdf::Sphere sphere; - double r = 0.1; + const double r = 0.1; sphere.SetRadius(r); diff --git a/src/Sphere_TEST.cc b/src/Sphere_TEST.cc index 028dd645d..e7da830dc 100644 --- a/src/Sphere_TEST.cc +++ b/src/Sphere_TEST.cc @@ -145,13 +145,13 @@ TEST(DOMSphere, CalculateInertial) sdf::Sphere sphere; // density of aluminium - double density = 2170; + const double density = 2170; sphere.SetRadius(-2); auto invalidSphereInertial = sphere.CalculateInertial(density); ASSERT_EQ(std::nullopt, invalidSphereInertial); - double r = 0.1; + const double r = 0.1; sphere.SetRadius(r); diff --git a/src/World_TEST.cc b/src/World_TEST.cc index d1f202a2e..8fca6f514 100644 --- a/src/World_TEST.cc +++ b/src/World_TEST.cc @@ -549,9 +549,9 @@ TEST(DOMWorld, CalculateInertial) root.CalculateInertials(errors, sdfParserConfig); - double l = 2; - double w = 2; - double h = 2; + const double l = 2; + const double w = 2; + const double h = 2; double expectedMass = l*w*h * 1240.0; double ixx = (1.0/12.0) * expectedMass * (w*w + h*h); From 1986cfd815ed1107dfffec0147257d91d865d354 Mon Sep 17 00:00:00 2001 From: Jasmeet Singh Date: Wed, 30 Aug 2023 10:23:10 +0530 Subject: [PATCH 88/88] Rename CalculateInertials() API in Root, World, Model & Link Signed-off-by: Jasmeet Singh --- include/sdf/Link.hh | 2 +- include/sdf/Model.hh | 2 +- include/sdf/ParserConfig.hh | 10 +++++----- include/sdf/Root.hh | 2 +- include/sdf/World.hh | 2 +- src/Box_TEST.cc | 4 ++-- src/Collision_TEST.cc | 4 ++-- src/Cylinder_TEST.cc | 4 ++-- src/Ellipsoid_TEST.cc | 4 ++-- src/Link.cc | 4 ++-- src/Link_TEST.cc | 14 +++++++------- src/Model.cc | 6 +++--- src/ParserConfig.cc | 12 ++++++------ src/ParserConfig_TEST.cc | 6 +++--- src/Root.cc | 10 +++++----- src/Root_TEST.cc | 6 +++--- src/World.cc | 6 +++--- src/World_TEST.cc | 4 ++-- 18 files changed, 51 insertions(+), 51 deletions(-) diff --git a/include/sdf/Link.hh b/include/sdf/Link.hh index 37de66dc2..c92d8d18d 100644 --- a/include/sdf/Link.hh +++ b/include/sdf/Link.hh @@ -335,7 +335,7 @@ namespace sdf /// \param[out] _errors A vector of Errors object. Each object /// would contain an error code and an error message. /// \param[in] _config Custom parser configuration - public: void CalculateInertials(sdf::Errors &_errors, + public: void ResolveAutoInertials(sdf::Errors &_errors, const ParserConfig &_config); /// \brief Get the pose of the link. This is the pose of the link diff --git a/include/sdf/Model.hh b/include/sdf/Model.hh index 43fd49f84..86cc3d422 100644 --- a/include/sdf/Model.hh +++ b/include/sdf/Model.hh @@ -513,7 +513,7 @@ namespace sdf /// \param[out] _errrors A vector of Errors objects. Each errors contains an /// Error code and a message. An empty errors vector indicates no errors /// \param[in] _config Custom parser configuration - public: void CalculateInertials(sdf::Errors &_errors, + public: void ResolveAutoInertials(sdf::Errors &_errors, const ParserConfig &_config); /// \brief Give the scoped PoseRelativeToGraph to be used for resolving diff --git a/include/sdf/ParserConfig.hh b/include/sdf/ParserConfig.hh index 810541736..24fd4c6b9 100644 --- a/include/sdf/ParserConfig.hh +++ b/include/sdf/ParserConfig.hh @@ -48,10 +48,10 @@ enum class EnforcementPolicy LOG, }; -/// \enum ConfigureCalculateInertial +/// \enum ConfigureResolveAutoInertials /// \brief Configuration options of how CalculateInertial() function /// would be used -enum class ConfigureCalculateInertial +enum class ConfigureResolveAutoInertials { /// \brief If this value is used, CalculateInertial() won't be /// called from inside the Root::Load() function @@ -176,14 +176,14 @@ class SDFORMAT_VISIBLE ParserConfig /// \brief Get the current configuration for the CalculateInertial() /// function - /// \return Current set value of the ConfigureCalculateInertial enum - public: ConfigureCalculateInertial CalculateInertialConfiguration() const; + /// \return Current set value of the ConfigureResolveAutoInertials enum + public: ConfigureResolveAutoInertials CalculateInertialConfiguration() const; /// \brief Set the configuration for the CalculateInertial() function /// \param[in] _configuration The configuration to set for the /// CalculateInertial() function public: void SetCalculateInertialConfiguration( - ConfigureCalculateInertial _configuration); + ConfigureResolveAutoInertials _configuration); /// \brief Registers a custom model parser. /// \param[in] _modelParser Callback as described in diff --git a/include/sdf/Root.hh b/include/sdf/Root.hh index da4cdd35a..9ad5f1632 100644 --- a/include/sdf/Root.hh +++ b/include/sdf/Root.hh @@ -234,7 +234,7 @@ namespace sdf /// \param[out] _errors A vector of Errors objects. Each errors contains an /// Error code and a message. An empty errors vector indicates no errors /// \param[in] _config Custom parser configuration - public: void CalculateInertials(sdf::Errors &_errors, + public: void ResolveAutoInertials(sdf::Errors &_errors, const ParserConfig &_config); /// \brief Create and return an SDF element filled with data from this diff --git a/include/sdf/World.hh b/include/sdf/World.hh index 29747bcad..7e449abab 100644 --- a/include/sdf/World.hh +++ b/include/sdf/World.hh @@ -521,7 +521,7 @@ namespace sdf /// \param[out] _errrors A vector of Errors objects. Each errors contains an /// Error code and a message. An empty errors vector indicates no errors /// \param[in] _config Custom parser configuration - public: void CalculateInertials(sdf::Errors &_errors, + public: void ResolveAutoInertials(sdf::Errors &_errors, const ParserConfig &_config); /// \brief Give the Scoped PoseRelativeToGraph to be passed on to child diff --git a/src/Box_TEST.cc b/src/Box_TEST.cc index eb57aeaa5..9fb631242 100644 --- a/src/Box_TEST.cc +++ b/src/Box_TEST.cc @@ -158,8 +158,8 @@ TEST(DOMBox, CalculateInertial) // density of aluminium double density = 2710; - // Invalid dimensions leading to std::nullopt return - // CalculateInertials() + // Invalid dimensions leading to std::nullopt return in + // CalculateInertial() box.SetSize(gz::math::Vector3d(-1, 1, 0)); auto invalidBoxInertial = box.CalculateInertial(density); ASSERT_EQ(std::nullopt, invalidBoxInertial); diff --git a/src/Collision_TEST.cc b/src/Collision_TEST.cc index 996912f22..b6aed7ec0 100644 --- a/src/Collision_TEST.cc +++ b/src/Collision_TEST.cc @@ -242,7 +242,7 @@ TEST(DOMCollision, CorrectBoxCollisionCalculateInertial) const sdf::Collision *collision = link->CollisionByIndex(0); sdf::Errors inertialErr; - root.CalculateInertials(inertialErr, sdfParserConfig); + root.ResolveAutoInertials(inertialErr, sdfParserConfig); const double l = 2; const double w = 2; @@ -305,7 +305,7 @@ TEST(DOMCollision, CalculateInertialPoseNotRelativeToLink) const sdf::Collision *collision = link->CollisionByIndex(0); sdf::Errors inertialErr; - root.CalculateInertials(inertialErr, sdfParserConfig); + root.ResolveAutoInertials(inertialErr, sdfParserConfig); const double l = 2; const double w = 2; diff --git a/src/Cylinder_TEST.cc b/src/Cylinder_TEST.cc index 018c4afd7..5c7d04ead 100644 --- a/src/Cylinder_TEST.cc +++ b/src/Cylinder_TEST.cc @@ -190,8 +190,8 @@ TEST(DOMCylinder, CalculateInertial) // density of aluminium const double density = 2170; - // Invalid dimensions leading to std::nullopt return - // CalculateInertials() + // Invalid dimensions leading to std::nullopt return in + // CalculateInertial() cylinder.SetLength(-1); cylinder.SetRadius(0); auto invalidCylinderInertial = cylinder.CalculateInertial(density); diff --git a/src/Ellipsoid_TEST.cc b/src/Ellipsoid_TEST.cc index 2a6fec0cd..cdba939b9 100644 --- a/src/Ellipsoid_TEST.cc +++ b/src/Ellipsoid_TEST.cc @@ -154,8 +154,8 @@ TEST(DOMEllipsoid, CalculateInertial) // density of aluminium const double density = 2170; - // Invalid dimensions leading to std::nullopt return - // CalculateInertials() + // Invalid dimensions leading to std::nullopt return in + // CalculateInertial() ellipsoid.SetRadii(gz::math::Vector3d(-1, 2, 0)); auto invalidEllipsoidInertial = ellipsoid.CalculateInertial(density); ASSERT_EQ(std::nullopt, invalidEllipsoidInertial); diff --git a/src/Link.cc b/src/Link.cc index 883028ead..7d8d5a182 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -597,7 +597,7 @@ Errors Link::ResolveInertial( } ///////////////////////////////////////////////// -void Link::CalculateInertials(sdf::Errors &_errors, const ParserConfig &_config) +void Link::ResolveAutoInertials(sdf::Errors &_errors, const ParserConfig &_config) { // If inertial calculations is set to automatic & the inertial values for the // link was not saved previously @@ -627,7 +627,7 @@ void Link::CalculateInertials(sdf::Errors &_errors, const ParserConfig &_config) // If CalculateInertial() was called with SAVE_CALCULATION // configuration then set autoInertiaSaved to true if (_config.CalculateInertialConfiguration() == - ConfigureCalculateInertial::SAVE_CALCULATION) + ConfigureResolveAutoInertials::SAVE_CALCULATION) { this->dataPtr->autoInertiaSaved = true; } diff --git a/src/Link_TEST.cc b/src/Link_TEST.cc index 0e0f23efe..524d003f5 100644 --- a/src/Link_TEST.cc +++ b/src/Link_TEST.cc @@ -279,7 +279,7 @@ TEST(DOMLink, InvalidInertia) } ///////////////////////////////////////////////// -TEST(DOMLink, CalculateInertialWithNoCollisionsInLink) +TEST(DOMLink, ResolveAutoInertialsWithNoCollisionsInLink) { std::string sdf = "" " " @@ -298,7 +298,7 @@ TEST(DOMLink, CalculateInertialWithNoCollisionsInLink) const sdf::Model *model = root.Model(); const sdf::Link *link = model->LinkByIndex(0); - root.CalculateInertials(errors, sdfParserConfig); + root.ResolveAutoInertials(errors, sdfParserConfig); ASSERT_FALSE(errors.empty()); // Default Inertial values set during load @@ -308,7 +308,7 @@ TEST(DOMLink, CalculateInertialWithNoCollisionsInLink) } ///////////////////////////////////////////////// -TEST(DOMLink, CalculateInertialWithMultipleCollisions) +TEST(DOMLink, ResolveAutoInertialsWithMultipleCollisions) { std::string sdf = "" "" @@ -347,7 +347,7 @@ TEST(DOMLink, CalculateInertialWithMultipleCollisions) const sdf::Model *model = root.Model(); const sdf::Link *link = model->LinkByIndex(0); - root.CalculateInertials(errors, sdfParserConfig); + root.ResolveAutoInertials(errors, sdfParserConfig); EXPECT_TRUE(errors.empty()); // Mass of cube(volume * density) + mass of cylinder(volume * density) @@ -394,7 +394,7 @@ TEST(DOMLink, InertialValuesGivenWithAutoSetToTrue) const sdf::Model *model = root.Model(); const sdf::Link *link = model->LinkByIndex(0); - root.CalculateInertials(errors, sdfParserConfig); + root.ResolveAutoInertials(errors, sdfParserConfig); EXPECT_TRUE(errors.empty()); EXPECT_NE(4.0, link->Inertial().MassMatrix().Mass()); @@ -404,7 +404,7 @@ TEST(DOMLink, InertialValuesGivenWithAutoSetToTrue) } ///////////////////////////////////////////////// -TEST(DOMLink, CalculateInertialCalledWithAutoFalse) +TEST(DOMLink, ResolveAutoInertialsCalledWithAutoFalse) { std::string sdf = "" " " @@ -423,7 +423,7 @@ TEST(DOMLink, CalculateInertialCalledWithAutoFalse) const sdf::Model *model = root.Model(); const sdf::Link *link = model->LinkByIndex(0); - root.CalculateInertials(errors, sdfParserConfig); + root.ResolveAutoInertials(errors, sdfParserConfig); EXPECT_TRUE(errors.empty()); // Default Inertial values set during load diff --git a/src/Model.cc b/src/Model.cc index 6d7312640..944e50b87 100644 --- a/src/Model.cc +++ b/src/Model.cc @@ -906,19 +906,19 @@ void Model::SetPoseRelativeTo(const std::string &_frame) } ///////////////////////////////////////////////// -void Model::CalculateInertials(sdf::Errors &_errors, +void Model::ResolveAutoInertials(sdf::Errors &_errors, const ParserConfig &_config) { // Loop through all the nested models, if there are any for (sdf::Model &model : this->dataPtr->models) { - model.CalculateInertials(_errors, _config); + model.ResolveAutoInertials(_errors, _config); } // Calculate and set inertials for all the links in the model for (sdf::Link &link : this->dataPtr->links) { - link.CalculateInertials(_errors, _config); + link.ResolveAutoInertials(_errors, _config); } } diff --git a/src/ParserConfig.cc b/src/ParserConfig.cc index 57b3c5e7f..96059afc6 100644 --- a/src/ParserConfig.cc +++ b/src/ParserConfig.cc @@ -46,8 +46,8 @@ class sdf::ParserConfig::Implementation /// \brief Configuration that is set for the CalculateInertial() function /// By default it is set to SKIP_CALCULATION_IN_LOAD which would cause /// Root::Load() to not call CalculateInertial() - public: ConfigureCalculateInertial calculateInertialConfiguration = - ConfigureCalculateInertial::SKIP_CALCULATION_IN_LOAD; + public: ConfigureResolveAutoInertials resolveAutoInertialsConfig = + ConfigureResolveAutoInertials::SKIP_CALCULATION_IN_LOAD; /// \brief Collection of custom model parsers. public: std::vector customParsers; @@ -164,16 +164,16 @@ EnforcementPolicy ParserConfig::DeprecatedElementsPolicy() const } ///////////////////////////////////////////////// -ConfigureCalculateInertial ParserConfig::CalculateInertialConfiguration() const +ConfigureResolveAutoInertials ParserConfig::CalculateInertialConfiguration() const { - return this->dataPtr->calculateInertialConfiguration; + return this->dataPtr->resolveAutoInertialsConfig; } ///////////////////////////////////////////////// void ParserConfig::SetCalculateInertialConfiguration( - ConfigureCalculateInertial _configuration) + ConfigureResolveAutoInertials _configuration) { - this->dataPtr->calculateInertialConfiguration = _configuration; + this->dataPtr->resolveAutoInertialsConfig = _configuration; } ///////////////////////////////////////////////// diff --git a/src/ParserConfig_TEST.cc b/src/ParserConfig_TEST.cc index 5c90041d8..00add6e3b 100644 --- a/src/ParserConfig_TEST.cc +++ b/src/ParserConfig_TEST.cc @@ -57,11 +57,11 @@ TEST(ParserConfig, Construction) EXPECT_EQ(sdf::EnforcementPolicy::WARN, config.UnrecognizedElementsPolicy()); EXPECT_EQ(sdf::EnforcementPolicy::WARN, config.DeprecatedElementsPolicy()); - EXPECT_EQ(sdf::ConfigureCalculateInertial::SKIP_CALCULATION_IN_LOAD, + EXPECT_EQ(sdf::ConfigureResolveAutoInertials::SKIP_CALCULATION_IN_LOAD, config.CalculateInertialConfiguration()); config.SetCalculateInertialConfiguration( - sdf::ConfigureCalculateInertial::SAVE_CALCULATION); - EXPECT_EQ(sdf::ConfigureCalculateInertial::SAVE_CALCULATION, + sdf::ConfigureResolveAutoInertials::SAVE_CALCULATION); + EXPECT_EQ(sdf::ConfigureResolveAutoInertials::SAVE_CALCULATION, config.CalculateInertialConfiguration()); EXPECT_FALSE(config.URDFPreserveFixedJoint()); diff --git a/src/Root.cc b/src/Root.cc index 960d62a2a..7a37f1466 100644 --- a/src/Root.cc +++ b/src/Root.cc @@ -394,9 +394,9 @@ Errors Root::Load(SDFPtr _sdf, const ParserConfig &_config) // Check if CalculateInertialConfiguration() is not set to skip in load if (_config.CalculateInertialConfiguration() != - ConfigureCalculateInertial::SKIP_CALCULATION_IN_LOAD) + ConfigureResolveAutoInertials::SKIP_CALCULATION_IN_LOAD) { - this->CalculateInertials(errors, _config); + this->ResolveAutoInertials(errors, _config); } return errors; @@ -598,19 +598,19 @@ void Root::Implementation::UpdateGraphs(sdf::Model &_model, } ///////////////////////////////////////////////// -void Root::CalculateInertials(sdf::Errors &_errors, const ParserConfig &_config) +void Root::ResolveAutoInertials(sdf::Errors &_errors, const ParserConfig &_config) { // Calculate and set Inertials for all the worlds for (sdf::World &world : this->dataPtr->worlds) { - world.CalculateInertials(_errors, _config); + world.ResolveAutoInertials(_errors, _config); } // Calculate and set Inertials for the model, if it is present if (std::holds_alternative(this->dataPtr->modelLightOrActor)) { sdf::Model &model = std::get(this->dataPtr->modelLightOrActor); - model.CalculateInertials(_errors, _config); + model.ResolveAutoInertials(_errors, _config); } } diff --git a/src/Root_TEST.cc b/src/Root_TEST.cc index c7814316c..d48382276 100644 --- a/src/Root_TEST.cc +++ b/src/Root_TEST.cc @@ -325,7 +325,7 @@ TEST(DOMRoot, FrameSemanticsOnMove) } ///////////////////////////////////////////////// -TEST(DOMRoot, CalculateInertialWithSaveCalculationConfiguration) +TEST(DOMRoot, ResolveAutoInertialsWithSaveCalculationConfiguration) { std::string sdf = "" " " @@ -354,10 +354,10 @@ TEST(DOMRoot, CalculateInertialWithSaveCalculationConfiguration) const sdf::Link *link = model->LinkByIndex(0); sdfParserConfig.SetCalculateInertialConfiguration( - sdf::ConfigureCalculateInertial::SAVE_CALCULATION); + sdf::ConfigureResolveAutoInertials::SAVE_CALCULATION); sdf::Errors inertialErr; - root.CalculateInertials(inertialErr, sdfParserConfig); + root.ResolveAutoInertials(inertialErr, sdfParserConfig); EXPECT_TRUE(inertialErr.empty()); ASSERT_TRUE(link->AutoInertiaSaved()); } diff --git a/src/World.cc b/src/World.cc index 9c413113f..d94ff685a 100644 --- a/src/World.cc +++ b/src/World.cc @@ -860,13 +860,13 @@ const NestedInclude *World::InterfaceModelNestedIncludeByIndex( } ///////////////////////////////////////////////// -void World::CalculateInertials(sdf::Errors &_errors, +void World::ResolveAutoInertials(sdf::Errors &_errors, const ParserConfig &_config) { - // Call CalculateInertials() function for all the models + // Call ResolveAutoInertials() function for all the models for (sdf::Model &model : this->dataPtr->models) { - model.CalculateInertials(_errors, _config); + model.ResolveAutoInertials(_errors, _config); } } diff --git a/src/World_TEST.cc b/src/World_TEST.cc index 8fca6f514..3100ce166 100644 --- a/src/World_TEST.cc +++ b/src/World_TEST.cc @@ -516,7 +516,7 @@ TEST(DOMWorld, AddLight) } ///////////////////////////////////////////////// -TEST(DOMWorld, CalculateInertial) +TEST(DOMWorld, ResolveAutoInertials) { std::string sdf = "" " " @@ -547,7 +547,7 @@ TEST(DOMWorld, CalculateInertial) const sdf::Model *model = world->ModelByIndex(0); const sdf::Link *link = model->LinkByIndex(0); - root.CalculateInertials(errors, sdfParserConfig); + root.ResolveAutoInertials(errors, sdfParserConfig); const double l = 2; const double w = 2;