Cannot control CartesianPose

[isabellahuang]

I'm currently having trouble commanding the CartesianPose of the robot. I am publishing a geometry_msgs/PoseStamped message to /iiwa/command/CartesianPose, but the robot is not moving. I see a potential solution on how to fix this in the message definition file but I'm unclear on which "value to copy" and which parameters to set to -1. In fact, I can't even find this "Cartesian Position tab" on the SmartPad. I would really appreciate some additional clarification!

Furthermore, my attempts to publish the CartesianPose command leads to this error on the SmartPad.

I'm pretty confident my numbers are within acceptable range. But I've also published the exact values from the current CartesianPose state as a sanity check (ie. no movement) but I get the same error. Thank you for reading!

[SalvoVirga]

I'm pretty confident my numbers are within acceptable range.

This can be, but the error from the SmartPad seems clear: the controller computer some path to reach your commanded destination and some configuration in joint space during that path would violate the robot limits. That could be possible, even if the cartesian pose is reachable, maybe it is not from the current robot configuration. Or maybe your commanded position is correct, but not the orientation.

• Did you try to command the same pose from another initial joint configuration?
• More importantly, did you try to command any other pose to the robot? Easy toy example: Take the current robot flange Cartesian Position, modify one components of its position by 1cm or so, send the modified pose (with the same orientation of the initial one). Does the robot move?

You should try to break down your errors to toy examples, if the robot moves with simple commanded poses (e.g. poses that are very close to the current one and therefore very likely to be correct, unless you are already very close to a joint limit), then you are probably not sending something correct. Or, you are very unlucky and the robot simply cannot achieve that end-effector pose from the current configuration.

[isabellahuang]

Thanks for the response. I have been trying these toy examples and I know they are valid positions because I command the JointPosition to different orientations, then read the CartesianPoses and use those for valid commands. However, the same error persists.

I would still love some additional clarification on what the comments in the CartesianPose msg file means. Ie, what parameters to set to -1? Perhaps this will be relevant to my issue. Thanks!

On Fri, Jun 21, 2019, 9:45 AM Salvo Virga notifications@github.com wrote:

I'm pretty confident my numbers are within acceptable range.

This can be, but the error from the SmartPad seems clear: the controller computer some path to reach your commanded destination and some configuration in joint space during that path would violate the robot limits. That could be possible, even if the cartesian pose is reachable, maybe it is not from the current robot configuration. Or maybe your commanded position is correct, but not the orientation.

• Did you try to command the same pose from another initial joint configuration?
• More importantly, did you try to command any other pose to the robot? Easy toy example: Take the current robot flange Cartesian Position, modify one components of its position by 1cm or so, send the modified pose (with the same orientation of the initial one). Does the robot move? You should try to break down your errors to toy examples, if the robot moves with simple commanded poses (e.g. poses that are very close to the current one and therefore very likely to be correct, unless you are already very close to a joint limit), then you are probably not sending something correct. Or, you are very unlucky and the robot simply cannot achieve that end-effector pose from the current configuration.

— You are receiving this because you authored the thread. Reply to this email directly, view it on GitHub https://github.com/IFL-CAMP/iiwa_stack/issues/186?email_source=notifications&email_token=AFJ2RHUY2KWMVYV435Y2WVLP3UATXA5CNFSM4HZOLPA2YY3PNVWWK3TUL52HS4DFVREXG43VMVBW63LNMVXHJKTDN5WW2ZLOORPWSZGODYI7H5Y#issuecomment-504493047, or mute the thread https://github.com/notifications/unsubscribe-auth/AFJ2RHTAZMEUTVSKW7E457DP3UATXANCNFSM4HZOLPAQ .

[exo-core]

When you want to ignore the redundancy data in a iiwa_msgs/CartesianPose you need to set status and turn to -1:

~$ rosmsg show iiwa_msgs/CartesianPose 
geometry_msgs/PoseStamped poseStamped
  std_msgs/Header header
    uint32 seq
    time stamp
    string frame_id
  geometry_msgs/Pose pose
    geometry_msgs/Point position
      float64 x
      float64 y
      float64 z
    geometry_msgs/Quaternion orientation
      float64 x
      float64 y
      float64 z
      float64 w
