Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Eliminate C array from members #140

Open
wants to merge 2 commits into
base: master
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions include/mujincontrollerclient/binpickingtask.h
Original file line number Diff line number Diff line change
Expand Up @@ -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<double,3> minMargins; ///< XYZ of how much to crop from min margins (value > 0 means crop inside)
std::array<double,3> maxMargins; ///< XYZ of how much to crop from min margins (value > 0 means crop inside)
};

typedef boost::shared_ptr<CropContainerMarginsXYZXYZ> CropContainerMarginsXYZXYZPtr;
Expand Down
78 changes: 39 additions & 39 deletions include/mujincontrollerclient/mujincontrollerclient.h
Original file line number Diff line number Diff line change
Expand Up @@ -279,9 +279,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<Real,4> maxrange; ///< X, Y, Z, Angle (deg)
std::array<Real,4> minrange; ///< X, Y, Z, Angle (deg)
std::array<Real,4> 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
Expand Down Expand Up @@ -310,12 +310,12 @@ struct PlacementsOptimizationParameters
}

// for every target, there's one setting:
boost::array<std::string, 2> 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<std::string, 2> 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<Real[4],2> maxranges; ///< X, Y, Z, Angle (deg)
boost::array<Real[4],2> minranges; ///< X, Y, Z, Angle (deg)
boost::array<Real[4],2> stepsizes; ///< X, Y, Z, Angle (deg)
boost::array<int,2> 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<std::string, 2> 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<std::string, 2> 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<std::array<Real,4>,2> maxranges; ///< X, Y, Z, Angle (deg)
std::array<std::array<Real,4>,2> minranges; ///< X, Y, Z, Angle (deg)
std::array<std::array<Real,4>,2> stepsizes; ///< X, Y, Z, Angle (deg)
std::array<int,2> 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
Expand Down Expand Up @@ -784,12 +784,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<Real,4> quaternion; // quaternion [w, x, y, z] = [cos(angle/2), sin(angle/2)*rotation_axis]
std::array<Real,3> translate;
bool visible;
Real diffusecolor[4];
std::array<Real,4> diffusecolor;
Real transparency;
Real half_extents[3];
std::array<Real,3> half_extents;
Real height;
Real radius;
Real topRadius;
Expand All @@ -811,9 +811,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<Real,4> quaternion; // quaternion [w, x, y, z] = [cos(angle/2), sin(angle/2)*rotation_axis]
std::array<Real,3> translation;
std::array<Real,3> direction;
Real angle;
};
typedef boost::shared_ptr<IkParamResource> IkParamResourcePtr;
Expand All @@ -831,7 +831,7 @@ class MUJINCLIENT_API ObjectResource : public WebResource

virtual void GetGeometries(std::vector<GeometryResourcePtr>& links);

virtual boost::shared_ptr<LinkResource> AddChildLink(const std::string& name, const Real quaternion[4], const Real translate[3]);
virtual boost::shared_ptr<LinkResource> AddChildLink(const std::string& name, const std::array<Real,4> &quaternion, const std::array<Real,3> &translate);

virtual void SetCollision(bool collision);
/// 0 -> off, 1 -> on
Expand All @@ -845,8 +845,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<Real,4> quaternion; // quaternion [w, x, y, z] = [cos(angle/2), sin(angle/2)*rotation_axis]
std::array<Real,3> translate;
bool collision;
};
typedef boost::shared_ptr<LinkResource> LinkResourcePtr;
Expand All @@ -859,7 +859,7 @@ class MUJINCLIENT_API ObjectResource : public WebResource

virtual void GetIkParams(std::vector<IkParamResourcePtr>& 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<Real,4> &quaternion, const std::array<Real,3> &translate);
virtual IkParamResourcePtr AddIkParam(const std::string& name, const std::string& iktype);

virtual void SetCollision(bool collision);
Expand All @@ -879,8 +879,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<Real,4> quaternion; // quaternion [w, x, y, z] = [cos(angle/2), sin(angle/2)*rotation_axis]
std::array<Real,3> translate;

protected:
ObjectResource(ControllerClientPtr controller, const std::string& resource, const std::string& pk);
Expand Down Expand Up @@ -915,30 +915,30 @@ class MUJINCLIENT_API RobotResource : public ObjectResource
std::string name;
std::string frame_origin;
std::string pk;
//Real direction[3];
//std::array<Real,3> direction;
std::array<Real,4> quaternion; // quaternion [w, x, y, z] = [cos(angle/2), sin(angle/2)*rotation_axis]
std::array<Real,3> translate;
std::string sensortype;

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<Real,5> distortion_coeffs;
std::string distortion_model;
Real focal_length;
int image_dimensions[3];
Real intrinsic[6];
std::array<int,3> image_dimensions;
std::array<Real,6> intrinsic;
Real measurement_time;
std::vector<Real> extra_parameters;
};
Expand Down Expand Up @@ -977,16 +977,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<Real,4> quaternion; // quaternion [w, x, y, z] = [cos(angle/2), sin(angle/2)*rotation_axis]
std::array<Real,3> 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<Real,3> direction;
std::array<Real,4> quaternion; // quaternion [w, x, y, z] = [cos(angle/2), sin(angle/2)*rotation_axis]
std::array<Real,3> translate;
};

class MUJINCLIENT_API Grab {
Expand All @@ -1012,8 +1012,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<Real,4> quaternion; // quaternion [w, x, y, z] = [cos(angle/2), sin(angle/2)*rotation_axis]
std::array<Real,3> translate;
};

void SetTransform(const Transform& t);
Expand All @@ -1028,8 +1028,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<Real,4> quaternion; // quaternion [w, x, y, z] = [cos(angle/2), sin(angle/2)*rotation_axis]
std::array<Real,3> translate;
std::vector<Grab> grabs;
std::vector<Link> links;
std::vector<Tool> tools;
Expand Down Expand Up @@ -1096,7 +1096,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<Real,4> &quaternion, const std::array<Real,3> &translate, double timeout = 300);

/// \brief deletes an inst object in scene
/// \param pk primary key of the object to delete
Expand Down
2 changes: 1 addition & 1 deletion src/controllerclientimpl.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -1084,7 +1084,7 @@ std::string ControllerClientImpl::CreateIkParam(const std::string& objectPk, con
return GetJsonValueByKey<std::string>(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<Real,4> &quaternion, const std::array<Real,3> &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]));
Expand Down
2 changes: 1 addition & 1 deletion src/controllerclientimpl.h
Original file line number Diff line number Diff line change
Expand Up @@ -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<Real,4> &quaternion, const std::array<Real,3> &translate, double timeout = 5);

/// \brief set geometry for an object
/// \param objectPk primary key for the object
Expand Down
6 changes: 3 additions & 3 deletions src/mujincontrollerclient.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -444,7 +444,7 @@ void ObjectResource::GetLinks(std::vector<ObjectResource::LinkResourcePtr>& 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<Real,4> &quaternion_, const std::array<Real,3> &translate_)
{
GETCONTROLLERIMPL();
const std::string linkPk = controller->CreateLink(this->pk, "", objname, quaternion_, translate_);
Expand All @@ -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<Real,4> &quaternion_, const std::array<Real,3> &translate_)
{
GETCONTROLLERIMPL();
const std::string linkPk = controller->CreateLink(this->objectpk, this->pk, objname, quaternion_, translate_);
Expand Down Expand Up @@ -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<Real,4> &quaternion, const std::array<Real,3> &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()));
Expand Down