mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-09-27 00:29:13 +08:00

Some checks are pending
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[CLANG_TIDY:true IMAGE:noble-ci-testing TARGET_CMAKE_… (push) Waiting to run
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[DOCKER_RUN_OPTS:-e PRELOAD=libasan.so.8 -e LSAN_OPTI… (push) Waiting to run
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[IMAGE:jammy-ci]) (push) Waiting to run
CI / ${{ matrix.env.IMAGE }}${{ matrix.env.NAME && ' • ' || ''}}${{ matrix.env.NAME }}${{ matrix.env.CATKIN_LINT && ' • catkin_lint' || ''}}${{ matrix.env.CLANG_TIDY && ' • clang-tidy' || '' }} (map[IMAGE:noble-ci NAME:ccov TARGET_CMAKE_ARGS:-DCMAKE_B… (push) Waiting to run
CI / doc (push) Blocked by required conditions
CI / deploy (push) Blocked by required conditions
Format / pre-commit (push) Waiting to run
78 lines
2.0 KiB
Python
Executable File
78 lines
2.0 KiB
Python
Executable File
#! /usr/bin/env python3
|
|
# -*- coding: utf-8 -*-
|
|
|
|
from std_msgs.msg import Header
|
|
from geometry_msgs.msg import TwistStamped, Twist, Vector3Stamped, Vector3
|
|
from moveit.task_constructor import core, stages
|
|
from math import pi
|
|
import time
|
|
|
|
from py_binding_tools import roscpp_init
|
|
|
|
roscpp_init("mtc_tutorial")
|
|
|
|
# [cartesianTut1]
|
|
group = "panda_arm"
|
|
# [cartesianTut1]
|
|
|
|
# [cartesianTut2]
|
|
# Cartesian and joint-space interpolation planners
|
|
cartesian = core.CartesianPath()
|
|
cartesian.max_cartesian_speed = 0.1 # m/s
|
|
cartesian.cartesian_speed_limited_link = "panda_hand"
|
|
jointspace = core.JointInterpolationPlanner()
|
|
# [cartesianTut2]
|
|
|
|
# [cartesianTut3]
|
|
task = core.Task()
|
|
task.name = "cartesian"
|
|
|
|
# start from current robot state
|
|
task.add(stages.CurrentState("current state"))
|
|
# [cartesianTut3]
|
|
|
|
# [initAndConfigMoveRelative]
|
|
# 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)
|
|
# [initAndConfigMoveRelative]
|
|
|
|
# [cartesianTut4]
|
|
# 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)
|
|
# [cartesianTut4]
|
|
|
|
# [cartesianTut5]
|
|
# rotate about z
|
|
move = stages.MoveRelative("rz +45°", cartesian)
|
|
move.group = group
|
|
move.setDirection(TwistStamped(header=header, twist=Twist(angular=Vector3(0, 0, pi / 4.0))))
|
|
task.add(move)
|
|
# [cartesianTut5]
|
|
|
|
# [cartesianTut6]
|
|
# Cartesian motion, defined as joint-space offset
|
|
move = stages.MoveRelative("joint offset", cartesian)
|
|
move.group = group
|
|
move.setDirection(dict(panda_joint1=pi / 6, panda_joint3=-pi / 6))
|
|
task.add(move)
|
|
# [cartesianTut6]
|
|
|
|
# [initAndConfigMoveTo]
|
|
# moveTo named posture, using joint-space interplation
|
|
move = stages.MoveTo("moveTo ready", jointspace)
|
|
move.group = group
|
|
move.setGoal("ready")
|
|
task.add(move)
|
|
# [initAndConfigMoveTo]
|
|
|
|
if task.plan():
|
|
task.publish(task.solutions[0])
|
|
time.sleep(100)
|