squirrel-project / squirrel_driver Goto Github PK

https://github.com/squirrel-project/squirrel_recommender/blob/master/docu/SquirrelDriverConvention.pdf

@bajo seems that alufr will not get the new shell and keep the old dynamixel unit, that means we need to keep at least the robotino_tilt_controller , as alufr don't have a pan axis I think make not sense a viewcontroller node for this hardware. @bajo what do you think?

In squirrel_driver/squirrel_sensing_node/src/sensing_drivers.cpp the network interface "eth0" should be made configurable. E.g., on the robotino it is "em1".

@ipa-nhg To make the viewcontroller behave correctly I had to change the direction of the calculated relative pan and tilt. I assume this is due to the orientation changes when updating the urdf file. I think the orientation of the neck_pan_link and the neck_tilt_link is rotated compared to the old links. Is this something intentionally? Or is the direction of the motor movement in the other direction compared to the dynamixel pan/tilt from before? Might be important for others to know as well.

When the wrist safety node is triggered it does not reset in the same way as the bumber and airskin. Could it be implemented similarly so that we do not have to manually reset the wrist. Also, the while loop (line 41) should be
while(ros::ok()){ ... }
not
while(1){ ... }
to ensure that the node is correctly terminated from the terminal.

squirrel_sensing_node does not provide anymore a launch file. why? can we please put that back there for specifying relevant devices instead of passing them as CL arguments? They generally should no change on the same robot given that proper udev rules are there.

The base controler as part of the full 8 DOF contoller accumulates drift. Also the 8 DOF planner does not replan when the path is no longer collision free.
Therefore for long base trajectories move_base should be used (replans when obstacle appears, and uses precise localisation)
I.e. manipulate locally using 8 DOF plan and control, with rather small base movements
-> navigate near target position using move_base
-> manipulate locally again
This way we get the best of both worlds.
Actually, there is nothing we have to do here. The top level planner takes care of moving between nav goals.

it seems to be impossible to move the motors controlled by idmind's controller board at the same time (would make sense to move neck and head at the same time) or short after another.

The resulting error is:

Traceback (most recent call last):
  File "/opt/ros/indigo/lib/python2.7/dist-packages/rospy/topics.py", line 720, in _invoke_callback
    cb(msg)
  File "/u/bajo/catkin_ws/src/squirrel_driver/squirrel_interaction/src/squirrel_interaction/board/board.py", line 77, in move_neck
    self._motor.move_to("neck", self.neck_destination)
  File "/u/bajo/catkin_ws/src/squirrel_driver/squirrel_interaction/src/squirrel_interaction/board/serial_api.py", line 58, in move_to
    response.extend(self.serial.read(5))
  File "/usr/local/lib/python2.7/dist-packages/serial/serialposix.py", line 501, in read
    'device reports readiness to read but returned no data '
SerialException: device reports readiness to read but returned no data (device disconnected or multiple access on port?)```
The first movement is executed, the second one fails without feedback for the "caller node", as it is handled via a publisher/subscriber. This is not a suitable solution.

Is it possible for idmind to implement a way to move the motors in parallel?

Airskin node provides a boolean topic /airskin/arm_bumper, which the arm controller needs to subscribe to.
If the airskin is pressed ( = collision with something) the data will be True and the arm has to stop the movement. At least until the data on the topic is back to False again.
@shangl @Senka2112 @qusaisuwan

ACTUATORS

NECK:
topic publisher: /joint_states[sensor_msgs/JointState]
topic subscriber: /neck_controller/joint_group_position_controller/command[std_msgs/Float64]

DOOR:
topic publisher: /joint_states [sensor_msgs/JointState]
service : /door_controller/command [request:std_msgs/String (open or close?) ; response:std_msgs/Bool]

LIGHT:
topic subscriber: /light/command [std_msgs/ColorRGBA] (also a service who answer with a bool could be possible)

SOUND:
service : /sound/say [request:std_msgs/String (text to speech) ; response:std_msgs/Bool]

FACE:
service: /face/emotion [request:std_msgs/String (emotion: confused,greeting,no,think); response:std_msgs/Bool]

@ipa-nhg @mzillich @edith-langer @tpatten @jibweb

We received the modified robot and tried to use it and found a lot of issues.

1. Raspberry pi not in the correct network -> unable to connect
2. Raspberry pi not configured for ROS with multiple machines (ROS_MASTER_URI, etc. misconfigured)
3. Wifi does not autostart on robotino's internal pc (needs manual ifdown wlan0 / ifup wlan0)
4. Unable to start roslaunch robotino_bringup tuw-robotino2.launch from any user except squirrel
   Error is `roslaunch robotino_bringup tuw-robotino2.launch