iiwa_msgs/RedundancyInformation redundancy
  float64 e1
  int32 status // set this to -1
  int32 turn   // and this as well

You can also take a look at this spline motion example.

[isabellahuang]

When you want to ignore the redundancy data in a iiwa_msgs/CartesianPose you need to set status and turn to -1:

~$ rosmsg show iiwa_msgs/CartesianPose 
geometry_msgs/PoseStamped poseStamped
  std_msgs/Header header
    uint32 seq
    time stamp
    string frame_id
  geometry_msgs/Pose pose
    geometry_msgs/Point position
      float64 x
      float64 y
      float64 z
    geometry_msgs/Quaternion orientation
      float64 x
      float64 y
      float64 z
      float64 w
iiwa_msgs/RedundancyInformation redundancy
  float64 e1
  int32 status // set this to -1
  int32 turn   // and this as well

You can also take a look at this spline motion example.

How should I set it if iiwa/command/CartesianPose only accepts the geometry_msgs/PoseStamped part rather than the entire iiwa_msgs/CartesianPose message?

[Deastan]

I have exactly the same error (on the real robot) and the same message! Did you find a solution ?

If I change the joints, it is working and go where I command but only in joints space.

I used this simple code and I have know no errors in the pad but no movement of the robot.

!/usr/bin/env python

''' Test to go from current cartesian pose to another one ''' import rospy

from iiwa_msgs.msg import CartesianPose from geometry_msgs.msg import PoseStamped import tf

def talker():

pub = rospy.Publisher('/iiwa/command/CartesianPose', PoseStamped, queue_size=1)

pub = rospy.Publisher('/iiwa/command/CartesianPose', CartesianPose, queue_size=1)
rospy.init_node('test_pub', anonymous=False)
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():

    goal = CartesianPose()
    # goal.header.seq

    # goal = PoseStamped()
    goal.poseStamped
    goal.poseStamped.header.seq = 1
    goal.poseStamped.header.stamp = rospy.Time.now()
    goal.poseStamped.header.frame_id = "world"

    goal.poseStamped.pose.position.x = 0.52
    goal.poseStamped.pose.position.y = 0.0
    goal.poseStamped.pose.position.z = 0.74

    quaternion = tf.transformations.quaternion_from_euler(0.0, 1.5707963, 0.0)

    goal.poseStamped.pose.orientation.x = quaternion[0]
    goal.poseStamped.pose.orientation.y = quaternion[1]
    goal.poseStamped.pose.orientation.z = quaternion[2]
    goal.poseStamped.pose.orientation.w = quaternion[3]

    goal.redundancy.e1 = 0
    goal.redundancy.status = -1
    goal.redundancy.turn = -1

    rospy.loginfo(goal)
    pub.publish(goal)
    rate.sleep()

if name == 'main': try: talker() except rospy.ROSInterruptException: pass

[isabellahuang]

@Deastan Unfortunately I have not been able to fix the problem! Same with me-- commands only work in joint space. I've been using TRAC-IK as a workaround.

Also, what's interesting is that when I try to control the Cartesian pose linearly (through iiwa/command/CartesianPoseLin) my robot jerks a little violently but is immediately stopped by the speed limit detection on the KUKA. I was excited because at least it moved. So I tried to set the linear Cartesian velocity to be slower through the services (see my issue #207) but to no avail. I am however able to control the joint velocity through these services.

I'd like to see what happens when you try to call the CartesianPoseLin command instead of the CartesianPose command. Could you let me know?

[Deastan]

Should I run Moveit before to command a desire pose ? I have no longer the error, but the robot doesn't move... I think it is not receiving the command.. There is a warning in the terminal :

