ra-mtp-ntnu / moto Goto Github PK

View Code? Open in Web Editor NEW

36.0 36.0 10.0 143 KB

A Python library for controlling Yaskawa MOTOMAN robots.

Home Page: https://github.com/tingelst/moto

License: Apache License 2.0

Python 100.00%

motoman

moto's People

Contributors

Stargazers

Watchers

Forkers

gavanderhoorn vebjorbb eastspawn mohshaf sebastiangrans pltlg acbuynak phantom1 vovodev motomanx

moto's Issues

Connection Refused when using sim with motoman

I'm trying to use MotoSim with motoman via ROS to get a feel for what it's capable of currently. I've created a small node to launch the sim:

#!/usr/bin/python

from moto.sim.motosim import ControlGroupSim, MotoSim
import rospy

if __name__ == "__main__":
    m = MotoSim("localhost", [ControlGroupSim(0, 6, [0.0] * 6)])
    m.start()
    while not rospy.is_shutdown():
        pass

and am launching motoman as follows:

    <include ns="$(arg robot_name)" file="$(find motoman_driver)/launch/robot_interface_streaming_yrc1000.launch">
      <arg name="robot_ip" value="localhost"/>
    </include>

However, I get the following errors on startup:

[ERROR] [1655978882.411580341] [/yaskawa/joint_state]: Failed to find topic_list parameter
[ WARN] [1655978882.412350151] [/yaskawa/joint_state]: Unable to find user-specified joint names in 'controller_joint_names'
[ WARN] [1655978882.413277066] [/yaskawa/joint_state]: Unable to find URDF joint names in 'robot_description'
[ERROR] [1655978882.413374606] [/yaskawa/joint_state]: Cannot find user-specified joint names. Tried ROS parameter 'controller_joint_names' and the URDF in 'robot_description'.
[ WARN] [1655978882.413569817] [/yaskawa/joint_state]: Unable to read 'controller_joint_names' param.  Using standard 6-DOF joint names.
[ERROR] [1655978882.413768800] [/yaskawa/joint_state]: Failed to connect to server, rc: -1. Error: 'Connection refused' (errno: 111)
[ERROR] [1655978882.415865930] [/yaskawa/joint_state]: Failed to connect to server, rc: -1. Error: 'Connection refused' (errno: 111)
[ WARN] [1655978882.415884926] [/yaskawa/joint_state]: Not connected, bytes not sent
[ERROR] [1655978882.415967579] [/yaskawa/joint_state]: Failed to receive message length
[ERROR] [1655978882.416037751] [/yaskawa/joint_state]: Failed to receive incoming message
[ WARN] [1655978882.416105299] [/yaskawa/joint_state]: Send failure, no callback support
...
[ WARN] [1655978882.476274494] [/yaskawa/io_relay]: Failed to get '~port' parameter: using default (50242)
[ERROR] [1655978882.493015715] [/yaskawa/io_relay]: Failed to connect to server, rc: -1. Error: 'Connection refused' (errno: 111)
[FATAL] [1655978882.493063405] [/yaskawa/io_relay]: Failed to connect
...
[ERROR] [1655978882.533790482] [/yaskawa/motion_streaming_interface]: Failed to find topic_list parameter
[ WARN] [1655978882.535837568] [/yaskawa/joint_trajectory_action]: Unable to find user-specified joint names in 'controller_joint_names'
[ WARN] [1655978882.536914233] [/yaskawa/joint_trajectory_action]: Unable to find URDF joint names in 'robot_description'
[ERROR] [1655978882.536943071] [/yaskawa/joint_trajectory_action]: Cannot find user-specified joint names. Tried ROS parameter 'controller_joint_names' and the URDF in 'robot_description'.
[ERROR] [1655978882.536972796] [/yaskawa/joint_trajectory_action]: Failed to initialize joint_names.
[ WARN] [1655978882.557413508] [/yaskawa/motion_streaming_interface]: Unable to find user-specified joint names in 'controller_joint_names'
[ WARN] [1655978882.558106726] [/yaskawa/motion_streaming_interface]: Unable to find URDF joint names in 'robot_description'
[ERROR] [1655978882.558181152] [/yaskawa/motion_streaming_interface]: Cannot find user-specified joint names. Tried ROS parameter 'controller_joint_names' and the URDF in 'robot_description'.
[ WARN] [1655978882.558324505] [/yaskawa/motion_streaming_interface]: Unable to read 'controller_joint_names' param.  Using standard 6-DOF joint names.
[ERROR] [1655978882.558618124] [/yaskawa/motion_streaming_interface]: Failed to connect to server, rc: -1. Error: 'Connection refused' (errno: 111)
[ WARN] [1655978882.559086520] [/yaskawa/motion_streaming_interface]: Unable to read velocity limits from 'robot_description' param.  Velocity validation disabled.
...
[ERROR] [1655978882.639311133] [/yaskawa/motion_streaming_interface]: Failed to connect to server, rc: -1. Error: 'Connection refused' (errno: 111)
[ERROR] [1655978882.889759774] [/yaskawa/motion_streaming_interface]: Timeout connecting to robot controller.  Send new motion command to retry.

