From 879b26e42bd7b38dc15ea6e14f733ae1daac2d6c Mon Sep 17 00:00:00 2001 From: Taiju Yamada Date: Wed, 23 Aug 2023 12:26:32 +0900 Subject: [PATCH] Eliminate C array from members --- .../mujincontrollerclient/binpickingtask.h | 4 +- .../mujincontrollerclient.h | 78 +++++++++---------- src/controllerclientimpl.cpp | 2 +- src/controllerclientimpl.h | 2 +- src/mujincontrollerclient.cpp | 6 +- 5 files changed, 46 insertions(+), 46 deletions(-) diff --git a/include/mujincontrollerclient/binpickingtask.h b/include/mujincontrollerclient/binpickingtask.h index 4eba5142..ecaa1b78 100644 --- a/include/mujincontrollerclient/binpickingtask.h +++ b/include/mujincontrollerclient/binpickingtask.h @@ -28,8 +28,8 @@ namespace mujinclient { /// Margins of the container to be cropped (or enlarged if negative), in order to define 3D container region under (calibration & shape) uncertainty - for pointcloud processing. struct CropContainerMarginsXYZXYZ { - double minMargins[3]; ///< XYZ of how much to crop from min margins (value > 0 means crop inside) - double maxMargins[3]; ///< XYZ of how much to crop from min margins (value > 0 means crop inside) + std::array minMargins; ///< XYZ of how much to crop from min margins (value > 0 means crop inside) + std::array maxMargins; ///< XYZ of how much to crop from min margins (value > 0 means crop inside) }; typedef boost::shared_ptr CropContainerMarginsXYZXYZPtr; diff --git a/include/mujincontrollerclient/mujincontrollerclient.h b/include/mujincontrollerclient/mujincontrollerclient.h index a49764f0..f233bf4f 100644 --- a/include/mujincontrollerclient/mujincontrollerclient.h +++ b/include/mujincontrollerclient/mujincontrollerclient.h @@ -258,9 +258,9 @@ struct RobotPlacementOptimizationParameters } std::string targetname; ///< the name of the target object to optimize for. Cannot be blank. Has to start with "0 instobject " is targetting an environment instance object. For Example "0 instobject myrobot". std::string framename; ///< The name of the frame to define the optimization parameters in. If blank, will use the targetname's coordinate system. For environment inst object frames, has to be "0 instobject mytargetname" - Real maxrange[4]; ///< X, Y, Z, Angle (deg) - Real minrange[4]; ///< X, Y, Z, Angle (deg) - Real stepsize[4]; ///< X, Y, Z, Angle (deg) + std::array maxrange; ///< X, Y, Z, Angle (deg) + std::array minrange; ///< X, Y, Z, Angle (deg) + std::array stepsize; ///< X, Y, Z, Angle (deg) int ignorebasecollision; ///< If 1, when moving the robot, allow collisions of the base with the environment, this allows users to search for a base placement and while ignoring small obstacles. By default this is 0. std::string unit; ///< the unit that information is used in. m, mm, nm, inch, etc @@ -289,12 +289,12 @@ struct PlacementsOptimizationParameters } // for every target, there's one setting: - boost::array targetnames; ///< the primary key of the target object to optimize for. If blank, will use robot. Has to start with "0 instobject " for environment inst object targets. - boost::array framenames; ///< The primary key of the frame to define the optimization parameters in. If blank, will use the target's coordinate system. Has to start iwth "0 instobject " for environment inst object frames. - boost::array maxranges; ///< X, Y, Z, Angle (deg) - boost::array minranges; ///< X, Y, Z, Angle (deg) - boost::array stepsizes; ///< X, Y, Z, Angle (deg) - boost::array ignorebasecollisions; ///< If 1, when moving the robot, allow collisions of the base with the environment, this allows users to search for a base placement and while ignoring small obstacles. By default this is 0. + std::array targetnames; ///< the primary key of the target object to optimize for. If blank, will use robot. Has to start with "0 instobject " for environment inst object targets. + std::array framenames; ///< The primary key of the frame to define the optimization parameters in. If blank, will use the target's coordinate system. Has to start iwth "0 instobject " for environment inst object frames. + std::array,2> maxranges; ///< X, Y, Z, Angle (deg) + std::array,2> minranges; ///< X, Y, Z, Angle (deg) + std::array,2> stepsizes; ///< X, Y, Z, Angle (deg) + std::array ignorebasecollisions; ///< If 1, when moving the robot, allow collisions of the base with the environment, this allows users to search for a base placement and while ignoring small obstacles. By default this is 0. // shared settings std::string unit; ///< the unit that information is used in. m, mm, nm, inch, etc @@ -757,12 +757,12 @@ class MUJINCLIENT_API ObjectResource : public WebResource std::string objectpk; std::string linkpk; std::string geomtype; - Real quaternion[4]; // quaternion [w, x, y, z] = [cos(angle/2), sin(angle/2)*rotation_axis] - Real translate[3]; + std::array quaternion; // quaternion [w, x, y, z] = [cos(angle/2), sin(angle/2)*rotation_axis] + std::array translate; bool visible; - Real diffusecolor[4]; + std::array diffusecolor; Real transparency; - Real half_extents[3]; + std::array half_extents; Real height; Real radius; Real topRadius; @@ -784,9 +784,9 @@ class MUJINCLIENT_API ObjectResource : public WebResource std::string name; std::string pk; std::string iktype; - Real quaternion[4]; // quaternion [w, x, y, z] = [cos(angle/2), sin(angle/2)*rotation_axis] - Real translation[3]; - Real direction[3]; + std::array quaternion; // quaternion [w, x, y, z] = [cos(angle/2), sin(angle/2)*rotation_axis] + std::array translation; + std::array direction; Real angle; }; typedef boost::shared_ptr IkParamResourcePtr; @@ -804,7 +804,7 @@ class MUJINCLIENT_API ObjectResource : public WebResource virtual void GetGeometries(std::vector& links); - virtual boost::shared_ptr AddChildLink(const std::string& name, const Real quaternion[4], const Real translate[3]); + virtual boost::shared_ptr AddChildLink(const std::string& name, const std::array &quaternion, const std::array &translate); virtual void SetCollision(bool collision); /// 0 -> off, 1 -> on @@ -818,8 +818,8 @@ class MUJINCLIENT_API ObjectResource : public WebResource std::string pk; std::string objectpk; std::string parentlinkpk; - Real quaternion[4]; // quaternion [w, x, y, z] = [cos(angle/2), sin(angle/2)*rotation_axis] - Real translate[3]; + std::array quaternion; // quaternion [w, x, y, z] = [cos(angle/2), sin(angle/2)*rotation_axis] + std::array translate; bool collision; }; typedef boost::shared_ptr LinkResourcePtr; @@ -832,7 +832,7 @@ class MUJINCLIENT_API ObjectResource : public WebResource virtual void GetIkParams(std::vector& ikparams); - virtual LinkResourcePtr AddLink(const std::string& name, const Real quaternion[4], const Real translate[3]); + virtual LinkResourcePtr AddLink(const std::string& name, const std::array &quaternion, const std::array &translate); virtual IkParamResourcePtr AddIkParam(const std::string& name, const std::string& iktype); virtual void SetCollision(bool collision); @@ -852,8 +852,8 @@ class MUJINCLIENT_API ObjectResource : public WebResource std::string scenepk; std::string unit; std::string uri; - Real quaternion[4]; // quaternion [w, x, y, z] = [cos(angle/2), sin(angle/2)*rotation_axis] - Real translate[3]; + std::array quaternion; // quaternion [w, x, y, z] = [cos(angle/2), sin(angle/2)*rotation_axis] + std::array translate; protected: ObjectResource(ControllerClientPtr controller, const std::string& resource, const std::string& pk); @@ -888,7 +888,7 @@ class MUJINCLIENT_API RobotResource : public ObjectResource std::string name; std::string frame_origin; std::string pk; - //Real direction[3]; + //std::array direction; std::array quaternion; // quaternion [w, x, y, z] = [cos(angle/2), sin(angle/2)*rotation_axis] std::array translate; std::string sensortype; @@ -896,22 +896,22 @@ class MUJINCLIENT_API RobotResource : public ObjectResource struct SensorData { public: bool operator!=(const SensorData& other) const { - return std::memcmp(distortion_coeffs, other.distortion_coeffs, 5 * sizeof(Real)) != 0 || + return distortion_coeffs != other.distortion_coeffs || distortion_model != other.distortion_model || focal_length != other.focal_length || - std::memcmp(image_dimensions, other.image_dimensions, 3 * sizeof(int)) != 0 || - std::memcmp(intrinsic, other.intrinsic, 6 * sizeof(Real)) != 0 || + image_dimensions != other.image_dimensions || + intrinsic != other.intrinsic || measurement_time != other.measurement_time || extra_parameters != other.extra_parameters; } bool operator==(const SensorData& other) const { return !operator!=(other); } - Real distortion_coeffs[5]; + std::array distortion_coeffs; std::string distortion_model; Real focal_length; - int image_dimensions[3]; - Real intrinsic[6]; + std::array image_dimensions; + std::array intrinsic; Real measurement_time; std::vector extra_parameters; }; @@ -950,16 +950,16 @@ class MUJINCLIENT_API SceneResource : public WebResource class MUJINCLIENT_API Link { public: std::string name; - Real quaternion[4]; // quaternion [w, x, y, z] = [cos(angle/2), sin(angle/2)*rotation_axis] - Real translate[3]; + std::array quaternion; // quaternion [w, x, y, z] = [cos(angle/2), sin(angle/2)*rotation_axis] + std::array translate; }; class MUJINCLIENT_API Tool { public: std::string name; - Real direction[3]; - Real quaternion[4]; // quaternion [w, x, y, z] = [cos(angle/2), sin(angle/2)*rotation_axis] - Real translate[3]; + std::array direction; + std::array quaternion; // quaternion [w, x, y, z] = [cos(angle/2), sin(angle/2)*rotation_axis] + std::array translate; }; class MUJINCLIENT_API Grab { @@ -985,8 +985,8 @@ class MUJINCLIENT_API SceneResource : public WebResource class MUJINCLIENT_API AttachedSensor { public: std::string name; - Real quaternion[4]; // quaternion [w, x, y, z] = [cos(angle/2), sin(angle/2)*rotation_axis] - Real translate[3]; + std::array quaternion; // quaternion [w, x, y, z] = [cos(angle/2), sin(angle/2)*rotation_axis] + std::array translate; }; void SetTransform(const Transform& t); @@ -1001,8 +1001,8 @@ class MUJINCLIENT_API SceneResource : public WebResource std::string object_pk; std::string reference_object_pk; std::string reference_uri; - Real quaternion[4]; // quaternion [w, x, y, z] = [cos(angle/2), sin(angle/2)*rotation_axis] - Real translate[3]; + std::array quaternion; // quaternion [w, x, y, z] = [cos(angle/2), sin(angle/2)*rotation_axis] + std::array translate; std::vector grabs; std::vector links; std::vector tools; @@ -1069,7 +1069,7 @@ class MUJINCLIENT_API SceneResource : public WebResource /// \param quaternion quaternion of the object /// \param translate translation of the object /// \return pointer to inst object created - virtual SceneResource::InstObjectPtr CreateInstObject(const std::string& name, const std::string& referenceUri, const Real quaternion[4], const Real translate[3], double timeout = 300); + virtual SceneResource::InstObjectPtr CreateInstObject(const std::string& name, const std::string& referenceUri, const std::array &quaternion, const std::array &translate, double timeout = 300); /// \brief deletes an inst object in scene /// \param pk primary key of the object to delete diff --git a/src/controllerclientimpl.cpp b/src/controllerclientimpl.cpp index 459c6efc..d7879e1a 100644 --- a/src/controllerclientimpl.cpp +++ b/src/controllerclientimpl.cpp @@ -1076,7 +1076,7 @@ std::string ControllerClientImpl::CreateIkParam(const std::string& objectPk, con return GetJsonValueByKey(pt, "pk"); } -std::string ControllerClientImpl::CreateLink(const std::string& objectPk, const std::string& parentlinkPk, const std::string& name, const Real quaternion[4], const Real translate[3], double timeout) +std::string ControllerClientImpl::CreateLink(const std::string& objectPk, const std::string& parentlinkPk, const std::string& name, const std::array &quaternion, const std::array &translate, double timeout) { rapidjson::Document pt(rapidjson::kObjectType); std::string data(str(boost::format("{\"name\":\"%s\", \"quaternion\":[%.15f,%.15f,%.15f,%.15f], \"translate\":[%.15f,%.15f,%.15f]")%name%quaternion[0]%quaternion[1]%quaternion[2]%quaternion[3]%translate[0]%translate[1]%translate[2])); diff --git a/src/controllerclientimpl.h b/src/controllerclientimpl.h index 87b636e5..e90f31c0 100644 --- a/src/controllerclientimpl.h +++ b/src/controllerclientimpl.h @@ -156,7 +156,7 @@ class ControllerClientImpl : public ControllerClient, public boost::enable_share std::string CreateObjectGeometry(const std::string& objectPk, const std::string& geometryName, const std::string& linkPk, const std::string& geomtype, double timeout = 5); std::string CreateIkParam(const std::string& objectPk, const std::string& name, const std::string& iktype, double timeout = 5); - std::string CreateLink(const std::string& objectPk, const std::string& parentlinkPk, const std::string& name, const Real quaternion[4], const Real translate[3], double timeout = 5); + std::string CreateLink(const std::string& objectPk, const std::string& parentlinkPk, const std::string& name, const std::array &quaternion, const std::array &translate, double timeout = 5); /// \brief set geometry for an object /// \param objectPk primary key for the object diff --git a/src/mujincontrollerclient.cpp b/src/mujincontrollerclient.cpp index 09eb3356..2cc45c0d 100644 --- a/src/mujincontrollerclient.cpp +++ b/src/mujincontrollerclient.cpp @@ -444,7 +444,7 @@ void ObjectResource::GetLinks(std::vector& link } } -ObjectResource::LinkResourcePtr ObjectResource::AddLink(const std::string& objname, const Real quaternion_[4], const Real translate_[3]) +ObjectResource::LinkResourcePtr ObjectResource::AddLink(const std::string& objname, const std::array &quaternion_, const std::array &translate_) { GETCONTROLLERIMPL(); const std::string linkPk = controller->CreateLink(this->pk, "", objname, quaternion_, translate_); @@ -455,7 +455,7 @@ ObjectResource::LinkResourcePtr ObjectResource::AddLink(const std::string& objna return link; } -ObjectResource::LinkResourcePtr ObjectResource::LinkResource::AddChildLink(const std::string& objname, const Real quaternion_[4], const Real translate_[3]) +ObjectResource::LinkResourcePtr ObjectResource::LinkResource::AddChildLink(const std::string& objname, const std::array &quaternion_, const std::array &translate_) { GETCONTROLLERIMPL(); const std::string linkPk = controller->CreateLink(this->objectpk, this->pk, objname, quaternion_, translate_); @@ -1017,7 +1017,7 @@ bool SceneResource::FindInstObject(const std::string& name, SceneResource::InstO return false; } -SceneResource::InstObjectPtr SceneResource::CreateInstObject(const std::string& name, const std::string& referenceUri, const Real quaternion[4], const Real translate[3], double timeout) +SceneResource::InstObjectPtr SceneResource::CreateInstObject(const std::string& name, const std::string& referenceUri, const std::array &quaternion, const std::array &translate, double timeout) { GETCONTROLLERIMPL(); const std::string uri(str(boost::format("scene/%s/instobject/?format=json&fields=pk,object_pk,reference_object_pk,reference_uri,dofvalues,quaternion,translate")%GetPrimaryKey()));