Merge branch 'main' into gh_package_name

chenkasirer · chenkasirer · commit 1d540c3b20cf · 2022-08-19T15:05:33.000+02:00
diff --git a/CHANGELOG.rst b/CHANGELOG.rst
@@ -12,10 +12,13 @@ Unreleased
 
 **Added**
 
+* Added support for attached and non-attached collision mesh visualization to the ``Robot Visualize`` GH component.
 * Added a prefix to all GH components.
 
 **Changed**
 
+* Changed behavior of ``Attach Tool`` GH component to only attach the tool but not add it to the planning scene state.
+
 **Fixed**
 
 * Fixed DH params for analytical IK solver of UR3e and UR10e.
diff --git a/docs/examples/03_backends_ros/02_robot_models.rst b/docs/examples/03_backends_ros/02_robot_models.rst
@@ -5,7 +5,7 @@ Robots in ROS
 .. note::
 
     The following examples use the `ROS <https://www.ros.org/>`_ backend
-    and the MoveI! planner for UR5 robots. Before running them, please
+    and the MoveIt! planner for UR5 robots. Before running them, please
     make sure you have the :ref:`ROS backend <ros_backend>` correctly
     configured and the :ref:`UR5 Demo <ros_bundles_list>` started.
 
diff --git a/docs/examples/03_backends_ros/03_forward_and_inverse_kinematics.rst b/docs/examples/03_backends_ros/03_forward_and_inverse_kinematics.rst
@@ -9,7 +9,7 @@ Forward and inverse kinematics
 .. note::
 
     The following examples use the `ROS <https://www.ros.org/>`_ backend
-    and the MoveI! planner for UR5 robots. Before running them, please
+    and the MoveIt! planner for UR5 robots. Before running them, please
     make sure you have the :ref:`ROS backend <ros_backend>` correctly
     configured and the :ref:`UR5 Demo <ros_bundles_list>` started.
 
diff --git a/docs/examples/03_backends_ros/04_plan_motion.rst b/docs/examples/03_backends_ros/04_plan_motion.rst
@@ -5,7 +5,7 @@ Plan motion
 .. note::
 
     The following examples use the `ROS <https://www.ros.org/>`_ backend
-    and the MoveI! planner for UR5 robots. Before running them, please
+    and the MoveIt! planner for UR5 robots. Before running them, please
     make sure you have the :ref:`ROS backend <ros_backend>` correctly
     configured and the :ref:`UR5 Demo <ros_bundles_list>` started.
 
diff --git a/docs/examples/03_backends_ros/05_collision_objects.rst b/docs/examples/03_backends_ros/05_collision_objects.rst
@@ -5,7 +5,7 @@ Planning scene and collision objects
 .. note::
 
     The following examples use the `ROS <https://www.ros.org/>`_ backend
-    and the MoveI! planner for UR5 robots. Before running them, please
+    and the MoveIt! planner for UR5 robots. Before running them, please
     make sure you have the :ref:`ROS backend <ros_backend>` correctly
     configured and the :ref:`UR5 Demo <ros_bundles_list>` started.
 
diff --git a/src/compas_fab/backends/kinematics/solvers/offset_wrist.py b/src/compas_fab/backends/kinematics/solvers/offset_wrist.py
@@ -35,32 +35,47 @@ def forward_kinematics_offset_wrist(joint_values, params):
     s23, c23 = sin(q23), cos(q23)
     s234, c234 = sin(q234), cos(q234)
 
