rs007n_demo.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 
3 import rospy
4 
5 from moveit_commander import MoveGroupCommander
6 
7 if __name__ == '__main__':
8 
9  #init_node()
10  rospy.init_node('message', anonymous=True)
11  group = MoveGroupCommander("manipulator")
12  exec_vel = 0.5
13 
14  while True:
15 
16  rospy.loginfo("joint1 start")
17  group.set_max_velocity_scaling_factor(exec_vel)
18  group.set_joint_value_target([0.1, 0.1, 0.1, 0.1, 0.1, 0.1])
19  group.go()
20  rospy.loginfo("joint1 end")
21 
22  rospy.loginfo("pose1 start")
23  group.set_max_velocity_scaling_factor(exec_vel)
24  group.set_pose_target([0.5,-0.2,0.2,0.0,1.0,0.0])
25  group.go()
26  rospy.loginfo("pose1 end")
27 
28  rospy.loginfo("pose2 start")
29  group.set_max_velocity_scaling_factor(exec_vel)
30  group.set_pose_target([0.5,-0.2,0.7,0.0,1.0,0.0])
31  group.go()
32  rospy.loginfo("pose2 end")
33 
34  rospy.loginfo("pose3 start")
35  group.set_max_velocity_scaling_factor(exec_vel)
36  group.set_pose_target([0.5,0.1,0.7,0.0,1.0,0.0])
37  group.go()
38  rospy.loginfo("pose3 end")
39 
40  rospy.loginfo("pose4 start")
41  group.set_max_velocity_scaling_factor(exec_vel)
42  group.set_pose_target([0.5,0.1,0.2,0.0,1.0,0.0])
43  group.go()
44  rospy.loginfo("pose4 end")


khi_rs007n_moveit_config
Author(s): matsui_hiro
autogenerated on Fri Mar 26 2021 02:34:34