def main():
rospy.init_node("bottle_shake")
robot = moveit_commander.RobotCommander()
arm = moveit_commander.MoveGroupCommander("arm")
arm.set_max_velocity_scaling_factor(0.3)
gripper = moveit_commander.MoveGroupCommander("gripper")
while len([s for s in rosnode.get_node_names() if 'rviz' in s]) == 0:
rospy.sleep(1.0)
rospy.sleep(1.0)
print("Group names:")
print(robot.get_group_names())
print("Current state:")
print(robot.get_current_state())
# アーム初期ポーズを表示
arm_initial_pose = arm.get_current_pose().pose
print("Arm initial pose:")
print(arm_initial_pose)
# 何かを掴んでいた時のためにハンドを開く
gripper.set_joint_value_target([0.9, 0.9])
gripper.go()
# SRDFに定義されている"home"の姿勢にする
arm.set_named_target("home")
arm.go()
gripper.set_joint_value_target([0.7, 0.7])
gripper.go()
# 掴む準備をする
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = 0.2
target_pose.position.y = 0.0
target_pose.position.z = 0.3
q = quaternion_from_euler(-3.14, 0.0, -3.14/2.0) # 上方から掴みに行く場合
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
arm.set_pose_target(target_pose) # 目標ポーズ設定
arm.go() # 実行
# ハンドを開く
gripper.set_joint_value_target([0.7, 0.7])
gripper.go()
# 掴みに行く
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = 0.2
target_pose.position.y = 0.0
target_pose.position.z = 0.1
q = quaternion_from_euler(-3.14, 0.0, -3.14/2.0) # 上方から掴みに行く場合
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
arm.set_pose_target(target_pose) # 目標ポーズ設定
arm.go() # 実行
# ハンドを閉じる
gripper.set_joint_value_target([0.4, 0.4]) #掴むobjectによって変更する
gripper.go()
# 持ち上げる
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = 0.2
target_pose.position.y = 0.0
target_pose.position.z = 0.3
q = quaternion_from_euler(-3.14, 0.0, -3.14/2.0) # 上方から掴みに行く場合
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
arm.set_pose_target(target_pose) # 目標ポーズ設定
arm.go() # 実行
arm.set_named_target("vertical")
arm.go()
# with open('swing_object.csv') as f: #csvファイルを読み込む
# reader = csv.reader(f)#, quoting=csv.QUOTE_NONNUMERIC)
# data = [row for row in reader]
# data = [[["a",1,20,0.3]],[["a",2,20,0.3]],[["a",3,-60,0.9],["a",5,0,0.9]],[["a",3,1,0.3],["a",5,30,0.3]],[["a",3,-60,0.9],["a",5,-30,0.9]],[["a",3,1,0.3],["a",5,30,0.3]]]
data = [[["a",1,20,1]],[["a",3,-60,0.4],["a",5,-40,0.4]],[["a",3,1,1],["a",5,40,1]],[["a",3,-60,0.4],["a",5,-40,0.4]],[["a",3,1,1],["a",5,40,1]],[["a",3,1,1],["a",5,40,1]],[["a",3,-60,0.4],["a",5,-40,0.4]],[["a",3,1,1],["a",5,40,1]]]
arm_joint_values = arm.get_current_joint_values()
for flame in range(len(data)):
for joint_data in range(len(data[flame])):
part=data[flame][joint_data][0]
joint=int(data[flame][joint_data][1])
angle = float(data[flame][joint_data][2])/180.0*math.pi
speed =float(data[flame][joint_data][3])
print(part, joint, angle, speed)
if part == "a":
# arm_joint_values = arm.get_current_joint_values()
arm_joint_values[joint] = angle
# elif part == "g":
# gripper_joint_values = gripper.get_current_joint_values()
# gripper_joint_values[joint] = angle
# gripper.set_joint_value_target(gripper_joint_values)
print("flame")
print(arm_joint_values)
arm.set_joint_value_target(arm_joint_values)
arm.set_max_velocity_scaling_factor(speed)
arm.go()
# gripper.go()
print("done")
def main():
# init node
rospy.init_node("grasp_commander")
# ========== publisher to jamming gripper ========== #
grasp_pub = rospy.Publisher('/jamming_grasp', Int32, queue_size=1)
# ========== Moveit init ========== #
# moveit_commander init
robot = moveit_commander.RobotCommander()
arm = moveit_commander.MoveGroupCommander("arm")
arm_initial_pose = arm.get_current_pose().pose
target_pose = geometry_msgs.msg.Pose()
# Set the planning time
arm.set_planning_time(10.0)
# ========== TF Lister ========== #
tf_buffer = tf2_ros.Buffer()
tf_listner = tf2_ros.TransformListener(tf_buffer)
for i in [0, 1]:
target = "object_" + str(i)
get_tf_flg = False
# Get target TF
while not get_tf_flg:
try:
trans = tf_buffer.lookup_transform('world', target,
rospy.Time(0),
rospy.Duration(10))
print "world -> ar_marker"
print trans.transform
print "Target Place & Pose"
# Go to up from target
target_pose.position.x = trans.transform.translation.x
target_pose.position.y = trans.transform.translation.y
target_pose.position.z = trans.transform.translation.z + 0.10
q = (trans.transform.rotation.x, trans.transform.rotation.y,
trans.transform.rotation.z, trans.transform.rotation.w)
(roll, pitch,
yaw) = tf.transformations.euler_from_quaternion(q)
# roll -= math.pi/6.0
pitch += math.pi / 2.0
# yaw += math.pi/4.0
tar_q = tf.transformations.quaternion_from_euler(
roll, pitch, yaw)
target_pose.orientation.x = tar_q[0]
target_pose.orientation.y = tar_q[1]
target_pose.orientation.z = tar_q[2]
target_pose.orientation.w = tar_q[3]
print target_pose
arm.set_pose_target(target_pose)
arm.go()
arm.clear_pose_targets()
#rospy.sleep(2)
# Get Grasp
# waypoints = []
# waypoints.append(arm.get_current_pose().pose)
# wpose = geometry_msgs.msg.Pose()
# wpose.orientation.w = 1.0
# wpose.position.x = waypoints[0].position.x
# wpose.position.y = waypoints[0].position.y - 0.085
# wpose.position.z = waypoints[0].position.z
# waypoints.append(copy.deepcopy(wpose))
# (plan, fraction) = arm.compute_cartesian_path(waypoints, 0.005, 0.0, False)
# target_pose.position.x = trans.transform.translation.x
# target_pose.position.y = trans.transform.translation.y + 0.04
# target_pose.position.z = trans.transform.translation.z + 0.243
# target_pose.orientation = target_pose.orientation
# print target_pose
# arm.set_pose_target(target_pose)
# arm.go()
# arm.clear_pose_targets()
# # Grasp
# grasp_pub.publish(1)
# print "!! Grasping !!"
# rospy.sleep(2.0)
# # Start Return
# # waypoints = []
# # waypoints.append(arm.get_current_pose().pose)
# # wpose = geometry_msgs.msg.Pose()
# # wpose.orientation.w = 1.0
# # wpose.position.x = waypoints[0].position.x
# # wpose.position.y = waypoints[0].position.y + 0.085
# # wpose.position.z = waypoints[0].position.z
# # waypoints.append(copy.deepcopy(wpose))
# # (plan, fraction) = arm.compute_cartesian_path(waypoints, 0.005, 0.0, False)
# target_pose.position.x = trans.transform.translation.x
# target_pose.position.y = trans.transform.translation.y + 0.04
# target_pose.position.z = trans.transform.translation.z + 0.32
# target_pose.orientation = target_pose.orientation
# print target_pose
# arm.set_pose_target(target_pose)
# arm.go()
# arm.clear_pose_targets()
# #rospy.sleep(2)
# # Go to Home Position
# target_pose.position.x = -0.13683 + (i-1)*0.08
# target_pose.position.y = -0.22166
# target_pose.position.z = 0.50554
# target_pose.orientation.x = 0.00021057
# target_pose.orientation.y = 0.70092
# target_pose.orientation.z = 0.00030867
# target_pose.orientation.w = 0.71324
# arm.set_pose_target(target_pose)
# arm.go()
# arm.clear_pose_targets()
# #rospy.sleep(2)
# target_pose.position.x = target_pose.position.x
# target_pose.position.y = target_pose.position.y
# target_pose.position.z -= 0.10
# target_pose.orientation = target_pose.orientation
# arm.set_pose_target(target_pose)
# arm.go()
# arm.clear_pose_targets()
# # Release
# print " !! Release !!"
# grasp_pub.publish(0)
# rospy.sleep(1)
get_tf_flg = True
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
continue
from gazebo_msgs.msg import LinkStates
from std_srvs.srv import Empty
from gazebo_msgs.msg import LinkState
from gazebo_msgs.srv import SetLinkState
from rosgraph_msgs.msg import Clock
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg
from std_msgs.msg import String
from moveit_commander.conversions import pose_to_list
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('trajectory_planning', anonymous=True)
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
group_name = "aarm_group"
move_group = moveit_commander.MoveGroupCommander(group_name)
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
planning_frame = move_group.get_planning_frame()
#print ("============ Planning frame: %s" % planning_frame)
#eef_link = move_group.get_end_effector_link()
#print "============ End effector link: %s" % eef_link
def __init__(self):
# print("__init__ of GazeboSmartBotPincherKinectEnv")
# Launch the simulation with the given launch file name
# launch file is in gym-gazebo/gym_gazebo/env/assets/launch/
gazebo_env.GazeboEnv.__init__(self,
"GazeboSmartBotPincherKinect_v0.launch")
self.link_state_pub = rospy.Publisher("/gazebo/set_link_state",
LinkState,
queue_size=5)
self.unpause_proxy = rospy.ServiceProxy("/gazebo/unpause_physics",
Empty)
self.pause_proxy = rospy.ServiceProxy("/gazebo/pause_physics", Empty)
self.reset_proxy = rospy.ServiceProxy("/gazebo/reset_simulation",
Empty)
# the state space (=observation space) are all possible depth images of the kinect camera
# Test for HER algorithm
self.observation_space = spaces.Dict(
dict(
desired_goal=spaces.Box(shape=(6, ), dtype='float32'),
achieved_goal=spaces.Box(shape=(6, ), dtype='float32'),
observation=spaces.Box(low=0,
high=255,
shape=[220 * 300],
dtype=np.uint8),
))
# the action space are all possible positions & orientations (6-DOF),
# which are bounded in the area in front of the robot arm where an object can lie (see reset())
boundaries_xAxis = [0.04,
0.3] # box position possiblities: (0.06, 0.22)
boundaries_yAxis = [-0.25,
0.25] # box position possiblities: (-0.2, 0.2)
boundaries_zAxis = [0, 0.05] # box position possiblities: (0.05, 0.1)
boundaries_roll = [0, 2 * np.pi] # box position possiblities: 0
boundaries_pitch = [0, 2 * np.pi] # box position possiblities: 0
boundaries_yaw = [0,
2 * np.pi] # box position possiblities: (0, np.pi)
# test with symmetric boundaries
# boundaries_xAxis = [-0.3, 0.3]
# boundaries_yAxis = [-0.25, 0.25]
# boundaries_zAxis = [-0.5, 0.5]
# boundaries_roll = [-np.pi, np.pi]
# boundaries_pitch = [-np.pi, np.pi]
# boundaries_yaw = [-np.pi, np.pi]
low = np.array([
boundaries_xAxis[0], boundaries_yAxis[0], boundaries_zAxis[0],
boundaries_roll[0], boundaries_pitch[0], boundaries_yaw[0]
])
high = np.array([
boundaries_xAxis[1], boundaries_yAxis[1], boundaries_zAxis[1],
boundaries_roll[1], boundaries_pitch[1], boundaries_yaw[1]
])
self.action_space = spaces.Box(low=low, high=high, dtype=np.float32)
self.reward_range = (-np.inf, np.inf)
self.seed()
# setting up the scene
print("inserting table plane")
self.insertObject(
0.4, 0, 0.025 / 2,
"/home/joel/Documents/pluginTest/smartBot_plugin/models/tablePlane/tablePlane.sdf",
"tablePlane")
print("inserting kinect sensor")
self.insertObject(
0.03,
0,
0.5,
"/home/joel/Documents/pluginTest/smartBot_plugin/models/kinect_ros/model.sdf",
"kinect_ros",
roll=0,
pitch=np.pi / 2,
yaw=0)
print("inserting object to pick up")
self.insertObject(
0.1, 0, 0.1,
"/home/joel/Documents/pluginTest/smartBot_plugin/models/objectToPickUp/objectToPickUp.sdf",
"objectToPickUp")
# initializing MoveIt variables
# it's necessary to unpause the simulation in order to initialize the MoveGroupCommander, printing robot state etc.
self.unpause_simulation()
moveit_commander.roscpp_initialize(sys.argv)
# rospy.init_node("move_group_python_interface", anonymous=True) # can't init another node, there is already one initialized in the superclass
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface()
self.group_arm = moveit_commander.MoveGroupCommander("arm")
# used to be: base_link, also possible: world (if defined in the urdf.xarco file)
self.REFERENCE_FRAME = "world"
# Allow some leeway in position (meters) and orientation (radians)
self.group_arm.set_goal_position_tolerance(0.01)
self.group_arm.set_goal_orientation_tolerance(0.01)
# Allow replanning to increase the odds of a solution
self.group_arm.allow_replanning(True)
# Set the right arm reference frame
self.group_arm.set_pose_reference_frame(self.REFERENCE_FRAME)
# Allow 5 seconds per planning attempt
self.group_arm.set_planning_time(5)
self.bridge = CvBridge()
self.pause_simulation()
def __init__(self):
super(MoveGroupPythonIntefaceTutorial, self).__init__()
## BEGIN_SUB_TUTORIAL setup
##
## First initialize `moveit_commander`_ and a `rospy`_ node:
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_group_python', anonymous=True)
## Instantiate a `RobotCommander`_ object. Provides information such as the robot's
## kinematic model and the robot's current joint states
robot = moveit_commander.RobotCommander()
## Instantiate a `PlanningSceneInterface`_ object. This provides a remote interface
## for getting, setting, and updating the robot's internal understanding of the
## surrounding world:
scene = moveit_commander.PlanningSceneInterface()
## Instantiate a `MoveGroupCommander`_ object. This object is an interface
## to a planning group (group of joints). In this tutorial the group is the primary
## arm joints in the Panda robot, so we set the group's name to "panda_arm".
## If you are using a different robot, change this value to the name of your robot
## arm planning group.
## This interface can be used to plan and execute motions:
group_name = "iiwa"
move_group = moveit_commander.MoveGroupCommander(group_name)
## Create a `DisplayTrajectory`_ ROS publisher which is used to display
## trajectories in Rviz:
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
## END_SUB_TUTORIAL
## BEGIN_SUB_TUTORIAL basic_info
##
## Getting Basic Information
## ^^^^^^^^^^^^^^^^^^^^^^^^^
# We can get the name of the reference frame for this robot:
planning_frame = move_group.get_planning_frame()
print "============ Planning frame: %s" % planning_frame
# We can also print the name of the end-effector link for this group:
eef_link = move_group.get_end_effector_link()
print "============ End effector link: %s" % eef_link
# We can get a list of all the groups in the robot:
group_names = robot.get_group_names()
print "============ Available Planning Groups:", robot.get_group_names(
)
# Sometimes for debugging it is useful to print the entire state of the
# robot:
print "============ Printing robot state"
print robot.get_current_state()
print ""
## END_SUB_TUTORIAL
# Misc variables
self.base_name = 'base'
self.w0_name = 'wheel0'
self.w1_name = 'wheel1'
self.top_name = 'top'
self.robot = robot
self.scene = scene
self.move_group = move_group
self.display_trajectory_publisher = display_trajectory_publisher
self.planning_frame = planning_frame
self.eef_link = eef_link
self.group_names = group_names
self.base_pose = geometry_msgs.msg.PoseStamped()
self.w0_pose = geometry_msgs.msg.PoseStamped()
self.w1_pose = geometry_msgs.msg.PoseStamped()
self.top_pose = geometry_msgs.msg.PoseStamped()
self.aruco_pose = geometry_msgs.msg.PoseStamped()
self.aruco_res = geometry_msgs.msg.PoseStamped()
def movo_moveit_test():
## BEGIN_TUTORIAL
##
## Setup
## ^^^^^
## CALL_SUB_TUTORIAL imports
##
## First initialize moveit_commander and rospy.
print "============ Starting tutorial setup"
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('movo_moveit_test', anonymous=True)
is_sim = rospy.get_param('~sim', False)
## Instantiate a RobotCommander object. This object is an interface to
## the robot as a whole.
robot = moveit_commander.RobotCommander()
## Instantiate a PlanningSceneInterface object. This object is an interface
## to the world surrounding the robot.
scene = moveit_commander.PlanningSceneInterface()
##Gripper action clients
lgripper = GripperActionTest('left')
rgripper = GripperActionTest('right')
## Instantiate a MoveGroupCommander object. This object is an interface
## to one group of joints. In this case the group is the joints in the left
## arm. This interface can be used to plan and execute motions on the left
## arm.
group = moveit_commander.MoveGroupCommander("upper_body")
## We create this DisplayTrajectory publisher which is used below to publish
## trajectories for RVIZ to visualize.
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=10)
rospy.sleep(2)
scene.remove_world_object("floor")
# publish a demo scene
p = geometry_msgs.msg.PoseStamped()
p.header.frame_id = robot.get_planning_frame()
p.pose.position.x = 0.0
p.pose.position.y = 0.0
p.pose.position.z = -0.01
p.pose.orientation.w = 1.0
scene.add_box("floor", p, (4.0, 4.0, 0.02))
## We can get a list of all the groups in the robot
print "============ Robot Groups:"
print robot.get_group_names()
## Sometimes for debugging it is useful to print the entire state of the
## robot.
print "============ Printing robot state"
print robot.get_current_state()
print "============"
group.set_planner_id("RRTConnectkConfigDefault")
## Planning to a Pose goal
## ^^^^^^^^^^^^^^^^^^^^^^^
## We can plan a motion for this group to a desired pose for the
## end-effector
if (True == is_sim):
gripper_closed = 0.96
gripper_open = 0.0
else:
gripper_closed = 0.0
gripper_open = 0.165
print "============ Testing with named targets"
group.set_named_target("homed")
plan = group.plan()
group.execute(plan)
lgripper.command(gripper_open)
rgripper.command(gripper_open)
lgripper.wait()
rgripper.wait()
rospy.sleep(2.0)
group.set_named_target("tucked")
plan = group.plan()
group.execute(plan)
lgripper.command(gripper_closed)
rgripper.command(gripper_closed)
lgripper.wait()
rgripper.wait()
rospy.sleep(2.0)
group.set_named_target("pos1")
plan = group.plan()
group.execute(plan)
lgripper.command(gripper_open)
rgripper.command(gripper_open)
lgripper.wait()
rgripper.wait()
rospy.sleep(2.0)
group.set_named_target("tucked")
plan = group.plan()
group.execute(plan)
lgripper.command(gripper_closed)
rgripper.command(gripper_closed)
lgripper.wait()
rgripper.wait()
## When finished shut down moveit_commander.
moveit_commander.roscpp_shutdown()
## END_TUTORIAL
print "============ STOPPING"
def main():
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node("joint_state_player_node")
waypoint_file = rospy.get_param('wpt_file', 'scan_waypoints2.npy')
limb_name = rospy.get_param('limb_name', 'left_arm')
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
arm = moveit_commander.MoveGroupCommander(limb_name)
waypoints = np.load(waypoint_file) #calib_sim3.npy #calib_real_right03.npy
rate = rospy.Rate(100)
joint_angle_target = np.zeros(7)
selected_idx = -1
while not rospy.is_shutdown():
c = getch()
if c in ['n']:
selected_idx = (selected_idx + 1) % len(waypoints)
joint_angle_target = waypoints[selected_idx]
print("Selected joint goal %d: " % (selected_idx, ),
joint_angle_target)
elif c in ['b']:
selected_idx -= 1
if selected_idx < 0:
selected_idx = len(waypoints) - 1
joint_angle_target = waypoints[selected_idx]
print("Selected joint goal %d: " % (selected_idx, ),
joint_angle_target)
elif c in ['a']:
current_joint_angles = arm.get_current_joint_values()
joint_angles = np.asarray(current_joint_angles)
waypoints = np.vstack([waypoints, joint_angles])
print("Added new waypoint. Number of waypoints %d." %
(len(waypoints, )))
print("Joint angles: ", joint_angles)
elif c in ['d']:
if len(waypoints) > 0:
waypoints = np.delete(waypoints, (selected_idx), axis=0)
print("Deleted %dth waypoint. Number of waypoints %d." %
(selected_idx + 1, len(waypoints, )))
else:
print("Waypoint list is empty.")
elif c in ['e']:
current_joint_angles = np.asarray(arm.get_current_joint_values())
before = np.array(waypoints[selected_idx])
waypoints[selected_idx] = current_joint_angles
print("Editting joint angles %d: " % (selected_idx, ), before)
print("Set to", current_joint_angles)
elif c in ['s']:
print("Saving current waypoint list.")
filename = raw_input("Filename:")
np.save('%s.npy' % (filename, ), waypoints)
print("Saved.")
elif c in ['g']:
try:
arm.set_joint_value_target(joint_angle_target)
arm.set_max_velocity_scaling_factor(0.35)
arm.set_max_acceleration_scaling_factor(0.35)
arm.go()
except:
print("Not possible to go to waypoint. Try another one")
elif c in ['\x1b', '\x03']:
rospy.signal_shutdown("Bye.")
rate.sleep()
moveit_commander.roscpp_shutdown()
def move_group_python_interface_tutorial():
## First initialize moveit_commander and rospy.
print "============ Starting setup"
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('collision_checker','move_group_python_interface_tutorial',
anonymous=True)
## Instantiate a RobotCommander object. This object is an interface to
## the robot as a whole.
robot = moveit_commander.RobotCommander()
## Instantiate a PlanningSceneInterface object. This object is an interface
## to the world surrounding the robot.
scene = moveit_commander.PlanningSceneInterface()
## Instantiate a MoveGroupCommander object. This object is an interface
## to one group of joints. In this case the group is the joints in the left
## arm. This interface can be used to plan and execute motions on the left
## arm.
group = moveit_commander.MoveGroupCommander("manipulator")
#group.set_goal_position_tolerance(0.01)
#group.set_goal_orientation_tolerance(0.1)
group.allow_replanning(True)
group.set_planner_id("ESTkConfigDefault") #RRTConnectkConfigDefault/SBLkConfigDefault/KPIECEkConfigDefault/BKPIECEkConfigDefault/LBKPIECEkConfigDefault/ ESTkConfigDefault
#group.set_planning_time(15)
group.set_num_planning_attempts(5)
replanning_num = 5
## We create this DisplayTrajectory publisher which is used below to publish
## trajectories for RVIZ to visualize.
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory)
## Getting Basic Information
## ^^^^^^^^^^^^^^^^^^^^^^^^^
##
## We can get the name of the reference frame for this robot
print "============ Reference frame: %s" % group.get_planning_frame()
## Sometimes for debugging it is useful to print the entire state of the
## robot.
print "============ Printing robot state"
print robot.get_current_state()
print "============"
print "============ Printing robot Pose"
print group.get_current_pose().pose
print "============"
## Set up the scene
rospack = rospkg.RosPack()
scene_path = rospack.get_path('abb_irb6640_support')
wall_1 = os.path.join(scene_path,'meshes/Scene/T1.stl')
wall_2 = os.path.join(scene_path,'meshes/Scene/T1.stl')
wall_3 = os.path.join(scene_path,'meshes/Scene/T3.stl')
Ceiling = os.path.join(scene_path,'meshes/Scene/TestBed_scene5.STL')
# Set the target pose in between the boxes and on the table
target_pose_panel = gen_pose_scene([1.862,0,1.325,0.5,-0.5,-0.5,0.5])#[0,-2,0.35,0.5,-0.5,-0.5,0.5]
target_pose_panel.header.frame_id = 'base_link'
#target_pose_wall_1 = gen_pose_scene([1.5384, -3.8262, 1,0,0,0,1])
#target_pose_wall_2 = gen_pose_scene([1.5384, 2.6, 1,0,0,0,1])
#target_pose_wall_3 = gen_pose_scene([-0.9, -0.6131, 1,0,0,0,1])
#target_pose_Ceiling = gen_pose_scene([-0.9, -4.377, 0, 0,-0.707, -0.707,0])
print "============ Starting Simulation ============"
x_All = []
y_All = []
Dist_Mean_All = []
Dist_Min_All = []
cc = CollisionChecker(gui=False)
for i_x in np.linspace(-1,-0.72,8): #-0.68,0.20,23
for i_y in np.linspace(-1.2,1.2,51): #(-1,1,51)
scene.remove_world_object('target')
#scene.remove_world_object('Wall_1')
#scene.remove_world_object('Wall_2')
#scene.remove_world_object('Wall_3')
#scene.remove_world_object('Ceiling')
rospy.sleep(2)
print "============ Iteration ============"
print i_x, i_y
np.savetxt('ScoreMap.out',(x_All,y_All,Dist_Mean_All,Dist_Min_All))
Dist = []
robot_pose = [i_x, i_y, 0, 0, 0, 0, 1]
set_robot_pose(robot_pose)
# Add the target object to the scene
scene.add_box('target', target_pose_panel, [2.33, 0.2, 2.08] )
#scene.add_mesh('Wall_1', target_pose_wall_1, wall_1 , size =(0.5,0.5,0.5))
#scene.add_mesh('Wall_2', target_pose_wall_2, wall_2 , size =(0.5,0.5,0.5))
#scene.add_mesh('Wall_3', target_pose_wall_3, wall_3 , size =(0.5,0.5,0.5))
#scene.add_mesh('Ceiling', target_pose_Ceiling, Ceiling)
## Wait for RVIZ to initialize. This sleep is ONLY to allow Rviz to come up.
print "============ Waiting for RVIZ..."
rospy.sleep(2)
print "============ Generating plan REST ============"
group.clear_pose_targets()
pose_target = gen_pose_target([1.862,0,1.955,0,-1,0,0])
group.set_pose_target(pose_target)
planR = group.plan()
cnt = 0;
while( (not planR.joint_trajectory.points) & (cnt< replanning_num)):
cnt = cnt+1;
print "============ Generating plan again"
planR = group.plan()
if (cnt == 5):
Dist=-1
group.detach_object('link_6')
x_All.append(i_x)
y_All.append(i_y)
Dist_Mean_All.append(np.mean(Dist))
Dist_Min_All.append(np.min(Dist))
continue
rospy.sleep(1)
print "============ Executing plan REST"
group.execute(planR)
print "============ Waiting while plan REST is Executed..."
rospy.sleep(1)
T1 = np.dot(translation_matrix(robot_pose[0:3]), quaternion_matrix(robot_pose[3:7]))
T2 = np.dot(translation_matrix([0,0,0]), quaternion_matrix([0,0,0,1]))
T3 = np.dot(T1,np.dot(translation_matrix([0,-2,0.35]), quaternion_matrix([0.5, 0.5, 0.5, 0.5])))
group.attach_object('target','link_6')
rospy.sleep(2)
collision_env = { 'Walls' : T2 }
print "============ Generating plan 2 ============"
group.clear_pose_targets()
pose_target = gen_pose_target([1.7-robot_pose[0],-1.165-robot_pose[1],0.62,0,-1,0,0])
group.set_pose_target(pose_target)
## Now, we call the planner to compute the plan
## and visualize it if successful
## Note that we are just planning, not asking move_group
## to actually move the robot
plan2 = group.plan()
cnt = 0;
while( (not plan2.joint_trajectory.points) & (cnt< replanning_num)):
cnt = cnt+1;
print "============ Generating plan again"
plan2 = group.plan()
if (cnt == 5):
Dist=-1
group.detach_object('link_6')
x_All.append(i_x)
y_All.append(i_y)
Dist_Mean_All.append(np.mean(Dist))
Dist_Min_All.append(np.min(Dist))
continue
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
for i_plan in range(0,len(plan2.joint_trajectory.points)):
tmp_array = plan2.joint_trajectory.points[i_plan].positions
T3 = np.dot(T1,np.dot(FK_Matrix2(tmp_array), quaternion_matrix([0.707, 0.707, 0, 0])))
#T3 = FK_Matrix(tmp_array)
joints = { 'irb6640_185_280_Testbed' : tmp_array }
collision_poi = { 'irb6640_185_280_Testbed' : T1, 'box' : T3 }
tmp_result = cc.check_safety(collision_poi, collision_env, joints)
print 'Safe (0.001 tolerance) =',tmp_result
Dist.append(tmp_result[1])
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
rospy.sleep(1)
print "============ Executing plan2"
group.execute(plan2)
print "============ Waiting while plan2 is Executed..."
rospy.sleep(1)
print "============ Generating plan REST ============"
group.clear_pose_targets()
pose_target = gen_pose_target([1.862,0,1.955,0,-1,0,0])
group.set_pose_target(pose_target)
planR = group.plan()
cnt = 0;
while( (not planR.joint_trajectory.points) & (cnt< replanning_num)):
cnt = cnt+1;
print "============ Generating plan again"
planR = group.plan()
if (cnt == 5):
Dist=-1
group.detach_object('link_6')
x_All.append(i_x)
y_All.append(i_y)
Dist_Mean_All.append(np.mean(Dist))
Dist_Min_All.append(np.min(Dist))
continue
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
for i_plan in range(0,len(planR.joint_trajectory.points)):
tmp_array = planR.joint_trajectory.points[i_plan].positions
T3 = np.dot(T1,np.dot(FK_Matrix2(tmp_array), quaternion_matrix([0.707, 0.707, 0, 0])))
joints = { 'irb6640_185_280_Testbed' : tmp_array }
collision_poi = { 'irb6640_185_280_Testbed' : T1, 'box' : T3 }
tmp_result = cc.check_safety(collision_poi, collision_env, joints)
print 'Safe (0.001 tolerance) =',tmp_result
Dist.append(tmp_result[1])
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
rospy.sleep(1)
print "============ Executing plan REST"
group.execute(planR)
print "============ Waiting while plan REST is Executed..."
rospy.sleep(1)
print "============ Generating plan 3 ============"
group.clear_pose_targets()
pose_target = gen_pose_target([1.7-robot_pose[0],1.165-robot_pose[1],0.62,0,-1,0,0])
group.set_pose_target(pose_target)
plan3 = group.plan()
cnt = 0;
while( (not plan3.joint_trajectory.points) & (cnt< replanning_num)):
cnt = cnt+1;
print "============ Generating plan again"
plan3 = group.plan()
if (cnt == 5):
Dist=-1
group.detach_object('link_6')
x_All.append(i_x)
y_All.append(i_y)
Dist_Mean_All.append(np.mean(Dist))
Dist_Min_All.append(np.min(Dist))
continue
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
for i_plan in range(0,len(plan3.joint_trajectory.points)):
tmp_array = plan3.joint_trajectory.points[i_plan].positions
T3 = np.dot(T1,np.dot(FK_Matrix2(tmp_array), quaternion_matrix([0.707, 0.707, 0, 0])))
joints = { 'irb6640_185_280_Testbed' : tmp_array }
collision_poi = { 'irb6640_185_280_Testbed' : T1, 'box' : T3 }
tmp_result = cc.check_safety(collision_poi, collision_env, joints)
print 'Safe (0.001 tolerance) =',tmp_result
Dist.append(tmp_result[1])
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
rospy.sleep(1)
print "============ Executing plan3"
group.execute(plan3)
print "============ Waiting while plan3 is Executed..."
rospy.sleep(1)
print "============ Generating plan REST ============"
group.clear_pose_targets()
pose_target = gen_pose_target([1.862,0,1.955,0,-1,0,0])
group.set_pose_target(pose_target)
planR = group.plan()
cnt = 0;
while( (not planR.joint_trajectory.points) & (cnt< replanning_num)):
cnt = cnt+1;
print "============ Generating plan again"
planR = group.plan()
if (cnt == 5):
Dist=-1
group.detach_object('link_6')
x_All.append(i_x)
y_All.append(i_y)
Dist_Mean_All.append(np.mean(Dist))
Dist_Min_All.append(np.min(Dist))
continue
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
for i_plan in range(0,len(planR.joint_trajectory.points)):
tmp_array = planR.joint_trajectory.points[i_plan].positions
T3 = np.dot(T1,np.dot(FK_Matrix2(tmp_array), quaternion_matrix([0.707, 0.707, 0, 0])))
joints = { 'irb6640_185_280_Testbed' : tmp_array }
collision_poi = { 'irb6640_185_280_Testbed' : T1, 'box' : T3 }
tmp_result = cc.check_safety(collision_poi, collision_env, joints)
print 'Safe (0.001 tolerance) =',tmp_result
Dist.append(tmp_result[1])
#++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
rospy.sleep(1)
print "============ Executing plan REST"
group.execute(planR)
print "============ Waiting while plan REST is Executed..."
rospy.sleep(1)
## Adding/Removing Objects and Attaching/Detaching Objects
## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
## First, we will define the collision object message
group.detach_object('link_6')
x_All.append(i_x)
y_All.append(i_y)
Dist_Mean_All.append(np.mean(Dist))
Dist_Min_All.append(np.min(Dist))
## END
scene.remove_world_object('target')
#scene.remove_world_object('Wall_1')
#scene.remove_world_object('Wall_2')
#scene.remove_world_object('Wall_3')
#scene.remove_world_object('Ceiling')
rospy.sleep(2)
print "============ STOPPING"
collision_object = moveit_msgs.msg.CollisionObject()
np.savetxt('ScoreMap.out',(x_All,y_All,Dist_Mean_All,Dist_Min_All))
## When finished shut down moveit_commander.
moveit_commander.roscpp_shutdown()
def main():
rospy.init_node("crane_x7_pick_and_place_controller")
robot = moveit_commander.RobotCommander()
arm = moveit_commander.MoveGroupCommander("arm")
arm.set_max_velocity_scaling_factor(0.1)
gripper = moveit_commander.MoveGroupCommander("gripper")
while len([s for s in rosnode.get_node_names() if 'rviz' in s]) == 0:
rospy.sleep(1.0)
rospy.sleep(1.0)
print("Group names:")
print(robot.get_group_names())
print("Current state:")
print(robot.get_current_state())
# アーム初期ポーズを表示
arm_initial_pose = arm.get_current_pose().pose
print("Arm initial pose:")
print(arm_initial_pose)
# 何かを掴んでいた時のためにハンドを開く
gripper.set_joint_value_target([0.9, 0.9])
gripper.go()
# SRDFに定義されている"home"の姿勢にする
arm.set_named_target("home")
arm.go()
gripper.set_joint_value_target([0.7, 0.7])
gripper.go()
# 掴む準備をする
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = 0.2
target_pose.position.y = 0.0
target_pose.position.z = 0.3
q = quaternion_from_euler(-3.14, 0.0, -3.14 / 2.0) # 上方から掴みに行く場合
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
arm.set_pose_target(target_pose) # 目標ポーズ設定
arm.go() # 実行
# ハンドを開く
gripper.set_joint_value_target([0.7, 0.7])
gripper.go()
# 掴みに行く
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = 0.2
target_pose.position.y = 0.0
target_pose.position.z = 0.13
q = quaternion_from_euler(-3.14, 0.0, -3.14 / 2.0) # 上方から掴みに行く場合
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
arm.set_pose_target(target_pose) # 目標ポーズ設定
arm.go() # 実行
# ハンドを閉じる
gripper.set_joint_value_target([0.4, 0.4])
gripper.go()
# 持ち上げる
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = 0.2
target_pose.position.y = 0.0
target_pose.position.z = 0.3
q = quaternion_from_euler(-3.14, 0.0, -3.14 / 2.0) # 上方から掴みに行く場合
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
arm.set_pose_target(target_pose) # 目標ポーズ設定
arm.go() # 実行
# 移動する
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = 0.2
target_pose.position.y = 0.2
target_pose.position.z = 0.3
q = quaternion_from_euler(-3.14, 0.0, -3.14 / 2.0) # 上方から掴みに行く場合
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
arm.set_pose_target(target_pose) # 目標ポーズ設定
arm.go() # 実行
# 下ろす
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = 0.2
target_pose.position.y = 0.2
target_pose.position.z = 0.13
q = quaternion_from_euler(-3.14, 0.0, -3.14 / 2.0) # 上方から掴みに行く場合
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
arm.set_pose_target(target_pose) # 目標ポーズ設定
arm.go() # 実行
# ハンドを開く
gripper.set_joint_value_target([0.7, 0.7])
gripper.go()
# 少しだけハンドを持ち上げる
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = 0.2
target_pose.position.y = 0.2
target_pose.position.z = 0.2
q = quaternion_from_euler(-3.14, 0.0, -3.14 / 2.0) # 上方から掴みに行く場合
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
arm.set_pose_target(target_pose) # 目標ポーズ設定
arm.go() # 実行
# SRDFに定義されている"home"の姿勢にする
arm.set_named_target("home")
arm.go()
print("done")
def main():
rospy.init_node("pose_groupstate_example")
robot = moveit_commander.RobotCommander()
arm = moveit_commander.MoveGroupCommander("arm")
arm.set_max_velocity_scaling_factor(0.1)
gripper = moveit_commander.MoveGroupCommander("gripper")
while len([s for s in rosnode.get_node_names() if 'rviz' in s]) == 0:
rospy.sleep(1.0)
rospy.sleep(1.0)
print("Group names:")
print(robot.get_group_names())
print("Current state:")
print(robot.get_current_state())
# アーム初期ポーズを表示
arm_initial_pose = arm.get_current_pose().pose
print("Arm initial pose:")
print(arm_initial_pose)
# 何かを掴んでいた時のためにハンドを開く
gripper.set_joint_value_target([0.9, 0.9])
gripper.go()
# SRDFに定義されている"home"の姿勢にする
print("home")
arm.set_named_target("home")
arm.go()
# SRDFに定義されている"vertical"の姿勢にする
print("vertical")
arm.set_named_target("vertical")
arm.go()
# ハンドを少し閉じる
gripper.set_joint_value_target([0.7, 0.7])
gripper.go()
# 手動で姿勢を指定するには以下のように指定
gripper.set_joint_value_target([0.9, 0.9])
gripper.go()
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = 0.0
target_pose.position.y = 0.0
target_pose.position.z = 0.0
q = quaternion_from_euler(-3.14, 0.0, -3.14 / 2) # 上方から掴み>に行く場合
target_pose = geometry_msgs.msg.Pose()
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
arm.set_pose_target(target_pose) # 目標ポーズ設定
arm.go()
gripper.set_joint_value_target([0.7, 0.7])
gripper.go()
print("あああああああああああ")
# 移動後の手先ポーズを表示
arm.set_named_target("home")
arm.go()
print("done")
def __init__(self, log_level):
rospy.init_node('cb_moveit_gui_execution', log_level=log_level)
# Subscriber
# rospy.Subscriber("clicked_point", PointStamped, self.clicked_cb)
rospy.Subscriber("pick_approach_plan", Bool, self.pick_approach_plan_cb)
rospy.Subscriber("pick_approach_execute", Bool, self.pick_approach_execute_cb)
rospy.Subscriber("pick_retreat_plan", Bool, self.pick_retreat_plan_cb)
rospy.Subscriber("pick_retreat_execute", Bool, self.pick_retreat_execute_cb)
rospy.Subscriber("place_approach_plan", Bool, self.place_approach_plan_cb)
rospy.Subscriber("place_approach_execute", Bool, self.place_approach_execute_cb)
rospy.Subscriber("place_retreat_plan", Bool, self.place_retreat_plan_cb)
rospy.Subscriber("place_retreat_execute", Bool, self.place_retreat_execute_cb)
rospy.Subscriber("approach_stop", Bool, self.approach_stop_cb)
rospy.Subscriber("move_xp", Bool, self.move_xp_cb)
rospy.Subscriber("move_xm", Bool, self.move_xm_cb)
rospy.Subscriber("move_yp", Bool, self.move_yp_cb)
rospy.Subscriber("move_ym", Bool, self.move_ym_cb)
rospy.Subscriber("move_zp", Bool, self.move_zp_cb)
rospy.Subscriber("move_zm", Bool, self.move_zm_cb)
rospy.Subscriber("waypoints/update", InteractiveMarkerUpdate, self.waypoints_update_cb)
rospy.Subscriber("distance", Int32, self.distance_cb)
rospy.Subscriber("solution_num", Int32, self.solution_cb)
rospy.Subscriber("pcl_record", Bool, self.record_move_cb)
rospy.Subscriber("compute_interpolation", Bool, self.stitch_plan_cb)
rospy.Subscriber("/cb_gui_moveit/pcl_capture", Bool, self.pcl_capture_cb)
rospy.Subscriber("/cb_gui_moveit/pcl_clear", Bool, self.pcl_clear_cb)
rospy.Subscriber("/cb_gui_moveit/grasp_object", Bool, self.grasp_object_cb)
rospy.Subscriber("/cb_gui_moveit/grasp_wand", Bool, self.grasp_wand_cb)
rospy.Subscriber("/cb_gui_moveit/scanning_plan", Bool, self.scanning_plan_req)
rospy.Subscriber("/cb_gui_moveit/scanning_execute", Bool, self.scanning_execute_req)
rospy.Subscriber("/cb_gui_moveit/j1", Bool, self.move_j1_cb)
rospy.Subscriber("/cb_gui_moveit/j4", Bool, self.move_j4_cb)
rospy.Subscriber("/cb_gui_moveit/j6", Bool, self.move_j6_cb)
# Publisher
self.instruction_pub = rospy.Publisher('/instruction', String, queue_size=1)
self.aco_pub = rospy.Publisher('/attached_collision_object', moveit_msgs.msg.AttachedCollisionObject, queue_size=100)
self.pcl_capture_pub = rospy.Publisher('/pcl_capture', Bool, queue_size=1)
self.remove_imarker_pub = rospy.Publisher("/remove_imarker", Bool, queue_size=1)
self.gripper_close_pub = rospy.Publisher("/gripper_close", Bool, queue_size=1)
# Service
rospy.wait_for_service('/pcl_fusion_node/reset')
rospy.Service("/cb_gui_moveit/scanning_plan", Trigger, self.scanning_plan_req)
rospy.Service("/cb_gui_moveit/scanning_execute", Trigger, self.scanning_execute_req)
# ready for listen tf
self.listener = tf.TransformListener()
# moveit commander
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface()
self.group = moveit_commander.MoveGroupCommander("ur5")
self.group.set_planner_id("RRTConnectkConfigDefault")
self.group.set_planning_time(5.0)
self.group.set_num_planning_attempts(10)
self.group.set_max_velocity_scaling_factor(0.05) # 0.1
self.group.set_max_acceleration_scaling_factor(0.05) # 0.1
self.pick_approach_plan = None
self.pick_retreat_plan = None
self.pick_retreat_plan1 = None
self.place_approach_plan = None
self.place_retreat_plan = None
# for compute cartesian path
self.scanning_plan1 = None
self.scanning_plan2 = None
self.jump_threshold = 0.0
self.eef_step = 0.01
# set after execution
self.pre_grasp_joint = None
self.post_grasp_joint = None
self.pick_retreat_step = 0
self.last_offset = 10.0
# IK
self.ur5_inv = ur5_inv_kin_wrapper()
self.last_selected_joint = None
self.last_sol_num = None
self.count = 0
# initialize
# from capture
#self.initialize(initial_joint_for_capture)
#self.grasp_wand = False
# from scan
self.initialize(initial_joint_for_scan)
self.grasp_wand = True
def __init__(self):
print("__init__ of GazeboSmartBotPincherKinectEnvREAL_ARM")
# Launch the moveit config for the real pincher arm
# launch file is in gym-gazebo/gym_gazebo/env/assets/launch/
gazebo_env.GazeboEnv.__init__(
self, "GazeboSmartBotPincherKinectREAL_ARM_v0.launch")
# the state space (=observation space) are all possible depth images of the kinect camera
if (self.flattenImage):
self.observation_space = spaces.Box(low=0,
high=255,
shape=[220 * 300],
dtype=np.uint8)
else:
self.observation_space = spaces.Box(low=0,
high=255,
shape=[220, 300],
dtype=np.uint8)
# the action space are all possible positions & orientations (6-DOF),
# which are bounded in the area in front of the robot arm where an object can lie (see reset())
boundaries_xAxis = [0.04,
0.3] # box position possiblities: (0.06, 0.22)
boundaries_yAxis = [-0.25,
0.25] # box position possiblities: (-0.2, 0.2)
boundaries_zAxis = [0.015, 0.05] # box z-position: ca. 0.04
boundaries_roll = [0, 2 * np.pi]
boundaries_pitch = [0, 2 * np.pi]
boundaries_yaw = [0, 2 * np.pi]
low = np.array([
boundaries_xAxis[0], boundaries_yAxis[0], boundaries_zAxis[0],
boundaries_roll[0], boundaries_pitch[0], boundaries_yaw[0]
])
high = np.array([
boundaries_xAxis[1], boundaries_yAxis[1], boundaries_zAxis[1],
boundaries_roll[1], boundaries_pitch[1], boundaries_yaw[1]
])
self.action_space = spaces.Box(low=low, high=high, dtype=np.float32)
self.reward_range = (-np.inf, np.inf)
self.seed()
rospy.sleep(2)
# MoveIt variables
moveit_commander.roscpp_initialize(sys.argv)
# rospy.init_node('move_group_python_interface', anonymous=True) # can't init another node, there is already one initialized in the superclass
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface()
self.group_arm = moveit_commander.MoveGroupCommander("arm")
self.group_gripper = moveit_commander.MoveGroupCommander("gripper")
# used to be: base_link, also possible: world (if defined in the urdf.xarco file)
self.REFERENCE_FRAME = 'world'
# Allow some leeway in position (meters) and orientation (radians)
self.group_arm.set_goal_position_tolerance(0.01)
self.group_arm.set_goal_orientation_tolerance(0.01)
# Allow replanning to increase the odds of a solution
self.group_arm.allow_replanning(True)
# Set the right arm reference frame
self.group_arm.set_pose_reference_frame(self.REFERENCE_FRAME)
# Allow 5 seconds per planning attempt
self.group_arm.set_planning_time(5)
self.bridge = CvBridge()
arm_right_group = moveit_commander.MoveGroupCommander("arm_right_torso");
arm_right_group.set_planner_id("RRTstarkConfigDefault");
#arm_right_group.set_planner_id("RRTConnectkConfigDefault");
arm_right_group.set_goal_tolerance(0.001);
arm_right_group.allow_replanning(True);
#pos_init(arm_left_group, arm_right_group);
arm_left_group.set_planning_time(Planning_time);
arm_right_group.set_planning_time(Planning_time);
print ">>>> Waiting for service `compute_ik` >>>>";
rospy.wait_for_service('compute_ik');
ik = rospy.ServiceProxy("compute_ik", GetPositionIK);
print ">>>> Start Testing >>>>"
pos_test(arm_right_group, ik);
motoman_sda10 = moveit_commander.RobotCommander();
start_state = motoman_sda10.get_current_state();
goal_state = [0.0, -0.7846225685744268, -0.44391901469000256, 2.0332152364478326, -0.9467777692180627, 1.094702439784921, -1.454522631785723, 2.482112603511986]
#constraint_planner(start_state, goal_state, arm_right_group, "RRTConnectkConfigDefault", 15, 10);
# w 0.70683; x 0.00024277; y 0.00024296; z 0.70739
# des_w = 0.68323; des_x = -0.68245; des_y = -0.18413; des_z = -0.18316;
orient_constraint = setOrientationConstraints(des_w, des_x, des_y, des_z, arm_right_group, weight = 1.0);
test_constraints = Constraints();
test_constraints.orientation_constraints.append(orient_constraint);
arm_right_group.set_path_constraints(test_constraints);
test_jnt_value_msg = Generate_joint_state_msg(arm_right_group,goal_state)
arm_right_group.set_joint_value_target(test_jnt_value_msg);
def __init__(self):
"""
The constructor for Ur5MoveitActionClient.
This function will initalize this node as ROS Action Client.
Moveit configurations for UR5_1 will also be setup here
"""
# Configuing Moveit
self._robot_ns = '/' + 'ur5_1'
self._planning_group = "manipulator"
self._commander = moveit_commander.roscpp_initialize(sys.argv)
self._robot = moveit_commander.RobotCommander(robot_description= self._robot_ns + "/robot_description", ns=self._robot_ns)
self._scene = moveit_commander.PlanningSceneInterface(ns=self._robot_ns)
self._group = moveit_commander.MoveGroupCommander(self._planning_group, robot_description= self._robot_ns + "/robot_description", ns=self._robot_ns)
self._display_trajectory_publisher = rospy.Publisher( self._robot_ns + '/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=1)
self._exectute_trajectory_client = actionlib.SimpleActionClient( self._robot_ns + '/execute_trajectory', moveit_msgs.msg.ExecuteTrajectoryAction)
self._exectute_trajectory_client.wait_for_server()
self._planning_frame = self._group.get_planning_frame()
self._eef_link = self._group.get_end_effector_link()
self._group_names = self._robot.get_group_names()
# Make it a publisher to publish on '/eyrc/vb/onConveyor'
self.onConveyor_pub_handle = rospy.Publisher("/eyrc/vb/onConveyor", onConveyor, queue_size = 10)
# A dict to store all the processed info on color of pkgs
self.package_pose_color = {}
# A dict to store all the incoming orders
self.orders_to_be_completed = {}
# Lists to store order number of their respective priority
# this helps to deliver higher priority order first
self.orders_to_be_completed_RED = []
self.orders_to_be_completed_YELLOW = []
self.orders_to_be_completed_GREEN = []
self.currently_processing = False
# Current State of the Robot is needed to add box to planning scene
self._curr_state = self._robot.get_current_state()
# Setup to play all the stored trajectories
rp = rospkg.RosPack()
self._pkg_path = rp.get_path('pkg_task5')
self._file_path = self._pkg_path + '/config/saved_trajectories/'
rospy.loginfo( "Package Path: {}".format(self._file_path) )
# Making it an Action Client
self._ac = actionlib.ActionClient('/action_ros_iot',
msgRosIotAction)
# Dictionary to store the goal handles
self._goal_handles = {}
# Wait for the action server to start
rospy.loginfo("[UR5_1] Waiting for Action Server ...")
self._ac.wait_for_server()
rospy.loginfo('\033[94m' + "[UR5_1]: >>> Ur5Moveit init done." + '\033[0m')
def shallow_insertion_start():
global gripper_command_publisher
## 1. Initialize robot environment
print "============ Starting tutorial setup"
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_group_python_interface_tutorial',
anonymous=True)
## Instantiate a RobotCommander object. This object is an interface to
## the robot as a whole.
robot = moveit_commander.RobotCommander()
## Instantiate a PlanningSceneInterface object. This object is an interface
## to the world surrounding the robot.
scene = moveit_commander.PlanningSceneInterface()
## Instantiate a MoveGroupCommander object. This object is an interface
## to one group of joints. In this case the group is the joints in the left
## arm. This interface can be used to plan and execute motions on the left
## arm.
##group = moveit_commander.MoveGroupCommander("left_arm")
group = moveit_commander.MoveGroupCommander("manipulator")
#set max velocity and acceleration scaler
group.set_max_velocity_scaling_factor(0.1);
group.set_max_acceleration_scaling_factor(0.1);
## We create this DisplayTrajectory publisher which is used below to publish
## trajectories for RVIZ to visualize.
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
#initialize the gripper publisher variable
gripper_command_publisher = rospy.Publisher(
'CModelRobotInputRob',
String,
queue_size=20)
## Wait for RVIZ to initialize. This sleep is ONLY to allow Rviz to come up.
print "============ Waiting for RVIZ..."
rospy.sleep(2)
print "============ Starting tutorial "
## Getting Basic Information
## ^^^^^^^^^^^^^^^^^^^^^^^^^
##
## We can get the name of the reference frame for this robot
print "============ Reference frame: %s" % group.get_planning_frame()
## We can also print the name of the end-effector link for this group
print "============ End effector: %s" % group.get_end_effector_link()
## We can get a list of all the groups in the robot
print "============ Robot Groups:"
print robot.get_group_names()
## Sometimes for debugging it is useful to print the entire state of the
## robot.
print "============ Printing group state"
print group.get_current_pose()
# ## Then, we will get the current set of joint values for the group
# group_variable_values = group.get_current_joint_values()
# print "============ Joint values: ", group_variable_values
## Now, let's modify one of the joints, plan to the new joint
## space goal and visualize the plan
group_variable_values = [-0.02749187151064092, -1.598161522542135, -1.8247435728656214, -3.7606776396380823, -1.3931792418109339, 1.796911358833313]
group.set_joint_value_target(group_variable_values)
plan2 = group.plan()
print "============ Waiting while RVIZ displays plan2..."
group.execute(plan2)
rospy.sleep(2)
# moveit_commander.roscpp_shutdown()
# return
# Initialize robot environment END
#2. Check if the gripper works well, then open it
gripper_init()
waypoints = []
#3. Go to the beginning position before closing the gripper and pick up the card
pose_target = geometry_msgs.msg.Pose()
pose_target.orientation.x = 0.5
pose_target.orientation.y = 0.5
pose_target.orientation.z = -0.5
pose_target.orientation.w = 0.5
pose_target.position.x = 0.02 -0.805
pose_target.position.y = 0.235 - 0.0125
pose_target.position.z = -0.12 + 0.46 - 0.03
group.set_pose_target(pose_target)
plan1 = group.plan()
group.execute(plan1)
rospy.sleep(2)
#4. Close the gripper
gripper_close()
#5. Pick up the card, and move to the top of the beginning point
pose_target.position.z = -0.12 + 0.46 + 0.1
group.set_pose_target(pose_target)
plan1 = group.plan()
group.execute(plan1)
rospy.sleep(2)
#6. Move to the top of the insertion beginning point
pose_target.position.x = 0.02 -0.805
pose_target.position.y = 0.235 - 0.0125 - 0.225 - 0.002
pose_target.position.z = -0.12 + 0.1 + 0.46
group.set_pose_target(pose_target)
plan1 = group.plan()
group.execute(plan1)
rospy.sleep(2)
##7. Go down and start the insertion process
pose_target.orientation.x = 0.48868426425
pose_target.orientation.y = 0.504593823773
pose_target.orientation.z = -0.51414136297
pose_target.orientation.w = 0.492170114665
pose_target.position.x = -0.77998840649
pose_target.position.y = 0.00084252595
pose_target.position.z = 0.369628519
group.set_pose_target(pose_target)
plan1 = group.plan()
group.execute(plan1)
print 'point 1'
rospy.sleep(2)
#calculate ARC waypoints
center_point = [-0.77998840649, 0.00084252595, 0.369628519 - 0.315]
rotate_axis = [0, 1, 0]
waypoints_new = calc_waypoints_ARC(pose_target, center_point, rotate_axis, 45)
#execute path with waypoints
## We want the cartesian path to be interpolated at a resolution of 1 cm
## which is why we will specify 0.01 as the eef_step in cartesian
## translation. We will specify the jump threshold as 0.0, effectively
## disabling it.
(plan3, fraction) = group.compute_cartesian_path(
waypoints_new, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
group.execute(plan3)
rospy.sleep(2)
#13. Widen the gripper
print 'gripper 200'
gripper_200()
#14. Move to the 8th way point in the insertion process
pose_target.orientation.x = 0.300275507697
pose_target.orientation.y = 0.622241259232
pose_target.orientation.z = -0.638282291773
pose_target.orientation.w = 0.339479234704
pose_target.position.x = -0.586538559597
pose_target.position.y = 0.010062837864
pose_target.position.z = 0.301186931067
group.set_pose_target(pose_target)
plan1 = group.plan()
group.execute(plan1)
print 'point 8'
rospy.sleep(2)
#15. Widen the gripper
gripper_open_degree('180')
#16. Move to the 9th way point in the insertion process
pose_target.orientation.x = 0.347952330633
pose_target.orientation.y = 0.63268202582
pose_target.orientation.z = -0.596866944095
pose_target.orientation.w = 0.349846367507
pose_target.position.x = -0.597923336817
pose_target.position.y = -0.00229020222052
pose_target.position.z = 0.305400357222
group.set_pose_target(pose_target)
plan1 = group.plan()
group.execute(plan1)
print 'point 9'
rospy.sleep(2)
#17. Move to the 10th way point in the insertion process
pose_target.orientation.x = 0.359182117133
pose_target.orientation.y = 0.630010602994
pose_target.orientation.z = -0.585982817717
pose_target.orientation.w = 0.361523144746
pose_target.position.x = -0.594702188085
pose_target.position.y = -0.00658854236333
pose_target.position.z = 0.31898880102
group.set_pose_target(pose_target)
plan1 = group.plan()
group.execute(plan1)
print 'point 10'
rospy.sleep(2)
#18. Move to the 11th way point in the insertion process
pose_target.orientation.x = 0.364964183344
pose_target.orientation.y = 0.628658362491
pose_target.orientation.z = -0.580710320412
pose_target.orientation.w = 0.366558770068
pose_target.position.x = -0.592416763802
pose_target.position.y = -0.00599166509442
pose_target.position.z = 0.320244273086
group.set_pose_target(pose_target)
plan1 = group.plan()
group.execute(plan1)
print 'point 11'
rospy.sleep(2)
#19. Move to the 12th way point in the insertion process
pose_target.orientation.x = 0.396925730678
pose_target.orientation.y = 0.583354341151
pose_target.orientation.z = -0.680227755847
pose_target.orientation.w = 0.198589720686
pose_target.position.x = -0.527264025781
pose_target.position.y = 0.194388091811
pose_target.position.z = 0.437013772921
group.set_pose_target(pose_target)
plan1 = group.plan()
group.execute(plan1)
print 'point 12'
rospy.sleep(2)
# The card should have fallen into the shallow hole
print "============ STOPPING"
# Shutdown moveit
moveit_commander.roscpp_shutdown()
return
def __init__(self):
############
# new
rclpy.init()
############
super(MoveGroupPythonIntefaceTutorial, self).__init__()
#######################################################
# new
# rclpyでのQoSを指定する方法
self.qos_profile = QoSProfile(depth=10)
#######################################################
## BEGIN_SUB_TUTORIAL setup
##
## First initialize `moveit_commander`_ and a `rospy`_ node:
moveit_commander.roscpp_initialize(sys.argv)
#######################################################################
# rclpy.nodeを使用することで、rclpy.init()の後はselfでnodeの代わりになる
# old
# rospy.init_node('move_group_python_interface_tutorial', anonymous=True)
########################################################################
## Instantiate a `RobotCommander`_ object. Provides information such as the robot's
## kinematic model and the robot's current joint states
robot = moveit_commander.RobotCommander()
## Instantiate a `PlanningSceneInterface`_ object. This provides a remote interface
## for getting, setting, and updating the robot's internal understanding of the
## surrounding world:
scene = moveit_commander.PlanningSceneInterface()
## Instantiate a `MoveGroupCommander`_ object. This object is an interface
## to a planning group (group of joints). In this tutorial the group is the primary
## arm joints in the Panda robot, so we set the group's name to "panda_arm".
## If you are using a different robot, change this value to the name of your robot
## arm planning group.
## This interface can be used to plan and execute motions:
group_name = "panda_arm"
move_group = moveit_commander.MoveGroupCommander(group_name)
## Create a `DisplayTrajectory`_ ROS publisher which is used to display
## trajectories in Rviz:
####################################################################################
# old
# display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
# moveit_msgs.msg.DisplayTrajectory,
# queue_size=20)
####################################################################################
# new
# 参考：https://docs.ros2.org/crystal/api/rclpy/api/node.html
# https://docs.ros.org/en/foxy/Contributing/Migration-Guide-Python.html#creating-a-publisher
# rospyとの違い：topicとmsg_typeが逆
display_trajectory_publisher = self.create_subscription(
moveit_msgs.msg.DisplayTrajectory,
'/move_group/display_planned_path', self.qos_profile)
####################################################################################
## END_SUB_TUTORIAL
## BEGIN_SUB_TUTORIAL basic_info
##
## Getting Basic Information
## ^^^^^^^^^^^^^^^^^^^^^^^^^
# We can get the name of the reference frame for this robot:
planning_frame = move_group.get_planning_frame()
print("============ Planning frame: %s" % planning_frame)
# We can also print the name of the end-effector link for this group:
eef_link = move_group.get_end_effector_link()
print("============ End effector link: %s" % eef_link)
# We can get a list of all the groups in the robot:
group_names = robot.get_group_names()
print("============ Available Planning Groups:",
robot.get_group_names())
# Sometimes for debugging it is useful to print the entire state of the
# robot:
print("============ Printing robot state")
print(robot.get_current_state())
print("")
## END_SUB_TUTORIAL
# Misc variables
self.box_name = ''
self.robot = robot
self.scene = scene
self.move_group = move_group
self.display_trajectory_publisher = display_trajectory_publisher
self.planning_frame = planning_frame
self.eef_link = eef_link
self.group_names = group_names
def __init__(self):
super(UR5PickupPlace, self).__init__()
## BEGIN_SUB_TUTORIAL setup
##
## First initialize `moveit_commander`_ and a `rospy`_ node:
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('UR5_pickup_place', anonymous=True)
## Instantiate a `RobotCommander`_ object. Provides information such as the robot's
## kinematic model and the robot's current joint states
robot = moveit_commander.RobotCommander()
## Instantiate a `PlanningSceneInterface`_ object. This provides a remote interface
## for getting, setting, and updating the robot's internal understanding of the
## surrounding world:
scene = moveit_commander.PlanningSceneInterface()
## Instantiate a `MoveGroupCommander`_ object. This object is an interface
## to a planning group (group of joints). In this tutorial the group is the primary
## arm joints in the Panda robot, so we set the group's name to "panda_arm".
## If you are using a different robot, change this value to the name of your robot
## arm planning group.
## This interface can be used to plan and execute motions:
group_name = "manipulator"
move_group_arm = moveit_commander.MoveGroupCommander(group_name)
move_group_arm.set_planning_time(20)
move_group_arm.allow_looking(True)
move_group_arm.set_num_planning_attempts(8)
group_name = "gripper"
move_group_gripper = moveit_commander.MoveGroupCommander(group_name)
move_group_gripper.set_planning_time(20)
move_group_gripper.allow_looking(True)
move_group_gripper.set_num_planning_attempts(8)
# Gazebo reset
self.pub_gazebo_object = rospy.Publisher('gazebo/set_model_state',
ModelState,
queue_size=1)
self.pub_running_state = rospy.Publisher('running_state',
String,
queue_size=1)
## BEGIN_SUB_TUTORIAL basic_info
##
## Getting Basic Information
## ^^^^^^^^^^^^^^^^^^^^^^^^^
# We can get the name of the reference frame for this robot:
planning_frame = move_group_arm.get_planning_frame()
# print "============ Planning frame: %s" % planning_frame
# We can also print the name of the end-effector link for this group:
eef_link = move_group_arm.get_end_effector_link()
# print "============ End effector link: %s" % eef_link
# We can get a list of all the groups in the robot:
group_names = robot.get_group_names()
# print "============ Available Planning Groups:", robot.get_group_names()
# Sometimes for debugging it is useful to print the entire state of the
# robot:
# print "============ Printing robot state"
# print robot.get_current_state()
# print ""
## END_SUB_TUTORIAL
self.box_name = ''
self.robot = robot
self.scene = scene
self.move_group_arm = move_group_arm
self.move_group_gripper = move_group_gripper
self.planning_frame = planning_frame
self.eef_link = eef_link
self.group_names = group_names
# Record the initial position of the object
object = rospy.wait_for_message('gazebo/model_states', ModelStates)
for i in range(len(object.name)):
if object.name[i] == 'object':
object_reset = ModelState()
object_reset.model_name = object.name[i]
object_reset.pose = object.pose[i]
self.object_initial_position = object_reset
def tilt_pivot_start():
global gripper_command_publisher
global odom_pub
global odom_c_pub
odom_pub = rospy.Publisher("/odom", Odometry, queue_size=50)
# diaplay center point
odom_c_pub = rospy.Publisher("/odom_c", Odometry, queue_size=50)
global display_trajectory_trigger_pub
display_trajectory_trigger_pub = rospy.Publisher(
'/display_trigger',
String,
queue_size=20)
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_group_python_interface_tutorial',
anonymous=True)
robot = moveit_commander.RobotCommander()
global group
group = moveit_commander.MoveGroupCommander("manipulator")
#set max velocity and acceleration scaler
group.set_max_velocity_scaling_factor(0.5);
group.set_max_acceleration_scaling_factor(0.5);
group.set_planning_time(3)
#initialize the gripper publisher variable
gripper_command_publisher = rospy.Publisher(
'CModelRobotInputRob',
String,
queue_size=20)
## Wait for RVIZ to initialize. This sleep is ONLY to allow Rviz to come up.
print "============ Waiting for RVIZ..."
rospy.sleep(2)
print "============ Starting tutorial "
for phi in range(0, 91, 10):
for alpha in range(0, -91, -10):
for beta in range(0, 91, 10):
for gamma in range(0, -91, -10):
card_length = 0.17
#go to camera point
clean_scene()
group_variable_values = [-55.77/ 180 * 3.1415926, -95.61/ 180 * 3.1415926, -72.13/ 180 * 3.1415926, 77.75/ 180 * 3.1415926, -89.71/ 180 * 3.1415926, 124.15/ 180 * 3.1415926]
group.set_joint_value_target(group_variable_values)
plan1 = group.plan()
group.execute(plan1)
# print 'go to the start point of the ARC'
rospy.sleep(2)
center_point = [-0.77998840649 + 0.5, 0.00084252595, 0.369628519 - 0.315]
rotate_axis = [0, 0, 1] # Set the rotation axis as 'z axis'
set_tool(-0.073, 0.005, 0.215)
f = open('/home/sslrayray/output.txt','a')
group_move_tool('z', alpha)
group_move_tool('y', 8)
pose_target = group.get_current_pose().pose
pose_target.position.y += 0.3
pose_target.position.z = 0.219 #0.269
group.set_pose_target(pose_target)
plan1 = group.plan()
group.execute(plan1)
tool_pose = get_tool_position()
external_axis_center = tool_pose
external_axis_center[1] += card_length
# print 'tilt center', external_axis_center
group_rotate_by_external_axis(external_axis_center, [-1, 0, 0], phi)
group_move_tool('y', beta)
group_move_tool('x', gamma)
final_tool_center = get_tool_position()
# print 'final tool center', final_tool_center
card_center = [(external_axis_center[0] + final_tool_center[0])/2, (external_axis_center[1] + final_tool_center[1])/2, (external_axis_center[2] + final_tool_center[2])/2 ]
# print 'card center =', card_center
# print 'angle = ', phi / 180.0 * 3.1415926
card_pose = [final_tool_center[0] - 0.07, final_tool_center[1] - 0.001, final_tool_center[2]+0.003, 3.1415926 - phi / 180.0 * 3.1415926, 3.1415926, -3.1415926]
add_planning_scene(card_pose)
res = check_collision()
if res:
print 'Phi:',phi, ', Alpha:',alpha, ', Beta:', beta, ', Gamma:', gamma, ', Collision Result: Yes'
string_val = str(phi) + ',' + str(alpha) + ',' + str(beta) + ',' + str(gamma) + ',' + 'Y\n'
else:
print 'Phi:',phi, ', Alpha:',alpha, ', Beta:', beta, ', Gamma:', gamma, ', Collision Result: No'
string_val = str(phi) + ',' + str(alpha) + ',' + str(beta) + ',' + str(gamma) + ',' + 'N\n'
f.write(string_val)
f.close()
raw_input()
rospy.sleep(3)
# Shutdown the program
moveit_commander.roscpp_shutdown()
print 'STOPPING'
def body_up():
# 取りに行く
robot = moveit_commander.RobotCommander()
arm = moveit_commander.MoveGroupCommander("arm")
arm.set_max_velocity_scaling_factor(1)
gripper = moveit_commander.MoveGroupCommander("gripper")
arm.set_named_target("home")
arm.go()
# ハンドを閉じる
gripper.set_joint_value_target([0.8, 0.8])
gripper.go()
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = 0.205045831868
target_pose.position.y = -0.0194597655966
target_pose.position.z = 0.309807278119
q = quaternion_from_euler(-3.14, 0.0, -3.14 / 2.0) # 上方から掴みに行く場合
target_pose.orientation.x = 0.474871942335
target_pose.orientation.y = 0.424353826113
target_pose.orientation.z = 0.514882237833
target_pose.orientation.w = 0.573861263557
arm.set_pose_target(target_pose) # 目標ポーズ設定
arm.go() # 実行take
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = 0.269119370689
target_pose.position.y = -0.0439712214052
target_pose.position.z = 0.138551614587
q = quaternion_from_euler(-3.14, 0.0, -3.14 / 2.0) # 上方から掴みに行く場合
target_pose.orientation.x = 0.717972912926
target_pose.orientation.y = 0.671395173952
target_pose.orientation.z = 0.10624984559
target_pose.orientation.w = 0.149847879569
arm.set_pose_target(target_pose) # 目標ポーズ設定
arm.go() # 実行take
# ハンドを閉じる
gripper.set_joint_value_target([0.1, 0.1])
gripper.go()
arm.set_named_target("home")
arm.go()
# 物体を持ち上げる
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = -0.0372058227489
target_pose.position.y = -0.246006903877
target_pose.position.z = 0.169424000349
q = quaternion_from_euler(-3.14, 0.0, -3.14 / 2.0) # 上方から掴みに行く場合
target_pose.orientation.x = -0.475279946015
target_pose.orientation.y = 0.431250538961
target_pose.orientation.z = -0.753061012631
target_pose.orientation.w = 0.145020884072
arm.set_pose_target(target_pose) # 目標ポーズ設定
arm.go() # 実行
gripper.set_joint_value_target([0.7, 0.7])
gripper.go()
rospy.sleep(2.0)
# ハンドを閉じる
gripper.set_joint_value_target([0.1, 0.1])
gripper.go()
arm.set_named_target("home")
arm.go()
print("done")
def main():
# Inicializo nodo - haptic_jointpose -
rospy.init_node('haptic_jointpose', anonymous=False)
# Moveit commander, robot commander y planning scene
moveit_commander.roscpp_initialize(sys.argv)
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
# Subscribo topics /Geomagic/button y /Geomagic/joint_status
sub_botones = rospy.Subscriber("/Geomagic/button", DeviceButtonEvent,
Callback_botones)
sub_joints = rospy.Subscriber("/Geomagic/joint_states", JointState,
Callback_joints)
# Subscribo topic fuerzas /wrench
sub_force = rospy.Subscriber("/wrench", WrenchStamped, Callback_force)
# Publish force feedback
pub = rospy.Publisher('/Geomagic/force_feedback',
DeviceFeedback,
queue_size=1)
#r = rospy.Rate(10)
# Instancio MoveGroupCommander, para UR5e - manipulator - (Robot planning group)
move_group = moveit_commander.MoveGroupCommander("manipulator")
# Publico topic /move_group/display_planned_path
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
# Inicializo joint vars
joint_goal = move_group.get_current_joint_values()
joint_haptic = [0, 0, 0, 0, 0, 0]
joint_goal = [
pi / 2, -0.2689 * 2, 0.6397 + pi / 2, -pi + pi / 5, -pi / 2 + pi, pi
]
df = DeviceFeedback()
############################
# Main while rospy running #
############################
iii = 1
while not rospy.is_shutdown():
######################
# Push button action #
######################
if (boton_gris.data == 1):
joint_haptic = [
waist.data, shoulder.data, elbow.data, yaw.data, pitch.data,
roll.data
]
print joint_haptic
#r.sleep()
joint_goal[0] = joint_goal[0]
joint_goal[1] = joint_goal[1] - 0.05
joint_goal[2] = joint_goal[2]
joint_goal[3] = joint_goal[3]
joint_goal[4] = joint_goal[4]
joint_goal[5] = joint_goal[5]
##### Final if grey button #####
# The go command can be called with joint values, poses, or without any
# parameters if you have already set the pose or joint target for the group
move_group.go(joint_goal, wait=True)
def __init__(self):
super(UR5GripperPythonInteface, self).__init__()
## First initialize `moveit_commander`_ and a `rospy`_ node:
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('ur5_gripper_python_interface', anonymous=True)
## Instantiate a `RobotCommander`_ object. Provides information such as the robot's
## kinematic model and the robot's current joint states
robot = moveit_commander.RobotCommander()
## Instantiate a `PlanningSceneInterface`_ object. This provides a remote interface
## for getting, setting, and updating the robot's internal understanding of the
## surrounding world:
scene = moveit_commander.PlanningSceneInterface()
## Create a `DisplayTrajectory`_ ROS publisher which is used to display
## trajectories in Rviz:
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
# We can get a list of all the groups in the robot:
group_names = robot.get_group_names()
print "============ Available Planning Groups:", robot.get_group_names()
# Sometimes for debugging it is useful to print the entire state of the
# robot:
print "============ Printing robot state"
print robot.get_current_state()
print ""
## END_SUB_TUTORIAL
# Misc variables
self.box_name = ''
self.robot = robot
self.scene = scene
self.display_trajectory_publisher = display_trajectory_publisher
self.group_names = group_names
init=False
def initialize_groups():
self.arm_group = self.robot.get_group("ur5_arm")
self.gripper_group = self.robot.get_group("robotiq140gripper")
return
start_time = time.time()
while(init == False):
try:
initialize_groups()
init = True
except:
time_now = time.time()
#try for 120 seconds
if(time_now - start_time > 120):
print "Could not initialize groups ..."
print "shutdown"
sys.exit(1)
print "Failed to initialize groups, trying again ..."
def __init__(self):
# Initialize the move_group API and node
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_ik_demo', anonymous=True)
robot = moveit_commander.RobotCommander()
def move_group_python_interface_tutorial():
## BEGIN_TUTORIAL
##
## Setup
## ^^^^^
## CALL_SUB_TUTORIAL imports
##
## First initialize moveit_commander and rospy.
print "============ Starting tutorial setup"
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_group_python_interface_tutorial', anonymous=True)
## Instantiate a RobotCommander object. This object is an interface to
## the robot as a whole.
robot = moveit_commander.RobotCommander()
## Instantiate a PlanningSceneInterface object. This object is an interface
## to the world surrounding the robot.
scene = moveit_commander.PlanningSceneInterface()
## Instantiate a MoveGroupCommander object. This object is an interface
## to one group of joints. In this case the group is the joints in the left
## arm. This interface can be used to plan and execute motions on the left
## arm.
group = moveit_commander.MoveGroupCommander("manipulator")
## We create this DisplayTrajectory publisher which is used below to publish
## trajectories for RVIZ to visualize.
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=10)
## Wait for RVIZ to initialize. This sleep is ONLY to allow Rviz to come up.
print "============ Waiting for RVIZ..."
rospy.sleep(1)
print "============ Starting tutorial "
## Getting Basic Information
## ^^^^^^^^^^^^^^^^^^^^^^^^^
##
## We can get the name of the reference frame for this robot
print "============ Reference frame: %s" % group.get_planning_frame()
## We can also print the name of the end-effector link for this group
print "============ Reference frame: %s" % group.get_end_effector_link()
## We can get a list of all the groups in the robot
print "============ Robot Groups:"
print robot.get_group_names()
## Sometimes for debugging it is useful to print the entire state of the
## robot.
print "============ Printing robot state"
print robot.get_current_state()
print "============"
## Planning to a Pose goal
## ^^^^^^^^^^^^^^^^^^^^^^^
## We can plan a motion for this group to a desired pose for the
## end-effector
print "============ Generating plan 1"
# Here I am making it go to some Pose (recall Pose has coordinates(x,y,z)
# as well as orientation (quaternions) )
# My robot is in the up position to start and this Pose will make its
# wrist dip down by 0.3 meters. 1.0-0.2=0.8 meters i.e. position.z
pose_target = geometry_msgs.msg.Pose()
quaternion = tf.transformations.quaternion_from_euler(0, 0, 0)
pose_target.orientation.x = quaternion[0]
pose_target.orientation.y = quaternion[1]
pose_target.orientation.z = quaternion[2]
pose_target.orientation.w = quaternion[3]
pose_target.position.x = 0
pose_target.position.y = 0.2
pose_target.position.z = 0.8
#tell ur5 that you want it to go to the Pose
group.set_pose_target(pose_target)
#if you don't have this line, the robot will go too fast and emergency stop
group.set_max_velocity_scaling_factor(0.2)
## Now, we call the planner to compute the plan
## and visualize it if successful
## Note that we are just planning, not asking move_group
## to actually move the robot
plan1 = group.plan()
print "============ Waiting while RVIZ displays plan1..."
rospy.sleep(0.5)
## You can ask RVIZ to visualize a plan (aka trajectory) for you. But the
## group.plan() method does this automatically so this is not that useful
## here (it just displays the same trajectory again).
print "============ Visualizing plan1"
display_trajectory = moveit_msgs.msg.DisplayTrajectory()
display_trajectory.trajectory_start = robot.get_current_state()
display_trajectory.trajectory.append(plan1)
display_trajectory_publisher.publish(display_trajectory)
print "============ Waiting while plan1 is visualized (again)..."
rospy.sleep(0.5)
## Moving to a pose goal
## ^^^^^^^^^^^^^^^^^^^^^
##
## Moving to a pose goal is similar to the step above
## except we now use the go() function. Note that
## the pose goal we had set earlier is still active
## and so the robot will try to move to that goal. We will
## not use that function in this tutorial since it is
## a blocking function and requires a controller to be active
## and report success on execution of a trajectory.
# Uncomment below line when working with a real robot
print "Press y to move robot to goal state. Press any other key to exit."
if raw_input() == 'y':
group.go(wait=True)
"""
## Planning to a joint-space goal
## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
##
## Let's set a joint space goal and move towards it.
## First, we will clear the pose target we had just set.
group.clear_pose_targets()
## Then, we will get the current set of joint values for the group
group_variable_values = group.get_current_joint_values()
print "============ Joint values: ", group_variable_values
## Now, let's modify one of the joints, plan to the new joint
## space goal and visualize the plan
group_variable_values[0] = 1.0
group.set_joint_value_target(group_variable_values)
plan2 = group.plan()
print "============ Waiting while RVIZ displays plan2..."
rospy.sleep(5)
## Cartesian Paths
## ^^^^^^^^^^^^^^^
## You can plan a cartesian path directly by specifying a list of waypoints
## for the end-effector to go through.
waypoints = []
# start with the current pose
waypoints.append(group.get_current_pose().pose)
# first orient gripper and move forward (+x)
wpose = geometry_msgs.msg.Pose()
wpose.orientation.w = 1.0
wpose.position.x = waypoints[0].position.x + 0.1
wpose.position.y = waypoints[0].position.y
wpose.position.z = waypoints[0].position.z
waypoints.append(copy.deepcopy(wpose))
# second move down
wpose.position.z -= 0.10
waypoints.append(copy.deepcopy(wpose))
# third move to the side
wpose.position.y += 0.05
waypoints.append(copy.deepcopy(wpose))
## We want the cartesian path to be interpolated at a resolution of 1 cm
## which is why we will specify 0.01 as the eef_step in cartesian
## translation. We will specify the jump threshold as 0.0, effectively
## disabling it.
(plan3, fraction) = group.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
print "============ Waiting while RVIZ displays plan3..."
rospy.sleep(5)
## Adding/Removing Objects and Attaching/Detaching Objects
## ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
## First, we will define the collision object message
collision_object = moveit_msgs.msg.CollisionObject()
## When finished shut down moveit_commander.
moveit_commander.roscpp_shutdown()
"""
## END_TUTORIAL
print "============ STOPPING==== Program will end now"
def go_target():
global cxm
global cym
global pub
#Initialize node
rospy.init_node('pick_place', anonymous=True)
rospy.Subscriber("/arbotix/force", Analog, callback_force)
rospy.Subscriber("/gpg/camera_info", CameraInfo, callback_camera)
pub = rospy.Publisher("/gripper_joint/command", Float64, queue_size=5)
tfBuffer = tf2_ros.Buffer()
listener = tf2_ros.TransformListener(tfBuffer)
direction = PointStamped()
direction.header.stamp = rospy.Time()
direction.header.frame_id = "camera_link"
robot = moveit_commander.RobotCommander()
group = moveit_commander.MoveGroupCommander("arm")
group.set_goal_tolerance(0.01)
group.set_planner_id('RRTstarkConfigDefault')
group.set_planning_time(0.5)
r = rospy.Rate(1)
time.sleep(1)
pub.publish(0)
time.sleep(1)
while not rospy.is_shutdown():
# Go to initial position
go_to_pinitial(group)
#Wait for user input
obj = raw_input("Write the object that you want to pick up or write 'quit' to exit: ")
if obj == 'quit':
break
print obj
#Read center of mass
f= open("/home/nvidia/catkin_ws/src/project/src/object.fcf","w")
f.write(obj)
f.close()
f = open("data.fcf",'r')
args = f.readline()
args = args.split(',')
args = numpy.array(args, dtype=numpy.float64)
#Calculate object position
cxm = int((args[1] + args[3])*0.5)
cym = int(args[2])
position = None
line = model.projectPixelTo3dRay((cxm,cym))
if cxm == 0 and cym == 0:
print "zero"
pass
direction.point.x = line[0]
direction.point.y = line[1]
direction.point.z = line[2]
try:
q = tfBuffer.transform(direction, "base_link")
direction.point.x = 0
direction.point.y = 0
direction.point.z = 0
p = tfBuffer.transform(direction, "base_link")
position = compute_position(p,q)
pose_target.position.x = position[0] + 0.01
pose_target.position.y = position[1]
if obj == 'bottle':
pose_target.position.z = position[2] + 0.065
else:
pose_target.position.z = position[2] + 0.03
if pose_target.position.z < -0.052:
pose_target.position.z = -0.052
angle = math.atan2(pose_target.position.y,pose_target.position.x)
quart = tf.transformations.quaternion_from_euler(0, 1.5707, angle)
pose_target.orientation.x = quart[0]
pose_target.orientation.y = quart[1]
pose_target.orientation.z = quart[2]
pose_target.orientation.w = quart[3]
except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e:
print e
r.sleep()
# Send the coordinates to Rviz
broadcaster = tf2_ros.TransformBroadcaster()
t = TransformStamped()
t.header.stamp = rospy.Time.now()
t.header.frame_id = "base_link"
t.child_frame_id = "object"
t.transform.translation.x = position[0]
t.transform.translation.y = position[1]
t.transform.translation.z = position[2]
t.transform.rotation.x = 0
t.transform.rotation.y = 0
t.transform.rotation.z = 0
t.transform.rotation.w = 1
broadcaster.sendTransform(t)
# Publish the object position
print "pose_target"
print pose_target
group.set_pose_target(pose_target)
# Pick up the object and come back to initial position
group.go(wait=True)
time.sleep(1)
get_object()
go_to_pinitial(group)
go_to_pfinal(group)
pub.publish(0)
time.sleep(0.5)
go_to_pinitial(group)
def send_move_command(self):
#We want orientation to always be the same
bMove = False
#stopped
if self.bstop:
rospy.sleep(1)
bMove = False
#move to basket
elif self.btoBasket:
position = self.moveMethod[1](self)
bMove = True
#move to last cut location
elif self.btoCut:
position = self.moveMethod[2](self)
bMove = True
#move by camera
elif self.bbyCamera:
position = self.moveMethod[3](self)
bMove = True
#move by scan
elif self.bbyScan:
position = self.moveMethod[4](self)
bMove = True
if bMove:
print position
if self.bgripper:
#make new message
goal = kinova_msgs.msg.SetFingersPositionGoal()
goal.fingers.finger1 = self.finger_turn
goal.fingers.finger2 = goal.fingers.finger1
goal.fingers.finger3 = goal.fingers.finger1
print goal
#send through client
self.fingerClient.send_goal(goal)
print "Attention: moving the gripper"
if self.fingerClient.wait_for_result(rospy.Duration(5.0)):
print 'Success'
else:
self.fingerClient.cancel_all_goals()
print ' Error: the cartesian action timed-out'
self.bgripper = False
if self.finger_turn == finger_minTurn:
#if we just opened fingers return to last cut location
self.btoBasket = False
self.btoCut = True
self.finger_turn = finger_maxTurn
else:
#if we just closed to fngers move to basket
self.finger_turn = finger_minTurn
self.btoBasket = True
else:
#basket has a different orientation
orientation = EulerXYZ2Quaternion(self.currentRPY)
if self.btoBasket:
orientation = EulerXYZ2Quaternion(self.basketRPY)
#send with Moveit using location gotten above to move to the new location
goal = geometry_msgs.msg.Pose()
goal.position = geometry_msgs.msg.Point(x=position[0],
y=position[1],
z=position[2])
goal.orientation = geometry_msgs.msg.Quaternion(
x=orientation[0],
y=orientation[1],
z=orientation[2],
w=orientation[3])
# Moveit
robot = moveit_commander.RobotCommander()
group = moveit_commander.MoveGroupCommander("arm")
planning_scene_interface = moveit_commander.PlanningSceneInterface(
)
# Planning to a Pose goal
group.set_pose_target(goal)
# Now, we call the planner to compute the plan and visualize it.
group.plan()
# we should add something hear to handle failures
# move the robot
print "Attention: moving the arm"
group.go()
def Test():
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('comportement_controller', anonymous=True)
# create a publisher
say = rospy.Publisher('/qt_robot/speech/say', String, queue_size=10)
emo = rospy.Publisher('/qt_robot/emotion/show',String, queue_size=15)
head_pub = rospy.Publisher('/qt_robot/head_position/command',Float64MultiArray, queue_size=1)
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
right_arm = moveit_commander.MoveGroupCommander("right_arm")
left_arm = moveit_commander.MoveGroupCommander("left_arm")
# head = moveit_commander.MoveGroupCommander("head")
rospy.sleep(3)
# We can get the name of the reference frame for this robot:
planning_frame_left = left_arm.get_planning_frame()
planning_frame_right= right_arm.get_planning_frame()
print "============Left reference frame: %s " % planning_frame_left
print "============Right reference frame: %s " %planning_frame_right
# We can also print the name of the end-effector link for this group:
eef_link_left = left_arm.get_end_effector_link()
eef_link_right = right_arm.get_end_effector_link()
print "============Left end effector: %s " % eef_link_left
print "============Right end effector: %s " % eef_link_right
# We can get a list of all the groups in the robot:
group_names = robot.get_group_names()
print "============ Robot Groups:", robot.get_group_names()
left_arm.allow_replanning(True)
left_arm.set_pose_reference_frame("base_link")
left_arm.set_planning_time(3.0)
left_arm.clear_path_constraints()
left_arm.clear_pose_targets()
right_arm.allow_replanning(True)
right_arm.set_pose_reference_frame("base_link")
right_arm.set_planning_time(3.0)
right_arm.clear_path_constraints()
right_arm.clear_pose_targets()
# Sometimes for debugging it is useful to print the entire state of the
# robot:
print "============ Printing robot state"
print robot.get_current_state()
print ""
## END_SUB_TUTORIAL
print "============ Generating plan 1"
pose_target = geometry_msgs.msg.Pose()
pose_target.orientation.w = 1.0
pose_target.position.x = 0.7
pose_target.position.y = -0.05
pose_target.position.z = 1.1
group.set_pose_target(pose_target)
## to actually move the robot
plan1 = left_arm.plan()
print "============ Waiting while RVIZ displays plan1..."
rospy.sleep(5)
def __init__(self):
super(MoveGroupPythonIntefaceTutorial, self).__init__()
## BEGIN_SUB_TUTORIAL setup
##
## First initialize `moveit_commander`_ and a `rospy`_ node:
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_group_python_interface_tutorial', anonymous=True)
## Instantiate a `RobotCommander`_ object. This object is the outer-level interface to
## the robot:
robot = moveit_commander.RobotCommander()
## Instantiate a `PlanningSceneInterface`_ object. This object is an interface
## to the world surrounding the robot:
scene = moveit_commander.PlanningSceneInterface()
## Instantiate a `MoveGroupCommander`_ object. This object is an interface
## to one group of joints. In this case the group is the joints in the Panda
## arm so we set ``group_name = panda_arm``. If you are using a different robot,
## you should change this value to the name of your robot arm planning group.
## This interface can be used to plan and execute motions on the Panda:
group_name = "manipulator"
group = moveit_commander.MoveGroupCommander(group_name)
## We create a `DisplayTrajectory`_ publisher which is used later to publish
## trajectories for RViz to visualize:
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
## END_SUB_TUTORIAL
## BEGIN_SUB_TUTORIAL basic_info
##
## Getting Basic Information
## ^^^^^^^^^^^^^^^^^^^^^^^^^
# We can get the name of the reference frame for this robot:
planning_frame = group.get_planning_frame()
print "============ Reference frame: %s" % planning_frame
# We can also print the name of the end-effector link for this group:
eef_link = group.get_end_effector_link()
print "============ End effector: %s" % eef_link
# We can get a list of all the groups in the robot:
group_names = robot.get_group_names()
print "============ Robot Groups:", robot.get_group_names()
# Sometimes for debugging it is useful to print the entire state of the
# robot:
print "============ Printing robot state"
print robot.get_current_state()
print ""
## END_SUB_TUTORIAL
# Misc variables
self.box_name = ''
self.robot = robot
self.scene = scene
self.group = group
self.display_trajectory_publisher = display_trajectory_publisher
self.planning_frame = planning_frame
self.eef_link = eef_link
self.group_names = group_names
def main():
rospy.init_node("crane_x7_pick_and_place_controller")
robot = moveit_commander.RobotCommander()
arm = moveit_commander.MoveGroupCommander("arm")
#アームの動きの速さを示している
arm.set_max_velocity_scaling_factor(0.5)
gripper = moveit_commander.MoveGroupCommander("gripper")
while len([s for s in rosnode.get_node_names() if 'rviz' in s]) == 0:
rospy.sleep(1.0)
rospy.sleep(1.0)
print("Group names:")
print(robot.get_group_names())
print("Current state:")
print(robot.get_current_state())
# アーム初期ポーズを表示
arm_initial_pose = arm.get_current_pose().pose
print("Arm initial pose:")
print(arm_initial_pose)
# SRDFに定義されている"home"の姿勢にする
arm.set_named_target("home")
arm.go()
gripper.set_joint_value_target([0.7, 0.7])
gripper.go()
#繰り返し呼び出すのでmove関数を定義する
def move(x, y, z):
target_pose = geometry_msgs.msg.Pose()
target_pose.position.x = x
target_pose.position.y = y
target_pose.position.z = z
q = quaternion_from_euler(-3.14, 0.0, -3.14 / 2.0) # 上方から掴みに行く場合
target_pose.orientation.x = q[0]
target_pose.orientation.y = q[1]
target_pose.orientation.z = q[2]
target_pose.orientation.w = q[3]
arm.set_pose_target(target_pose) # 目標ポーズ設定
arm.go() # 実行
# 掴む準備をする
move(0.2, 0.0, 0.2)
# 掴みに行く
move(0.2, 0.0, 0.1)
# 持ち上げる
move(0.2, 0.0, 0.2)
# 移動する
move(0.2, 0.3, 0.2)
# 下ろす
move(0.2, 0.3, 0.1)
# 少しだけハンドを持ち上げる
move(0.2, 0.3, 0.15)
# SRDFに定義されている"home"の姿勢にする
# srdfファイルがある場所 (crane_x7_ros/crane_x7_moveit_config/config/crane_x7.srdf)
arm.set_named_target("home")
arm.go()
print("done")
def handle_req(req):
## First initialize `moveit_commander`_ and a `rospy`_ node:
moveit_commander.roscpp_initialize(sys.argv)
## Instantiate a `RobotCommander`_ object. Provides information such as the robot's
## kinematic model and the robot's current pose states
robot = moveit_commander.RobotCommander()
## Instantiate a `PlanningSceneInterface`_ object. This provides a remote interface
## for getting, setting, and updating the robot's internal understanding of the
## surrounding world:
scene = moveit_commander.PlanningSceneInterface()
## Instantiate a `MoveGroupCommander`_ object. This object is an interface
## to a planning group (group of poses).
## This interface can be used to plan and execute motions:
group_name = req.group
## print "============ Robot Groups:"
print robot.get_group_names()
## print "============ Printing robot state"
print robot.get_current_state()
move_group = moveit_commander.MoveGroupCommander(group_name.data)
print "Pose is: "
print move_group.get_current_pose().pose
print "Orientation is: "
print req.orientation
print "x is: "
print req.posex
print "y is: "
print req.posey
print "z is: "
print req.posez
## Create a `DisplayTrajectory`_ ROS publisher which is used to display
## trajectories in Rviz:
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
# We can get the pose values from the group and adjust some of the values:
print "============ Generating plan.."
pose_goal = geometry_msgs.msg.Pose()
print pose_goal
# plan a motion for this group
pose_goal.orientation.w = req.orientation.data
pose_goal.position.x = req.posex.data
pose_goal.position.y = req.posey.data
pose_goal.position.z = req.posez.data
move_group.set_pose_target(pose_goal)
# The go command can be called with pose values, poses, or without any
# parameters if you have already set the pose or pose target for the group
move_group.go(pose_goal, wait=True)
# Calling ``stop()`` ensures that there is no residual movement
move_group.stop()
# It is always good to clear your targets after planning with poses.
move_group.clear_pose_targets()
rep = udm_poseService()
rep.message = "This action was planned at %s" % rospy.get_time()
rep.res = True
return rep
def main():
arg_fmt = argparse.RawDescriptionHelpFormatter
parser = argparse.ArgumentParser(formatter_class=arg_fmt,
description=main.__doc__)
required = parser.add_argument_group('required arguments')
required.add_argument('-l',
'--limb',
required=True,
choices=['left', 'right'],
help='send joint trajectory to which limb')
args = parser.parse_args(rospy.myargv()[1:])
limb = args.limb
rospy.init_node('stackit')
iksvc, ns = ik_command.connect_service(limb)
moveit_commander.roscpp_initialize(sys.argv)
rate = rospy.Rate(1)
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
group = moveit_commander.MoveGroupCommander(limb + "_arm")
group.allow_replanning(True)
print("Getting robot state... ")
rs = baxter_interface.RobotEnable(CHECK_VERSION)
print("Enabling robot... ")
rs.enable()
#Calibrate gripper
gripper_if = baxter_interface.Gripper(limb)
if not gripper_if.calibrated():
print "Calibrating gripper"
gripper_if.calibrate()
obj_manager = ObjectManager()
while len(obj_manager.collision_objects) <= 0:
rate.sleep()
objects = obj_manager.collision_objects
object_height = params['object_height']
"""if len(objects) > 1:
stack_pose = projectPose(objects[len(objects)-1].primitive_poses[0])
stack_pose = incrementPoseMsgZ(stack_pose, object_height*2.0)
objects.pop(len(objects)-1)
elif len(objects) == 1:
stack_pose = projectPose(objects[0].primitive_poses[0])"""
stack_pose = Pose(position=Point(0.593, -0.212, object_height - 0.130),
orientation=Quaternion(0.6509160466, 0.758886809948,
-0.0180992582839,
-0.0084573527776))
processed_win = "Processed image"
raw_win = "Hand camera"
cv2.namedWindow(processed_win)
#cv2.namedWindow(raw_win)
rospy.on_shutdown(obj_manager.remove_known_objects)
for obj in objects:
obj_pose = obj.primitive_poses[0]
#group.pick(obj.id)
#group.place(obj.id, stack_pose)
print "Got pose:", obj.primitive_poses[0]
pose = projectPose(obj.primitive_poses[0])
pose = incrementPoseMsgZ(pose, object_height * 1.7)
print "Modified pose:", pose
#if obj.id in obj_manager.id_operations:
# obj_manager.id_operations[obj.id] = CollisionObject.REMOVE
#obj_manager.publish(obj)
print "setting target to pose"
# Move to the next block
group.clear_pose_targets()
group.set_start_state_to_current_state()
group.set_pose_target(pose)
plan = group.plan()
# is there a better way of checking this?
plan_found = len(plan.joint_trajectory.points) > 0
if plan_found:
#print "============ Waiting while RVIZ displays plan1..."
#rospy.sleep(3)
group.go(wait=True)
imgproc = ObjectFinder("star", None, None)
imgproc.subscribe("/cameras/" + limb + "_hand_camera/image")
imgproc.publish(limb)
vc = VisualCommand(iksvc, limb)
vc.subscribe()
while (not vc.done) and (not rospy.is_shutdown()):
blobArray = []
for centroid, axis in zip(imgproc.centroids, imgproc.axes):
blob = BlobInfo()
centroid = Point(*centroid)
blob.centroid = centroid
if axis is None:
axis = -1 * numpy.ones(6)
blob.axis = Polygon([
Point(*axis[0:3].tolist()),
Point(*axis[3:6].tolist())
])
blobArray.append(blob)
msg = BlobInfoArray()
msg.blobs = blobArray
imgproc.handler_pub.publish(msg)
if imgproc.processed is not None:
cv2.imshow(processed_win, imgproc.processed)
cv2.waitKey(10)
vc.unsubscribe()
imgproc.unsubscribe()
print "Adding attached message"
#Add attached message
#obj.primitive_poses[0] = incrementPoseMsgZ(obj.primitive_poses[0], *object_height) #this is in the base frame...?
obj_manager.publish_attached(obj, limb)
#touch_links = [limb+"_gripper", limb+"_gripper_base", limb+"_hand_camera", limb+"_hand_range"]
#group.attach_object(obj.id, limb+"_gripper", touch_links = touch_links)
# Carefully rise away from the object before we plan another path
pose = incrementPoseMsgZ(pose, 2 * object_height) # test this
ik_command.service_request_pose(iksvc, pose, limb, blocking=True)
else:
print "Unable to plan path"
continue
# what to do?
# Move to the stacking position
group.clear_pose_targets()
group.set_start_state_to_current_state()
group.set_pose_target(stack_pose)
plan = group.plan()
plan_found = len(plan.joint_trajectory.points) > 0
if plan_found:
#print "============ Waiting while RVIZ displays plan2..."
#rospy.sleep(3)
group.go(wait=True)
gripper_if.open(block=True)
group.detach_object(obj.id)
# Carefully rise away from the object before we plan another path
pose = incrementPoseMsgZ(stack_pose, 2 * object_height)
ik_command.service_request_pose(iksvc, pose, limb, blocking=True)
obj.operation = CollisionObject.REMOVE
obj_manager.publish(obj)
# Get the next stack pose
stack_pose = incrementPoseMsgZ(stack_pose, object_height * 3 / 4)
"""if obj.id in obj_manager.id_operations:
