def createDisplacement(group, displacement):
move = stages.MoveRelative("displace", cartesian)
move.group = group
move.ik_frame = PoseStamped(header=Header(frame_id="tool0"))
dir = Vector3Stamped(header=Header(frame_id="base_link"),
vector=Vector3(*displacement))
move.setDirection(dir)
move.restrictDirection(stages.MoveRelative.Direction.FORWARD)
return move
def test_MoveRelative(self):
stage = stages.MoveRelative("move", self.planner)
self._check(stage, "group", "group")
self._check(stage, "ik_frame", PoseStamped())
self._check(stage, "min_distance", 0.5)
self._check(stage, "max_distance", 0.25)
self._check(stage, "path_constraints", Constraints())
stage.setDirection(TwistStamped())
stage.setDirection(Vector3Stamped())
def test_MoveRelative(self):
task = core.Task()
task.add(stages.CurrentState("current"))
move = stages.MoveRelative("move", core.JointInterpolationPlanner())
move.group = self.PLANNING_GROUP
move.setDirection({"joint_1": 0.2, "joint_2": 0.4})
task.add(move)
task.enableIntrospection()
task.init()
task.plan()
self.assertEqual(len(task.solutions), 1)
for s in task.solutions:
print(s)
s = task.solutions[0]
task.execute(s)
roscpp_init("mtc_tutorial")
group = "panda_arm"
eef_frame = "panda_link8"
# Cartesian and joint-space interpolation planners
cartesian = core.CartesianPath()
jointspace = core.JointInterpolationPlanner()
task = core.Task()
# start from current robot state
task.add(stages.CurrentState("current state"))
# move along x
move = stages.MoveRelative("x +0.2", cartesian)
move.group = group
header = Header(frame_id="world")
move.setDirection(Vector3Stamped(header=header, vector=Vector3(0.2, 0, 0)))
task.add(move)
# move along y
move = stages.MoveRelative("y -0.3", cartesian)
move.group = group
move.setDirection(Vector3Stamped(header=header, vector=Vector3(0, -0.3, 0)))
task.add(move)
# rotate about z
move = stages.MoveRelative("rz +45°", cartesian)
move.group = group
move.setDirection(
from moveit.python_tools import roscpp_init
roscpp_init("mtc_tutorial")
group = "panda_arm"
eef_frame = "panda_link8"
# Cartesian interpolation planner
cartesian = core.CartesianPath()
task = core.Task()
# start from current robot state
task.add(stages.CurrentState("current state"))
# move along x
move = stages.MoveRelative("x +0.2", cartesian)
move.group = group
header = Header(frame_id = "world")
move.setDirection(Vector3Stamped(header=header, vector=Vector3(0.2,0,0)))
task.add(move)
# move along y
move = stages.MoveRelative("y -0.3", cartesian)
move.group = group
move.setDirection(Vector3Stamped(header=header, vector=Vector3(0,-0.3,0)))
task.add(move)
# rotate about z
move = stages.MoveRelative("z +90°", cartesian)
move.group = group
move.setDirection(TwistStamped(header=header, twist=Twist(angular=Vector3(0,0,pi/4.))))