-    T = [0. for _ in range(4*4)]
-
-    T[0] = c234*c1*s5 - c5*s1
-    T[1] = c6*(s1*s5 + c234*c1*c5) - s234*c1*s6
-    T[2] = -s6*(s1*s5 + c234*c1*c5) - s234*c1*c6
-    T[3] = d6*c234*c1*s5 - a3*c23*c1 - a2 * \
-        c1*c2 - d6*c5*s1 - d5*s234*c1 - d4*s1
-    T[4] = c1*c5 + c234*s1*s5
-    T[5] = -c6*(c1*s5 - c234*c5*s1) - s234*s1*s6
-    T[6] = s6*(c1*s5 - c234*c5*s1) - s234*c6*s1
-    T[7] = d6*(c1*c5 + c234*s1*s5) + d4*c1 - \
-        a3*c23*s1 - a2*c2*s1 - d5*s234*s1
-    T[8] = -s234*s5
-    T[9] = -c234*s6 - s234*c5*c6
-    T[10] = s234*c5*s6 - c234*c6
-    T[11] = d1 + a3*s23 + a2*s2 - d5 * \
-        (c23*c4 - s23*s4) - d6*s5*(c23*s4 + s23*c4)
+    T = [0.0 for _ in range(4 * 4)]
+
+    T[0] = c234 * c1 * s5 - c5 * s1
+    T[1] = c6 * (s1 * s5 + c234 * c1 * c5) - s234 * c1 * s6
+    T[2] = -s6 * (s1 * s5 + c234 * c1 * c5) - s234 * c1 * c6
+    T[3] = (
+        d6 * c234 * c1 * s5
+        - a3 * c23 * c1
+        - a2 * c1 * c2
+        - d6 * c5 * s1
+        - d5 * s234 * c1
+        - d4 * s1
+    )
+    T[4] = c1 * c5 + c234 * s1 * s5
+    T[5] = -c6 * (c1 * s5 - c234 * c5 * s1) - s234 * s1 * s6
+    T[6] = s6 * (c1 * s5 - c234 * c5 * s1) - s234 * c6 * s1
+    T[7] = (
+        d6 * (c1 * c5 + c234 * s1 * s5)
+        + d4 * c1
+        - a3 * c23 * s1
+        - a2 * c2 * s1
+        - d5 * s234 * s1
+    )
+    T[8] = -s234 * s5
+    T[9] = -c234 * s6 - s234 * c5 * c6
+    T[10] = s234 * c5 * s6 - c234 * c6
+    T[11] = (
+        d1
+        + a3 * s23
+        + a2 * s2
+        - d5 * (c23 * c4 - s23 * s4)
+        - d6 * s5 * (c23 * s4 + s23 * c4)
+    )
     T[15] = 1.0
 
-    frame = Frame((T[3], T[7], T[11]),
-                  (T[0], T[4], T[8]), (T[1], T[5], T[9]))
+    frame = Frame((T[3], T[7], T[11]), (T[0], T[4], T[8]), (T[1], T[5], T[9]))
 
     return frame
 
 
