Problem with SmartServo

[JerryKayson]

Hi!

I'm currently doing my bachelor's working with a KUKA IIWA7 using the iiwa_stack and sadly have run into some issues in using the stack to its full capacity. Moveit! works but since it frequently determines that the ideal path is a 360 roundhouse movement for moving the arm half a meter I'm trying to get the SmartServoLin-function to work and allow for a more linear motion.

Sadly I can't get the arm to respond to SmartServo-commands unless they are relative to what the arm believes its own position is - i.e. "my_iiwa.setCartesianPose( my_iiwa.getCartesianPose+some offset)".

If I try printing my_iiwa.getCartesianPose I get some really odd values, see image 4 below.

I believe the issue is in the Java-part understanding what the Tool and TCP is in my URDF.

The cabinet and Sunrise project was installed by a currently unavailable professional whose work I'd rather not ruin so I've just taken some pictures on external files trying to discern what the setting for the URDF should be.

In images 1,2 and 5 you can see what the professional has setup in the Sunrise project, a tool named RobotiqGripper and some referenceframe called /TCP ???

What should my settings be in the URDF and SRDF to make the robot respond to SmartServo-commands? And what should the rosparam iiwa/toolName be set to?

I've tried have a link called RobotiqGripper_link in the URDF and setting the rosparam to RobotiqGripper but that didn't work.

I would appreciate any help I can get in this issue.

[exo-core]

Hi Jerry,

the TCP of the tool needs to have a frame named <TOOL_NAME>_link_ee (e.g. RobotiqGripper_link_ee) to load it via the iiwa_tool rosparam (if I remember correctly).

I also recommend to work with the latest development branch as a lot has been change in the handling of Cartesian parameters (e.g. adding TF support on the Sunrise site).

[JerryKayson]

Would this setup be correct? Apparently there was staff available that knows how the IIWA works and had access to the required software, etc.

[JerryKayson]

Ok, looking into the Java code it all makes much more sense now, for example seeing the different Application options from the SmartPad in the SunriseProject( *extends RoboticsAPIApplication).

Will probably get it to work tomorrow. I've set the project up with the SmartLin-Feature-branch that I used in my own code and debugged everything; might look into the development branch as you mentioned exo-core.

[JerryKayson]

Is there something off with the SmartLin-feature-branch when CartesianPoseLin commands are set for positions out of range?

I got it to work as intended both with linear movement and speed, but as soon as the tool/Endeffector was commanded to go to a position out of range(as might happen with the code below) it made some odd jerky movement exceeding velocity speedlimits and ending the control loop.

while (!my_iiwa.getCartesianPose(command_cartesian_position)) ; command_cartesian_position.pose.position.y += 0.2; my_iiwa.setCartesianPoseLin(command_cartesian_position); sleepForMotion(my_iiwa, 2.0);

Does the development branch check for invalid goalPositions?

[exo-core]

As the SmartServo interface is supposed to offer a low-level command interface to the robot the reacts relatively fast it entirely comes without any checks like that. The checks are only included in standard PTP motions (which are available triggered by the ROS action interfaces).

[JerryKayson]

Hmm, what's most likely the issue when getting a "Could not calculate BackwardTrafo" from GetInverseKinetmaticsFromReferenceRequest with the development branch?

The code is just a simple snippet

nh.setParam("iiwa/toolName", "RobotiqGripper"); iiwa_ros::state::CartesianPose cart_state; iiwa_ros::command::CartesianPose goCartPose; cart_state.init("iiwa"); goCartPose.init("iiwa");

command_cartesian_position.pose = cart_state.getPose().poseStamped.pose; command_cartesian_position.pose.position.y += 0.2; goCartPose.setPose(command_cartesian_position); ros::Duration(2).sleep();

Could it be that I haven't initialized the header of the poseStamped?

By the way, what ActionType am I to use for a P2P action when writing to the iiwaActionServer ? "/action/move_to_cartesian_pose" expects a MoveToCartesianPoseAction probably with a goalPose and actiontype CommandType.POINT_TO_POINT?? but I can't find that in any of the common ros/iiwa libraries.

actionlib::SimpleActionClient MoveBaseClient;

The cross-over from the Java-code makes it a little bit difficult. I'll check what the topic expects the messagetype to be tomorrow and construct something from that, if there isn't something available already of course. :)

[exo-core]

I must admit that I never checked what happens if you leave the header empty. I'd recommend you to fill at least the frame_id field. You can find the action definition here.

[JerryKayson]

Nice! Thanks for the help.

[JerryKayson]

For some reason the iiwa/action/move_to_cartesian_pose/ goal and result topics are all quiet when I publish to the action-topic like below - same goes for the ROS_INFO. Any idea what the issue might be? Am I publishing the action incorrectly?

[exo-core]

Could you give some more details? Does the arm move and the action result is missing or is nothing happening at all?

I know there is an issue with the initialization of P2P motions. It heps to send a single command on /iiwa/command/JointPosition before using the action. Alternatively you could use the workouround from PR https://github.com/IFL-CAMP/iiwa_stack/pull/147.

[JerryKayson]

I'm trying to debug but it's quite the hassle since losing the connection/stopping the SmartServo program does not fully disconnect the robot in this branch. I get a "address already in use" error when trying to start up the SmartServo program again, forcing a hard restart of the robot to get it to work.

I used the config.txt for configuration like in the master-branch instead of the applicationData since it worked then without any problems.

[JerryKayson]

Seems like it's a result from the choice from master to dev-branch to bind specific XML and TCP-ports(30000-30007) instead of defaulting to 0 when setting up the nodes for configuration, action_server(dev.branch), sub and pub.

Edit: Just changing back the to the config.txt file and default ports like before stops the "address alrdy in use" error from occurring for me at least. :)

[JerryKayson]

Back to the action

So the robot is receiving the goal, visible both on the SmartPad and the Topic. but something is off. It believes that the goal is already finished and therefor doesn't initiate any motion.

The goalPosition is "it's on own position + position.y+=0.2"

Something odd is how the position and orientation in my targetPose and the one published in the goalTopic differ. Some position and orientation variables are the same numbers but swapped around for some reason. Look at orientation.w for goalPose and both z,y for my sent targetPose; OR goalPose z coordinate and targetPose x coordinate.