... logging to /home/bajo/.ros/log/668f2dfc-9ec6-11e7-bfb3-00190f153f00/roslaunch-robotino-16659.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.

Traceback (most recent call last):
File "/opt/ros/indigo/share/xacro/xacro.py", line 62, in
xacro.main()
File "/opt/ros/indigo/lib/python2.7/dist-packages/xacro/init.py", line 696, in main
eval_self_contained(doc)
File "/opt/ros/indigo/lib/python2.7/dist-packages/xacro/init.py", line 626, in eval_self_contained
eval_all(doc.documentElement, macros, symbols)
File "/opt/ros/indigo/lib/python2.7/dist-packages/xacro/init.py", line 552, in eval_all
(",".join(params), str(node.tagName)))
xacro.XacroException: Parameters [has_8dof] were not set for macro xacro:base
Invalid tag: Cannot load command parameter [robot_description]: command [/opt/ros/indigo/share/xacro/xacro.py '/home/bajo/catkin_ws/src/squirrel_robotino/robotino_bringup/robots/tuw-robotino2/urdf/robotino.urdf.xacro'] returned with code [1].

Param xml is
The traceback for the exception was written to the log file
`

5. Unable to start the new arm controller roslaunch squirrel_control squirrel_control.launch
   Error is `ROS_MASTER_URI=http://robotino:11311

core service [/rosout] found
process[robotino_node-1]: started with pid [16797]
process[robotino_safety_node-2]: started with pid [16798]
process[robotino_odometry_node-3]: started with pid [16799]
process[twist_mux-4]: started with pid [16815]
process[twist_marker-5]: started with pid [16832]
process[arm_controller/squirrel_control_node-6]: started with pid [16855]
process[arm_controller/ros_control_controller_manager-7]: started with pid [16863]
process[arm_controller/robot_state_publisher-8]: started with pid [16880]
[ INFO] [1506001035.005349583]: Topic handler 'teleop' subscribed to topic 'cmd_teleop': timeout = 0.250000s, priority = 100
process[arm_controller/joint_states_relay-9]: started with pid [16888]
[ INFO] [1506001035.048606554]: Topic handler 'navigation' subscribed to topic 'cmd_navigation': timeout = 0.250000s, priority = 90
[ INFO] [1506001035.079042288]: Odometry connected to Robotino.
[ INFO] [1506001035.094627644]: Topic handler 'pushing' subscribed to topic 'cmd_pushing': timeout = 0.250000s, priority = 80
[ INFO] [1506001035.119603430]: Topic handler 'rotatory' subscribed to topic 'cmd_rotatory': timeout = 0.500000s, priority = 70
[ INFO] [1506001035.140738376]: Topic handler 'simulation' subscribed to topic 'cmd_sim': timeout = 0.500000s, priority = 1
[ INFO] [1506001035.160900205]: Topic handler 'pause_navigation' subscribed to topic 'twist_mux/locks/pause_navigation': timeout = None, priority = 101
[ INFO] [1506001035.187610348]: Topic handler 'pause_pushing' subscribed to topic 'twist_mux/locks/pause_pushing': timeout = None, priority = 91
[ INFO] [1506001035.194989512]: Topic handler 'pause_rotatory' subscribed to topic 'twist_mux/locks/pause_rotatory': timeout = None, priority = 81
[ INFO] [1506001035.199064080]: RobotinoNode connected to Robotino.
[ INFO] [1506001035.200156496]: Topic handler 'pause_sim' subscribed to topic 'twist_mux/locks/pause_rotatory': timeout = None, priority = 1
[ INFO] [1506001036.124309038]: Waiting for model URDF on the ROS param server at location: //arm_controller/robot_description
Found 0 motors
Motors started.
[ INFO] [1506001038.799179925]: SquirrelHWInterface ready.
Intializing loop...
Looping...
[arm_controller/squirrel_control_node-6] process has died [pid 16855, exit code -11, cmd /home/squirrel/catkin_ws/devel/lib/squirrel_control/squirrel_hw_main __name:=squirrel_control_node __log:=/home/squirrel/.ros/log/668f2dfc-9ec6-11e7-bfb3-00190f153f00/arm_controller-squirrel_control_node-6.log].
log file: /home/squirrel/.ros/log/668f2dfc-9ec6-11e7-bfb3-00190f153f00/arm_controller-squirrel_control_node-6*.log
[arm_controller/squirrel_control_node-6] restarting process
process[arm_controller/squirrel_control_node-6]: started with pid [17168]
[ INFO] [1506001040.125660436]: Waiting for model URDF on the ROS param server at location: //arm_controller/robot_description`
Not sure if the URDF model is the also an issue, but it seems it cannot find any motors of the arm.

