diff --git a/.buildinfo b/.buildinfo
new file mode 100644
index 0000000..8b64dae
--- /dev/null
+++ b/.buildinfo
@@ -0,0 +1,4 @@
+# Sphinx build info version 1
+# This file hashes the configuration used when building these files. When it is not found, a full rebuild will be done.
+config: b91813fa1201a767cc40d0635dfb4bee
+tags: 645f666f9bcd5a90fca523b33c5a78b7
diff --git a/.doctrees/changelog.doctree b/.doctrees/changelog.doctree
new file mode 100644
index 0000000..315c258
Binary files /dev/null and b/.doctrees/changelog.doctree differ
diff --git a/.doctrees/contributing.doctree b/.doctrees/contributing.doctree
new file mode 100644
index 0000000..6480300
Binary files /dev/null and b/.doctrees/contributing.doctree differ
diff --git a/.doctrees/environment.pickle b/.doctrees/environment.pickle
new file mode 100644
index 0000000..c5b2090
Binary files /dev/null and b/.doctrees/environment.pickle differ
diff --git a/.doctrees/index.doctree b/.doctrees/index.doctree
new file mode 100644
index 0000000..024d827
Binary files /dev/null and b/.doctrees/index.doctree differ
diff --git a/.doctrees/installation.doctree b/.doctrees/installation.doctree
new file mode 100644
index 0000000..b8806af
Binary files /dev/null and b/.doctrees/installation.doctree differ
diff --git a/.doctrees/license.doctree b/.doctrees/license.doctree
new file mode 100644
index 0000000..db7dc28
Binary files /dev/null and b/.doctrees/license.doctree differ
diff --git a/.doctrees/reference.doctree b/.doctrees/reference.doctree
new file mode 100644
index 0000000..a4c359e
Binary files /dev/null and b/.doctrees/reference.doctree differ
diff --git a/.doctrees/reference/compas_mrr.arbitrary_pts_localization.doctree b/.doctrees/reference/compas_mrr.arbitrary_pts_localization.doctree
new file mode 100644
index 0000000..69b68b4
Binary files /dev/null and b/.doctrees/reference/compas_mrr.arbitrary_pts_localization.doctree differ
diff --git a/.doctrees/reference/compas_mrr.doctree b/.doctrees/reference/compas_mrr.doctree
new file mode 100644
index 0000000..45c8201
Binary files /dev/null and b/.doctrees/reference/compas_mrr.doctree differ
diff --git a/.doctrees/reference/compas_mrr.measurement_point.doctree b/.doctrees/reference/compas_mrr.measurement_point.doctree
new file mode 100644
index 0000000..5312a4b
Binary files /dev/null and b/.doctrees/reference/compas_mrr.measurement_point.doctree differ
diff --git a/.doctrees/reference/compas_mrr.three_pts_localization.doctree b/.doctrees/reference/compas_mrr.three_pts_localization.doctree
new file mode 100644
index 0000000..6ecd10f
Binary files /dev/null and b/.doctrees/reference/compas_mrr.three_pts_localization.doctree differ
diff --git a/.doctrees/reference/compas_mrr.utils.doctree b/.doctrees/reference/compas_mrr.utils.doctree
new file mode 100644
index 0000000..722c78a
Binary files /dev/null and b/.doctrees/reference/compas_mrr.utils.doctree differ
diff --git a/.doctrees/reference/compas_mrr.xforms.doctree b/.doctrees/reference/compas_mrr.xforms.doctree
new file mode 100644
index 0000000..10206c8
Binary files /dev/null and b/.doctrees/reference/compas_mrr.xforms.doctree differ
diff --git a/.doctrees/usage.doctree b/.doctrees/usage.doctree
new file mode 100644
index 0000000..7e0cbd9
Binary files /dev/null and b/.doctrees/usage.doctree differ
diff --git a/.nojekyll b/.nojekyll
new file mode 100644
index 0000000..e69de29
diff --git a/_modules/compas_mrr.html b/_modules/compas_mrr.html
new file mode 100644
index 0000000..a0f3cea
--- /dev/null
+++ b/_modules/compas_mrr.html
@@ -0,0 +1,375 @@
+
+
+
+
Source code for compas_mrr.arbitrary_pts_localization
+"""
+*******************************************************************************
+Arbitrary points method for robot relocalization.
+*******************************************************************************
+"""
+
+from__future__importabsolute_import
+from__future__importdivision
+from__future__importprint_function
+
+importsys
+importtempfile
+fromfunctoolsimportreduce
+
+importnumpyasnp
+fromscipy.optimizeimportminimize
+
+fromcompas_mrr.utilsimportTYPE_CHECKING
+
+try:
+ frompathlibimportPath
+exceptImportError:
+ ifsys.version_info.major<3:
+ raiseException("Python 3 is required to run this module.")
+ else:
+ raise
+
+ifTYPE_CHECKING:
+ fromtypingimportList# noqa: F401
+
+ fromscipy.optimizeimportOptimizeResult# noqa: F401
+
+
+def_objective_function(
+ x,# type: List[float]
+ rcs_coords,# type: List[List[float]]
+ wcs_coords,# type: List[List[float]]
+):# type: (...) -> float
+"""Objective function for the optimization problem.
+
+ Parameters
+ ----------
+ x
+ The optimization variable (9x1)
+ args : :obj:`tuple`
+ The localization points and the measurements as a tuple of two
+ dimensional arrays where each row is one point. The columns are the X,
+ Y and Z coordinates.
+
+ Returns
+ -------
+ :obj:`float`
+ The cost for the given optimization variable values.
+ """
+ origin=np.array(x[0:3])
+ x_vec=np.array(x[3:6])
+ y_vec=np.array(x[6:9])
+ z_vec=np.cross(x_vec,y_vec)
+
+ cost=0
+ forpoint,measurementinzip(rcs_coords,wcs_coords):
+ # Calculate the deviation from the measurement using the given
+ # coordinate system (optimization variable) and add the square of it to
+ # the cost.
+ deviation=np.power(
+ origin
+ +point[0]*x_vec
+ +point[1]*y_vec
+ +point[2]*z_vec
+ -measurement,
+ 2,
+ )
+ cost+=sum(deviation)
+
+ returncost
+
+
+def_nonlinear_constraints(x):# type: (List[float]) -> List[float]
+"""Constraints for the optimization problem.
+
+ Parameters
+ ----------
+ x
+ The optimization variable (9x1).
+
+ Returns
+ -------
+ :obj:`list` of :obj:`float`
+ An array that contains the values when the constraints are evaluated at
+ `x`.
+ """
+ return[
+ # x and y need to be orthogonal (i.e. scalar product = 0)
+ x[3]*x[6]+x[4]*x[7]+x[5]*x[8],
+ x[3]**2+x[4]**2+x[5]**2-1,# |x| = 1
+ x[6]**2+x[7]**2+x[8]**2-1,# |y| = 1
+ ]
+
+
+def_nonlinear_jacobian(x):# type: (List[float]) -> List[List[float]]
+"""Jacobian for the constraints.
+
+ Parameters
+ ----------
+ x
+ The optimization variable (9x1).
+
+ Returns
+ -------
+ The jacobian of the nonlinear constraints.
+ """
+ return[
+ [0,0,0,x[6],x[7],x[8],x[3],x[4],x[5]],
+ [0,0,0,2*x[3],2*x[4],2*x[5],0,0,0],
+ [0,0,0,0,0,0,2*x[6],2*x[7],2*x[8]],
+ ]
+
+
+def_plot(
+ rcs_coords,# type: List[List[float]]
+ wcs_coords,# type: List[List[float]]
+ results,# type: OptimizeResult
+):# type: (...) -> None
+"""Create plots to visualize multiple consecutive results from a solver."""
+ importmatplotlib.pyplotasplt
+
+ rcs_coords=np.array(rcs_coords)
+ wcs_coords=np.array(wcs_coords)
+
+ plot_dir=Path(tempfile.mkdtemp(prefix="localization_"))
+ print("Saving plots to {}".format(plot_dir))
+
+ fori,resinenumerate(results):
+ dir_=plot_dir/str(i)
+ dir_.mkdir()
+
+ _plot_result(rcs_coords,wcs_coords,res,dir_)
+
+ # Create a plot summarizing the the different runs
+ objective_values=[result.funforresultinresults]
+ summary_file_path=plot_dir/"summary.png"
+ plt.figure()
+ plt.plot(range(len(objective_values)),objective_values,"ro")
+ plt.ylabel("Objective value")
+ plt.xlabel("Run")
+ plt.title("Objective value for different x_0")
+ plt.savefig(summary_file_path)
+
+
+def_plot_result(
+ rcs_coords,# type: np.ndarray
+ wcs_coords,# type: np.ndarray
+ result,# type: OptimizeResult
+ plot_dir,# type: Path
+):# type: (...) -> None
+"""Create some plots that illustrate the result.
+
+ Parameters
+ ----------
+ rcs_coords
+ wcs_coords
+ result
+ The result from the solver
+ folder
+ The folder in which the plots should be stored.
+
+ Returns
+ -------
+ :obj:`str`
+ The path at which the plots were stored.
+ """
+ importmatplotlib.pyplotasplt
+
+ origin=result.x[0:3]
+ x_vec=result.x[3:6]
+ y_vec=result.x[6:9]
+ z_vec=np.cross(x_vec,y_vec)
+
+ x_axis=origin+1000*x_vec
+ y_axis=origin+1000*y_vec
+ # z_axis = origin + 1000 * z_vec
+
+ # Calculate the localization points in the new coordinate system
+ _transformed_points=[]
+ forpointinrcs_coords:
+ _transformed_points.append(
+ origin+point[0]*x_vec+point[1]*y_vec+point[2]*z_vec
+ )
+ transformed_points=np.array(_transformed_points)
+
+ plt.figure()
+ plt.plot(wcs_coords.T[0],wcs_coords.T[1],"bo")
+ plt.plot(transformed_points.T[0],wcs_coords.T[1],"rx")
+ plt.plot(origin[0],origin[1],"bx")
+ plt.plot(x_axis[0],x_axis[1],"rx")
+ plt.plot(y_axis[0],y_axis[1],"gx")
+ plt.xlabel("x")
+ plt.ylabel("y")
+ plt.title("X-Y projection")
+ plt.savefig(plot_dir/"rcs_matching_xy.png")
+
+ plt.figure()
+ plt.plot(wcs_coords.T[0],wcs_coords.T[2],"bo")
+ plt.plot(transformed_points.T[0],wcs_coords.T[2],"rx")
+ plt.plot(origin[0],origin[2],"bx")
+ plt.plot(x_axis[0],x_axis[2],"rx")
+ plt.plot(y_axis[0],y_axis[2],"gx")
+ plt.xlabel("x")
+ plt.ylabel("z")
+ plt.title("X-Z projection")
+ plt.savefig(plot_dir/"rcs_matching_xz.png")
+
+ plt.figure()
+ plt.plot(wcs_coords.T[1],wcs_coords.T[2],"bo")
+ plt.plot(transformed_points.T[1],wcs_coords.T[2],"rx")
+ plt.plot(origin[1],origin[2],"bx")
+ plt.plot(x_axis[1],x_axis[2],"rx")
+ plt.plot(y_axis[1],y_axis[2],"gx")
+ plt.xlabel("y")
+ plt.ylabel("z")
+ plt.title("Y-Z projection")
+ plt.savefig(plot_dir/"rcs_matching_yz.png")
+
+
+
[docs]defarbitrary_pts_localization(
+ rcs_coords,# type: List[List[float]]
+ wcs_coords,# type: List[List[float]]
+ plot_results=False,# type: bool
+ maxiter=200,# type: int
+):# type: (...) -> List[List[float]]
+"""Calculate the RCS origin frame.
+
+ Finding the origin is formulated as an optimization problem where we want
+ to find the origin and two orthonormal vectors defining the coordinate
+ system. At the same time, the position of the localization points in this
+ new coordinate system should match the measurements as close as possible.
+ Therefore the deviation from the measurements is used as the cost function.
+ The only constraints are that the x and y vector need to have length 1 and
+ be orthogonal to each other. The optimization variable is a vector with 9
+ entries: X = [o, x, y] where o is the origin of the coordinate system and
+ x, y the vectors spanning the x-y-plane. Each of them is a 3 dimensional
+ vector. **Important**: Ensure that the order of rcs_coords and
+ measurements is identical. I.e. the i-th entry in measurements is the
+ measurement of the i-th localization point.
+
+ Parameters
+ ----------
+ rcs_coords
+ The points where the robot endeffector was positioned to take
+ measurements. These points are in the RCS.
+ wcs_coords
+ The measurements taken in the world coordinate system (WCS) with the
+ total station. These are the coordinates of the rcs_coords in
+ the WCS.
+
+ Returns
+ -------
+ :obj:`list` of :obj:`list` of :obj:`float`
+ A tuple of 3 vectors (lists with 3 elements) where the first represents
+ the origin of the RCS, the second is the direction of the x axis and
+ the third the direction of the y axis. The x and y axis are vectors
+ with length 1.
+ """
+ # Setup the constraints
+ constraints={"type":"eq","fun":_nonlinear_constraints}
+
+ results=[]
+ slices=4
+ foriinrange(slices):
+ radians=np.deg2rad(360.0/float(slices)*i)
+ c,s=np.cos(radians),np.sin(radians)
+ rotation=np.array(((c,-s,0),(s,c,0),(0,0,1)))
+
+ x=rotation.dot(np.array([1,0,0]))
+ y=rotation.dot(np.array([0,1,0]))
+
+ # We use the standard coordinate system as an initial guess.
+ x0=np.array(np.concatenate(([0,0,0],x,y)))
+ res=minimize(
+ _objective_function,
+ x0,
+ args=(rcs_coords,wcs_coords),
+ method="SLSQP",# Default method for problems with constraints
+ constraints=constraints,
+ options={"disp":True,"maxiter":maxiter},
+ )
+
+ results.append(res)
+
+ ifplot_results:
+ _plot(rcs_coords,wcs_coords,results)
+
+ # Pick the result with the lowest objective value
+ result=reduce((lambdax,y:xifx.fun<y.funelsey),results)
+
+ origin=result.x[0:3].tolist()
+ x_vec=result.x[3:6].tolist()
+ y_vec=result.x[6:9].tolist()
+
+ return[origin,x_vec,y_vec]
+
+
+# Make MeasurementPoint available from utils for backwards compatibility
+# with compas_mrr <= v1.0.4
+utils.MeasurementPoint=MeasurementPoint# type: ignore[attr-defined]
+
[docs]defthree_pts_localization(rcs_coords,wcs_coords):# type: (List[Point], List[Point]) -> Frame
+"""Get the robot base frame in WCS using three points method.
+
+ Parameters
+ ----------
+ rcs_coords
+ List of the RCS coordinates used for measurements.
+ wcs_coords
+ List of the WCS coordinates used for measurements.
+
+ Returns
+ -------
+ The base frame of the robot in WCS.
+ """
+ recorded_frame_rcs=_coerce_frame(rcs_coords)
+ recorded_frame_wcs=_coerce_frame(wcs_coords)
+
+ T=Transformation.from_frame_to_frame(recorded_frame_rcs,recorded_frame_wcs)
+
+ wcs_robot_base=Frame.worldXY()
+ wcs_robot_base.transform(T)
+
+ returnwcs_robot_base
[docs]defworldxy_to_robot_base_xform(robot_base_frame):
+ # type: (cg.Frame) -> cg.Transformation
+"""Get transformation from WCS origin to RCS origin.
+
+ Parameters
+ ----------
+ robot_base_frame
+ Robot base frame in WCS. The frame origin is the location of the RCS origo
+ in WCS, the X axis and Y axis are the X and Y axes of the RCS in WCS.
+ """
+ frame_from=cg.Frame.worldXY()
+ frame_to=robot_base_frame
+
+ returncg.Transformation.from_change_of_basis(frame_from,frame_to)
+
+
+
[docs]defxform_to_xyz_quaternion(xform):# type: (Any) -> List[float]
+"""Convert transformation to :obj:`list` of coords and quaternion values.
+
+ Parameters
+ ----------
+ xform
+ Transformation to be converted. Can be given as
+ :class:`Rhino.Geometry.Transform`,
+ :class:`compas.geometry.Transformation`,
+ :class:`numpy.ndarray`, or :obj:`list` of :obj:`list` of :obj:`float`.
+
+ Returns
+ -------
+ X, Y, Z, QW, QX, QY, QZ values as a list.
+
+ >>> from compas.geometry import Frame, Rotation, Translation
+ >>> Tr = Translation.from_vector([100, 100, 100])
+ >>> R = Rotation.from_frame(Frame.worldYZ())
+ >>> T = Tr * R
+ >>> xform_to_xyz_quaternion(T)
+ [100.0, 100.0, 100.0, 0.5, 0.5, 0.5, 0.5]
+ """
+ xform=_coerce_cg_xform(xform)
+
+ xyz=list(xform.translation_vector)
+
+ wxyz=cg.Quaternion.from_rotation(xform.rotation).wxyz
+
+ returnxyz+wxyz
Monkeypatch added for compas._os.prepare_environment applied if compas version
+is less than v0.19.2 to get around
+a bug affecting ``compas.rpc.proxy` <https://github.com/compas-dev/compas/issues/701>`_
MeasurementPoint moved to its own module and made available as second level
+import (compas_mobile_robot_reloc.MeasurementPoint). Still available from
+compas_mobile_robot_reloc.utils for backwards compatibility.
No changes, version bumped to give give a clear indication of which conda
+package to use. (Lingering package
+compas-mobile-robot-reloc
+on conda forge.)
Ercan, Selen, Sandro Meier, Fabio Gramazio, and Matthias Kohler. 2019.
+“Automated Localization of a Mobile Construction Robot with an External
+Measurement Device.” In Proceedings of the 36th International Symposium on
+Automation and Robotics in Construction (ISARC 2019), 929-36. International
+Association on Automation and Robotics in Construction.
+https://doi.org/10.3929/ethz-b-000328442.
+
+
@inproceedings{ercan_automated_2019,
+title={Automated {Localization} of a {Mobile} {Construction} {Robot} with an {External} {Measurement} {Device}},
+copyright={http://rightsstatements.org/page/InC-NC/1.0/},
+url={https://www.research-collection.ethz.ch/handle/20.500.11850/328442},
+doi={10.3929/ethz-b-000328442},
+language={en},
+booktitle={Proceedings of the 36th {International} {Symposium} on {Automation} and {Robotics} in {Construction} ({ISARC} 2019)},
+publisher={International Association on Automation and Robotics in Construction},
+author={Ercan, Selen and Meier, Sandro and Gramazio, Fabio and Kohler, Matthias},
+year={2019},
+note={Accepted: 2019-09-11T08:51:48Z},
+pages={929--936},
+}
+
Monkeypatch added for compas._os.prepare_environment applied if compas version
+is less than v0.19.2 to get around
+a bug affecting ``compas.rpc.proxy` <https://github.com/compas-dev/compas/issues/701>`_
MeasurementPoint moved to its own module and made available as second level
+import (compas_mobile_robot_reloc.MeasurementPoint). Still available from
+compas_mobile_robot_reloc.utils for backwards compatibility.
No changes, version bumped to give give a clear indication of which conda
+package to use. (Lingering package
+compas-mobile-robot-reloc
+on conda forge.)
Finding the origin is formulated as an optimization problem where we want
+to find the origin and two orthonormal vectors defining the coordinate
+system. At the same time, the position of the localization points in this
+new coordinate system should match the measurements as close as possible.
+Therefore the deviation from the measurements is used as the cost function.
+The only constraints are that the x and y vector need to have length 1 and
+be orthogonal to each other. The optimization variable is a vector with 9
+entries: X = [o, x, y] where o is the origin of the coordinate system and
+x, y the vectors spanning the x-y-plane. Each of them is a 3 dimensional
+vector. Important: Ensure that the order of rcs_coords and
+measurements is identical. I.e. the i-th entry in measurements is the
+measurement of the i-th localization point.
+
+
Parameters:
+
+
rcs_coords – The points where the robot endeffector was positioned to take
+measurements. These points are in the RCS.
+
wcs_coords – The measurements taken in the world coordinate system (WCS) with the
+total station. These are the coordinates of the rcs_coords in
+the WCS.
+
+
+
Returns:
+
A tuple of 3 vectors (lists with 3 elements) where the first represents
+the origin of the RCS, the second is the direction of the x axis and
+the third the direction of the y axis. The x and y axis are vectors
+with length 1.
robot_base_frame – Robot base frame in WCS. The frame origin is the location of the RCS origo
+in WCS, the X axis and Y axis are the X and Y axes of the RCS in WCS.