CODEWITHSUNDEEP
pythondrake
BjorniFPV
BjorniFPV

Reputation: 1

Constrain depending joint angles - Trajectory Planning with Drake Direct Collabration - Robotics

The goal is to prevent the end effector from reaching a certain area

Initial:

dircol = DirectCollocation(acromonk_plant,
                                    acromonk_plant.CreateDefaultContext(),
                                    num_time_samples=30,
                                    minimum_timestep=0.01,\
                                    maximum_timestep=0.5,
                                    input_port_index = acromonk_plant.get_actuation_input_port().get_index())

# Add Equal time interval constraint so that DirectCollocation doesn't stretch/shrink time
# in our trajectory.
_ = dircol.AddEqualTimeIntervalsConstraints()

I know how to block single joint angels:

state = dircol.state()
_ = dircol.AddConstraintToAllKnotPoints(state[1] >= np.deg2rad(-150))

I want to constrain multiple depending joint angles. Like so: If theta1 is in Range 34°-36°, theta2 should be blocked in Range 116°-118°

How can I do that?

I also tried to use a barrier function. But i prefer this method. The documentation did not help.

Upvotes: 0

Views: 44

Answers (1)

Russ Tedrake
Russ Tedrake

Reputation: 5533

The question is not very clear. Of course, you could call that method multiple times, e.g.

dircol.AddConstraintToAllKnotPoints(state[1] >= np.deg2rad(-150))
dircol.AddConstraintToAllKnotPoints(state[2] >= np.deg2rad(-116))

but I suspect you are asking about writing the vector form? e.g. you'd like to write

dircol.AddConstraintToAllKnotPoints(state >= [0.1, 0.2])

if that's the case, then please see: https://github.com/RobotLocomotion/drake/issues/8315#issuecomment-480773969 basically, you can do

from pydrake.all import ge
dircol.AddConstraintToAllKnotPoints(ge(state,[0.1, 0.2]))

you can do this for lt, le, eq, ne, ge, and gt.

Upvotes: 0

Related Questions