Orientation error in pose with speedl()

Dear,

I’m trying to figure out how to use speedl(). I’ve attached the script that I’m using. As long as I use speedl() to make a pure translation move, it works perfect. But as soon as I demand a orientation change of the tool, it gets messed up. Please note that I coded it to be a 1:1 replacement for a movel() (which works fine). I intend to modify it, so it becomes more then just a movel() replacement (which is silly).

What’s wrong with my orientation part in the speedl()?? (my code is an adaption of an UR sample)

def speed2target(target,linear_speed,linear_acc):
  act_pos=get_actual_tcp_pose()
  tgt_pos=get_forward_kin(target)
  rot_spd=0
	
  dist=point_dist(act_pos,tgt_pos)
	
  dt_a=linear_speed/linear_acc
  ds_a=(linear_acc/2)*pow(dt_a,2)
  dt_v=(dist-(2*ds_a))/linear_speed
	
  time=dt_a+dt_v
	
  dir=pose_sub(tgt_pos,act_pos)
  translation=sqrt(pow(dir[0],2)+pow(dir[1],2)+pow(dir[2],2))
  rotation=sqrt(pow(dir[3],2)+pow(dir[4],2)+pow(dir[5],2))
	
  spl=linear_speed/translation
  spr=linear_speed/rotation
  speed_vec=[spl*dir[0],spl*dir[1],spl*dir[2],spr*dir[3],spr*dir[4],spr*dir[5]]

  speedl(speed_vec,linear_acc,time)
  stopl(linear_acc)
end

I’ve been busy playing around with speedl(). I ran into problems. 1) I can’t figure out how to handle rotations, what does pose_sub do EXACTLY? How does it handle rotational differences? It’s poorly documented… 2) What are the rules for using speedl()? I learned the hard way, that you cannot use speedl() let the robot move and then check for a command to see if it should stop or move into another direction. The controller exits brutal by stating that a motion wasn’t stopped properly before a wait was encountered.

I would like to see some more clarification on speedl() and pose_sub(). An example using both WITH ROTATIONAL DIFFERENCES is much appreciated.

I agree about better documentation on some of these functions, including servoj as we have been seeing behavior that we would not expect with that when doing something similar.

Just as a test, have you tried just manually using a speedl() with some linear and joint speeds hardcoded just to see what the behavior is? I have only ever used speedl() in a linear fashion so am curious myself.

@mbush

Yes, I did. Lineair moves work fine, although without any wait operations. As soon as rotations come in to play, the robots’ behaviour becomes quite mysterious…

When you are judging the motion of the robot you need to be aware of where the TCP is defined. Is it at the tool flange or some offset from there? The speedl command is working on moving the TCP so if you aren’t aware of where that TCP is then it’s path might seem confusing.

Hi,

Yes, we are aware of that. We tested both with and without a tool, with and without a tool centre point. There’s no difference in behaviour. If the tool or flange makes a translation only, regardless in whatever direction, speedl works just fine. As soon as we demand a tool re-orientation, say 45 degree rotation about the tool tip, the robot steers off into the wild and crashes into itself. same move in movel works just fine. We took care not to be near singularities and move with slow speeds, so no joint would ever exceed maximum joint / torque speeds.

We need a worked example that includes different tool orientations.

Second, I’d like to use speedl to start of a move in a certain direction, check some TCP/IP messages and then decide whether I want to stop the move or continue it, or maybe alter the path a little. Speedl crashes with the message that you cannot have a wait while a speedl is executing…… Then, what’s the use of speedl?

I’d like to use it, so I can stitch multiple movel together, without the annoying stops in between.

Met vriendelijke groet,
With kind regards,

Mit freundlichen Grüßen,

Med venlig hilsen,

Con cordiali saluti,

Max van Rooij

So let me clarify one more thing about Rx/Ry/Rz rotation values with our robots. The rotations that these functions are expecting are in ‘rotation vector’ form and NOT roll/pitch/yaw. There are urscript functions that can be used to make these conversions. Are you keeping this in mind as you develop an experiment and judge the motion?

Hi,

Yes I am. It seems that the pose_dif() gives some weird results… I can compose any desired pose and use it with movel, I can supply both the pose or the motor angles. I can use rotation matrix, rotation vector or RPY. Maybe a clarification on how pose_diff works would already clear up a lot. Especially how it deals with two poses that have a different tool orientation (2 poses with only a translation between them is handled well by pose_dif)

Met vriendelijke groet,
With kind regards,

Mit freundlichen Grüßen,

Med venlig hilsen,

Con cordiali saluti,

Max van Rooij

It seems you are trying to develop some hybridized version of moveL. Instead, why not just use moveL with blend radii between the points to avoid the stops?

Well, if I can (pseudo code):

  1. movel() with some blend radius

  2. check TCP/IP for next-thing-to-do

  3. do another movel() again with some blend radius

  4. repeat from 2)

… without the robot stalling, because some wait occurred, sure, I’ll try that. Does the movel() return before the move is complete? Will it stop grfefully if I do not provide a new movel in time?

So, if I command a movel of about 1cm, which takes approx. 1 second to complete (I’m moving slow), can I start the movel, set blend radius to 1cm, check TCP/IP and then issue a new 1cm move with again a blend radius of 1cm?

Met vriendelijke groet,
Max van Rooij

There are a few aspects of how the controller functions that will prevent this from working as you might hope :
A) Issuing commands between moveL commands that cause some physical wait are going to cause the motion to stop abruptly IF there is a blend radius around those moveL waypoints. In your psuedo code example the operation in step #2 to check the tcp/ip connection for more data is going to cause it to do that.
B) IF you are attempting to do any of what you describe above (either with moveL or speedL commands) you are probably not going to get good results if you are doing this over the client interfaces (ports 30001-30003). To do what you describe above (checking the tcp/ip connection for new data while a move is executing) you should really be doing that operation in a background thread.

I think it would help if i had a better description of what your ultimate goal is with this.
a) Is this a CB3 robot or eseries and do you have a real robot to test this on?
b) Are you trying to send urscript commands via the client interfaces or is all the control programming being done on the robot?
c) If you are only trying to change the path between waypoints dynamically then there is a new feature in 5.6 called Online Path Offset that is the better way to solve this problem. This is only available on eseries software though.
d) IF you are trying to dynamically change the waypoints ‘on the fly’ as it executes a previous waypoint, getting the new waypoint data from an external source then that is an entirely different approach than if the goal is ‘c’.

1 Like