6. dynamixel_controllers do not start as the device /dev/ttyTilt is not available anymore.
   Is pan and tilt on the raspberrypi now?

Hi all,

in dynamixel_tool.cpp line 97 the path to the model_info.list is hardcoded. However, the standard robotino setup does not install anything in that path. We should consider change that path to the local resources in this package or change the robot setup so that those files are installed in the predefined path /usr/local/dynamixel/models/model_info.list.

Best

This happened during testing on tuw-robotino2 with the latest stable version of squirrel_driver package.

terminate called after throwing an instance of 'motor_controller::MotorException'
  what():  failed to commicate with motor
[real/robotino/robot_controller_node-29] process has died [pid 26937, exit code -6, cmd /home/squirrel/catkin_ws/devel/lib/uibk_robot_driver/robot_controller_node cmd_rotatory:=/cmd_rotatory __name:=robot_controller_node __log:=/home/squirrel/.ros/log/cb43e11c-6064-11e6-99e1-08bd4388fa79/real-robotino-robot_controller_node-29.log].
log file: /home/squirrel/.ros/log/cb43e11c-6064-11e6-99e1-08bd4388fa79/real-robotino-robot_controller_node-29*.log

add configuration to disable pads if they are not needed

Missed add_dependencies(squirrel_hw_control_loop squirrel_safety_msgs_gencpp) at line 94

This library should be automatically installed with "catkin_make", it is possible that the problem is the name of the library FT17, the ros package name is ft17_driver...

the driver don't update a current state of the joints during a movement, the joint_states (real/../get_state) is updated at the end of the movement, when the position is reached.

• network connection settings
• start commands

....

During testing on tuw-robotino2 we found this issue.

Sending commands to /real/robotino/joint_control/ptp first the robot/arm are not moving.
If sending a command to /real/robotino/settings/switch_mode the robot and the arm start moving.
This is not a good behavior, unexpected and completely unsafe as it is unclear to the caller what data might already be queued up in the arm controller.

@shangl @qusaisuwan please change the behavior to something that ignores the data before the mode is switched. @Senka2112 mentioned that it was originally implemented like this by her. Maybe you can use some parts of that original code.

Currently squirrel_controller publishes commands to the joints and base constantly, as part of the SquirrelHWControlLoop::update() loop. It should hovever only send commands while executing a trajectory. Otherwise the squirrel_controller will "freeze" the base, and prevent move_base and even the joystick from moving the base.
So a flag or something "executing_trajectgory" would need to be added.

Dear all,

