根据关节角度进行约束 - 与 Drake 直接协作进行轨迹规划 - 机器人学

Question

目标是防止末端执行器到达某个区域

首字母：

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()

我知道如何阻挡单关节天使：

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

我想约束多个相关的关节角度。就像这样： 如果 theta1 在 34°-36° 范围内，则 theta2 应在 116°-118° 范围内被阻挡

我该怎么做？

我也尝试过使用屏障函数。但我更喜欢这种方法。该文档没有帮助。

Answer 1

问题不是很清楚。当然，您可以多次调用该方法，例如

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

但我怀疑你是在问写向量形式？ 例如你想写吗

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

如果是这种情况，请参阅： https://github.com/RobotLocomotion/drake/issues/8315#issuecomment-480773969 基本上，你可以做

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

您可以对

lt

、

le

、

eq

、

ne

、

ge

和

gt

执行此操作。