Adding Constraints Index Error

[rachelholladay]

When you add a constraint it calls something like: problem.AddConstraint(f, [(timestep, i) for i in inds], fntype.value, fnname)

Where inds is generated a few lines above: inds = ([i_dofs[dof] for dof in dofs] if dofs is not None else range(n_dofs))

The logic behind this (if I may paraphrase my understanding from the code) is that if the constraint dictionary has anything entered for the 'dofs' entry (say xrange(7) - since its signature is that it must be an array of ints) then inds is an array of the active dof indices. So on the right arm something like [15, 17, 19, 20, 21, 22, 23]. This would cause a C++ out of bounds error when calling AddConstraint.

If instead the dictionary 'dof' entry was passed 'None' then inds becomes range(number of dofs), which successfully adds the constraint and does not error.

Unless I am misunderstanding something it would seem as though instead of inds = ([i_dofs[dof] for dof in dofs] if dofs is not None else range(n_dofs))

It should be inds = range(n_dofs).

Since this would seem to be the only place that the dictionary entry 'dof' is used, it seems as though it really shouldn't be an entry in the dictionary.

Unless I misunderstand and there is a time when you would want to pass the indices of the active DOFs. But the C++ error leads me to believe trajopt isn't expecting that..

(while I've been referring to constraints the same logic is used in adding a cost)

[psigen]

I need some time to parse this...

[rachelholladay]

Sorry. Maybe I can try to clarify. The third argument to addConstraint / addErrorCost specifies what variables to apply over. I'm wondering why the DOFs would be applying over should be anything except for range(number of dofs).

[psigen]

I think it was originally intended for situations where a constraint function only applies over a subset of the DOFs that trajopt is optimizing.

Suppose we want to write a constraint that HERB's right hand is holding something, and his left hand is holding something else. If we want to write these constraints in a general way, we could write a constraint function that is applied over the DOFs of a WAM and constrains its EE frame to match some transform.

Now suppose we want to now plan bimanually. The active DOFs of the robot would be both arms, but each constraint would apply to only a subset of those DOFs. In this case, we would pass the constraint functions, but specify that each only applies to the 7 DOFs associated with their respective arm.

The line of code that you are looking at takes this list of DOF indices on HERB (e.g. [15, 17, 19, 20, 21, 22, 23]) and compares it to the list of indices that are currently active (e.g. [0, 1, 2, 3, 4, 5, 6, 15, 17, 19, 20, 21, 22, 23]). It then figures where in the current list of active indices each of the input DOFs is (e.g. [7, 8, 9, 10. 11, 12, 13]) and returns that list. This tells trajopt that of all the active DOFs it is optimizing, only the ones in this specific list can affect this constraint.

There might still be a bug in the code attempting to implement this logic though. Do you have a test case?

[rachelholladay]

Thanks for the thorough explanation. That better clarified what I thought was going on.

I was planning constraints for the right arm, having set the right arm to be the active DOF so the active DOF indices were [15 17 19 20 21 22 23] and the active DOF was 7. At first, perhaps naive in retrospect, giving 'range(7)' for the DOF dictionary input. That led to the C++ error listed above. Based on your explanation I was tempted to give the arm DOFs as the dictionary input but that throws an obvious indexing error at the creation of 'inds'.

Maybe its that I'm missing something simple. If you were writing a constraint for say the right arm, what would the intended dictionary DOF argument?

Also as a side note independent of logic I'm pretty sure

inds = ([i_dofs.index(dof) for dof in dofs]
  if dofs is not None else range(n_dofs))

Should be

 inds = ([i_dofs[dof] for dof in dofs]
  if dofs is not None else range(n_dofs))

Considering i_dofs is a numpy array, not a list. I hadn't pushed the tweak in case there was something else to be changed in the logic.

[mkoval]

@rachelholladay I think that code is correct. i_dofs seems to be a list of DOF indices. The snippet

[i_dofs.index(dof) for dof in dofs]

maps from "DOF index" to "optimization variable index;" i.e. the position of the DOF index in the i_dofs array. It defaults to range(n_dofs); i.e. all DOFs.

@psigen Is this correct? The Trajopt documentation is unclear about whether AddConstraint takes DOF indices or optimization variable indices. All it says is:

This argument is telling the optimizer which variables the provided function should be applied to. This parameter must be a list of ordered pairs (timestep, dof).

[psigen]

inds = ([i_dofs.index(dof) for dof in dofs] if dofs is not None else range(n_dofs))

Should be

inds = ([i_dofs[dof] for dof in dofs] if dofs is not None else range(n_dofs))

Well, one issue is that these two things are not equivalent. i_dofs.index(dof) does not mean "get the dof element of the i_dofs array", it means "get the first index in the i_dofs array such that i_dofs[index] = dof". It's a search function. I think the closest equivalent is numpy.where().

So maybe the issue is that you are passing a numpy array, and I'm expecting a list? If so, we can convert everything to one or the other on input.

[mkoval]

Yes, I am saying that your version is incorrect. :smile: (Assuming that Trajopt is expecting a list of optimization variable indices, not DOF indices.)

[psigen]

I'm not sure, but I think that trajopt expects optimization variable indices here because the default of range(n_dofs) would make no sense for most cases otherwise.

[psigen]

Ok, I guess at the end of the day, my assumption is that if trajopt is optimizing over active indices: [15, 17, 19, 20, 21, 22, 23]

and you pass in a constraint that is applied to OpenRAVE robot DOFs: [15, 17, 19, 20, 21, 22, 23]

then the AddConstraint function should get passed: [0, 1, 2, 3, 4, 5, 6].

[mkoval]

I looked through the Trajopt source code and I'm honestly not sure. It's only called in one place with range(7), so it could go either way. It's also possible that they never tested this function on a subset of the active DOFs.

[psigen]

If that's the case, then passing range(7) would never work with the right arm only (as the underlying DOFs are pretty arbitrary) but I'm pretty sure it does.

[rachelholladay]

To confirm range(7) was the only thing I could get to work, and while I am currently only dealing with the right arm it definitely applied the desired constraint.

i_dofs a numpy array because its the active dof indices. I am passing in a list, (range(7)). I now see what you are talking about what it should be doing but can't think of the preferred way to implement that.

[psigen]

Numpy arrays have a tolist() function that converts them to a normal python list. Can you quickly try using that as i_dofs that and see if it works?

If so, we can just sanitize the input a little bit and we should be good.

[rachelholladay]

(I always ask because I have not yet fully mastered the preferred conventions)

The following two methods seem to work identically (and correctly):

0. Passing nothing as the DOFs dictionary entry, thus having inds default to range(n_dofs)
1. Passing robot.GetActiveDOFIndices() (or robot.GetActiveDOFIndices.tolist(), no noticeable difference) as the DOFs dictionary entry (with the right am active) and the small edit of

inds = ([(i_dofs.tolist()).index(dof) for dof in dofs]
  if dofs is not None else range(n_dofs))

I'll happily push this small edit if this is the desired behavior.

[psigen]

Ok, cool. I think this is a numpy.array/list issue then. I think we can make a slightly generalized version of (2) that handles both lists and numpy arrays, and then we'll be good to go.

On Mon, Jul 13, 2015 at 1:18 PM, Rachel Holladay notifications@github.com wrote:

(I always ask because I have not yet fully mastered the preferred conventions)

The following two methods seem to work identically (and correctly):

0. Passing nothing as the DOFs dictionary entry, thus having inds default to range(n_dofs)
1. Passing robot.GetActiveDOFIndices() as the DOFs dictionary entry (with the right am active) and the small edit of

inds = ([(i_dofs.tolist()).index(dof) for dof in dofs] if dofs is not None else range(n_dofs))

I'll happily push this small edit if this is the desired behavior.

— Reply to this email directly or view it on GitHub https://github.com/personalrobotics/or_trajopt/issues/6#issuecomment-120997261 .