I noticed that the controller sometimes does not properly track the base trajectory. This might be due to the fact that the control is not computed in /map frame but rather in the /origin or /odom frame. If this is the case, this is generally wrong because the transformation between /map and /origin (or /odom) are not constant (that's what the localizer is computing) and therefore the controller is affected by the errors/drifts in the odometry (which is not negligible).

Best

some joint directions seem to be reversed. I guess this is a mismatch between motor driving directions and joint directions as defined in the URDF.

In order to have a general overview of the squirrel software and be more efficient during the integration meetings, our software should be properly documented. On one hand, with installation and execution instructions and, on the other hand, with a software architecture diagram to see the interconnections and dependencies between the modules. It is not necessary a diagram per node, only the most important nodes that subscribe or publish topics used by other components.

The squirrel_recommender package holds the templates and a small tutorial about how to create the diagram https://github.com/squirrel-project/squirrel_recommender#Documentation-and-Templates (2.Documentation and Templates). Please use these templates to create your documentation.

In https://github.com/squirrel-project/squirrel_driver/blob/indigo_dev/squirrel_interaction/src/squirrel_interaction/board/serial_api.py multiple functions (get_position, move_to, set_mouth_led_colors, ...) access the serial port. On of them is constantly called to update the joint_states of the robot.
Calling any of the available commands to do something else (eg. move neck, camera, head, door, etc.) can lead to the node dying with:

Traceback (most recent call last):
  File "/u/bajo/catkin_ws/src/squirrel_driver/squirrel_interaction/src/squirrel_interaction/board/board.py", line 119, in <module>
    controller.run()
  File "/u/bajo/catkin_ws/src/squirrel_driver/squirrel_interaction/src/squirrel_interaction/board/board.py", line 63, in run
    joint_state_msg.position = [radians(self._motor.get_position("head")), radians(self._motor.get_position("neck")), radians(self._motor.get_position("camera")), radians(self._motor.get_position("door"))]
  File "/u/bajo/catkin_ws/src/squirrel_driver/squirrel_interaction/src/squirrel_interaction/board/serial_api.py", line 72, in get_position
    response.extend(self.serial.read(7))
  File "/usr/local/lib/python2.7/dist-packages/serial/serialposix.py", line 501, in read
    'device reports readiness to read but returned no data '
serial.serialutil.SerialException: device reports readiness to read but returned no data (device disconnected or multiple access on port?)

@ipa-nhg buildbot fails with W: Failed to fetch http://lcas.lincoln.ac.uk/repos/release/dists/trusty/main/binary-amd64/Packages 503 Service Unavailable

As we no longer need their repo and have our own now we should take this out of all our code.

I tested the movement of the neck by sending it ~10 degree increments and printed the current and the new position before the movement is actually executed.

These are the results printed from within board.py

current neck position: 10
next neck position: 21
current neck position: 21
next neck position: 32
current neck position: 71
next neck position: 82
current neck position: 81
next neck position: 92
current neck position: 91
next neck position: 102
current neck position: 101
next neck position: 112
current neck position: 111
next neck position: 122
current neck position: 121
next neck position: 132
current neck position: 131
next neck position: 142

After the second movement the data was off by 40 degree!!!!
After the last position I again hit the issue reported in #142
@mzillich @ipa-nhg I guess this needs to be sorted out as soon as possible.

There are just some issues with the base limits we should fix, i.e., enforceLimits() for now removes all base commands as they apparently are not within the limits specified in the URDF - this could also be caused by the base only accepting velocities and no positions which is why this information probably is not inside the URDF? We can have a chat if you want!

Can we at least stop the execution of the 8 DOF trajectory when an obstacle enters the planned trajectory (to then have the top level planner replan?

Currently we have a small issue with the 8dof planner generating long trajectories, but it has also exposed a problem with the controller. The base has a really large error. The arm joints look good, just the base is way off (the example in the first image is a very extreme case but nonetheless you can see how bad it is - blue is desired pose, white is actual pose). Is the controller properly tuned? Or is this a ros control issue? Or is it due to the odom drift during the execution of the trajectory? I suspect it could be last because after we generate a new plan to the same location and send the goal, the robot is much closer to the desired location (see second image).

If the arm stops moving because the safety node is triggered, the commands seem to buffer and are suddenly applied when the safety node is reset. Results in very quick and dangerous movements.

Hi,

is the arm namespace necessary in the launch file?

Removing the namespace would also overcome the need of joint_states_relay.

Furthermore, the controller controls also the base, not just the arm.

Dear all,

I noticed that /squirrel_control_node sometimes does not release the control of the base (and probably of the arm too). I also experienced this straight after booting the robot. For instance, it happens to me that I could not reposition the robot with the joystick cause the low level controller moved the robot to pose where the robot was switched on.

I think /squirrel_control_node should execute the control loop if and only if requested. It might be also useful to create a service that kills the control loop just in case the a controller has been called via topic /trajectory_trajectory_controller/command and without the action server/client.