[WARN] [WallTime: 1565979025.424510] Could not process inbound connection: topic types do not match: [geometry_msgs/PoseStamped] vs. [iiwa_msgs/CartesianPose]{'topic': '/iiwa/command/CartesianPose', 'message_definition': '# A Pose with reference coordinate frame and timestamp\nHeader header\nPose pose\n', 'md5sum': 'd3812c3cbc69362b77dc0b19b345f8f5', 'type': 'geometry_msgs/PoseStamped', 'callerid': '/iiwa/iiwa_subscriber'}

@isabelleahuang: I have published under the topic: /iiwa/command/CartesianPoseLin but nothing happended too.

[isabellahuang]

I'm not currently using Moveit and I don't think it's necessary in theory. Yeah, that error shows up when you publish an /iiwa_msgs/CartesianPose message instead of a geometry_msgs/PoseStamped to the /iiwa/command/CartesianPose or /iiwa/command/CartesianPoseLin topics.

It's a little bit confusing because reading the state /iiwa/state/CartesianPose returns a iiwa_msgs/CartesianPose message but commanding it requires a PoseStamped message. You could also follow Salvo's example code for using the helper functions that he typed up for me in #185 .

When you publish to the topic /iiwa/command/CartesianPoseLin are you getting any errors?

[Deastan]

I have no error/no warning.

I have the git from March... because I have changed a lot of code and have adapted for gazebo because I was building an RL environment...

I didn't have every message because in March they used less iiwa_msgs. I have added this msg CartesianPose by copy/past in my version of iiwa_stack. Maybe something else is missing. :/

[isabellahuang]

Sorry, what do you mean by adding msg : CartesianPose? I don't quite understand.

I know a lot of changes were pushed in June so it could be worth trying to send a basic CartesianPose command with the updated code if that's feasible for you. Though I'm updated with the master branch and still having errors.

I also have a suspicion that attaching tools to the arm may affect whether commanding the CartesianPose works. I saw #159 has the same error we have. I personally have attached a tool but have yet to remove it and try the Cartesian commands again. Are you maybe also using a tool?

On Fri, Aug 16, 2019, 11:29 AM Deastan notifications@github.com wrote:

I have no error/no warning.

I have the git from March... because I change and adapt for gazebo because I was building an RL environment... I have added this msg : CartesianPose because it wasn't on iiwa_msgs. Maybe something else is missing. :/

— You are receiving this because you authored the thread. Reply to this email directly, view it on GitHub https://github.com/IFL-CAMP/iiwa_stack/issues/186?email_source=notifications&email_token=AFJ2RHTPTC4KQ6LALZW4HTLQE3WY5A5CNFSM4HZOLPA2YY3PNVWWK3TUL52HS4DFVREXG43VMVBW63LNMVXHJKTDN5WW2ZLOORPWSZGOD4PLVYI#issuecomment-522107617, or mute the thread https://github.com/notifications/unsubscribe-auth/AFJ2RHS4NROE37DO7NMRYETQE3WY5ANCNFSM4HZOLPAQ .

[Deastan]

So I found out ! I will answer you soon. But I still don't understand why.

@isabell: I update a bit my last message. I am working on the ancient version of iiwa_stack because I took the repo in March... and they have changed a lot of thing since.

Edit:

My code to publish my desire in the following python code. I insisted and moving with the pad everywhere in joint space was working. And then I started in cartesian space from the top so x=0.0, y=0.0, z=1.22 and it moved. I continued to where I'd like to go with my first try but step by step and finally went to my desired position.

The issue is the limit of the joints. So you cannot go from one pose to an other one if you are close to the limit. For me it should find a way to reach the goal without breaking the joints limits...but it seems to not... I think an issue is the chosen orientation which can be quite fast off the limit!

!/usr/bin/env python

''' Test to go from current cartesian pose to another one ''' import rospy

from iiwa_msgs.msg import CartesianPose from geometry_msgs.msg import PoseStamped import tf

def talker(): CartesianPose_bool = False if CartesianPose_bool: pub = rospy.Publisher('/iiwa/command/CartesianPose', CartesianPose, queue_size=10) else:

pub = rospy.Publisher('/iiwa/command/CartesianPose', PoseStamped, queue_size=1)

    pub = rospy.Publisher('/iiwa/command/CartesianPose', PoseStamped, queue_size=1)