-def inverse_kinematics_offset_wrist(frame, params, q6_des=0.):
+def inverse_kinematics_offset_wrist(frame, params, q6_des=0.0):
     """Inverse kinematics function for offset wrist 6-axis robots.
 
     Parameters
@@ -101,57 +116,57 @@ def inverse_kinematics_offset_wrist(frame, params, q6_des=0.):
     A = d6 * T12 - T13
     B = d6 * T02 - T03
     R = A * A + B * B
-    if(fabs(A) < ZERO_THRESH):
+    if fabs(A) < ZERO_THRESH:
         div = 0.0
-        if(fabs(fabs(d4) - fabs(B)) < ZERO_THRESH):
+        if fabs(fabs(d4) - fabs(B)) < ZERO_THRESH:
             div = -sign(d4) * sign(B)
         else:
             div = -d4 / B
         arcsin = asin(div)
-        if(fabs(arcsin) < ZERO_THRESH):
+        if fabs(arcsin) < ZERO_THRESH:
             arcsin = 0.0
-        if(arcsin < 0.0):
+        if arcsin < 0.0:
             q1[0] = arcsin + 2.0 * pi
         else:
             q1[0] = arcsin
         q1[1] = pi - arcsin
 
-    elif(fabs(B) < ZERO_THRESH):
+    elif fabs(B) < ZERO_THRESH:
         div = 0.0
-        if(fabs(fabs(d4) - fabs(A)) < ZERO_THRESH):
-            div = sign(d4)*sign(A)
+        if fabs(fabs(d4) - fabs(A)) < ZERO_THRESH:
+            div = sign(d4) * sign(A)
         else:
             div = d4 / A
         arccos = acos(div)
         q1[0] = arccos
         q1[1] = 2.0 * pi - arccos
 
-    elif(d4 * d4 > R):
+    elif d4 * d4 > R:
         raise ValueError("No solutions")
     else:
         arccos = acos(d4 / sqrt(R))
         arctan = atan2(-B, A)
         pos = arccos + arctan
         neg = -arccos + arctan
-        if(fabs(pos) < ZERO_THRESH):
+        if fabs(pos) < ZERO_THRESH:
             pos = 0.0
-        if(fabs(neg) < ZERO_THRESH):
+        if fabs(neg) < ZERO_THRESH:
             neg = 0.0
-        if(pos >= 0.0):
+        if pos >= 0.0:
             q1[0] = pos
         else:
-            q1[0] = 2.0*pi + pos
-        if(neg >= 0.0):
+            q1[0] = 2.0 * pi + pos
+        if neg >= 0.0:
             q1[1] = neg
         else:
-            q1[1] = 2.0*pi + neg
+            q1[1] = 2.0 * pi + neg
 
     # wrist 2 joint (q5)
     q5 = [[0, 0], [0, 0]]
     for i in range(2):
-        numer = (T03 * sin(q1[i]) - T13*cos(q1[i]) - d4)
+        numer = T03 * sin(q1[i]) - T13 * cos(q1[i]) - d4
         div = 0.0
-        if(fabs(fabs(numer) - fabs(d6)) < ZERO_THRESH):
+        if fabs(fabs(numer) - fabs(d6)) < ZERO_THRESH:
             div = sign(numer) * sign(d6)
         else:
             div = numer / d6
@@ -168,62 +183,66 @@ def inverse_kinematics_offset_wrist(frame, params, q6_des=0.):
             q6 = 0.0
 
             # wrist 3 joint (q6)
-            if(fabs(s5) < ZERO_THRESH):
+            if fabs(s5) < ZERO_THRESH:
                 q6 = q6_des
             else:
-                q6 = atan2(sign(s5)*-(T01*s1 - T11*c1),
-                           sign(s5)*(T00*s1 - T10*c1))
-            if(fabs(q6) < ZERO_THRESH):
+                q6 = atan2(
+                    sign(s5) * -(T01 * s1 - T11 * c1), sign(s5) * (T00 * s1 - T10 * c1)
+                )
+            if fabs(q6) < ZERO_THRESH:
                 q6 = 0.0
-            if(q6 < 0.0):
-                q6 += 2.0*pi
+            if q6 < 0.0:
+                q6 += 2.0 * pi
 
             # RRR joints (q2,q3,q4)
             q2, q3, q4 = [0, 0], [0, 0], [0, 0]
 
             c6 = cos(q6)
             s6 = sin(q6)
-            x04x = -s5*(T02*c1 + T12*s1) - c5 * \
-                (s6*(T01*c1 + T11*s1) - c6*(T00*c1 + T10*s1))
-            x04y = c5*(T20*c6 - T21*s6) - T22*s5
-            p13x = d5*(s6*(T00*c1 + T10*s1) + c6*(T01*c1 + T11*s1)
-                       ) - d6*(T02*c1 + T12*s1) + T03*c1 + T13*s1
-            p13y = T23 - d1 - d6*T22 + d5*(T21*c6 + T20*s6)
-
-            c3 = (p13x*p13x + p13y*p13y - a2*a2 - a3*a3) / (2.0*a2*a3)
-            if(fabs(fabs(c3) - 1.0) < ZERO_THRESH):
+            x04x = -s5 * (T02 * c1 + T12 * s1) - c5 * (
+                s6 * (T01 * c1 + T11 * s1) - c6 * (T00 * c1 + T10 * s1)
+            )
+            x04y = c5 * (T20 * c6 - T21 * s6) - T22 * s5
+            p13x = (
+                d5 * (s6 * (T00 * c1 + T10 * s1) + c6 * (T01 * c1 + T11 * s1))
+                - d6 * (T02 * c1 + T12 * s1)
+                + T03 * c1
+                + T13 * s1
+            )
+            p13y = T23 - d1 - d6 * T22 + d5 * (T21 * c6 + T20 * s6)
+
+            c3 = (p13x * p13x + p13y * p13y - a2 * a2 - a3 * a3) / (2.0 * a2 * a3)
+            if fabs(fabs(c3) - 1.0) < ZERO_THRESH:
                 c3 = sign(c3)
-            elif(fabs(c3) > 1.0):
+            elif fabs(c3) > 1.0:
                 solutions.extend([None, None])
                 continue
 
             arccos = acos(c3)
             q3[0] = arccos
-            q3[1] = 2.0*pi - arccos
-            denom = a2*a2 + a3*a3 + 2*a2*a3*c3
+            q3[1] = 2.0 * pi - arccos
+            denom = a2 * a2 + a3 * a3 + 2 * a2 * a3 * c3
             s3 = sin(arccos)
-            A = (a2 + a3*c3)
-            B = a3*s3
-            q2[0] = atan2((A*p13y - B*p13x) / denom,
-                          (A*p13x + B*p13y) / denom)
-            q2[1] = atan2((A*p13y + B*p13x) / denom,
-                          (A*p13x - B*p13y) / denom)
-            c23_0 = cos(q2[0]+q3[0])
-            s23_0 = sin(q2[0]+q3[0])
-            c23_1 = cos(q2[1]+q3[1])
-            s23_1 = sin(q2[1]+q3[1])
-            q4[0] = atan2(c23_0*x04y - s23_0*x04x, x04x*c23_0 + x04y*s23_0)
-            q4[1] = atan2(c23_1*x04y - s23_1*x04x, x04x*c23_1 + x04y*s23_1)
+            A = a2 + a3 * c3
+            B = a3 * s3
+            q2[0] = atan2((A * p13y - B * p13x) / denom, (A * p13x + B * p13y) / denom)
+            q2[1] = atan2((A * p13y + B * p13x) / denom, (A * p13x - B * p13y) / denom)
+            c23_0 = cos(q2[0] + q3[0])
+            s23_0 = sin(q2[0] + q3[0])
+            c23_1 = cos(q2[1] + q3[1])
+            s23_1 = sin(q2[1] + q3[1])
+            q4[0] = atan2(c23_0 * x04y - s23_0 * x04x, x04x * c23_0 + x04y * s23_0)
+            q4[1] = atan2(c23_1 * x04y - s23_1 * x04x, x04x * c23_1 + x04y * s23_1)
 
             for k in range(2):
-                if(fabs(q2[k]) < ZERO_THRESH):
+                if fabs(q2[k]) < ZERO_THRESH:
                     q2[k] = 0.0
-                elif(q2[k] < 0.0):
-                    q2[k] += 2.0*pi
-                if(fabs(q4[k]) < ZERO_THRESH):
+                elif q2[k] < 0.0:
+                    q2[k] += 2.0 * pi
+                if fabs(q4[k]) < ZERO_THRESH:
                     q4[k] = 0.0
-                elif(q4[k] < 0.0):
-                    q4[k] += 2.0*pi
+                elif q4[k] < 0.0:
+                    q4[k] += 2.0 * pi
 
                 solutions.append([q1[i], q2[k], q3[k], q4[k], q5[i][j], q6])
 
@@ -232,8 +251,9 @@ def inverse_kinematics_offset_wrist(frame, params, q6_des=0.):
 
 if __name__ == "__main__":
     from compas.geometry import allclose
+
     params = [0.089159, -0.42500, -0.39225, 0.10915, 0.09465, 0.0823]  # ur5
     q = [0.2, 5.5, 1.4, 1.3, 2.6, 3.6]
     frame = forward_kinematics_offset_wrist(q, params)
     sol = inverse_kinematics_offset_wrist(frame, params)
-    assert(allclose(sol[0], q))
+    assert allclose(sol[0], q)
diff --git a/src/compas_fab/backends/kinematics/utils.py b/src/compas_fab/backends/kinematics/utils.py
@@ -17,13 +17,16 @@ def fit_within_bounds(angle, lower, upper):
         angle += 2 * math.pi
     while angle > upper:
         angle -= 2 * math.pi
-    assert(angle >= lower and angle <= upper), "Joint angle out of bounds."
+    assert angle >= lower and angle <= upper, "Joint angle out of bounds."
     return angle
 
 
 def joint_angles_to_configurations(robot, solutions, group=None):
     joint_names = robot.get_configurable_joint_names(group=group)
-    return [Configuration.from_revolute_values(q, joint_names=joint_names) if q else None for q in solutions]
+    return [
+        Configuration.from_revolute_values(q, joint_names=joint_names) if q else None
+        for q in solutions
+    ]
 
 
 def try_to_fit_configurations_between_bounds(robot, configurations, group=None):
diff --git a/src/compas_fab/ghpython/components/Cf_AttachTool/code.py b/src/compas_fab/ghpython/components/Cf_AttachTool/code.py
@@ -8,7 +8,6 @@
 from compas_rhino.conversions import plane_to_compas_frame
 
 from compas.geometry import Frame
-from compas_fab.robots import PlanningScene
 from compas_fab.robots import Tool
 
 
@@ -27,8 +26,6 @@ def RunScript(self, robot, visual_mesh, collision_mesh, tcf_plane, group):
                 frame = plane_to_compas_frame(tcf_plane)
             tool = Tool(c_visual_mesh, frame, c_collision_mesh)
 
-            scene = PlanningScene(robot)
             robot.attach_tool(tool, group)
-            scene.add_attached_tool()
 
         return robot
diff --git a/src/compas_fab/ghpython/components/Cf_VisualizeRobot/code.py b/src/compas_fab/ghpython/components/Cf_VisualizeRobot/code.py
diff --git a/src/compas_fab/ghpython/components/Cf_VisualizeRobot/metadata.json b/src/compas_fab/ghpython/components/Cf_VisualizeRobot/metadata.json
