Re-enabling IK elements from a script

Typically: "How do I... ", "How can I... " questions
Post Reply
mfloyd
Posts: 21
Joined: 29 Oct 2014, 23:06

Re-enabling IK elements from a script

Post by mfloyd » 30 Jul 2015, 01:14

I have a script that will temporarily override the IK joint mode (to torque control if hybrid and to passive if not). When I stop the simulation or if I change the joints back to IK during the simulation I notice that the IK behavior doesn't return to normal. I found the cause to be that in some cases an IK element will remain disabled after the simulation has ended or even when all the joints are changed back to IK mode. This usually happens if all the joints for the IK element have been changed away from IK mode, but not if only some of them have been changed. Is there a way to programmatically re-enable the elements?

Some of things I've looked at don't seem to work because my script is meant to be generic and is attached to dummy which could be moved to arbitrary models.
Here's what I've tried:
  • I have no way of getting the ikGroupHandle from simGetIkGroupHandle because I don't know the name of the ik groups before attaching my script to the model. I don't know how to read that information otherwise. It doesn't show when reading through the entire object tree, (and there's no related scene object type to identify it if it were there).
  • I've seen that simSetIkElementProperties has been used by others to manually disable or enable an element (the ABB IRB 360 model), but I don't know the GroupHandle and I wouldn't know which constraints to apply for the constraints argument since different models may require different constraints. It also only clears or sets the constraints rather than truly enabling the element after its been disabled by vrep.
  • I've thought of trying to access the IK groups or elements through the dummies, but there are no dummy properties to read. There is simGetLinkDummy, but it only gives the handle of the other dummy and doesn't tell even what type of link it is or if there's any associated Calculation Modules (IK, GC, ...).

mfloyd
Posts: 21
Joined: 29 Oct 2014, 23:06

Re: Re-enabling IK elements from a script

Post by mfloyd » 30 Jul 2015, 18:32

I found somewhere else on the forum here where a similar problem was encountered. I also cannot set the "Element is active" checkbox automatically when the ik element fails. I think the suggestion there to use simSetExplicitHandling(ikGroup,1) would work, but the problem is compounded in my case because I can't search the model in the generic case to find the related IK groups (names or handles). I don't know before hand what model my script (and dummy) will be attached to and it could change several times during a simulation run.

coppelia
Site Admin
Posts: 6867
Joined: 14 Dec 2012, 00:25

Re: Re-enabling IK elements from a script

Post by coppelia » 30 Jul 2015, 18:38

Hello,

many questions, I'll start with 2:
  • When you change the mode joints are operating, and for instance your mode is passive, and you happen to call the simHandleIkGroup upon that mechanism (explicitely or implicitely via the main script), then the algorithm notices that the IK task is not valid (since no joint is flagged as IK, i.e. a rigid block), and disables that related IK element. You can avoid this by not calling simHandleIKGroup once the mode has been changed, or, if the IK group is implicitely handled via the main script, flag that IK group as explicit handling before switching the joint modes (with simSetExplicitHandling(ikGroupHandle,1)).
  • You are right, if you don't know the IK group name, you cannot access it. I'll have to think about something..
Cheers

coppelia
Site Admin
Posts: 6867
Joined: 14 Dec 2012, 00:25

Re: Re-enabling IK elements from a script

Post by coppelia » 30 Jul 2015, 18:40

Can you tell me what exactly you want to do with that dummy attached to models?

mfloyd
Posts: 21
Joined: 29 Oct 2014, 23:06

Re: Re-enabling IK elements from a script

Post by mfloyd » 31 Jul 2015, 20:25

I'll describe first, then I'll have a video of what I have right now (and the problems).

I have a generic script attached to a dummy meant for testing and applying joint level controls (later there will be high level controls as well). It performs some common functions that I would like to perform for any manipulator I come across. For most cases the objects don't need modifications before hand, and ideally none would require changes. (Right now I often have to disable scripts attached to models, and for the irb360 I also had to uncheck explicit handling on all the ik groups and disable one of the competing ik elements usually handled by the script).

After I attach the script object to a model and then remove it (or reset the scene because of stopping the scene), I don't want there to be any permanent changes to the models. I want them to work the same way they did before attaching the script object. This is not the case if the ik elements were disabled (despite joint mode changes manually reverted after script cleanup).