# pub = rospy.Publisher('/iiwa/command/CartesianPoseLin', CartesianPose, queue_size=10)
rospy.init_node('test_pub', anonymous=False)
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
    if CartesianPose_bool:
        goal = CartesianPose()
        # goal.header.seq

        # goal = PoseStamped()
        goal.poseStamped
        goal.poseStamped.header.seq = 1
        goal.poseStamped.header.stamp = rospy.Time.now()
        goal.poseStamped.header.frame_id = "world"

        goal.poseStamped.pose.position.x = 0.46875
        goal.poseStamped.pose.position.y = 0.0
        goal.poseStamped.pose.position.z = 0.97862

        quaternion = tf.transformations.quaternion_from_euler(0.0, 1.099, 0.0)#1.5707963

        goal.poseStamped.pose.orientation.x = quaternion[0]
        goal.poseStamped.pose.orientation.y = quaternion[1]
        goal.poseStamped.pose.orientation.z = quaternion[2]
        goal.poseStamped.pose.orientation.w = quaternion[3]

        # goal.redundancy.e1 = 0
        goal.redundancy.status = -1
        goal.redundancy.turn = -1

        rospy.loginfo(goal)
        pub.publish(goal)
        rate.sleep()
    else:
        goal = PoseStamped()

        goal.header.seq = 1
        goal.header.stamp = rospy.Time.now()
        goal.header.frame_id = "world"

        goal.pose.position.x = 0.46875
        goal.pose.position.y = 0.3
        goal.pose.position.z = 0.7

        quaternion = tf.transformations.quaternion_from_euler(0.0, 0.0, 0.0)#1.5707963

        goal.pose.orientation.x = quaternion[0]
        goal.pose.orientation.y = quaternion[1]
        goal.pose.orientation.z = quaternion[2]
        goal.pose.orientation.w = quaternion[3]

        rospy.loginfo(goal)
        pub.publish(goal)
        rate.sleep()

if name == 'main': try: talker() except rospy.ROSInterruptException: pass

[isabellahuang]

Oh, that's great that it works! I'm still trying to make very small incremental changes to my current CartesianPose but the same error persists...

[Deastan]

If you check the joints limits space on the pad what is happening ? 1) Try first to reach x = 0, y = 0 and z = 2 meter. 2) look at the pad if the error message changes to something like "cannot compute the inverse kinematic"

What is the result ? Is it the same error code ? What is happening ? What you could also try is my my python scrip if it is simple to set for you or send a topic with rqt (Be careful with the orientation with RQT)

[davidpaulius]

This may be really late, but I had a similar issue when using the CartesianPose message type with the CartesianPose command. Instead, I had to use the PoseStamped message type to get the robot to move. Hope this helps anyone who is stumped on this issue!

[shrutic12]

Hey all, I have a strange error, where publishing on CartesianPose or CartesianPoseLin does not move the robot, however, when I go to moveit path planning and manually move the robot along the same direction, it works. So, in this case I am certain the joint limits have not been reached. But it seems like the publishing to the topic gives no result. Any ideas what I could try? I also have no errors.

[exo-core]

Hey all, I have a strange error, where publishing on CartesianPose or CartesianPoseLin does not move the robot, however, when I go to moveit path planning and manually move the robot along the same direction, it works. So, in this case I am certain the joint limits have not been reached. But it seems like the publishing to the topic gives no result. Any ideas what I could try? I also have no errors.

Did you forget about the namespace? It should be /iiwa/CartesianPose or /iiwa/CartesianPoseLin, if I remember correctly...

[shrutic12]

Hi, No, I did not forget the namespace, it was just to keep the post short. I am using a KUKA LBR Med robot with arealsense camera attached. I am using a trained neural network to detect some markers and I would like to have a visual servoing type application where the robot is able to track the markers. Currently, I compute the pose in the camera frame, which is then transformed to the base frame of the robot and I am sending the pose commands to /iiwa/CartesianPose. But the tracking becomes unstable and the robot stops moving. Any ideas/suggestions would be great!