diff --git a/include/mujincontrollerclient/binpickingtask.h b/include/mujincontrollerclient/binpickingtask.h index ddcf4ef4..f11606d6 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) + double minMargins[3] = {0, 0, 0}; ///< XYZ of how much to crop from min margins (value > 0 means crop inside) + double maxMargins[3] = {0, 0, 0}; ///< XYZ of how much to crop from min margins (value > 0 means crop inside) }; typedef boost::shared_ptr CropContainerMarginsXYZXYZPtr; @@ -56,24 +56,24 @@ class MUJINCLIENT_API BinPickingTaskResource : public TaskResource std::string object_uri; // "object_uri": "mujin:/box0.mujin.dae" Transform transform; ///< pose of the object in the world coordinate system std::string confidence; - unsigned long long timestamp; ///< sensor timestamp in ms (from Linux epoch) + unsigned long long timestamp = 0; ///< sensor timestamp in ms (from Linux epoch) std::string extra; // (OPTIONAL) "extra": {"type":"randombox", "length":100, "width":100, "height":100} - bool isPickable; ///< whether the object is pickable + bool isPickable = false; ///< whether the object is pickable }; struct MUJINCLIENT_API PointCloudObstacle { std::string name; std::vector points; ///< consecutive x,y,z values in meter - Real pointsize; ///< size of each point in meter + Real pointsize = 0; ///< size of each point in meter }; struct MUJINCLIENT_API SensorOcclusionCheck { std::string bodyname; ///< name of body whose visibility is to be checked std::string cameraname; ///< name of camera - unsigned long long starttime; ///< milisecond - unsigned long long endtime; ///< milisecond + unsigned long long starttime = 0; ///< milisecond + unsigned long long endtime = 0; ///< milisecond }; struct MUJINCLIENT_API ResultBase @@ -86,8 +86,8 @@ class MUJINCLIENT_API BinPickingTaskResource : public TaskResource { AddPointOffsetInfo() : zOffsetAtBottom(0), zOffsetAtTop(0) { } - double zOffsetAtBottom; - double zOffsetAtTop; + double zOffsetAtBottom = 0; + double zOffsetAtTop = 0; }; typedef boost::shared_ptr AddPointOffsetInfoPtr; @@ -106,7 +106,7 @@ class MUJINCLIENT_API BinPickingTaskResource : public TaskResource virtual ~ResultMoveJoints(); void Parse(const rapidjson::Value& pt); std::string robottype; - int numpoints; + int numpoints = 0; std::vector timedjointvalues; //Real elapsedtime; }; @@ -122,14 +122,14 @@ class MUJINCLIENT_API BinPickingTaskResource : public TaskResource { void Parse(const rapidjson::Value& pt); std::vector translation; - std::vector quaternion; + std::vector quaternion = {1, 0, 0, 0}; std::vector direction; - Real angleXZ; - Real angleYX; - Real angleZY; - Real angleX; - Real angleY; - Real angleZ; + Real angleXZ = 0; + Real angleYX = 0; + Real angleZY = 0; + Real angleX = 0; + Real angleY = 0; + Real angleZ = 0; }; struct MUJINCLIENT_API ResultComputeIKFromParameters : public ResultBase @@ -158,7 +158,7 @@ class MUJINCLIENT_API BinPickingTaskResource : public TaskResource { mujin::SensorSelectionInfo sensorSelectionInfo; std::string bodyname; - int isocclusion; + int isocclusion = 0; }; ResultGetBinpickingState(); @@ -167,24 +167,24 @@ class MUJINCLIENT_API BinPickingTaskResource : public TaskResource std::string statusPickPlace; std::string statusDescPickPlace; std::string statusPhysics; - bool isDynamicEnvironmentStateEmpty; + bool isDynamicEnvironmentStateEmpty = true; - int pickAttemptFromSourceId; - unsigned long long timestamp; ///< ms - unsigned long long lastGrabbedTargetTimeStamp; ///< ms + int pickAttemptFromSourceId = 0; + unsigned long long timestamp = 0; ///< ms + unsigned long long lastGrabbedTargetTimeStamp = 0; ///< ms //bool isRobotOccludingSourceContainer; ///? std::vector vOcclusionResults; std::vector activeLocationTrackingInfos; std::vector locationExecutionInfos; - bool isGrabbingTarget; - bool isGrabbingLastTarget; + bool isGrabbingTarget = false; + bool isGrabbingLastTarget = false; bool hasRobotExecutionStarted=false; - int orderNumber; ///< -1 if not initiaized - int numLeftInOrder; ///< -1 if not initiaized - int numLeftInSupply; ///< -1 if not initiaized - int placedInDest; ///< -1 if not initiaized + int orderNumber = -1; ///< -1 if not initiaized + int numLeftInOrder = -1; ///< -1 if not initiaized + int numLeftInSupply = -1; ///< -1 if not initiaized + int placedInDest = -1; ///< -1 if not initiaized std::string cycleIndex; ///< index of the published cycle that pickworker is currently executing /** @@ -206,32 +206,32 @@ class MUJINCLIENT_API BinPickingTaskResource : public TaskResource struct MinViableRegionInfo { MinViableRegionInfo(); - std::array size2D; ///< width and length on the MVR - std::array maxPossibleSize; ///< the max possible size of actual item - std::array maxPossibleSizeOriginal; ///< the maxPossibleSize that has originally been given from vision - uint8_t cornerMaskOriginal; ///< the cornerMask that has originally been given from vision - uint8_t cornerMask; ///< Represents the corner(s) used for corner based detection. 4 bit. -x-y = 1, +x-y = 2, -x+y = 4, +x+y = 8 + std::array size2D = {0, 0}; ///< width and length on the MVR + std::array maxPossibleSize = {0, 0, 0}; ///< the max possible size of actual item + std::array maxPossibleSizeOriginal = {0, 0, 0}; ///< the maxPossibleSize that has originally been given from vision + uint8_t cornerMaskOriginal = 0; ///< the cornerMask that has originally been given from vision + uint8_t cornerMask = 0; ///< Represents the corner(s) used for corner based detection. 4 bit. -x-y = 1, +x-y = 2, -x+y = 4, +x+y = 8 } minViableRegion; std::string locationName; ///< The name of the location where the minViableRegion was triggered for - std::array translation; ///< Translation of the 2D MVR plane (height = 0) - std::array quaternion; ///< Rotation of the 2D MVR plane (height = 0) - double objectWeight; ///< If non-zero, use this weight fo registration. unit is kg. zero means unknown. + std::array translation = {0, 0, 0}; ///< Translation of the 2D MVR plane (height = 0) + std::array quaternion = {1, 0, 0, 0}; ///< Rotation of the 2D MVR plane (height = 0) + double objectWeight = 0; ///< If non-zero, use this weight fo registration. unit is kg. zero means unknown. std::string objectType; ///< The type of the object - uint64_t sensorTimeStampMS; ///< Same as DetectedObject's timestamp sent to planning - double robotDepartStopTimestamp; ///< Force capture after robot stops - std::array liftedWorldOffset; ///< [dx, dy, dz], mm in world frame - std::array maxCandidateSize; ///< the max candidate size expecting - std::array minCandidateSize; ///< the min candidate size expecting - double transferSpeedPostMult; ///< transfer speed multiplication factor + uint64_t sensorTimeStampMS = 0; ///< Same as DetectedObject's timestamp sent to planning + double robotDepartStopTimestamp = 0; ///< Force capture after robot stops + std::array liftedWorldOffset = {0, 0, 0}; ///< [dx, dy, dz], mm in world frame + std::array maxCandidateSize = {0, 0, 0}; ///< the max candidate size expecting + std::array minCandidateSize = {0, 0, 0}; ///< the min candidate size expecting + double transferSpeedPostMult = 0; ///< transfer speed multiplication factor rapidjson::Document graspModelInfo; ///< Parameters used for grasping model generation for the object - double minCornerVisibleDist; ///< how much distance along with uncertain edge from uncertain corner robot exposes to camera - double minCornerVisibleInsideDist; ///< how much distance inside MVR robot exposes to camera - double maxCornerAngleDeviation; ///< how much angle deviation around uncertain corner is considered to expose to camera - uint8_t occlusionFreeCornerMask; ///< mask of corners that robot exposes to camera. 4 bit. -x-y = 1, +x-y = 2, -x+y = 4, +x+y = 8 + double minCornerVisibleDist = 0; ///< how much distance along with uncertain edge from uncertain corner robot exposes to camera + double minCornerVisibleInsideDist = 0; ///< how much distance inside MVR robot exposes to camera + double maxCornerAngleDeviation = 0; ///< how much angle deviation around uncertain corner is considered to expose to camera + uint8_t occlusionFreeCornerMask = 0; ///< mask of corners that robot exposes to camera. 4 bit. -x-y = 1, +x-y = 2, -x+y = 4, +x+y = 8 mujin::MinViableRegionRegistrationMode registrationMode; ///< lift, drag or perpendicular drag - bool skipAppendingToObjectSet; ///< if true, skip appending newly created registration data into an active object set - double maxPossibleSizePadding; ///< how much to additionally expose max possible size region to vision + bool skipAppendingToObjectSet = true; ///< if true, skip appending newly created registration data into an active object set + double maxPossibleSizePadding = 0; ///< how much to additionally expose max possible size region to vision std::vector fullDofValues; ///< robot configuration state on capturing std::vector connectedBodyActiveStates; ///< robot configuration state on capturing bool IsEmpty() const { @@ -251,7 +251,7 @@ class MUJINCLIENT_API BinPickingTaskResource : public TaskResource struct TriggerDetectionCaptureInfo { TriggerDetectionCaptureInfo(); - double timestamp; ///< timestamp this request was sent. If non-zero, then valid. + double timestamp = 0; ///< timestamp this request was sent. If non-zero, then valid. std::string triggerType; ///< The type of trigger this is. For now can be: "phase1Detection", "phase2Detection" std::string locationName; ///< The name of the location for this detection trigger. std::string targetUpdateNamePrefix; ///< if not empty, use this new targetUpdateNamePrefix for the triggering, otherwise do not change the original targetUpdateNamePrefix @@ -297,7 +297,7 @@ class MUJINCLIENT_API BinPickingTaskResource : public TaskResource std::vector translation; std::vector extents; std::vector rotationmat; // row major - std::vector quaternion; // the quaternion + std::vector quaternion = {1, 0, 0, 0}; // the quaternion }; struct MUJINCLIENT_API ResultInstObjectInfo : public ResultBase @@ -331,7 +331,7 @@ class MUJINCLIENT_API BinPickingTaskResource : public TaskResource void Parse(const rapidjson::Value& pt); std::string status; ResultGetBinpickingState taskstate; - Real timestamp; + Real timestamp = 0; std::string msg; std::string _slaverequestid; }; diff --git a/include/mujincontrollerclient/mujincontrollerclient.h b/include/mujincontrollerclient/mujincontrollerclient.h index 964c6446..a08d2bc9 100644 --- a/include/mujincontrollerclient/mujincontrollerclient.h +++ b/include/mujincontrollerclient/mujincontrollerclient.h @@ -113,8 +113,8 @@ class DebugResource; struct FileEntry { std::string filename; - double modified; // in epoch seconds - size_t size; // file size in bytes + double modified = 0; // in epoch seconds + size_t size = 0; // file size in bytes }; typedef boost::shared_ptr ControllerClientPtr; @@ -193,7 +193,7 @@ JobStatusCode GetStatusCode(const std::string& str); struct JobStatus { - JobStatus() : code(JSC_Unknown) { + JobStatus() : code(JSC_Unknown), elapsedtime(0.0) { } JobStatusCode code; ///< status code on whether the job is active std::string type; ///< the type of job running @@ -791,16 +791,16 @@ 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]; - bool visible; - Real diffusecolor[4]; - Real transparency; - Real half_extents[3]; - Real height; - Real radius; - Real topRadius; - Real bottomRadius; + Real quaternion[4] = {1, 0, 0, 0}; // quaternion [w, x, y, z] = [cos(angle/2), sin(angle/2)*rotation_axis] + Real translate[3] = {0, 0, 0}; + bool visible = true; + Real diffusecolor[4] = {0, 0, 0, 0}; + Real transparency = 0; + Real half_extents[3] = {0, 0, 0}; + Real height = 0; + Real radius = 0; + Real topRadius = 0; + Real bottomRadius = 0; virtual void GetMesh(std::string& primitive, std::vector >& indices, std::vector >& vertices); virtual void SetGeometryFromRawSTL(const std::vector& rawstldata, const std::string& unit, double timeout = 5.0); @@ -818,10 +818,10 @@ 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]; - Real angle; + Real quaternion[4] = {1, 0, 0, 0}; // quaternion [w, x, y, z] = [cos(angle/2), sin(angle/2)*rotation_axis] + Real translation[3] = {0, 0, 0}; + Real direction[3] = {0, 0, 0}; + Real angle = 0; }; typedef boost::shared_ptr IkParamResourcePtr; @@ -852,9 +852,9 @@ 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]; - bool collision; + Real quaternion[4] = {1, 0, 0, 0}; // quaternion [w, x, y, z] = [cos(angle/2), sin(angle/2)*rotation_axis] + Real translate[3] = {0, 0, 0}; + bool collision = true; }; typedef boost::shared_ptr LinkResourcePtr; @@ -877,17 +877,17 @@ class MUJINCLIENT_API ObjectResource : public WebResource virtual int GetVisible(); std::string name; - int nundof; + int nundof = 0; std::string datemodified; std::string geometry; - bool isrobot; + bool isrobot = false; std::string pk; std::string resource_uri; 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]; + Real quaternion[4] = {1, 0, 0, 0}; // quaternion [w, x, y, z] = [cos(angle/2), sin(angle/2)*rotation_axis] + Real translate[3] = {0, 0, 0}; protected: ObjectResource(ControllerClientPtr controller, const std::string& resource, const std::string& pk); @@ -907,9 +907,9 @@ class MUJINCLIENT_API RobotResource : public ObjectResource std::string frame_origin; std::string frame_tip; std::string pk; - std::array direction; - std::array quaternion; // quaternion [w, x, y, z] = [cos(angle/2), sin(angle/2)*rotation_axis] - std::array translate; + std::array direction = {0, 0, 0}; + std::array quaternion = {1, 0, 0, 0}; // quaternion [w, x, y, z] = [cos(angle/2), sin(angle/2)*rotation_axis] + std::array translate = {0, 0, 0}; }; typedef boost::shared_ptr ToolResourcePtr; @@ -923,8 +923,8 @@ class MUJINCLIENT_API RobotResource : public ObjectResource std::string frame_origin; std::string pk; //Real direction[3]; - std::array quaternion; // quaternion [w, x, y, z] = [cos(angle/2), sin(angle/2)*rotation_axis] - std::array translate; + std::array quaternion = {1, 0, 0, 0}; // quaternion [w, x, y, z] = [cos(angle/2), sin(angle/2)*rotation_axis] + std::array translate = {0, 0, 0}; std::string sensortype; struct SensorData { @@ -941,12 +941,12 @@ class MUJINCLIENT_API RobotResource : public ObjectResource bool operator==(const SensorData& other) const { return !operator!=(other); } - Real distortion_coeffs[5]; + Real distortion_coeffs[5] = {0, 0, 0, 0, 0}; std::string distortion_model; - Real focal_length; - int image_dimensions[3]; - Real intrinsic[6]; - Real measurement_time; + Real focal_length = 0; + int image_dimensions[3] = {0, 0, 0}; + Real intrinsic[6] = {0, 0, 0, 0, 0, 0}; + Real measurement_time = 0; std::vector extra_parameters; }; SensorData sensordata; @@ -964,7 +964,7 @@ class MUJINCLIENT_API RobotResource : public ObjectResource // attachments // ikparams // images - int numdof; + int numdof = 0; std::string simulation_file; }; @@ -984,16 +984,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]; + Real quaternion[4] = {1, 0, 0, 0}; // quaternion [w, x, y, z] = [cos(angle/2), sin(angle/2)*rotation_axis] + Real translate[3] = {0, 0, 0}; }; 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]; + Real direction[3] = {0, 0, 0}; + Real quaternion[4] = {1, 0, 0, 0}; // quaternion [w, x, y, z] = [cos(angle/2), sin(angle/2)*rotation_axis] + Real translate[3] = {0, 0, 0}; }; class MUJINCLIENT_API Grab { @@ -1019,8 +1019,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]; + Real quaternion[4] = {1, 0, 0, 0}; // quaternion [w, x, y, z] = [cos(angle/2), sin(angle/2)*rotation_axis] + Real translate[3] = {0, 0, 0}; }; void SetTransform(const Transform& t); @@ -1035,8 +1035,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]; + Real quaternion[4] = {1, 0, 0, 0}; // quaternion [w, x, y, z] = [cos(angle/2), sin(angle/2)*rotation_axis] + Real translate[3] = {0, 0, 0}; std::vector grabs; std::vector links; std::vector tools; @@ -1257,7 +1257,7 @@ class MUJINCLIENT_API DebugResource : public WebResource std::string name; std::string pk; std::string resource_uri; - size_t size; + size_t size = 0; protected: DebugResource(ControllerClientPtr controller, const std::string& resource, const std::string& pk);