如何仅为Plant中的单个对象添加单位四元数约束？

问题场景 #

桌面上放置一个箱体，需要为该对象添加四元数约束。查阅文档发现AddUnitQuaternionConstraintOnPlant()会对Plant中所有对象生效。已将对象的广义位置声明为决策变量：

q_o_vars = prog.NewContinuousVariables(nq_o, N, "q_o")

随后执行：

AddUnitQuaternionConstraintOnPlant(plant, q_o_vars[:, i], prog)

但出现错误：condition 'q_vars.rows() == plant.num_positions()' failed。请问是否存在现成函数可替代手动为Plant中的单个对象添加约束？

解决方案 #

• 不要使用AddUnitQuaternionConstraintOnPlant()，这个函数要求输入变量的维度等于整个Plant的总位置数，这正是报错的原因。
• 改用AddUnitQuaternionConstraint()函数，可针对单个刚体的四元数部分单独添加约束，具体步骤如下：
  1. 获取目标刚体在Plant中的索引：

     body_index = plant.GetBodyByName("box_body").index()
     

  2. 获取该刚体四元数在其广义位置数组中的切片范围：

     quat_slice = plant.GetBodyPoseInArrayIndices(body_index)
     

  3. 从已定义的变量中提取对应四元数部分：

     quat_vars = q_o_vars[quat_slice, i]
     

  4. 添加单位四元数约束：

     AddUnitQuaternionConstraint(prog, quat_vars)
     

这样就能仅为目标箱体添加四元数约束，无需处理Plant中的其他刚体。

内容的提问来源于stack exchange，提问作者Xiao