#! /usr/bin/env python3 # -*- coding: utf-8 -*- from moveit.task_constructor import core, stages from py_binding_tools import roscpp_init import time roscpp_init("mtc_tutorial") # Create a task task = core.Task() task.name = "fix collision objects" # Add the current state to the task hierarchy task.add(stages.CurrentState("current state")) # [initAndConfig] # check for collisions and find corrections fixCollisionObjects = stages.FixCollisionObjects("FixCollisionObjects") # cut off length for collision fixing fixCollisionObjects.max_penetration = 0.01 # Add the stage to the task hierarchy task.add(fixCollisionObjects) # [initAndConfig] if task.plan(): task.publish(task.solutions[0]) time.sleep(1)