In the following video I show my script in action. First I show that unmodified IRB140 and redundantRobot work through manipulation of the IK spheres (without my script attached). Then at 0:24 I move and attach GhostController (which has my script attached) to each of them and control them with GhostController. My warning dialogue box shows up saying that they may need to be manually fixed later. Notice redundantRobot sagging after I move GhostController to another robot because the IK is no longer working. When GhostController is moved to IRB140 and because its script was not disabled, its own IK failure warning pops up. At 0:55 I move the GhostController to other models and control them (the Jaco arm, irb360, and Baxter). I interact with a block dynamically with the Jaco arm. Note that irb360 is the only static model and the others are dynamic. About 2:04 I stop the simulation and (almost) everything is restored to what it was at the beginning. I then start the simulation again and move the IK manipulation spheres of the IRB140 and redundantRobot and show they no longer work like they did before (because the IK elements were disabled).

Here's the link to the video.

mfloyd
Posts: 21
Joined: 29 Oct 2014, 23:06

Re: Re-enabling IK elements from a script

Post by mfloyd » 01 Aug 2015, 02:00

I have another related problem while trying to make this script:

Many dynamic models use dynamic overlap constraints in order to perform loop closure. One example is the "Robotiq 85", as well as many other grippers, including some models I've made myself. When my script creates a static model from such a dynamic model, ideally it would need to convert these dynamic overlap constraints to equivalent IK calculations. Without being able to read what kind of link type the dummy has, and without being able to read which dummies are already used for IK calculations [the robotiq 85 uses both IK dummies and dynamic overlap dummies], I would have to guess from the structure of the original model whether a dummy is already used in an IK element by checking all joints in the chain leading up to the dummy that has a linked dummy. I can find pairs of linked dummies using simGetLinkDummy. I would assume that a set of paired dummies where one has IK mode joints among its ancestors would be an IK dummy; I would also assume that other linked dummies that have free-torque joints are ones to be included as dynamics overlap constraint dummies and that I could use those dummies in creating new IK groups for the static model. It ignores GCS altogether (I haven't seen it used in any example models yet, and haven't needed to use it myself). It's not a robust solution however, especially if GCS constraints show up. I'm also not sure how to change the link type of dummy when trying to create an IK group: I copy the model and if the dummy started as dynamics overlap constraint and then I cant use it in an IK group. I can clear the link and reset it, but it defaults to dynamics overlap.

This is just another example where being able to read/set the link type of dummy would be useful (like point 3 in the first post above), and/or being able to retrieve the related IK groups/elements of a dummy found in a model tree.

coppelia
Site Admin
Posts: 6867
Joined: 14 Dec 2012, 00:25

Re: Re-enabling IK elements from a script

Post by coppelia » 03 Aug 2015, 17:00

What you are trying to do is fairly complex it seems.

The first idea that came to my mind would be to simply copy and paste a robot model, and have the original one made invisible. The copy could then be modified (but beforehand made static and non-respondable). Those changes would be done with simSetModelProperty. Once you are done with your manipulation, you would erase the modified model, and make the original one visible, dynamic and respondable again. When doing those changes, make sure you do tis without interruption (i.e. if you are running a threaded child script, use simSetThreadAutomaticSwitch(false) and after the changes simSetThreadAutomaticSwitch(true) again). The idea there is to keep all the models almost untouched, so that they would always be functional (i.e. you wouldn't break them with your modifications)

For your second problem: converting a mechanism from dynamic to Ik (or vice-versa) is almost impossible when you don't know what mechanism you are dealing with and the function of the loop closures/etc. If I understood what you are trying to achieve correctly: you temporarily want to manipulate the various robots via direct joint control, then switch back to the original way the robot is controlled, but by applying some values (e.g. joint target positions, etc.). Is that correct?

Cheers

mfloyd
Posts: 21
Joined: 29 Oct 2014, 23:06

Re: Re-enabling IK elements from a script

Post by mfloyd » 04 Aug 2015, 19:48

I got around setting dummy links as IK by placing a pair of dummies already linked as an IK tip/target pair under my ghost control dummy/script. I then copy and paste those as needed in my ghost and replace the dynamic overlap constraints. It works well for simple models (such as the Jaco or Mico hands). My script needs some more work to get it working with more complicated structures such as the Robotiq gripper, but shouldn't take much more to make it happen.

For the first problem of breaking IK in the original model, I think your suggestion is the only one that would work without changes to V-rep (or its api at least). I will try it soon (copying the original and making it invisible and unable to interact with the simulation otherwise). I think it will complicate the scene hierarchy a bit, since then there will be three copies of the model, one that is the "original" (keeping all IK elements un-modified so the model can be restored later, but changing the model otherwise to prevent it from interacting with the scene or from being seen), the "dynamic copy of the original" (a copy that keeps any dynamic and respondable elements, but with a few temporary changes such as IK removed), and the "Ghost" (A second copy that is changed to a static, non-respondable model that previews control commands before sending them to the dynamic model).

Post Reply