I believe some of these errors may be due to me still having a fake MoveIt "controller" set up, however, I was expecting motoman would still be able to connect to the server (the ports and IP look correct). Any help would be much appreciated.

TimeoutError

Hello,

I am currently working on a hc10 from Yaskawa and was interested by your library to read and write on the ports of the robot in python.
I am not a socket specialist and went through this error which I can't find how to resolve:

➜  scripts git:(main) ✗ python3 test_moto.py       
Traceback (most recent call last):
  File "moto_controll.py", line 7, in <module>
    m = Moto(
  File "/home/aip/.local/lib/python3.8/site-packages/moto/__init__.py", line 155, in __init__
    self._state_connection.start()
  File "/home/aip/.local/lib/python3.8/site-packages/moto/state_connection.py", line 76, in start
    raise TimeoutError(
TimeoutError: Did not receive at least one of each message before timeout occured. Try increasing the timeout period. Timeout currently set to 5.0

I already tried to increase the timeout, and checked if the ports (like TCP_PORT_STATE = 50241) were the good ones. To do this I checked the ports used by a working MoveIt application on this robot. The controller is yrc1000, and the ip adress has been changed.

Please let me know if you have an idea,

Sincerely

Remove unnecessary abstraction? (Motion/MotionConnection, etc.)

Currently, the only point of the Motion, State, and IO classes are to call their corresponding <*>Connection instance.

What was the initial idea behind this design decision?

I propose we remove the <*>Connection classes unless there's a good reason for this abstraction.

Difficulties connecting to RealTimeMotionServerSim()

I have been trying to run the real-time motion server using the examples: test_moto_rt_motion_simulation.py and test_moto.py (replacing the IP with localhost)

When I run them I get this on the server side:

INFO:root:Waiting for connection
INFO:root:Got connection from ('127.0.0.1', 64822)
<socket.socket fd=568, family=AddressFamily.AF_INET, type=SocketKind.SOCK_STREAM, proto=0, laddr=('127.0.0.1', 50243), raddr=('127.0.0.1', 64822)>
INFO:root:Waiting for connection

and this on the client side:

ConnectionRefusedError: [WinError 10061] Kan ikke koble til fordi målmaskinen avslo tilkobling

The connection port does not seem to correspond with the TCP port.

Does anyone know how to deal with this issue?

Status of MotoSim?

There seems to be support for a form of 'simulation', which starts a set of server threads which implement subsets of the Simple Message services and topics supported by MotoROS.

It doesn't appear to be complete though.

I'm wondering what the status of it is, and whether there are any plans to complete it?

`StateConnection.start()` does not completely protect againt unexpected states.

After establishing a StateConnection with the robot, there's a moment before any state packets have arrived. This causes the internal variables to be None which is annoying to deal with. PR #8 tried to fix this (and PR #4).

My initial pull request (#8) seemed to be working, but I discovered that it was just a lucky coincidence. I then "fixed it" and pushed directly, although there is still a potential issue.

My design idea was that StateConnection.start() would block until all the internal variables had be set by incoming messages. In the current state, it is done as follows:

if not self._initial_response.is_set() and (
                isinstance(self.robot_status(), RobotStatus)
                and any(isinstance(elem, JointFeedback) for elem in self._joint_feedback)
                and isinstance(self.joint_feedback_ex(), JointFeedbackEx)
            ):
                self._initial_response.set()

Problem

The problem is that the list self._joint_feedback is a None list of length 4. If the user instantiate Moto with two control groups, we could still run into the scenario that only StateConnection has received a message from only one of them. I.e. Moto.state.joint_feedback(2) would return None.

Suggested solution

When the user constructs an instance of Moto, they provide a list of control group definitions, and hence we know how many instances of JointFeedback the user is expecting to query.

Changes to StateConnection:

def __init__(self, ip_address: str, number_of_control_groups: int = MOT_MAX_GR):
    ...
    self._number_of_control_groups = number_of_control_groups
    self._check_function = any if self._number_of_control_groups == 0 else all
    ....

def _worker(self): 
    ...
    # Inside if initial response set statement 
    and self_check_function(isinstance(elem, JointFeedback) for elem in self._joint_feedback[:MOT_MAX_GR])

Add to Moto:

self._state_connection: StateConnection = StateConnection(self._robot_ip, len(control_group_defs)

For a user who instantiates Moto, the functions Moto.state.robot_status() and others will behave as expected.

And the user who instantiates StateConnection is at least partially protected from receiving None values.

Motosim server unpack error

Hi again!

I am getting an unpack error when I run the simulation server and moto_test.py
python -i sim.py

from moto.sim.motosim import ControlGroupSim, MotoSim

m = MotoSim("localhost", [ControlGroupSim(0, 6, [0.0] * 6)])
m.start()

python moto_test.py

from moto import motion_connection
from moto import Moto, ControlGroupDefinition
from moto.simple_message import (
    JointFeedbackEx,
    JointTrajPtExData,
    JointTrajPtFull,
    JointTrajPtFullEx,
    ValidFields,
)

import time
import numpy as np
import copy

m = Moto(
    "localhost",
    [
        ControlGroupDefinition(
            groupid="robot",
            groupno=0,
            num_joints=6,
            joint_names=[
                "joint_1_s",
                "joint_2_l",
                "joint_3_u",
                "joint_4_r",
                "joint_5_b",
                "joint_6_t",
            ],
        ),
    ],
    start_motion_connection=True,
    start_state_connection=True,
)


robot = m.control_groups["robot"]

time.sleep(1)


robot_joint_feedback: JointFeedbackEx = m.state.joint_feedback(groupno=0)

p0 = JointTrajPtFullEx(
    number_of_valid_groups=1,
    sequence=0,
    joint_traj_pt_data=[
        JointTrajPtExData(
            groupno=0,
            valid_fields=ValidFields.TIME | ValidFields.POSITION | ValidFields.VELOCITY,
            time=0.0,
            pos=robot_joint_feedback.pos,
            vel=[0.0] * 10,
            acc=[0.0] * 10,
        ),
    ],
)

p1 = JointTrajPtFullEx(
    number_of_valid_groups=1,
    sequence=1,
    joint_traj_pt_data=[
        JointTrajPtExData(
            groupno=0,
            valid_fields=ValidFields.TIME | ValidFields.POSITION | ValidFields.VELOCITY,
            time=5.0,
            pos=np.deg2rad([10.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]),
            vel=[0.0] * 10,
            acc=[0.0] * 10,
        ),
    ],
)

Console:

DEBUG:root:Setting motion update rate (hz): 100.000000
DEBUG:root:Setting initial joint state: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
DEBUG:root:Setting motion buffer size: 0
DEBUG:root:Starting motion worker in motion controller simulator
[motion_server]: Waiting for connection
[state_server]: Waiting for connection
[io_server]: Waiting for connection
>>> [motion_server]: Got connection from ('127.0.0.1', 59086)
[state_server]: Got connection from ('127.0.0.1', 59087)
[io_server]: Got connection from ('127.0.0.1', 59088)
Exception in thread Thread-2:
Traceback (most recent call last):
Exception in thread Thread-3:
  File "C:\Users\tehtt\AppData\Local\Programs\Python\Python39\lib\threading.py", line 954, in _bootstrap_inner
Traceback (most recent call last):
  File "C:\Users\tehtt\AppData\Local\Programs\Python\Python39\lib\threading.py", line 954, in _bootstrap_inner
    self.run()
  File "C:\Users\tehtt\AppData\Local\Programs\Python\Python39\lib\threading.py", line 892, in run
    self.run()
  File "C:\Users\tehtt\AppData\Local\Programs\Python\Python39\lib\threading.py", line 892, in run
    self._target(*self._args, **self._kwargs)
  File "C:\Users\tehtt\AppData\Local\Programs\Python\Python39\lib\site-packages\moto\sim\motosim.py", line 112, in _worker
    self._target(*self._args, **self._kwargs)
  File "C:\Users\tehtt\AppData\Local\Programs\Python\Python39\lib\site-packages\moto\sim\motosim.py", line 187, in _worker
    msg = SimpleMessage.from_bytes(self._conn.recv(1024))
  File "C:\Users\tehtt\AppData\Local\Programs\Python\Python39\lib\site-packages\moto\simple_message.py", line 986, in from_bytes
    self._conn.sendall(msg.to_bytes())
ConnectionResetError: [WinError 10054] An existing connection was forcibly closed by the remote host
    header = Header.from_bytes(bytes_[4:16])
  File "C:\Users\tehtt\AppData\Local\Programs\Python\Python39\lib\site-packages\moto\simple_message.py", line 174, in from_bytes
    return cls(*cls.struct_.unpack(bytes_))
struct.error: unpack requires a buffer of 12 bytes

Is this a bug? Or am I doing something wrong? Any help is appreciated.

Is there a way to get the torque data using the simple message?

What I am trying to retrieve is actually a MotoRos function from the ControlGroup.h torque data using Ros_CtrlGroup_GetTorque function. Could you please guide me on how to achieve this using simple message?

enable robot is not implemented yet?

I saw a mention about m.motion.enable_robot() to start the robot, but didn't find the corresponding function anywhere in the repo.

Is it still under development??

P.S. I really appreciate the work you did with this and am willing to contribute with my best effort.

ra-mtp-ntnu / moto Goto Github PK

moto's People

Contributors

Stargazers

Watchers

Forkers

moto's Issues

Problem

Suggested solution

Recommend Projects

Recommend Topics

Recommend Org

Jobs