Merge pull request #112 from compas-dev/start_full_configuration

start FULL configuration for planning service calls

Author: romanarust

CHANGELOG.rst

@@ -13,10 +13,20 @@ Unreleased
 
 **Added**
 
-* ..
+* Added optional ``joint_names`` to ``Configuration``
+* Added ``Configuration.scaled``
+* Added ``full_joint_state`` to ``Robot.inverse_kinematics``
+* Added ``Semantics.get_all_configurable_joints``
 
 **Changed**
 
+* Construct ``full_configuration`` with ``values``, ``types``, ``joint_names`` in ``Robot`` rather than in ``MoveItPlanner``
+* ``MoveItPlanner`` returns ``start_configuration`` with set ``joint_names``
+* Removed parameter ``names`` from ``RobotArtist.update``
+* Updated Grasshopper examples
+* ``Robot``: ``forward_kinematics`` returns now ``frame_WCF``
+* ``MoveItPlanner``: ``forward_kinematics`` takes now instance of ``Configuration`` and ``robot``
+* ``MoveItPlanner``: ``inverse_kinematics`` takes now instance of ``Configuration`` and ``robot``
 * Property :class:`compas_fab.robots.Robot.artist` does not try to scale robot
   geometry if links and/or joints are not defined.
 * In :class:``compas_fab.robots.constraints.JointConstraint``, added ``tolerance_above`` and

docs/examples/02_description_models/files/02_robot_artist_grasshopper.ghx

@@ -4,10 +4,7 @@
 robot = Robot()
 configuration = Configuration.from_revolute_values([-2.238, -1.153, -2.174, 0.185, 0.667, 0.])
 
-frame_RCF = robot.forward_kinematics(configuration)
-frame_WCF = robot.to_world_coords(frame_RCF)
+frame_WCF = robot.forward_kinematics(configuration)
 
-print("Frame in the robot's coordinate system")
-print(frame_RCF)
 print("Frame in the world coordinate system")
 print(frame_WCF)

docs/examples/03_backends_ros/files/02_forward_kinematics.py

@@ -6,10 +6,7 @@
     robot = Robot(client)
     configuration = Configuration.from_revolute_values([-2.238, -1.153, -2.174, 0.185, 0.667, 0.])
 
-    frame_RCF = robot.forward_kinematics(configuration)
-    frame_WCF = robot.to_world_coords(frame_RCF)
+    frame_WCF = robot.forward_kinematics(configuration)
 
-    print("Frame in the robot's coordinate system")
-    print(frame_RCF)
     print("Frame in the world coordinate system")
     print(frame_WCF)

docs/examples/03_backends_ros/files/02_forward_kinematics_ros.py

@@ -6,10 +6,7 @@
 
     configuration = Configuration.from_prismatic_and_revolute_values([0.], [-2.238, -1.153, -2.174, 0.185, 0.667, 0.])
 
-    frame_RCF = robot.forward_kinematics(configuration)
-    frame_WCF = robot.to_world_coords(frame_RCF)
+    frame_WCF = robot.forward_kinematics(configuration)
 
-    print("Frame in the robot's coordinate system")
-    print(frame_RCF)
     print("Frame in the world coordinate system")
     print(frame_WCF)

docs/examples/03_backends_ros/files/02_forward_kinematics_ros_loader.py

@@ -6,7 +6,7 @@
     robot = Robot(client)
 
     frame_WCF = Frame([0.3, 0.1, 0.5], [1, 0, 0], [0, 1, 0])
-    start_configuration = robot.init_configuration()
+    start_configuration = robot.zero_configuration()
 
     configuration = robot.inverse_kinematics(frame_WCF, start_configuration)
 

docs/examples/03_backends_ros/files/02_inverse_kinematics.py

@@ -5,7 +5,7 @@
     robot = client.load_robot()
 
     frame_WCF = Frame([0.3, 0.1, 0.5], [1, 0, 0], [0, 1, 0])
-    start_configuration = robot.init_configuration()
+    start_configuration = robot.zero_configuration()
 
     configuration = robot.inverse_kinematics(frame_WCF, start_configuration)
 

docs/examples/03_backends_ros/files/02_inverse_kinematics_ros_loader.py

@@ -0,0 +1,23 @@
+"""Calculate the robot's forward kinematic.
+    Inputs:
+        robot: :class:`compas_fab.robots.Robot`
+            The robot.
+        group: str, optional
+            The planning group used for calculation. Defaults to the robot's
+            main planning group.
+        full_configuration : :class:`compas_fab.robots.Configuration`
+            The full configuration to calculate the forward kinematic for. If no
+            full configuration is passed, the zero-joint state for the other
+            configurable joints is assumed.
+    Output:
+        :class:`Frame`
+            The frame in the world coordinate frame (WCF).
+"""
+from __future__ import print_function
+
+if robot and robot.client and full_configuration:
+    if robot.client.is_connected:
+        frame_WCF = robot.forward_kinematics(full_configuration, group, backend='model')
+        print(frame_WCF)
+    else:
+        print("Robot is not connected")

docs/examples/03_backends_ros/files/0x_load_robot_6.ghx

@@ -0,0 +1,35 @@
+"""Calculate the robot's inverse kinematic for a given plane.
+    Inputs:
+        robot: The robot
+        group: str, optional
+            The planning group used for calculation. Defaults to the robot's
+            main planning group.
+        plane: The plane in world coordinate frame to calculate the inverse for.
+        start_configuration: :class:`compas_fab.robots.Configuration`, optional
+            If passed, the inverse will be calculated such that the calculated
+            joint positions differ the least from the start_configuration.
+            Defaults to the init configuration.
+        avoid_collisions: bool, optional
+            Whether or not to avoid collisions. Defaults to True.
+    Output:
+        group_configuration: The group configuration
+        full_configuration: The full configuration
+"""
+from __future__ import print_function
+
+from compas.geometry import Frame
+
+
+frame = Frame(plane.Origin, plane.XAxis, plane.YAxis)
+
+if robot and robot.client:
+    if robot.client.is_connected:
+        full_configuration = robot.inverse_kinematics(frame,
+                                                      start_configuration,
+                                                      group=group,
+                                                      avoid_collisions=avoid_collisions,
+                                                      return_full_configuration=True)
+        group_configuration = robot.get_group_configuration(group, full_configuration)
+        print(group_configuration)
+    else:
+        print("Robot client is not connected")