mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-09-27 00:29:13 +08:00
fix stage "FixCollisionObjects"
... correction wasn't initialized to zero, resulting in random NaNs
This commit is contained in:
parent
a0a9738a00
commit
1e39da2388
@ -74,6 +74,7 @@ void FixCollisionObjects::computeBackward(const InterfaceState &to)
|
||||
|
||||
bool computeCorrection(const std::vector<cd::Contact>& contacts, Eigen::Vector3d& correction, double max_penetration)
|
||||
{
|
||||
correction.setZero();
|
||||
for (const cd::Contact& c : contacts) {
|
||||
if ((c.body_type_1 != cd::BodyTypes::WORLD_OBJECT &&
|
||||
c.body_type_2 != cd::BodyTypes::WORLD_OBJECT)) {
|
||||
|
Loading…
Reference in New Issue
Block a user