1+
2+ # https://ros-planning.github.io/moveit_tutorials/
3+
4+ from __future__ import print_function
5+ from six .moves import input
6+
7+ import sys
8+ import copy
9+ import rospy
10+ import moveit_commander
11+ import moveit_msgs .msg
12+ import geometry_msgs .msg
13+
14+ try :
15+ from math import pi , tau , dist , fabs , cos
16+ except : # For Python 2 compatibility
17+ from math import pi , fabs , cos , sqrt
18+
19+ tau = 2.0 * pi
20+
21+ def dist (p , q ):
22+ return sqrt (sum ((p_i - q_i ) ** 2.0 for p_i , q_i in zip (p , q )))
23+
24+
25+ from std_msgs .msg import String
26+ from moveit_commander .conversions import pose_to_list
27+
28+
29+ def main ():
30+ moveit_commander .roscpp_initialize (sys .argv )
31+ rospy .init_node ("move_group_python_interface_tutorial" , anonymous = True )
32+ robot = moveit_commander .RobotCommander ()
33+ scene = moveit_commander .PlanningSceneInterface ()
34+ group_name = "panda_arm"
35+ move_group = moveit_commander .MoveGroupCommander (group_name )
36+ display_trajectory_publisher = rospy .Publisher (
37+ "/move_group/display_planned_path" ,
38+ moveit_msgs .msg .DisplayTrajectory ,
39+ queue_size = 20 ,
40+ )
41+ # We can get the name of the reference frame for this robot:
42+ planning_frame = move_group .get_planning_frame ()
43+ print ("============ Planning frame: %s" % planning_frame )
44+
45+ # We can also print the name of the end-effector link for this group:
46+ eef_link = move_group .get_end_effector_link ()
47+ print ("============ End effector link: %s" % eef_link )
48+
49+ # We can get a list of all the groups in the robot:
50+ group_names = robot .get_group_names ()
51+ print ("============ Available Planning Groups:" , robot .get_group_names ())
52+
53+ # Sometimes for debugging it is useful to print the entire state of the
54+ # robot:
55+ print ("============ Printing robot state" )
56+ print (robot .get_current_state ())
57+ print ("" )
58+
59+ from scipy .spatial .transform import Rotation as R
60+ import math
61+ import numpy as np
62+
63+ pose_goal = geometry_msgs .msg .Pose ()
64+ rot = R .from_euler ('xyz' , [90 , 90 , 0 ], degrees = True )
65+ pose_goal .orientation .w = rot .as_quat ()[- 1 ]
66+ breakpoint ()
67+ pose_goal .position .x = 0.4
68+ pose_goal .position .y = 0.2
69+ pose_goal .position .z = 0.4
70+
71+ move_group .set_pose_target (pose_goal )
72+
73+ # `go()` returns a boolean indicating whether the planning and execution was successful.
74+ success = move_group .go (wait = True )
75+ # Calling `stop()` ensures that there is no residual movement
76+ move_group .stop ()
77+ # It is always good to clear your targets after planning with poses.
78+ # Note: there is no equivalent function for clear_joint_value_targets().
79+
80+ ## Moving according to waypoint
81+
82+ # move_group.clear_pose_targets()
83+ # waypoints = []
84+ # scale = 1
85+ # wpose = move_group.get_current_pose().pose
86+ # wpose.orientation.w = 0
87+ # wpose.position.x -= scale * 0.1 # First move up (z)
88+ # wpose.position.y -= scale * 0.2 # and sideways (y)
89+ # waypoints.append(copy.deepcopy(wpose))
90+
91+ # wpose.position.x += scale * 0.1 # Second move forward/backwards in (x)
92+ # waypoints.append(copy.deepcopy(wpose))
93+
94+ # wpose.position.y -= scale * 0.1 # Third move sideways (y)
95+ # waypoints.append(copy.deepcopy(wpose))
96+
97+ # We want the Cartesian path to be interpolated at a resolution of 1 cm
98+ # which is why we will specify 0.01 as the eef_step in Cartesian
99+ # translation. We will disable the jump threshold by setting it to 0.0,
100+ # ignoring the check for infeasible jumps in joint space, which is sufficient
101+ # for this tutorial.
102+ # (plan, fraction) = move_group.compute_cartesian_path(
103+ # waypoints, 0.01, 0.0 # waypoints to follow # eef_step
104+ # ) # jump_threshold
105+ # breakpoint()
106+ # move_group.execute(plan, wait=True)
107+
1108if __name__ == "__main__" :
2- pass
109+ main ()
0 commit comments