mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
auto-format python code with black
This commit is contained in:
parent
10adb63f4a
commit
fd25a0626a
@ -27,7 +27,7 @@ class Test(unittest.TestCase):
|
|||||||
task.add(stages.CurrentState("current"))
|
task.add(stages.CurrentState("current"))
|
||||||
move = stages.MoveRelative("move", core.JointInterpolationPlanner())
|
move = stages.MoveRelative("move", core.JointInterpolationPlanner())
|
||||||
move.group = self.PLANNING_GROUP
|
move.group = self.PLANNING_GROUP
|
||||||
move.setDirection({"joint_1" : 0.2, "joint_2" : 0.4})
|
move.setDirection({"joint_1": 0.2, "joint_2": 0.4})
|
||||||
task.add(move)
|
task.add(move)
|
||||||
|
|
||||||
task.enableIntrospection()
|
task.enableIntrospection()
|
||||||
@ -42,14 +42,12 @@ class Test(unittest.TestCase):
|
|||||||
|
|
||||||
def test_Merger(self):
|
def test_Merger(self):
|
||||||
cartesian = core.CartesianPath()
|
cartesian = core.CartesianPath()
|
||||||
|
|
||||||
def createDisplacement(group, displacement):
|
def createDisplacement(group, displacement):
|
||||||
move = stages.MoveRelative("displace", cartesian)
|
move = stages.MoveRelative("displace", cartesian)
|
||||||
move.group = group
|
move.group = group
|
||||||
move.ik_frame = PoseStamped(header = Header(frame_id = "tool0"))
|
move.ik_frame = PoseStamped(header=Header(frame_id="tool0"))
|
||||||
dir = Vector3Stamped(
|
dir = Vector3Stamped(header=Header(frame_id="base_link"), vector=Vector3(*displacement))
|
||||||
header = Header(frame_id = "base_link"),
|
|
||||||
vector = Vector3(*displacement)
|
|
||||||
)
|
|
||||||
move.setDirection(dir)
|
move.setDirection(dir)
|
||||||
move.restrictDirection(stages.MoveRelative.Direction.FORWARD)
|
move.restrictDirection(stages.MoveRelative.Direction.FORWARD)
|
||||||
return move
|
return move
|
||||||
@ -58,7 +56,7 @@ class Test(unittest.TestCase):
|
|||||||
task.add(stages.CurrentState("current"))
|
task.add(stages.CurrentState("current"))
|
||||||
merger = core.Merger("merger")
|
merger = core.Merger("merger")
|
||||||
merger.insert(createDisplacement(self.PLANNING_GROUP, [-0.2, 0, 0]))
|
merger.insert(createDisplacement(self.PLANNING_GROUP, [-0.2, 0, 0]))
|
||||||
merger.insert(createDisplacement(self.PLANNING_GROUP, [ 0.2, 0, 0]))
|
merger.insert(createDisplacement(self.PLANNING_GROUP, [0.2, 0, 0]))
|
||||||
task.add(merger)
|
task.add(merger)
|
||||||
|
|
||||||
task.enableIntrospection()
|
task.enableIntrospection()
|
||||||
@ -67,6 +65,6 @@ class Test(unittest.TestCase):
|
|||||||
task.publish(task.solutions[0])
|
task.publish(task.solutions[0])
|
||||||
|
|
||||||
|
|
||||||
if __name__ == '__main__':
|
if __name__ == "__main__":
|
||||||
roscpp_init("test_mtc")
|
roscpp_init("test_mtc")
|
||||||
rostest.rosrun("", "", Test)
|
rostest.rosrun("", "", Test)
|
||||||
|
|||||||
@ -143,9 +143,11 @@ class TestStages(unittest.TestCase):
|
|||||||
except TypeError as e:
|
except TypeError as e:
|
||||||
pass
|
pass
|
||||||
except:
|
except:
|
||||||
self.fail("Assigning {} did raise wrong exception: {}".format(value, sys.exc_info()[0]))
|
msg = "Assigning {} did raise wrong exception: {}"
|
||||||
|
self.fail(msg.format(value, sys.exc_info()[0]))
|
||||||
else:
|
else:
|
||||||
self.fail("Assigning {} did not raise an exception, result: {}".format(value, getattr(stage, name)))
|
msg = "Assigning {} did not raise an exception, result: {}"
|
||||||
|
self.fail(msg.format(value, getattr(stage, name)))
|
||||||
|
|
||||||
def test_CurrentState(self):
|
def test_CurrentState(self):
|
||||||
stage = stages.CurrentState("current")
|
stage = stages.CurrentState("current")
|
||||||
@ -181,7 +183,7 @@ class TestStages(unittest.TestCase):
|
|||||||
stage.setGoal(PointStamped())
|
stage.setGoal(PointStamped())
|
||||||
stage.setGoal(RobotState())
|
stage.setGoal(RobotState())
|
||||||
stage.setGoal("named pose")
|
stage.setGoal("named pose")
|
||||||
stage.setGoal(dict(joint1 = 1.0, joint2 = 2.0))
|
stage.setGoal(dict(joint1=1.0, joint2=2.0))
|
||||||
self._check(stage, "path_constraints", Constraints())
|
self._check(stage, "path_constraints", Constraints())
|
||||||
|
|
||||||
def test_MoveRelative(self):
|
def test_MoveRelative(self):
|
||||||
@ -194,7 +196,7 @@ class TestStages(unittest.TestCase):
|
|||||||
self._check(stage, "path_constraints", Constraints())
|
self._check(stage, "path_constraints", Constraints())
|
||||||
stage.setDirection(TwistStamped())
|
stage.setDirection(TwistStamped())
|
||||||
stage.setDirection(Vector3Stamped())
|
stage.setDirection(Vector3Stamped())
|
||||||
stage.setDirection({'joint': 0.1})
|
stage.setDirection({"joint": 0.1})
|
||||||
|
|
||||||
def test_Connect(self):
|
def test_Connect(self):
|
||||||
planner = core.PipelinePlanner()
|
planner = core.PipelinePlanner()
|
||||||
@ -290,5 +292,5 @@ class TestTask(unittest.TestCase):
|
|||||||
task.add(stages.FixedState())
|
task.add(stages.FixedState())
|
||||||
|
|
||||||
|
|
||||||
if __name__ == '__main__':
|
if __name__ == "__main__":
|
||||||
unittest.main()
|
unittest.main()
|
||||||
|
|||||||
@ -5,8 +5,8 @@ from catkin_pkg.python_setup import generate_distutils_setup
|
|||||||
|
|
||||||
d = generate_distutils_setup(
|
d = generate_distutils_setup(
|
||||||
# list of packages to setup
|
# list of packages to setup
|
||||||
packages = ['moveit', 'moveit.python_tools', 'moveit.task_constructor'],
|
packages=["moveit", "moveit.python_tools", "moveit.task_constructor"],
|
||||||
# specify location of root ('') package dir
|
# specify location of root ("") package dir
|
||||||
package_dir = {'' : 'python/src'}
|
package_dir={"": "python/src"},
|
||||||
)
|
)
|
||||||
setup(**d)
|
setup(**d)
|
||||||
|
|||||||
@ -8,6 +8,7 @@ from math import pi
|
|||||||
import time
|
import time
|
||||||
|
|
||||||
from moveit.python_tools import roscpp_init
|
from moveit.python_tools import roscpp_init
|
||||||
|
|
||||||
roscpp_init("mtc_tutorial")
|
roscpp_init("mtc_tutorial")
|
||||||
|
|
||||||
group = "panda_arm"
|
group = "panda_arm"
|
||||||
@ -25,26 +26,26 @@ task.add(stages.CurrentState("current state"))
|
|||||||
# move along x
|
# move along x
|
||||||
move = stages.MoveRelative("x +0.2", cartesian)
|
move = stages.MoveRelative("x +0.2", cartesian)
|
||||||
move.group = group
|
move.group = group
|
||||||
header = Header(frame_id = "world")
|
header = Header(frame_id="world")
|
||||||
move.setDirection(Vector3Stamped(header=header, vector=Vector3(0.2,0,0)))
|
move.setDirection(Vector3Stamped(header=header, vector=Vector3(0.2, 0, 0)))
|
||||||
task.add(move)
|
task.add(move)
|
||||||
|
|
||||||
# move along y
|
# move along y
|
||||||
move = stages.MoveRelative("y -0.3", cartesian)
|
move = stages.MoveRelative("y -0.3", cartesian)
|
||||||
move.group = group
|
move.group = group
|
||||||
move.setDirection(Vector3Stamped(header=header, vector=Vector3(0,-0.3,0)))
|
move.setDirection(Vector3Stamped(header=header, vector=Vector3(0, -0.3, 0)))
|
||||||
task.add(move)
|
task.add(move)
|
||||||
|
|
||||||
# rotate about z
|
# rotate about z
|
||||||
move = stages.MoveRelative("rz +45°", cartesian)
|
move = stages.MoveRelative("rz +45°", cartesian)
|
||||||
move.group = group
|
move.group = group
|
||||||
move.setDirection(TwistStamped(header=header, twist=Twist(angular=Vector3(0,0,pi/4.))))
|
move.setDirection(TwistStamped(header=header, twist=Twist(angular=Vector3(0, 0, pi / 4.0))))
|
||||||
task.add(move)
|
task.add(move)
|
||||||
|
|
||||||
# Cartesian motion, defined as joint-space offset
|
# Cartesian motion, defined as joint-space offset
|
||||||
move = stages.MoveRelative("joint offset", cartesian)
|
move = stages.MoveRelative("joint offset", cartesian)
|
||||||
move.group = group
|
move.group = group
|
||||||
move.setDirection(dict(panda_joint1=pi/6, panda_joint3=-pi/6))
|
move.setDirection(dict(panda_joint1=pi / 6, panda_joint3=-pi / 6))
|
||||||
task.add(move)
|
task.add(move)
|
||||||
|
|
||||||
# moveTo named posture, using joint-space interplation
|
# moveTo named posture, using joint-space interplation
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user