Merge branch 'master' of https://github.com/compas-dev/compas

Author: tomvanmele

src/compas/robots/model/geometry.py

@@ -48,35 +48,15 @@ class Origin(Frame):
 
     def __init__(self, point, xaxis, yaxis):
         super(Origin, self).__init__(point, xaxis, yaxis)
-        self.init = None  # keep a copy to the initial, not transformed origin
 
     @classmethod
     def from_urdf(cls, attributes, elements, text):
         xyz = _parse_floats(attributes.get('xyz', '0 0 0'))
         rpy = _parse_floats(attributes.get('rpy', '0 0 0'))
         return cls.from_euler_angles(rpy, static=True, axes='xyz', point=xyz)
 
-    @property
-    def init_transformation(self):
-        return Transformation.from_frame(self.init)
-
-    def create(self, transformation):
-        self.transform(transformation)
-        self.init = self.copy()
-
-    def reset_transform(self):
-        if self.init:
-            # TODO: Transform back into initial state does not always work...
-            # T = init_transformation * Transformation.from_frame(self).inverse()
-            # self.transform(T)
-            cp = self.init.copy()
-            self.point = cp.point
-            self.xaxis = cp.xaxis
-            self.yaxis = cp.yaxis
-
     def scale(self, factor):
         self.point = self.point * factor
-        self.init = self.copy()
 
 
 class BaseShape(object):

src/compas/robots/model/joint.py

@@ -114,34 +114,18 @@ def __init__(self, xyz='0 0 0'):
         self.x = xyz[0]
         self.y = xyz[1]
         self.z = xyz[2]
-        self.init = None
 
     def copy(self):
         cls = type(self)
         return cls("%f %f %f" % (self.x, self.y, self.z))
 
-    def reset_transform(self):
-        if self.init:
-            cp = self.init.copy()
-            self.x = cp.x
-            self.y = cp.y
-            self.z = cp.z
-
     def transform(self, transformation):
         xyz = transform_vectors(
             [[self.x, self.y, self.z]], transformation.matrix)
         self.x = xyz[0][0]
         self.y = xyz[0][1]
         self.z = xyz[0][2]
 
-    def create(self, transformation):
-        """Stores the initial direction of the axis.
-
-        This is called when the robot is created.
-        """
-        self.transform(transformation)
-        self.init = self.copy()
-
     @property
     def vector(self):
         return Vector(self.x, self.y, self.z)
@@ -208,6 +192,8 @@ def __init__(self, name, type, parent, child, origin=None, axis=None, calibratio
         self.attr = kwargs
         self.child_link = None
         self.position = 0
+        self.init_origin = origin.copy() if origin else None
+        self.init_axis = axis.copy() if axis else None
 
         switcher = {
             Joint.REVOLUTE: self.calculate_revolute_transformation,
@@ -229,16 +215,16 @@ def current_transformation(self):
 
     @property
     def init_transformation(self):
-        if self.origin:
-            return self.origin.init_transformation.copy()
+        if self.init_origin:
+            return Transformation.from_frame(self.init_origin)
         else:
             return Transformation()
 
     def reset_transform(self):
-        if self.origin:
-            self.origin.reset_transform()
-        if self.axis:
-            self.axis.reset_transform()
+        if self.init_origin:
+            self.origin = self.init_origin.copy()
+        if self.init_axis:
+            self.axis = self.init_axis.copy()
 
     def transform(self, transformation):
         if self.origin:

src/compas/robots/model/link.py

@@ -138,54 +138,6 @@ def __init__(self, name, type=None, visual=[], collision=[], inertial=None, **kw
         self.joints = []
         self.parent_joint = None
 
-    # TODO: Check
-    def update(self, joint_state, parent_transformation, reset_transformation, collision=True):
-        """Recursive function to apply the transformations given by the joint
-            state.
-
-        Joint_states are given absolute, so it is necessary to reset the current
-        transformation.
-
-        Args:
-            joint_state (dict): A dictionary with the joint names as keys and
-                values in radians and m (depending on the joint type)
-            parent_transformation (:class:`Transformation`): The transfomation
-                of the parent joint
-            reset_transformation (:class:`Transformation`): The transfomation
-                to reset the current transformation of the link's geometry.
-            collision (bool): If collision geometry should be transformed as
-                well. Defaults to True.
-        """
-        relative_transformation = parent_transformation * reset_transformation
-
-        for item in self.visual:
-            item.geometry.shape.transform(relative_transformation)
-
-        if collision:
-            for item in self.collision:
-                item.geometry.shape.transform(relative_transformation)
-
-        for joint in self.joints:
-            # 1. Get reset transformation
-            reset_transformation = joint.calculate_reset_transformation()
-            # 2. Reset
-            joint.reset_transform()
-            # joint.transform(reset_transformation) # why does this not work properly....
-
-            # 3. Calculate transformation for next joints in the chain
-            if joint.name in joint_state.keys():
-                position = joint_state[joint.name]
-                transformation = joint.calculate_transformation(position)
-                transformation = parent_transformation * transformation
-                joint.position = position
-            else:
-                transformation = parent_transformation
-
-            # 4. Apply on joint
-            joint.transform(transformation)
-            # 4. Apply function to all children in the chain
-            joint.child_link.update(joint_state, transformation, reset_transformation, collision)
-
     # TODO: Check
     def scale(self, factor):
         from compas.geometry import Scale

src/compas/robots/model/robot.py

@@ -225,21 +225,6 @@ def iter_chain(self, start=None, end=None):
         for name in shortest_chain:
             yield name
 
-    def update(self, names, positions, collision=True):
-        """Updates the joints and link geometries.
-
-        Args:
-            names (list of str): a list of the joints names that are updated
-            positions (list_of float): a list of the respective joint positions,
-                in radians and m
-            collision (bool): If collision geometry should be transformed as
-                well. Defaults to True.
-        """
-        if len(names) != len(positions):
-            return ValueError("len(names): %d is not len(positions) %d" % (len(names), len(positions)))
-        joint_state = dict(zip(names, positions))
-        self.root.update(joint_state, Transformation(), Transformation(), collision)
-
     def get_configurable_joints(self):
         joints = self.iter_joints()
         return [joint for joint in joints if joint.is_configurable()]
@@ -322,6 +307,7 @@ def axes(self):
                 axes.append(joint.axis.vector)
         return axes
 
+    # TODO: Check
     def scale(self, factor):
         names = self.get_configurable_joint_names()
         # bring to init configuration