search

HaddingtonDynamics / Dexter

GNU General Public License v3.0
363 stars 84 forks source link

Add ROS support to Dexter #97

Open JamesNewton opened 4 years ago

JamesNewton commented 4 years ago

Desired ROS messages:

Units: https://ros.org/reps/rep-0103.html

Bidirectional

http://docs.ros.org/api/sensor_msgs/html/msg/JointState.html

From host to Robot:

http://docs.ros.org/melodic/api/geometry_msgs/html/msg/Pose.html

Processing the quaternion requires pretty extensive calculation, which is currently only supported in DDE. We might be able to extract that, but it's not "lightweight". So this message requires that we stick with DDE via the Job Engine on Dexter.

http://wiki.ros.org/joint_trajectory_controller (not sure if this is part of the "action" system which we perhaps don't want?) http://docs.ros.org/melodic/api/trajectory_msgs/html/msg/JointTrajectory.html

This is unusual because we would need to change velocity and acceleration to meet the time stamps for each position. Again, this seems pretty complex, and something that needs doing in DDE / Job Engine. Apparently you don't send all the items. You might accept the set of (Position, and time) and of (Position, Velocity, and Accelerations) and whatever is missing is up to the robot; e.g. the time stamp can be ignored.

From the robot back to host:

http://wiki.ros.org/xacro format to convey URDF (robot desciption data) data via ROS PARAM? The point here is to allow the URDF to be updated with the actual lengths from the robot.

ROS publisher, publish joint space and measured forces. We get to make this up? Or is there a standard? One topic for

Notes

List of sensor messages: http://wiki.ros.org/sensor_msgs?distro=kinetic

As far as I can find, there is no version of Pose which includes this time constraint, and no way to move all joints without the time constraint. How strange! I had really expected that ROS would be doing lower level control, and have a more flexible set of messages. I probably just don't understand it at this point. Edit: Yep, I found the Joint State messages: http://docs.ros.org/api/sensor_msgs/html/msg/JointState.html

We could just start the ROS Job Engine job every time the robot starts like we do with PHUI, but this would mean the robot would then be a ROS only configured robot, rather than every robot being ready to do ROS if it gets a ROS message.

To ensure that every Dexter understands ROS if contacted on the main port (ROS "master" port is 11311), we should use the node server just like we do for ModBus or Scratch or the web Job Engine and chat interface. Then, once we get a ROS request, we can start a job and communicate with it via the child process stdin / stdout as we already do for the browser Job Engine interface. Jobs take a second to start, so we will need to receive a "get ready" message which starts the ROS job, and then pass on ROS messages to that job. TODO: This desperately needs to be documented. @cfry

ROS "action" is different more complex and harder. "move to position by this time" Robot sends back status as it moves, then sends "ok, I'm there". This support is not desired at this time.

JamesNewton commented 4 years ago

Sumo contributed the following node.js process, which uses the source code of DDE to implement communications with the robot. This allows DDE to manage the creation of the oplets and waiting for, and decoding, the status response. The repo includes all the files needed, copied from DDE, but the following file (and the config file in the same folder) is the heart of the application. https://github.com/schaikunsaeng/DexterRos/blob/master/ros_control_loop/ros_control_loop.js

Note that this implements the http://docs.ros.org/api/sensor_msgs/html/msg/JointState.html message as a subscription to get new joint angle sets, and advertises the same to publish current joint angle positions.

This also shows an interesting method for accessing sections of DDE without loading all of it. In this case, just the core job functionality is used. I see no reason why this can't be expanded to load the Math and Kin libraries into the node web server for Pose kin and trajectory spline calculations. We can get that working via DDE GUI on the PC, (assuming some way to receive the same message data) and then move that to the job engine at a minimum, for launch from the node server, or perhaps into the node server directly.

JamesNewton commented 4 years ago

Basic Setup

It appears the ROS Melodic can not be installed on Ubuntu 16.04. It requires 18 and up. Currently, Dexter can not be advanced beyond Xenial / 16.04 See #25 for more on how Dexter's OS is upgraded. The latest ROS version which can be applied to 16.04 is Lunar but that is EOL. Kinetic also supports 16.04 and is supported until April of 2021; about 9 months.

It's our understanding that as long as the message doesn't change, different versions of ROS1 can be used. E.g. a ROS Melodic running on a PC should be able to work with ROS Kinetic on the Robot.

