Merge pull request #5 from SINTEFMedtek/develop

Develop

README.md

@@ -1,6 +1,8 @@
 libromocc
 =========
 
+[![Build badge](https://github.com/SINTEFMedtek/libromocc/workflows/cmake_build.yml/badge.svg?branch=master&event=push)](https://github.com/SINTEFMedtek/libromocc/actions)
+
 libromocc is a lightweight C++ library for **ro**bot **mo**delling, **c**ontrol and **c**ommunication. It
 also contains Python wrappers with a corresponding PyPI distribution.
 

cmake/ExternalEigen.cmake

@@ -5,7 +5,7 @@ include(cmake/Externals.cmake)
 ExternalProject_Add(eigen
         PREFIX ${ROMOCC_EXTERNAL_BUILD_DIR}/eigen
         BINARY_DIR ${ROMOCC_EXTERNAL_BUILD_DIR}/eigen/build
-        URL "https://bitbucket.org/eigen/eigen/get/f3a22f35b044.tar.gz"
+        URL "https://gitlab.com/libeigen/eigen/-/archive/603e213d13311af286c8c1abd4ea14a8bd3d204e/eigen-603e213d13311af286c8c1abd4ea14a8bd3d204e.tar.gz"
         INSTALL_DIR ${ROMOCC_EXTERNAL_INSTALL_DIR}
         CMAKE_CACHE_ARGS
         -DCMAKE_BUILD_TYPE:STRING=Release

source/pyromocc/examples/robot_move_adaptive.py

@@ -0,0 +1,51 @@
+from pyromocc import Robot
+import numpy as np
+import matplotlib.pyplot as plt
+from time import sleep
+
+np.set_printoptions(precision=3)
+fig = plt.figure()
+ax3d = fig.add_subplot(1, 2, 1, projection='3d')
+
+figj, axj = plt.subplots(nrows=2, ncols=6)
+
+robot = Robot(ip="192.168.153.128", port=30003, manipulator="UR5", sw_version="3.0")
+robot.connect()
+
+sleep(2.0)
+print(robot.joint_config)
+
+target_q = [0.0, -np.pi/2, -np.pi/2, -np.pi/2, 0, 0]
+target_pose_aa = robot.forward_kinematics(target_q, format='axis_angle')
+
+robot.movej(target_q, 5, 10)
+print(robot.inverse_jacobian())
+print(robot.jacobian())
+
+count = 0
+while count < 1000:
+    x, y, z, rx, ry, rz = robot.pose_aa
+    ax3d.scatter(x, y, z, color='b')
+    ax3d.scatter(target_pose_aa[0], target_pose_aa[1], target_pose_aa[2], color='r')
+
+    axj[0, 0].plot(count, robot.joint_config[0], 'ro')
+    axj[0, 1].plot(count, robot.joint_config[1], 'ro')
+    axj[0, 2].plot(count, robot.joint_config[2], 'ro')
+    axj[0, 3].plot(count, robot.joint_config[3], 'ro')
+    axj[0, 4].plot(count, robot.joint_config[4], 'ro')
+    axj[0, 5].plot(count, robot.joint_config[5], 'ro')
+
+    axj[1, 0].plot(count, robot.joint_velocity[0], 'ro')
+    axj[1, 1].plot(count, robot.joint_velocity[1], 'ro')
+    axj[1, 2].plot(count, robot.joint_velocity[2], 'ro')
+    axj[1, 3].plot(count, robot.joint_velocity[3], 'ro')
+    axj[1, 4].plot(count, robot.joint_velocity[4], 'ro')
+    axj[1, 5].plot(count, robot.joint_velocity[5], 'ro')
+
+    count += 1
+    plt.draw()
+    plt.pause(1/125.0)
+
+sleep(1.0)
+print(robot.joint_config)
+print(robot.pose)

source/pyromocc/examples/robotiq_gripper.py

@@ -0,0 +1,9 @@
+from pyromocc import Robot
+from pyromocc.tools import RobotiqGripper
+
+robot = Robot(ip="10.218.93.156", port=30003, manipulator="UR5", sw_version="3.0")
+robot.connect()
+
+robotiqgrip = RobotiqGripper(robot=robot, force=255, speed=50)
+robotiqgrip.open_gripper()
+robotiqgrip.close_gripper()

source/pyromocc/pyromocc/robot.py

@@ -1,4 +1,5 @@
 from .pyromocc import *
+from typing import Union
 
 
 class Robot(RobotBase):
@@ -35,6 +36,10 @@ def __init__(self, ip: str, port: int = 30003, manipulator: str = None, **kwargs
     def joint_config(self):
         return self.get_state().get_joint_config()
 
+    @property
+    def joint_velocity(self):
+        return self.get_state().get_joint_velocity()
+
     @property
     def pose(self):
         return self.get_state().get_pose()
@@ -102,4 +107,30 @@ def rz(self):
     def rz(self, val):
         p = self.pose_aa
         p[5] = val
-        self.movep(p, 50, 100)
+        self.movep(p, 50, 100)
+
+    def forward_kinematics(self, joint_config, format="homogeneous"):
+        pose = self.get_state().joint_to_pose(joint_config)
+        if format == "axis_angle":
+            return pose_to_vector(pose)
+        return pose
+
+    def inverse_kinematics(self, pose):
+        pose[:, 3][:3] = pose[:, 3][:3] / 1000
+        joint_config = self.get_state().pose_to_joint(pose)
+        return joint_config
+
+    def jacobian(self):
+        return self.get_state().get_jacobian()
+
+    def inverse_jacobian(self):
+        return self.get_state().get_inverse_jacobian()
+
+    def send_program(self, program: Union[str, bytes]):
+        """
+        Sends program to the robot in URScript format.
+        """
+        program.strip()
+        if not isinstance(program, bytes):
+            program = program.encode()
+        self._send_program(program)

source/pyromocc/pyromocc/tools/__init__.py

@@ -0,0 +1 @@
+from .robotiq_gripper import RobotiqGripper

source/pyromocc/pyromocc/tools/robotiq_gripper.py

@@ -0,0 +1,125 @@
+import time
+from .urscript import URScript
+
+
+class RobotiqScript(URScript):
+    """
+    Object for generating scripts for the Robotiq gripper. Adapted from python-urx.
+    """
+
+    def __init__(self, socket_host="127.0.0.1", socket_port=63352, socket_name="gripper_socket"):
+        self.socket_host = socket_host
+        self.socket_port = socket_port
+        self.socket_name = socket_name
+        super(RobotiqScript, self).__init__()
+
+        # Reset connection to gripper
+        self.socket_close(self.socket_name)
+        self.socket_open(self.socket_host, self.socket_port, self.socket_name)
+
+    def set_gripper_activate(self):
+        self.socket_set_var("GTO", 1, self.socket_name)
+
+    def set_gripper_force(self, value):
+        """
+        FOR is the variable
+        range is 0 - 255
+        0 is no force
+        255 is full force
+        """
+        value = self._constrain_unsigned_char(value)
+        self.socket_set_var("FOR", value, self.socket_name)
+
+    def set_gripper_position(self, value):
+        """
+        SPE is the variable
+        range is 0 - 255
+        0 is no speed
+        255 is full speed
+        """
+        value = self._constrain_unsigned_char(value)
+        self.socket_set_var("POS", value, self.socket_name)
+
+    def set_gripper_speed(self, value):
+        """
+        SPE is the variable
+        range is 0 - 255
+        0 is no speed
+        255 is full speed
+        """
+        value = self._constrain_unsigned_char(value)
+        self.socket_set_var("SPE", value, self.socket_name)
+
+    def set_robot_activate(self):
+        self.socket_set_var("ACT", 1, self.socket_name)
+
+
+class RobotiqGripper(object):
+    def __init__(self, robot, payload=0.85, speed=255, force=50, socket_host="127.0.0.1", socket_port=63352,
+                 socket_name="gripper_socket"):
+
+        self.robot = robot
+        self.payload = payload
+        self.speed = speed
+        self.force = force
+        self.socket_host = socket_host
+        self.socket_port = socket_port
+        self.socket_name = socket_name
+
+    def _setup_urscript(self):
+        """
+        Set up a new URScript to communicate with gripper
+        """
+        urscript = RobotiqScript(socket_host=self.socket_host, socket_port=self.socket_port,
+                                 socket_name=self.socket_name)
+
+        # Set input and output voltage ranges
+        urscript.set_analog_input_range(0, 0)
+        urscript.set_analog_input_range(1, 0)
+        urscript.set_analog_input_range(2, 0)
+        urscript.set_analog_input_range(3, 0)
+        urscript.set_analog_outputdomain(0, 0)
+        urscript.set_analog_outputdomain(1, 0)
+        urscript.set_tool_voltage(0)
+        urscript.set_runstate_outputs()
+
+        # Set payload, speed and force
+        urscript.set_payload(self.payload)
+        urscript.set_gripper_speed(self.speed)
+        urscript.set_gripper_force(self.force)
+
+        # Initialize the gripper
+        urscript.set_robot_activate()
+        urscript.set_gripper_activate()
+
+        # Wait on activation to avoid USB conflicts
+        urscript.sleep(0.1)
+
+        return urscript
+
+    def gripper_action(self, value):
+        """
+        Activate the gripper to a given value from 0 to 255
+
+        0 is open
+        255 is closed
+        """
+        urscript = self._setup_urscript()
+
+        # Move to the position
+        sleep = 2.0
+        urscript.set_gripper_position(value)
+        urscript.sleep(sleep)
+
+        # Send the script
+        self.robot.send_program(urscript())
+
+        # sleep the code the same amount as the urscript to ensure that
+        # the action completes
+        time.sleep(sleep)
+
+    def open_gripper(self):
+        self.gripper_action(0)
+
+    def close_gripper(self):
+        self.gripper_action(255)

source/pyromocc/pyromocc/tools/urscript.py

@@ -0,0 +1,128 @@
+import warnings
+
+
+class URScript(object):
+    """
+    Object for generating URScript. Adapted from python-urx.
+    """
+
+    def __init__(self, **kwargs):
+        self.header = None
+        self.program = None
+
+        self.controller_ports = kwargs.get("controller_ports", [0, 1])
+        self.controller_voltage = kwargs.get("controller_voltage", [0, 2])
+        self.tool_ports = kwargs.get("tool_ports", [2, 3])
+        self.tool_voltage = kwargs.get("tool_voltage", [0, 1, 2])
+        self.output_domain_voltage = kwargs.get("output_domain_voltage", [0, 1])
+
+    def __call__(self):
+        if self.program is None:
+            warnings.warn("URScript program is empty")
+            return None
+
+        # Construct the program
+        myprog = """def myProg():{}\nend""".format(self.program)
+
+        # Construct the full script
+        script = ""
+        if self.header:
+            script = "{}\n\n".format(self.header)
+        script = "{}{}".format(script, myprog)
+        return script
+
+    def reset(self):
+        self.__init__()
+
+    def add_header_to_program(self, header_line):
+        self.header = "{}\n{}".format(self.header, header_line)
+
+    def add_line_to_program(self, new_line):
+        self.program = "{}\n\t{}".format(self.program, new_line)
+
+    def _constrain_unsigned_char(self, value):
+        """
+        Ensure that unsigned char values are constrained to between 0 and 255.
+        """
+        assert(isinstance(value, int))
+        if value < 0:
+            value = 0
+        elif value > 255:
+            value = 255
+        return value
+
+    def set_analog_input_range(self, port, vrange):
+        if port in self.controller_ports:
+            assert(vrange in self.controller_voltage)
+        elif port in self.tool_ports:
+            assert(vrange in self.tool_voltage)
+        msg = "set_analog_inputrange({},{})".format(port, vrange)
+        self.add_line_to_program(msg)
+
+    def set_analog_output(self, input_id, signal_level):
+        assert(input_id in [0, 1])
+        assert(signal_level in [0, 1])
+        msg = "set_analog_output({}, {})".format(input_id, signal_level)
+        self.add_line_to_program(msg)
+
+    def set_analog_outputdomain(self, port, domain):
+        assert(domain in self.output_domain_voltage)
+        msg = "set_analog_outputdomain({},{})".format(port, domain)
+        self.add_line_to_program(msg)
+
+    def set_payload(self, mass, cog=None):
+        msg = "set_payload({}".format(mass)
+        if cog:
+            assert(len(cog) == 3)
+            msg = "{},{}".format(msg, cog)
+        msg = "{})".format(msg)
+        self.add_line_to_program(msg)
+
+    def set_runstate_outputs(self, outputs=None):
+        if not outputs:
+            outputs = []
+        msg = "set_runstate_outputs({})".format(outputs)
+        self.add_line_to_program(msg)
+
+    def set_tool_voltage(self, voltage):
+        assert(voltage in [0, 12, 24])
+        msg = "set_tool_voltage({})".format(voltage)
+        self.add_line_to_program(msg)
+
+    def sleep(self, value):
+        msg = "sleep({})".format(value)
+        self.add_line_to_program(msg)
+
+    def socket_close(self, socket_name):
+        msg = "socket_close(\"{}\")".format(socket_name)
+        self.add_line_to_program(msg)
+
+    def socket_get_var(self, var, socket_name):
+        msg = "socket_get_var(\"{}\",\"{}\")".format(var, socket_name)
+        self.add_line_to_program(msg)
+        self.sync()
+
+    def socket_open(self, socket_host, socket_port, socket_name):
+        msg = "socket_open(\"{}\",{},\"{}\")".format(socket_host,
+                                                     socket_port,
+                                                     socket_name)
+        self.add_line_to_program(msg)
+
+    def socket_read_byte_list(self, nbytes, socket_name):
+        msg = "global var_value = socket_read_byte_list({},\"{}\")".format(nbytes, socket_name)  # noqa
+        self.add_line_to_program(msg)
+        self.sync()
+
+    def socket_send_string(self, message, socket_name):
+        msg = "socket_send_string(\"{}\",\"{}\")".format(message, socket_name)  # noqa
+        self.add_line_to_program(msg)
+        self.sync()
+
+    def socket_set_var(self, var, value, socket_name):
+        msg = "socket_set_var(\"{}\",{},\"{}\")".format(var, value, socket_name)  # noqa
+        self.add_line_to_program(msg)
+        self.sync()
+
+    def sync(self):
+        msg = "sync()"
+        self.add_line_to_program(msg)

source/pyromocc/setup.py

@@ -11,8 +11,9 @@
 
 setup(name=package_name,
       version='0.0.3',
-      author="Andreas Østvik",
+      author="Andreas Oestvik",
       packages=find_packages(exclude=['third_party', 'examples']),
       install_requires=['numpy'],
+      extras_require={'examples': ["matplotlib"]},
       include_package_data=True,
       package_data=package_data)