In any case, actually installing and running ROS on the robot is probably of limited value. The FPGA is a stunningly powerful processor, but the dual core ARM 7 processors running the OS are not terribly powerful (about the same as a good tablet or cell phone) and so ROS's higher end processing probably isn't useful on the arm. Also, supporting Dexter with native ROS would require several steps:

  1. Learn ROS (which is not our core environment)
  2. Write an extension to ROS in C which replaces DexRun.c (or extend DexRun to add in ROS messaging; unlikely given it's complexity)
  3. Switch the robot from supporting multiple interfaces to supporting only ROS (assuming a separate ROS only firmware) for those clients which want ROS.

Another possibility is: https://www.npmjs.com/package/roslib which is used extensively in the browser environment for ROS but is also available as a node package. Documentation seems very biased towards the browser side with very little for it's use in node. This example looks pretty good: https://github.com/RobotWebTools/roslibjs/blob/develop/examples/node_simple.js I'm concerned that this supports only websocket connections and I think ROS normally uses raw sockets?

rosnodejs

Instead, we can implement a lite weight ROS communications interface on the robot via the rosnodejs NPM package: https://www.npmjs.com/package/rosnodejs http://wiki.ros.org/rosnodejs https://roscon.ros.org/2017/presentations/ROSCon%202017%20rosnodejs.pdf and then do the work to support desired messages on the robot side via Javascript / DDE / Job Engine.

There is no requirement listed on the NPM package for ROS to be installed, but on the ROS wiki page, it indicates that ROS must be installed and that only Kinetic is supported. It would be nice to NOT require ROS on the robot in order to be as lite as possible. Luckily, some nerd ask this very question last year: https://github.com/RethinkRobotics-opensource/rosnodejs/issues/131 and the answer came back:

Yes, you can, if you:

  • use the onTheFly option (see here), and
  • you have the message definition files of the message types you want to use in your ROS_PACKAGE_PATH (path pointed to by that env variable).

It looks like you also need to have an environment variable defined for CMAKE_PREFIX_PATH. On the standard install of Kinetic, ROS_PACKAGE_PATH="/opt/ros/kinetic/share" CMAKE_PREFIX_PATH="/opt/ros/kinetic" To emulate those as close as possible, a ros folder is added under /srv/samba/share, and a "share" folder under that. Those can be added permanently^ with:

echo export ROS_PACKAGE_PATH=/srv/samba/share/www/ros/share/>>/etc/environment 
echo export CMAKE_PREFIX_PATH=/srv/samba/share/www/ros/>>/etc/environment

Wherever they point, there needs to be a set of folders under the share folder with message package.xml and msg folder with the .msg files. Something like you find from /opt/ros/kinetic/share/ in a ROS installation. e.g. the std_msgs folder. But watch out for depends which must also be included. e.g. the package.xml for std_msgs lists:

<build_depend>message_generation</build_depend>
<run_depend>message_runtime</run_depend>

so the message_generation and message_runtime folders must also be included. To support std_msgs and sensor_msgs, I ended up copying in

This allows messages with all the standard string, int32, etc... types from std_msgs, as well as JointState (from sensor_msgs), and Accel, Pose, Quaternion, and others from geometry_msgs. It does not support logging, which seems to have a massive list of dependencies. Not sure what to do about that. Having a light www.zip file that can be dropped into customers robots seems like a good idea.

JamesNewton commented 4 years ago

Configuration

To work on the ROS network, a few things must be explained to the robot. e.g. where the ROS "Master" (I will call it the "Server" instead) is, and (I think) where the Parameter Server is. Other environmental settings are listed here: http://wiki.ros.org/ROS/EnvironmentVariables

ROS Server

To set this, on the robot: export ROS_MASTER_URI=http://$1:11311; or add that to /etc/environment

Hostname issues

As per the rosnodejs authors, getting messages to actually pass back and forth involves "editing your /etc/hosts file as well, because ROS is really picky about the way machines are named".

On the PC, I had to add 192.168.1.142 dex-197121 to the /etc/hosts file were the IP address is that of Dexter and the dex-197121 is the name in /etc/hostname which was put there to support #83 . This /might/ not be necessary on other Dexters if that hasn't been set?

Also had to do the same to the /etc/hosts file on Dexter to get it to be able to receive messages. Not only adding the entry for Dexter, but also for the PC. Otherwise, when I sent a message from the PC to Dexter, I would get [WARN] [1597208618.701] (ros.rosnodejs): Error requesting topic on /chatter: Error: getaddrinfo ENOTFOUND nameofpc nameofpc:40861, undefined and the only way around that seems to be to put the pc's ip address and hostname into the robots /etc/hostname file. Which is a hack at best.

Something will need to be done about this.

ROS_HOSTNAME=$IP

One way of avoiding the issue is to set the ROS_HOSTNAME environmental variable on both the PC and the robot to the local IP address. Then when a message is sent, it will give that "hostname" as the source and the other end gets the actual IP address for response instead. Many thanks to CFritz for this script, from https://github.com/RethinkRobotics-opensource/rosnodejs/issues/157#issuecomment-672346984

setserver.sh

TARGET_IP=$1;
IP=`ip route get $TARGET_IP | head -n 1 | sed "s/.*src \(\S*\) .*/\1/"`;
echo -e "${GREEN}using $1 ($TARGET_IP) via $IP${NORMAL}";
export ROS_MASTER_URI=http://$1:11311;
export ROS_HOSTNAME=$IP;
export ROS_IP=$IP

which has been added to the /srv/samba/share/www/ros folder as setserver.sh and chmod +x setserver.sh. This can be called on the robot from /srv/samba/share via . www/ros/setserver.sh server-ip where the server-ip is replaced with the servers ip. This sets up the URI of the server so the robot can find it, and sets the ROS_HOSTNAME to whatever IP address is being used to route messages to the server. At that point, the ROS configuration is done on the robot side.

Some method for triggering a connection attempt by the node server must be added. I like the idea of a ROS config web page served from Dexter. Perhaps it could also update and run this script, or set the environmental variables itself, and then trigger the node server to connect to the ROS server and show status.

On the ROS server, or any other node which will publish messages to the robot, the hostname must be set to the local IP address, or to a hostname that the robot can resolve to an IP via DNS or some other method. If this isn't done, the robot won't be able to ack messages it receives.

JamesNewton commented 4 years ago

At this point, the following test program is working:

const rosnodejs = require('rosnodejs');
//rosnodejs.loadAllPackages(); NO! Not for onTheFly / Kinetic and up.

rosnodejs.initNode('Dexter', { onTheFly: true
  //,rosMasterUri: `http://192.168.0.134:11311` 
  //not needed if set in environmental variable via www/ros/setserver.sh script
  }).then(() => {
    console.log("Initialized")
    const nh = rosnodejs.nh;
    const stdMsgs = rosnodejs.require('std_msgs');
    const StringMsg = stdMsgs.msg.String;
    const subChatter = nh.subscribe('/chatter', StringMsg, (msg) => { 
      console.log("recvd:"+JSON.stringify(msg)+".") 
      });
    //rostopic pub -1 /chatter std_msgs/String  -- "hello3"
    const pubChatter = nh.advertise('/chatter', StringMsg);
    pubChatter.on('connection', () => {
      console.log("chat connected");
      pubChatter.publish({data: "hi2\n"});
      });
    //pub.publish({data: "hi1\n"}); //this doesn't work

    const SensorMsgs = rosnodejs.require('sensor_msgs');
    console.log("joints:"+JSON.stringify(SensorMsgs.msg))
    const subJointState = nh.subscribe('/joint_states', 'sensor_msgs/JointState', (msg) => { 
      console.log("recvd:"+JSON.stringify(msg)+".") 
      });
//rostopic pub /joint_states sensor_msgs/JointState '{header: {seq: 0, stamp: {secs: 0, nsecs: 0}, frame_id: ""}, name: ["J1", "J2", "J3"], position: [150.0, 0], velocity: [0.0, 0], effort: [0.0, 0]}' --once

    const joints = {header: {seq: 0, stamp: {secs: 0, nsecs: 0}, frame_id:""}
    ,name: ["J1","J2","J3","J4","J5","J6","J7"]
    ,position: [0,0,0,0,0,0,0]
    ,velocity: [0,0,0,0,0,0,0]
    ,effort:   [0,0,0,0,0,0,0]
    };
    const pubJointState = nh.advertise('/joint_states', 'sensor_msgs/JointState');
    pubJointState.on('connection', () => {
      console.log("joint connected");
      pubJointState.publish(joints);
      });

//rostopic pub /test geometry_msgs/Pose '{position: {"x":0.1,"y":0.2,"z":0.3}, orientation: {"w":1, "x":0, "y":0, "z":0}}' --once

    const subPose = nh.subscribe('/pose', 'geometry_msgs/Pose', (msg) => { 
      console.log("recvd:"+JSON.stringify(msg)+".") 
      });

    } //initnode callback
  );

Custom Message

https://github.com/RethinkRobotics-opensource/rosnodejs#generating-messages

When generating on the fly, messages can not be required until the node has initialized.

const rosnodejs = require('rosnodejs');
await rosnodejs.initNode('my_node', { onTheFly: true })
const stdMsgs = rosnodejs.require('std_msgs');

Hopefully that won't be an issue. I believe that using the node server, Dexter will /always/ be ready to receive messages from a ROS "host" (moving away from "master" as a term) and the advertised topics should be accessible immediately as well. If we need to use the job engine, then some means of starting that job will be required (init message) or the robot can be configured to start that ROS job on boot. (again, that would limit the robot to being /only/ ROS).

Now to figure out the ROS message definition files...

JamesNewton commented 4 years ago

URDF

Spec at: https://wiki.ros.org/urdf/XML most of that is very simple, but the physical properties are quite confusing.

This video provided an excellent overview of URDF files, their capabilities and limitations: https://www.youtube.com/watch?v=g9WHxOpAUns

Testing a URDF file: check-urdf <filename>. Visualize a URDF with rviz.

You need the Display Model add on, but the parameter containing the contents of the urdf file must be set. By default, the parameter is called /robot_description. To load it from a file: rosparam set /robot_description --textfile <filename> We should be able to do the same thing from the node server on Dexter when it's configured / the ROS server is setup.

To move joints around, you need the joint_state_publisher but that has been broken off into a separate "gui" which isn't installed by default, so: sudo apt install ros-kinetic-joint-state-publisher-gui and there are a number of components which must be launched, so it's easy to just re-use the urdf_tutorial launch file and substitute your own file: roslaunch urdf_tutorial display.launch model:=<filename>

Here is the current Dexter.urdf file for 5 joints:

<?xml version="1.0"?>

<robot name="Dexter" xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:property name="cadpath" value=""/>
<!-- To view with CAD files, download the "LowRes*.stl" files from 
https://drive.google.com/drive/folders/1JNs-h3x_sM75Rum5aerc5YCDE0ybJRp3
then edit the above line to point to the location of those files, e.g.:
<xacro:property name="cadpath" value="file:///home/jamesn/Documents/"/>  -->

<xacro:property name="LINK1" value="0.245252"/>
<xacro:property name="LINK2" value="0.339092"/>
<xacro:property name="LINK3" value="0.307500"/>
<xacro:property name="LINK4" value="0.059500"/>
<xacro:property name="LINK5" value="0.082440"/>

<xacro:property name="L2Xoff" value="-0.056"/>
<xacro:property name="L2Zadd" value="0.15"/> //L2 motor counterbalance
<xacro:property name="L2Xsize" value="0.095"/> //Approx thick
<xacro:property name="L2Ysize" value="0.11"/> //Approx width

<xacro:property name="L3Xoff" value="-0.066"/>
<xacro:property name="L3Xsize" value="0.05"/> //Approx thick
<xacro:property name="L3Ysize" value="0.07"/> //Approx width

<xacro:property name="L4Xoff" value="-0.020"/>
<xacro:property name="L5Zoff" value="-0.021"/>

  <material name="onyx">
    <color rgba="0.1 0.1 0.1 1"/>
  </material>

  <link name="base_link">
    <visual>
<xacro:if value="${cadpath==''}">
      <geometry>
        <cylinder length="${LINK1-0.025}" radius="0.05"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 ${(LINK1-0.025)/2}"/> 
</xacro:if>
<xacro:unless value="${cadpath==''}">
      <geometry>
        <mesh filename="${cadpath}LowRes_DexterHDIKinematic_Link1_Bottom.stl" scale="0.001 0.001 0.001"/> 
      </geometry>
      <origin rpy="1.5708 0 -1.5708" xyz="0 0 0"/>
</xacro:unless>
      <material name="onyx"/>
    </visual>
    <collision>
      <geometry>
        <cylinder length="${LINK1-0.025}" radius="0.05"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 ${(LINK1-0.025)/2}"/> 
    </collision>
  </link>

  <link name="link1">
    <visual>
<xacro:if value="${cadpath==''}">
      <geometry>
        <box size="0.1 0.05 0.05"/>
      </geometry>
      <origin rpy="0 0 0" xyz="-0.025 0 0"/>
</xacro:if>
<xacro:unless value="${cadpath==''}">
      <geometry>
        <mesh filename="${cadpath}LowRes_DexterHDIKinematic_Link1_Top.stl" scale="0.001 0.001 0.001"/> 
      </geometry>
      <origin rpy="1.5708 0 -1.5708" xyz="0 0 -0.0375"/>
</xacro:unless>
      <material name="onyx"/>
    </visual>
    <collision>
      <geometry>
        <box size="0.1 0.05 0.05"/>
      </geometry>
      <origin rpy="0 0 0" xyz="-0.025 0 0"/>
    </collision>
  </link>

  <link name="link2">
    <visual>
<xacro:if value="${cadpath==''}">
      <geometry>
        <box size="${L2Xsize} ${L2Ysize} ${LINK2+L2Ysize+L2Zadd}"/> //add Ysize to account for joint curve
      </geometry>
      <origin rpy="0 0 0" xyz="${L2Xoff-L2Xsize/2} 0 ${(LINK2+L2Zadd+L2Ysize)/2-L2Zadd-L2Ysize/2}"/>
</xacro:if>
<xacro:unless value="${cadpath==''}">
      <geometry>
        <mesh filename="${cadpath}LowRes_DexterHDIKinematic_Link2.stl" scale="0.001 0.001 0.001"/> 
      </geometry>
      <origin rpy="1.57079 0 -1.5708" xyz="${L2Xoff} 0 0"/>
</xacro:unless>
      <material name="onyx"/>
    </visual>
    <collision>
      <geometry>
        <box size="${L2Xsize} ${L2Ysize} ${LINK2+L2Ysize+L2Zadd}"/> //add Ysize to account for joint curve
      </geometry>
      <origin rpy="0 0 0" xyz="${L2Xoff-L2Xsize/2} 0 ${(LINK2+L2Zadd+L2Ysize)/2-L2Zadd-L2Ysize/2}"/>
    </collision>
  </link>

  <link name="link3">
    <visual>
<xacro:if value="${cadpath==''}">
      <geometry>
        <box size="${L3Xsize} ${L3Ysize} ${(LINK3+L3Ysize)}"/>
      </geometry>
      <origin rpy="0 0 0" xyz="${L3Xoff+L3Xsize/2} 0 ${(LINK3+L3Ysize)/2-L3Ysize/2}"/>
</xacro:if>
<xacro:unless value="${cadpath==''}">
      <geometry>
        <mesh filename="${cadpath}LowRes_DexterHDIKinematic_Link3.stl" scale="0.001 0.001 0.001"/> 
      </geometry>
      <origin rpy="1.57079 0 -1.5708" xyz="${L3Xoff} 0 0"/>
</xacro:unless>
      <material name="onyx"/>
    </visual>
    <collision>
      <geometry>
        <box size="${L3Xsize} ${L3Ysize} ${(LINK3+L3Ysize)}"/>
      </geometry>
      <origin rpy="0 0 0" xyz="${L3Xoff+L3Xsize/2} 0 ${(LINK3+L3Ysize)/2-L3Ysize/2}"/>
    </collision>
  </link>

  <link name="link4">
    <visual>
<xacro:if value="${cadpath==''}">
      <geometry>
        <cylinder length="0.05" radius="0.027"/> 
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0.015"/>
</xacro:if>
<xacro:unless value="${cadpath==''}">
      <geometry>
        <mesh filename="${cadpath}LowRes_DexterHDIKinematic_Link4.stl" scale="0.001 0.001 0.001"/> 
      </geometry>
      <origin rpy="1.57079 0 -1.5708" xyz="${L4Xoff} 0 0"/>
</xacro:unless>
      <material name="onyx"/>
    </visual>
    <collision>
      <geometry>
        <cylinder length="0.05" radius="0.027"/> 
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0.015"/>
    </collision>
  </link>

  <link name="link5">
    <visual>
<xacro:if value="${cadpath==''}">
      <geometry>
        <cylinder length="${LINK5}" radius="0.027"/> 
      </geometry>
      <origin rpy="1.57079 0 0" xyz="0 -0.013 0"/>
</xacro:if>
<xacro:unless value="${cadpath==''}">
      <geometry>
        <mesh filename="${cadpath}LowRes_DexterHDIKinematic_Link5.stl" scale="0.001 0.001 0.001"/> 
      </geometry>
      <origin rpy="1.57079 0 -1.5708" xyz="0 0 ${L5Zoff}"/>
</xacro:unless>
      <material name="onyx"/>
    </visual>
    <collision>
      <geometry>
        <cylinder length="${LINK5}" radius="0.027"/> 
      </geometry>
      <origin rpy="1.57079 0 0" xyz="0 -0.013 0"/>
    </collision>
  </link>

<!--S, LinkLengths, 82551, 50801, 330201, 320676, 228600 -->
  <joint name="J1" type="revolute">
     <parent link="base_link"/>
     <child link="link1"/>
     <axis xyz="0 0 1"/>
     <origin xyz="0 0 0.228600"/><!-- z=L1 -->
     <dynamics damping="0.0" friction="0.0"/>
     <limit effort="30" velocity="1.0" lower="-3.316" upper="3.316" /> <!-- bounds in radians -->
   </joint>

  <joint name="J2" type="revolute">
     <parent link="link1"/>
     <child link="link2"/>
     <axis xyz="1 0 0"/>
     <origin xyz="0 0 0"/><!-- y=L1cl to back of j2 drive Z=0 because already at L1-->
     <limit effort="30" velocity="1.0" lower="-1.745" upper="1.745" /> <!-- bounds in radians -->
   </joint>

  <joint name="J3" type="revolute">
     <parent link="link2"/>
     <child link="link3"/>
     <axis xyz="1 0 0"/>
     <origin xyz="0 0 ${LINK2}"/><!-- y=half way from back of j3 drive to L1cl z=L2  -->
     <limit effort="30" velocity="1.0" lower="-2.8797" upper="2.8797" /> <!-- bounds in radians -->
   </joint>

  <joint name="J4" type="revolute">
     <parent link="link3"/>
     <child link="link4"/>
     <axis xyz="1 0 0"/>
     <origin xyz="0 0 ${LINK3}"/><!-- y=rest of the way from back of j3 drive to L1cl z=L3 -->
     <limit effort="30" velocity="1.0" lower="-2.1816" upper="2.1816" /> <!-- bounds in radians -->
   </joint>

  <joint name="J5" type="revolute">
     <parent link="link4"/>
     <child link="link5"/>
     <axis xyz="0 0 1"/>
     <origin xyz="0 0 ${LINK4}"/><!-- z=L4 -->
     <limit effort="30" velocity="1.0" lower="-3.3161" upper="3.3161" /> <!-- bounds in radians -->
   </joint>

</robot>
JamesNewton commented 4 years ago

Services

http://wiki.ros.org/ROS/Tutorials/UnderstandingServicesParams

Perhaps an /init or /reset service should be provided by the robot to start the ROS job engine and get it ready for communications? This would avoid any issues with slow response on the first ROS message send to the robot as it starts the job engine.

JamesNewton commented 4 years ago

ROS.dde

A job engine job to receive and process ROS messages in JSON format has been written by @JamesWigglesworth /srv/samba/share/dde_apps/ROS.dde

//Written by James Wigglesworth
//Started: 8_18_20
//Modified: 8_18_20

Dexter.LINK1 = 0.2352
Dexter.LINK2 = 0.339092
Dexter.LINK3 = 0.3075
Dexter.LINK4 = 0.0595
Dexter.LINK5 = 0.08244

new Job({
    name: "ROS",
    inter_do_item_dur: 0,
    show_instructions: false,
    keep_history: false,
    user_data: {
        ws_message: false,
        xyz: [0, 0.5, 0.4],
        stiffnesses: [12.895, 12.895, 1.2568, 0.1503, 0.1503],
        StepAngles: "[0, 0, 0, 0, 0]",
        step_angles: [0, 0, 0, 0, 0],
        position_prev: [0, 0, 0, 0, 0, 0, 0],
        time_prev: Date.now() / 1000 / _ns
    },
    do_list: [
        function(){
            out("ROS.dde has been started")
        },
        Robot.loop(true, function(){
            let CMD = []
            if(this.user_data.ws_message){
                let message
                try{
                    message = JSON.parse(this.user_data.ws_message)
                }catch(err){
                    out("ws_message is not JSON parsable:")
                    console.log(message)
                    this.user_data.ws_message = false
                    return
                }

                if(typeof (message) === "object"){
                    CMD.push(ROS_to_DDE(message))
                }

                this.user_data.ws_message = false
            }else{
                CMD.push(Dexter.get_robot_status())
                CMD.push(function(){
                    let StepAngles = Vector.multiply(JSON.parse(this.user_data.StepAngles), _arcsec)
                    this.user_data.step_angles = StepAngles
                })
            }

            send_ROS_message(DDE_to_ROS(this))

            return CMD
        })
    ]
})

function send_ROS_message(string){
    //out(string) //TODO: slow this down. 
}

function ROS_to_DDE(message){
    let position = message.position
    let velocity = message.velocity
    let effort = message.effort
    let pose = message.pose
    let CMD = []

    if(position && !velocity && !effort){
        CMD.push(Dexter.move_all_joints(Vector.multiply(position, _rad)))
    }else if(!position && velocity && !effort){
        //i_moves go here
    }else if(!position && !velocity && effort){
        //i_move with torque feedback goes here
    }else if(pose){
        let quaternion = [1, 0, 0, 0]
        let xyz = [0, 0.4, 0.4]
        let DCM = Vector.quaternion_to_DCM(quaternion)
        let dir = Vector.multiply(-1, Vector.pull(DCM, [0, 2], 2))
        out("dir: " + dir)
        CMD.push(Dexter.pid_move_to(xyz, dir))
    }

    return CMD   
}

function DDE_to_ROS(job){
    let rs = job.robot.robot_status
    let position = [
        rs[Dexter.J1_MEASURED_ANGLE],
        rs[Dexter.J2_MEASURED_ANGLE],
        rs[Dexter.J3_MEASURED_ANGLE],
        rs[Dexter.J4_MEASURED_ANGLE],
        rs[Dexter.J5_MEASURED_ANGLE],
        rs[Dexter.J6_MEASURED_ANGLE],
        rs[Dexter.J7_MEASURED_ANGLE]
    ]
    let position_rad = Vector.multiply(1 / _rad, position)

    let time = rs[Dexter.START_TIME] + rs[Dexter.STOP_TIME] / 1000000
    //out(time, "blue", true)
    let time_nano_secs = time / _ns
    //out(time_nano_secs, "blue", true)
    let position_prev = job.user_data.position_prev
    let time_prev = job.user_data.time_prev
    let velocity = Vector.divide(Vector.subtract(position_rad, position_prev), time_nano_secs - time_prev)

    let torque = [...compute_torque(position, job.user_data.step_angles, job.user_data.stiffnesses), rs[Dexter.J6_MEASURED_TORQUE], rs[Dexter.J7_MEASURED_TORQUE]]

    let message = {
        header: {
            seq: 0,
            stamp: {
                secs: time,
                nsecs: time_nano_secs
            },
            frame_id: ""
        },
        name: ["J1", "J2", "J3", "J4", "J5", "J6", "J7"],
        position: position_rad,
        velocity: velocity,
        effort: torque
    }

    job.user_data.position_prev = position_rad
    job.user_data.time_prev = time_nano_secs

    return JSON.stringify(message)
}

function compute_torque(measured_angles, step_angles, stiffnesses, hysterhesis_low = [0, 0, 0, 0, 0], hysterhesis_high = [0, 0, 0, 0, 0]){
    let torques = [0, 0, 0, 0, 0]
    let displacement
    for(let i = 0; i < 5; i++){
        displacement = measured_angles[i] - step_angles[i]
        torques[i] = 0
        if(displacement < hysterhesis_low[i]){
            torques[i] = (measured_angles[i] - step_angles[i] - hysterhesis_low[i]) * stiffnesses[i]
        }else if(displacement > hysterhesis_high[i]){
            torques[i] = (measured_angles[i] - step_angles[i] - hysterhesis_high[i]) * stiffnesses[i]
        }
    }
    return torques
}

For now, it can be tested from the debug console in chrome after starting the job via the browser job engine interface and then executing: web_socket.send('{"job_name_with_extension":"ROS.dde","ws_message":{"header":{"seq":0,"stamp":{"secs":0,"nsecs":0},"frame_id":""},"name":["J1","J2","J3","J4","J5","J6","J7"],"position":[0,0,1,0,0,0,0]}}')

VV1919 commented 3 years ago

(snip quote of above comment)

there are few commenting mistakes. updated code:

<?xml version="1.0"?>

<robot name="Dexter" xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:property name="cadpath" value=""/>
<!-- To view with CAD files, download the "LowRes*.stl" files from 
https://drive.google.com/drive/folders/1JNs-h3x_sM75Rum5aerc5YCDE0ybJRp3
then edit the above line to point to the location of those files, e.g.:
<xacro:property name="cadpath" value="file:///home/jamesn/Documents/"/>  -->

<xacro:property name="LINK1" value="0.245252"/>
<xacro:property name="LINK2" value="0.339092"/>
<xacro:property name="LINK3" value="0.307500"/>
<xacro:property name="LINK4" value="0.059500"/>
<xacro:property name="LINK5" value="0.082440"/>

<xacro:property name="L2Xoff" value="-0.056"/>
<xacro:property name="L2Zadd" value="0.15"/> <!-- L2 motor counterbalance -->
<xacro:property name="L2Xsize" value="0.095"/> <!-- Approx thick -->
<xacro:property name="L2Ysize" value="0.11"/> <!-- Approx width-->

<xacro:property name="L3Xoff" value="-0.066"/>
<xacro:property name="L3Xsize" value="0.05"/> <!-- Approx thick -->
<xacro:property name="L3Ysize" value="0.07"/> <!-- Approx width -->

<xacro:property name="L4Xoff" value="-0.020"/>
<xacro:property name="L5Zoff" value="-0.021"/>

  <material name="onyx">
    <color rgba="0.1 0.1 0.1 1"/>
  </material>

  <link name="base_link">
    <visual>
<xacro:if value="${cadpath==''}">
      <geometry>
        <cylinder length="${LINK1-0.025}" radius="0.05"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 ${(LINK1-0.025)/2}"/> 
</xacro:if>
<xacro:unless value="${cadpath==''}">
      <geometry>
        <mesh filename="${cadpath}LowRes_DexterHDIKinematic_Link1_Bottom.stl" scale="0.001 0.001 0.001"/> 
      </geometry>
      <origin rpy="1.5708 0 -1.5708" xyz="0 0 0"/>
</xacro:unless>
      <material name="onyx"/>
    </visual>
    <collision>
      <geometry>
        <cylinder length="${LINK1-0.025}" radius="0.05"/>
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 ${(LINK1-0.025)/2}"/> 
    </collision>
  </link>

  <link name="link1">
    <visual>
<xacro:if value="${cadpath==''}">
      <geometry>
        <box size="0.1 0.05 0.05"/>
      </geometry>
      <origin rpy="0 0 0" xyz="-0.025 0 0"/>
</xacro:if>
<xacro:unless value="${cadpath==''}">
      <geometry>
        <mesh filename="${cadpath}LowRes_DexterHDIKinematic_Link1_Top.stl" scale="0.001 0.001 0.001"/> 
      </geometry>
      <origin rpy="1.5708 0 -1.5708" xyz="0 0 -0.0375"/>
</xacro:unless>
      <material name="onyx"/>
    </visual>
    <collision>
      <geometry>
        <box size="0.1 0.05 0.05"/>
      </geometry>
      <origin rpy="0 0 0" xyz="-0.025 0 0"/>
    </collision>
  </link>

  <link name="link2">
    <visual>
<xacro:if value="${cadpath==''}">
      <geometry>
        <box size="${L2Xsize} ${L2Ysize} ${LINK2+L2Ysize+L2Zadd}"/> <!-- add Ysize to account for joint curve -->
      </geometry>
      <origin rpy="0 0 0" xyz="${L2Xoff-L2Xsize/2} 0 ${(LINK2+L2Zadd+L2Ysize)/2-L2Zadd-L2Ysize/2}"/>
</xacro:if>
<xacro:unless value="${cadpath==''}">
      <geometry>
        <mesh filename="${cadpath}LowRes_DexterHDIKinematic_Link2.stl" scale="0.001 0.001 0.001"/> 
      </geometry>
      <origin rpy="1.57079 0 -1.5708" xyz="${L2Xoff} 0 0"/>
</xacro:unless>
      <material name="onyx"/>
    </visual>
    <collision>
      <geometry>
        <box size="${L2Xsize} ${L2Ysize} ${LINK2+L2Ysize+L2Zadd}"/> <!-- add Ysize to account for joint curve -->
      </geometry>
      <origin rpy="0 0 0" xyz="${L2Xoff-L2Xsize/2} 0 ${(LINK2+L2Zadd+L2Ysize)/2-L2Zadd-L2Ysize/2}"/>
    </collision>
  </link>

  <link name="link3">
    <visual>
<xacro:if value="${cadpath==''}">
      <geometry>
        <box size="${L3Xsize} ${L3Ysize} ${(LINK3+L3Ysize)}"/>
      </geometry>
      <origin rpy="0 0 0" xyz="${L3Xoff+L3Xsize/2} 0 ${(LINK3+L3Ysize)/2-L3Ysize/2}"/>
</xacro:if>
<xacro:unless value="${cadpath==''}">
      <geometry>
        <mesh filename="${cadpath}LowRes_DexterHDIKinematic_Link3.stl" scale="0.001 0.001 0.001"/> 
      </geometry>
      <origin rpy="1.57079 0 -1.5708" xyz="${L3Xoff} 0 0"/>
</xacro:unless>
      <material name="onyx"/>
    </visual>
    <collision>
      <geometry>
        <box size="${L3Xsize} ${L3Ysize} ${(LINK3+L3Ysize)}"/>
      </geometry>
      <origin rpy="0 0 0" xyz="${L3Xoff+L3Xsize/2} 0 ${(LINK3+L3Ysize)/2-L3Ysize/2}"/>
    </collision>
  </link>

  <link name="link4">
    <visual>
<xacro:if value="${cadpath==''}">
      <geometry>
        <cylinder length="0.05" radius="0.027"/> 
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0.015"/>
</xacro:if>
<xacro:unless value="${cadpath==''}">
      <geometry>
        <mesh filename="${cadpath}LowRes_DexterHDIKinematic_Link4.stl" scale="0.001 0.001 0.001"/> 
      </geometry>
      <origin rpy="1.57079 0 -1.5708" xyz="${L4Xoff} 0 0"/>
</xacro:unless>
      <material name="onyx"/>
    </visual>
    <collision>
      <geometry>
        <cylinder length="0.05" radius="0.027"/> 
      </geometry>
      <origin rpy="0 0 0" xyz="0 0 0.015"/>
    </collision>
  </link>

  <link name="link5">
    <visual>
<xacro:if value="${cadpath==''}">
      <geometry>
        <cylinder length="${LINK5}" radius="0.027"/> 
      </geometry>
      <origin rpy="1.57079 0 0" xyz="0 -0.013 0"/>
</xacro:if>
<xacro:unless value="${cadpath==''}">
      <geometry>
        <mesh filename="${cadpath}LowRes_DexterHDIKinematic_Link5.stl" scale="0.001 0.001 0.001"/> 
      </geometry>
      <origin rpy="1.57079 0 -1.5708" xyz="0 0 ${L5Zoff}"/>
</xacro:unless>
      <material name="onyx"/>
    </visual>
    <collision>
      <geometry>
        <cylinder length="${LINK5}" radius="0.027"/> 
      </geometry>
      <origin rpy="1.57079 0 0" xyz="0 -0.013 0"/>
    </collision>
  </link>

<!--S, LinkLengths, 82551, 50801, 330201, 320676, 228600 -->
  <joint name="J1" type="revolute">
     <parent link="base_link"/>
     <child link="link1"/>
     <axis xyz="0 0 1"/>
     <origin xyz="0 0 0.228600"/><!-- z=L1 -->
     <dynamics damping="0.0" friction="0.0"/>
     <limit effort="30" velocity="1.0" lower="-3.316" upper="3.316" /> <!-- bounds in radians -->
   </joint>

  <joint name="J2" type="revolute">
     <parent link="link1"/>
     <child link="link2"/>
     <axis xyz="1 0 0"/>
     <origin xyz="0 0 0"/><!-- y=L1cl to back of j2 drive Z=0 because already at L1-->
     <limit effort="30" velocity="1.0" lower="-1.745" upper="1.745" /> <!-- bounds in radians -->
   </joint>

  <joint name="J3" type="revolute">
     <parent link="link2"/>
     <child link="link3"/>
     <axis xyz="1 0 0"/>
     <origin xyz="0 0 ${LINK2}"/><!-- y=half way from back of j3 drive to L1cl z=L2  -->
     <limit effort="30" velocity="1.0" lower="-2.8797" upper="2.8797" /> <!-- bounds in radians -->
   </joint>

  <joint name="J4" type="revolute">
     <parent link="link3"/>
     <child link="link4"/>
     <axis xyz="1 0 0"/>
     <origin xyz="0 0 ${LINK3}"/><!-- y=rest of the way from back of j3 drive to L1cl z=L3 -->
     <limit effort="30" velocity="1.0" lower="-2.1816" upper="2.1816" /> <!-- bounds in radians -->
   </joint>

  <joint name="J5" type="revolute">
     <parent link="link4"/>
     <child link="link5"/>
     <axis xyz="0 0 1"/>
     <origin xyz="0 0 ${LINK4}"/><!-- z=L4 -->
     <limit effort="30" velocity="1.0" lower="-3.3161" upper="3.3161" /> <!-- bounds in radians -->
   </joint>

</robot>
ramamoorthyluxman commented 2 years ago

For those looking for moveit-config package of the dexter, here is the package. With parameters set in launch file, gazebo simulation is possible. https://github.com/ramamoorthyluxman/dexter_ros/

ramamoorthyluxman commented 2 years ago

I am trying to develop a python based ros driver node to control the robot arm. I am able to move the robot arm to a given joints values using the 'a' oplet. However, it is not clear to me what oplet should I use to send a bunch of waypoints and ask the robot to follow the trajectory with an overall trapezoidal velocity profile. Can you help with this please ?

sflc2 commented 2 years ago

I am trying to develop a python based ros driver node to control the robot arm. I am able to move the robot arm to a given joints values using the 'a' oplet. However, it is not clear to me what oplet should I use to send a bunch of waypoints and ask the robot to follow the trajectory with an overall trapezoidal velocity profile. Can you help with this please ?

I need this too!

JamesNewton commented 2 years ago

@ramamoorthyluxman @sflc2 Please raise a separate issue for separate (even if related) issues.

HerringTin commented 2 years ago

Does "i_move" refer to something that exists currently?

if(position && !velocity && !effort){
      CMD.push(Dexter.move_all_joints(Vector.multiply(position, _rad)))
    }else if(!position && velocity && !effort){
      //i_moves go here
    }else if(!position && !velocity && effort){
      //i_move with torque feedback goes here
    }else if(pose){
JamesNewton commented 2 years ago

I am unsure of the context of that code?

HerringTin commented 2 years ago

Oh sorry. Its a snippet from ROS.dde in your comment written above https://github.com/HaddingtonDynamics/Dexter/issues/97#issuecomment-675809917

JamesNewton commented 2 years ago

I'm told that code was not completed.