def move_arm():
print "Starting tutorial setup"
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_arm_node')
print "move_arm_node should be initialized"
print "Initializing Robot Commander"
robot = moveit_commander.RobotCommander()
print "Initializing Planning Scene Interface"
scene = moveit_commander.PlanningSceneInterface()
print "Let's move left arm first"
group = moveit_commander.MoveGroupCommander("left_arm")
print "Create publisher to display_planned_path"
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',moveit_msgs.msg.DisplayTrajectory)
robot_pose = geometry_msgs.msg.Pose()
robot_pose.orientation.x = 1
robot_pose.position.x = 0.7
robot_pose.position.y = 0.4
robot_pose.position.z = 0.3
group.set_pose_target(robot_pose)
plan1 = group.plan()
rospy.sleep(5)
group.go(wait=True)
moveit_commander.roscpp_shutdown()
print "Finishing"
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
# Initialize the MoveIt! commander for the right arm
arm = moveit_commander.MoveGroupCommander('arm_with_torso')
arm.set_end_effector_link('wrist_roll_link')
# Get the name of the end-effector link
end_effector_link = arm.get_end_effector_link()
# Set the reference frame for pose targets
reference_frame = 'base_link'
# Set the right arm reference frame accordingly
arm.set_pose_reference_frame(reference_frame)
# Allow replanning to increase the odds of a solution
arm.allow_replanning(True)
# Allow some leeway in position (meters) and orientation (radians)
arm.set_goal_position_tolerance(0.01)
arm.set_goal_orientation_tolerance(0.05)
# Set the start pose. This particular pose has the gripper oriented horizontally
# 0.75 meters above the base and 0.72 meters in front of the robot.
target_pose = PoseStamped()
target_pose.header.frame_id = reference_frame
target_pose.header.stamp = rospy.Time.now()
target_pose.pose.position.x = 0.609889
target_pose.pose.position.y = 0.207002
target_pose.pose.position.z = 1.10677
target_pose.pose.orientation.x = 0.0
target_pose.pose.orientation.y = 0.0
target_pose.pose.orientation.z = 0.0
target_pose.pose.orientation.w = 1.0
# Set the start state to the current state
arm.set_start_state_to_current_state()
# Set the goal pose of the end effector to the stored pose
arm.set_pose_target(target_pose, end_effector_link)
# Plan the trajectory to the goal
traj = arm.plan()
# Execute the planned trajectory
arm.execute(traj)
# Pause for a couple of seconds
rospy.sleep(2)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit MoveIt
moveit_commander.os._exit(0)
def starting_pose():
rospy.sleep(20)
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_to_starting_pose',
anonymous=True)
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
group = moveit_commander.MoveGroupCommander("arm_gp")
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory)
print "============ Waiting for RVIZ..."
pose_target = geometry_msgs.msg.Pose()
pose_target.orientation.x = 0.0
pose_target.orientation.y = -1.0
pose_target.orientation.z = 0.0
pose_target.orientation.w = 0.0
pose_target.position.x = 0.3
pose_target.position.y = 0.0
pose_target.position.z = 0.38
group.set_pose_target(pose_target)
group.go(wait=True)
## When finished shut down moveit_commander.
moveit_commander.roscpp_shutdown()
def move_group_python_interface_tutorial():
## 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("arm")
## Sometimes for debugging it is useful to print the entire state of the
print "============ Printing robot end-effector pose"
#print group.get_current_pose()
print group.get_current_pose().pose.position.x, group.get_current_pose().pose.position.y, group.get_current_pose().pose.position.z
print group.get_current_rpy()
## When finished shut down moveit_commander.
moveit_commander.roscpp_shutdown()
def stateCallback(self, data): self.newPose = data #newPose = self.group.get_current_pose().pose #newPose.position.x = newPose.position.x +0.0005 #newPose.position.y = newPose.position.y +0.0005 #newPose.position.z = newPose.position.z +0.0005 self.group.set_pose_target(self.newPose) # Compute plan and visualize if successful print self.newPose newPlan = self.group.plan() # Wait for Rviz to display print "Rviz displaying" rospy.sleep(5) # Perform plan on Robot self.group.go(wait=True) rospy.sleep(5) self.group.clear_pose_targets() #print self.group.get_planning_frame() ## 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()
def main():
rospy.init_node('smach_example_actionlib')
sm = smach.StateMachine(outcomes=['succeeded','aborted','preempted'])
sm.userdata.tgt_pose = geometry_msgs.msg.Pose()
with sm:
# Initialize robot pose
smach.StateMachine.add('INIT', Init(),
transitions={'done':'READ_NEXT_POSE'})
# Read next pose from KB
smach.StateMachine.add('READ_NEXT_POSE', ReadNextGoal(),
transitions={'cont':'MOVE_ARM',
'finished':'succeeded'},
remapping={'next_pose':'tgt_pose'})
# Move to pose
smach.StateMachine.add('MOVE_ARM', MoveIt(),
transitions={'done':'READ_NEXT_POSE'},
remapping={'goal_pose':'tgt_pose'})
# start visualizer and introspection server
sis = smach_ros.IntrospectionServer('smach_tests', sm, '/SM_ROOT')
sis.start()
# Execute SMACH plan
outcome = sm.execute()
rospy.spin()
sis.stop()
moveit_commander.roscpp_shutdown()
rospy.signal_shutdown('All done.')
def __init__(self):
# rospy.sleep(10) # wait for other files to be launched successfully
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('cw3_q1')
self.tf_buffer = tf2_ros.Buffer()
self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
# self.touch = [0,0,0,0]
# rospy.Subscriber("/contacts/lh_ff/distal", ContactsState, self.call_ff)
# rospy.Subscriber("/contacts/lh_mf/distal", ContactsState, self.call_mf)
# rospy.Subscriber("/contacts/lh_rf/distal", ContactsState, self.call_rf)
# rospy.Subscriber("/contacts/lh_th/distal", ContactsState, self.call_th)
# # Robot contains the entire state of the robot (iiwa and shadow hand)
# robot = moveit_commander.RobotCommander()
# # We can get a list of all the groups in the robot
# print('============ Robot Groups:')
# print('{}\n\n'.format(robot.get_group_names()))
self.iiwa_group = moveit_commander.MoveGroupCommander('hand_iiwa')
self.hand_group = moveit_commander.MoveGroupCommander('sr_hand')
rospy.sleep(1)
# object tracking
[all_object_name,all_object_pose] = self.get_object_position()
# create a service proxy for add and remove objects to allowed collision matrix
add_to_acm = rospy.ServiceProxy('/add_object_acm', ChangeCollisionObject)
remove_from_acm = rospy.ServiceProxy('/remove_object_acm', ChangeCollisionObject)
# open the hand
self.hand_open()
for i in range(0,3):
# i = 2 # object no.
# move the iiwa to just above the object
self.move_iiwa_to_object(all_object_pose,i)
# add object to allowed collision matrix
add_to_acm(all_object_name[i])
# close the hand (grasping)
self.hand_close()
# hold the grasping position for 10 seconds
rospy.sleep(10)
# open the hand (releasing)
self.hand_open()
# remove object from allowed collision matrix
remove_from_acm(all_object_name[i])
moveit_commander.roscpp_shutdown()
rospy.loginfo("Task finished")
def main():
try:
grasp_analyzer_node = GraspAnalyzerNode()
loop = rospy.Rate(10)
while not rospy.is_shutdown():
loop.sleep()
moveit_commander.roscpp_shutdown()
except rospy.ROSInterruptException:
rospy.signal_shutdown()
def main():
try:
crui_manager = CRUIManager()
loop = rospy.Rate(10)
while not rospy.is_shutdown():
loop.sleep()
moveit_commander.roscpp_shutdown()
except rospy.ROSInterruptException:
rospy.signal_shutdown(reason="Interrupted")
def cleanup(self):
rospy.loginfo("Stopping the robot")
# Stop any current arm movement
self._arm.stop()
#Shut down MoveIt! cleanly
rospy.loginfo("Shutting down Moveit!")
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __init__(self):
rospy.init_node('xm_arm_moveit_ik_test', anonymous=True)
moveit_commander.roscpp_initialize(sys.argv)
xm_arm = moveit_commander.MoveGroupCommander('xm_arm')
end_effector_link = xm_arm.get_end_effector_link()
reference_frame = 'base_footprint'
xm_arm.set_pose_reference_frame(reference_frame)
xm_arm.allow_replanning(True)
xm_arm.set_goal_position_tolerance(5)
xm_arm.set_goal_orientation_tolerance(5)
xm_arm.set_planner_id("RRTConnectkConfigDefault")
xm_arm.set_named_target('initial')
xm_arm.go()
rospy.sleep(2)
print "-------------------- Test Arm Moveit IK ----------------------"
print "Input the position of object(reference frame is base's center)"
print "Such as [object/o x y z x y z w] "
while not rospy.is_shutdown():
print ">>> ",
keyboard_cmd = raw_input().split(" ")
try:
if keyboard_cmd[0] == "object" or keyboard_cmd[0] == "o":
target_pose = PoseStamped()
target_pose.header.frame_id = reference_frame
target_pose.header.stamp = rospy.Time.now()
target_pose.pose.position.x = float(keyboard_cmd[1])
target_pose.pose.position.y = float(keyboard_cmd[2])
target_pose.pose.position.z = float(keyboard_cmd[3])
target_pose.pose.orientation.x = float(keyboard_cmd[4])
target_pose.pose.orientation.y = float(keyboard_cmd[5])
target_pose.pose.orientation.z = float(keyboard_cmd[6])
target_pose.pose.orientation.w = float(keyboard_cmd[7])
print ("position[x y z] %s, %s, %s" % (keyboard_cmd[1],
keyboard_cmd[2],
keyboard_cmd[3]))
print "orientation[x y z w] %s, %s, %s, %s" % (
keyboard_cmd[4],
keyboard_cmd[5],
keyboard_cmd[6],
keyboard_cmd[7])
xm_arm.set_start_state_to_current_state()
xm_arm.set_pose_target(target_pose, end_effector_link)
trajectory = xm_arm.plan()
xm_arm.execute(trajectory)
rospy.sleep(1)
elif keyboard_cmd[0] == "exit" or keyboard_cmd[0] == "e":
exit()
except Exception as exce:
print "Error!", exce
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the ROS node
rospy.init_node('moveit_demo', anonymous=True)
GRIPPER_OPEN = [0.030, 0.030]
GRIPPER_CLOSED = [0.002, 0.002]
GRIPPER_NEUTRAL = [0.01, 0.01]
# Connect to the arm move group
arm = moveit_commander.MoveGroupCommander('arm')
# Connect to the gripper move group
gripper = moveit_commander.MoveGroupCommander('gripper')
rospy.loginfo("mw gripper name= " + gripper.get_name())
rospy.loginfo("mw gripper active joints= " + str(gripper.get_active_joints()))
rospy.loginfo("mw gripper current joint values= " + str(gripper.get_current_joint_values()))
# Get the name of the end-effector link
end_effector_link = arm.get_end_effector_link()
# Display the name of the end_effector link
rospy.loginfo("The end effector link is: " + str(end_effector_link))
# Set a small tolerance on joint angles
arm.set_goal_joint_tolerance(0.001)
gripper.set_goal_joint_tolerance(0.002)
# Set the gripper target to neutal position using a joint value target
gripper.set_joint_value_target(GRIPPER_OPEN)
# Plan and execute the gripper motion
gripper.go()
rospy.sleep(0.1)
rospy.loginfo("mw gripper current joint values= " + str(gripper.get_current_joint_values()))
start_time = datetime.datetime.now()
# gripper.set_joint_value_target(GRIPPER_CLOSED)
# gripper.go()
# rospy.sleep(0.1)
stop_time = datetime.datetime.now()
print "took: %s\n" % str(stop_time - start_time)
# Cleanly shut down MoveIt
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
def __init__(self):
# Initialize the move_group API and node
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_fk_demo', anonymous=True)
# Use the groups of SmartPal5
arm = moveit_commander.MoveGroupCommander(GROUP_NAME_ARM)
gripper_left = moveit_commander.MoveGroupCommander(GROUP_NAME_GRIPPER)
# Set a goal joint tolerance
arm.set_goal_joint_tolerance(0.001)
gripper_left.set_goal_joint_tolerance(0.001)
# Use the pose stored in the SRDF file
# 1. Set the target pose
# 2. Plan a trajectory
# 3. Execute the planned trajectory
arm.set_named_target('l_arm_init')
traj = arm.plan()
arm.execute(traj)
rospy.sleep(1)
gripper_left.set_named_target('l_gripper_init')
gripper_left.go()
rospy.sleep(1)
# Use the joint positions with FK
# 1. Set the target joint values
# 2. Plan a trajectory
# 3. Execute the planned trajectory
joint_positions = [0.0, 0.07, 0.0, 1.5707, 0.0, 0.0, 0.0]
arm.set_joint_value_target(joint_positions)
arm.go()
rospy.sleep(1)
gripper_left.set_named_target('l_gripper_open')
gripper_left.go()
rospy.sleep(1)
# Return
arm.set_named_target('l_arm_init')
traj = arm.plan()
arm.execute(traj)
rospy.sleep(1)
gripper_left.set_named_target('l_gripper_init')
gripper_left.go()
rospy.sleep(1)
# Cleanly shut down MoveIt
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
right_arm = moveit_commander.MoveGroupCommander('right_arm')
right_arm.set_named_target('resting')
right_arm.go()
end_effector_link = right_arm.get_end_effector_link()
reference_frame = '/odom'
right_arm.set_pose_reference_frame(reference_frame)
rospy.loginfo(str(end_effector_link))
rospy.loginfo(right_arm.get_planning_frame())
rospy.loginfo(right_arm.get_pose_reference_frame())
rospy.loginfo(right_arm.get_current_pose())
right_arm.allow_replanning(True)
right_arm.set_goal_position_tolerance(0.01)
right_arm.set_goal_orientation_tolerance(0.05)
target_pose = PoseStamped()
target_pose.header.frame_id = reference_frame
target_pose.header.stamp = rospy.Time.now()
target_pose.pose.position.x = 0.441402636719
target_pose.pose.position.y = -0.279191735455
target_pose.pose.position.z = 0.863870298601
target_pose.pose.orientation.x = -0.0281863349718
target_pose.pose.orientation.y = 0.063611003310
target_pose.pose.orientation.z = 0.765618014453
target_pose.pose.orientation.w = 0.63952187353
right_arm.set_start_state_to_current_state()
right_arm.set_pose_target(target_pose, end_effector_link)
traj = right_arm.plan()
right_arm.execute(traj)
rospy.sleep(1)
right_arm.set_named_target('resting')
right_arm.go()
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('cobra_test_cartesian', anonymous=True)
arm = MoveGroupCommander(ARM_GROUP)
arm.allow_replanning(True)
rospy.sleep(2)
pose = PoseStamped().pose
# same orientation for all
q = quaternion_from_euler(0.0, 0.0, 0.0) # roll, pitch, yaw
pose.orientation = Quaternion(*q)
i = 1
while i <= 10:
waypoints = list()
j = 1
while j <= 5:
# random pose
rand_pose = arm.get_random_pose(arm.get_end_effector_link())
pose.position.x = rand_pose.pose.position.x
pose.position.y = rand_pose.pose.position.y
pose.position.z = rand_pose.pose.position.z
waypoints.append(copy.deepcopy(pose))
j += 1
(plan, fraction) = arm.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
print "====== waypoints created ======="
# print waypoints
# ======= show cartesian path ====== #
arm.go()
rospy.sleep(10)
i += 1
print "========= end ========="
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
# Initialize the move group for the right arm
right_arm = MoveGroupCommander('right_arm')
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Allow some leeway in position(meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.02)
right_arm.set_goal_orientation_tolerance(0.1)
# Start the arm in the "resting" pose stored in the SRDF file
right_arm.set_named_target('resting')
right_arm.go()
# Move to the named "straight_forward" pose stored in the SRDF file
right_arm.set_named_target('straight_forward')
right_arm.go()
rospy.sleep(2)
# Move back to the resting position at roughly 1/4 speed
right_arm.set_named_target('resting')
# Get back the planned trajectory
traj = right_arm.plan()
# Scale the trajectory speed by a factor of 0.25
new_traj = scale_trajectory_speed(traj, 0.25)
# Execute the new trajectory
right_arm.execute(new_traj)
rospy.sleep(1)
# Exit MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
def main():
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_quadrotor', anonymous=True)
q = Quadrotor(display=False, debug=True)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
p = q.get_current_pose()
p.x += random.uniform(-2.5, 2.5)
p.y += random.uniform(-2.5, 2.5)
p.z += random.uniform(-2.5, 2.5)
p.yaw += random.uniform(angles.d2r(-90.0), angles.d2r(90.0))
p.z = max(p.z, 0.5)
q.moveto_pose(p, wait=True)
rate.sleep()
moveit_commander.roscpp_shutdown()
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('cobra_move_position', anonymous=True)
arm = MoveGroupCommander(ARM_GROUP)
arm.allow_replanning(True)
arm.set_planning_time(10)
arm.set_named_target(START_POSITION)
arm.go()
rospy.sleep(2)
print "======== create new goal position ========"
start_pose = PoseStamped()
start_pose.header.frame_id = arm.get_planning_frame()
# Test Position
start_pose.pose.position.x = 0.2 # 20 cm
start_pose.pose.position.y = -0.11 # -11 cm
start_pose.pose.position.z = 0.6 # 60 cm
q = quaternion_from_euler(0.0, 0.0, 0.0)
start_pose.pose.orientation = Quaternion(*q)
print start_pose
print "====== move to position ======="
# KDL
# arm.set_joint_value_target(start_pose, True)
# IK Fast
arm.set_position_target([start_pose.pose.position.x, start_pose.pose.position.y, start_pose.pose.position.z],
arm.get_end_effector_link())
arm.go()
rospy.sleep(2)
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def FindBlock(self):
print "==============="
print "FIND BLOCK"
print "==============="
#Get information about the markers found
rospy.Subscriber("/demo/found_markers",Int16MultiArray, callback1)
msg1 = rospy.wait_for_message('/demo/found_markers',Pose)
print msg1.data
detectedMarkers = msg1.data
chosenMarker = self.GetTargetBlock(detectedMarkers)
if chosenMarker != -1:
try:
msg = Pose()
position = Vector3()
orientation = Quaternion()
targetMarker = 'ar_marker_'+str(chosenMarker)
print "the marker frame chosen", targetMarker
(trans,rot) = self.listener.lookupTransform('/base',targetMarker,rospy.Time(0))
Px = trans[0]
Py = trans[1]
Pz = -0.171 #this value dependent on height of table
Qx = 0.0
Qy = 1.0
Qz = 0.0
Qw = 0.0
# Return the position/orientation of the target
self.Block_Position = [Px,Py,Pz]
self.Block_Quaternion = [Qx,Qy,Qz,Qw]
print self.Block_Position
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
pass
else:
user_input = raw_input('No blocks left. Please give me more blocks. Press ''Enter'' when ready!!!')
if user_input == 's':
moveit_commander.roscpp_shutdown()
else:
self.FindBlock()
def move_group_python_interface():
print "============ Starting move_group_python_interface setup"
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_group_python_interface', 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.
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.
rospy.sleep(1)
group = moveit_commander.MoveGroupCommander("arm")
## 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 "============"
rospy.spin()
moveit_commander.roscpp_shutdown()
def launch_script():
#======================================INVERSE LEG=====================================================
#print("====================================================")
#print("Kill node l_leg_ik")
#t = time.time()-t_start
#print(t)
#print("====================================================")
#os.system('rosnode kill l_leg_ik')
#print("====================================================")
#print("Killed in")
#print(time.time()-t_start-t)
#print("seconds")
#print("====================================================")
print "============ SPINNING NOW ========================"
moveit_commander.roscpp_shutdown()
rospy.spin()
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_ik_demo')
# 初始化需要使用move group控制的机械臂中的arm group
arm = moveit_commander.MoveGroupCommander('manipulator')
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
# 设置目标位置所使用的参考坐标系
reference_frame = 'base_link'
arm.set_pose_reference_frame(reference_frame)
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.001)
arm.set_goal_orientation_tolerance(0.01)
# 设置允许的最大速度和加速度
arm.set_max_acceleration_scaling_factor(0.5)
arm.set_max_velocity_scaling_factor(0.5)
# 控制机械臂先回到初始化位置
arm.set_named_target('home')
arm.go()
rospy.sleep(1)
# 设置机械臂工作空间中的目标位姿，位置使用x、y、z坐标描述，
# 姿态使用四元数描述，基于base_link坐标系
target_pose = PoseStamped()
target_pose.header.frame_id = reference_frame
target_pose.header.stamp = rospy.Time.now()
target_pose.pose.position.x = 0.2593
target_pose.pose.position.y = 0.0636
target_pose.pose.position.z = 0.1787
target_pose.pose.orientation.x = 0.70692
target_pose.pose.orientation.y = 0.0
target_pose.pose.orientation.z = 0.0
target_pose.pose.orientation.w = 0.70729
# 设置机器臂当前的状态作为运动初始状态
arm.set_start_state_to_current_state()
# 设置机械臂终端运动的目标位姿
arm.set_pose_target(target_pose, end_effector_link)
# 规划运动路径
traj = arm.plan()
# 按照规划的运动路径控制机械臂运动
arm.execute(traj)
rospy.sleep(1)
# 控制机械臂终端向右移动5cm
# arm.shift_pose_target(1, 0.1, end_effector_link)
# arm.go()
# rospy.sleep(1)
# 控制机械臂终端反向旋转90度
# arm.shift_pose_target(3, -1.57, end_effector_link)
# arm.go()
# rospy.sleep(1)
# 控制机械臂回到初始化位置
arm.set_named_target('home')
arm.go()
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
p = PoseStamped()
p.header.frame_id = acHan.planning_frame()
p.pose.position.x = 0.5
p.pose.position.y = 0.0
p.pose.position.z = 0.0
p.pose.orientation.w = 1.0
#size = [0.1,0.1,0.1]
#scene.remove_world_object("1")
#x = scene.add_box("1", p, size)
pos = acHan.current_jointState()
print(pos)
pos.position[0] = pos.position[0] + 0.5
acHan.Transport_Empty(4, p)
rospy.signal_shutdown("Done")
os._exit(0)
roscpp_shutdown()
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_dem')
# Initialize the move group for the right arm
right_arm = moveit_commander.MoveGroupCommander('arm')
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Set the reference frame for pose targets
reference_frame = 'base_footprint'
# Set the right arm reference frame accordingly
right_arm.set_pose_reference_frame(reference_frame)
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Allow some leeway in position (meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.01)
right_arm.set_goal_orientation_tolerance(0.05)
# Start the arm in the "resting" pose stored in the SRDF file
right_arm.set_named_target('straight_forward')
right_arm.go()
rospy.sleep(2)
# Set the target pose. This particular pose has the gripper oriented horizontally
# 0.85 meters above the ground, 0.10 meters to the right and 0.20 meters ahead of
# the center of the robot base.
target_pose = PoseStamped()
target_pose.header.frame_id = reference_frame
target_pose.header.stamp = rospy.Time.now()
target_pose.pose.position.x = 0.10
target_pose.pose.position.y = 0.10
target_pose.pose.position.z = 0.85
target_pose.pose.orientation.x = 0.0
target_pose.pose.orientation.y = 0.0
target_pose.pose.orientation.z = 0.0
target_pose.pose.orientation.w = 1.0
# Set the start state to the current state
right_arm.set_start_state_to_current_state()
# Set the goal pose of the end effector to the stored pose
right_arm.set_pose_target(target_pose, end_effector_link)
# Plan the trajectory to the goal
traj = right_arm.plan()
# Execute the planned trajectory
right_arm.execute(traj)
# Pause for a second
rospy.sleep(1)
# Shift the end-effector to the right 5cm
right_arm.shift_pose_target(1, -0.05, end_effector_link)
right_arm.go()
rospy.sleep(1)
# Rotate the end-effector 90 degrees
right_arm.shift_pose_target(3, -1.57, end_effector_link)
right_arm.go()
rospy.sleep(1)
# Store this pose as the new target_pose
saved_target_pose = right_arm.get_current_pose(end_effector_link)
# Move to the named pose "wave"
#right_arm.set_named_target('wave')
#right_arm.go()
#rospy.sleep(1)
# Go back to the stored target
#right_arm.set_pose_target(saved_target_pose, end_effector_link)
#right_arm.go()
#rospy.sleep(1)
# Finish up in the resting position
#right_arm.set_named_target('resting')
#right_arm.go()
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit MoveIt
moveit_commander.os._exit(0)
def close(self):
print("closing GazeboSmartBotPincherKinectEnv")
# When finished, shut down moveit_commander.
moveit_commander.roscpp_shutdown()
super(GazeboSmartBotPincherKinectEnv, self).close()
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():
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the move group for the ur5_arm
arm = moveit_commander.MoveGroupCommander("ur5_arm")
try:
cjs = rospy.get_param('current_joint_state')
except:
cjs = [0, 0, 0, 0, 0, 0]
jt = RobotState()
jt.joint_state.header.frame_id = '/base_link'
jt.joint_state.name = [
'front_left_wheel', 'front_right_wheel', 'rear_left_wheel',
'rear_right_wheel', 'ur5_arm_shoulder_pan_joint',
'ur5_arm_shoulder_lift_joint', 'ur5_arm_elbow_joint',
'ur5_arm_wrist_1_joint', 'ur5_arm_wrist_2_joint',
'ur5_arm_wrist_3_joint', 'left_tip_hinge', 'right_tip_hinge'
]
jt.joint_state.position = [
0, 0, 0, 0, cjs[0], cjs[1], cjs[2], cjs[3], cjs[4], cjs[5], 0, 0
]
# Set the start state to the current state
arm.set_start_state(jt)
arm.set_joint_value_target('ur5_arm_shoulder_pan_joint', 1.57)
arm.set_joint_value_target('ur5_arm_shoulder_lift_joint', 1.57)
arm.set_joint_value_target('ur5_arm_elbow_joint', -1.57)
arm.set_joint_value_target('ur5_arm_wrist_1_joint', 1.57)
arm.set_joint_value_target('ur5_arm_wrist_2_joint', cjs[4])
arm.set_joint_value_target('ur5_arm_wrist_3_joint', cjs[5]) #3.14159)
traj = arm.plan()
arm.execute(traj)
rospy.sleep(5.0)
# Stop any current arm movement
arm.stop()
pub = rospy.Publisher('gripper/cmd_vel', Twist, queue_size=1)
twist = Twist()
speed = .5
turn = 1
x = 0
y = 0
z = 0
th = 1 # To open gripper (1) use th = 1
twist.linear.x = x * speed
twist.linear.y = y * speed
twist.linear.z = z * speed
twist.angular.x = 0
twist.angular.y = 0
twist.angular.z = th * turn
ct = 0
rest_time = 0.1
tot_time = 1
while ct * rest_time < tot_time:
pub.publish(twist)
rospy.sleep(0.1)
ct = ct + 1
#rospy.set_param('smach_state','turnedValve')
#Shut down MoveIt! cleanly
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_cartesian_demo', anonymous=True)
# 是否需要使用笛卡尔空间的运动规划
cartesian = True
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('left_manipulator')
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 设置目标位置所使用的参考坐标系
arm.set_pose_reference_frame('leftbase_link')
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.01)
arm.set_goal_orientation_tolerance(0.1)
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
# 获取当前位姿数据最为机械臂运动的起始位姿
start_pose = arm.get_current_pose(end_effector_link).pose
# 初始化路点列表
waypoints = []
# 将初始位姿加入路点列表
if cartesian:
waypoints.append(start_pose)
# 设置第二个路点数据，并加入路点列表
# 第二个路点需要向后运动0.2米，向右运动0.2米
wpose = deepcopy(start_pose)
wpose.position.x -= 0.2
wpose.position.y -= 0.2
if cartesian:
waypoints.append(deepcopy(wpose))
else:
arm.set_pose_target(wpose)
arm.go()
rospy.sleep(1)
# 设置第三个路点数据，并加入路点列表
wpose.position.x += 0.05
wpose.position.y += 0.15
wpose.position.z -= 0.15
if cartesian:
waypoints.append(deepcopy(wpose))
else:
arm.set_pose_target(wpose)
arm.go()
rospy.sleep(1)
# 设置第四个路点数据，回到初始位置，并加入路点列表
if cartesian:
waypoints.append(deepcopy(start_pose))
else:
arm.set_pose_target(start_pose)
arm.go()
rospy.sleep(1)
if cartesian:
fraction = 0.0 #路径规划覆盖率
maxtries = 100 #最大尝试规划次数
attempts = 0 #已经尝试规划次数
# 设置机器臂当前的状态作为运动初始状态
arm.set_start_state_to_current_state()
# 尝试规划一条笛卡尔空间下的路径，依次通过所有路点
while fraction < 1.0 and attempts < maxtries:
(plan, fraction) = arm.compute_cartesian_path(
waypoints, # waypoint poses，路点列表
0.01, # eef_step，终端步进值
0.0, # jump_threshold，跳跃阈值
True) # avoid_collisions，避障规划
# 尝试次数累加
attempts += 1
# 打印运动规划进程
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) +
" attempts...")
# 如果路径规划成功（覆盖率100%）,则开始控制机械臂运动
if fraction == 1.0:
rospy.loginfo("Path computed successfully. Moving the arm.")
arm.execute(plan)
rospy.loginfo("Path execution complete.")
# 如果路径规划失败，则打印失败信息
else:
rospy.loginfo("Path planning failed with only " +
str(fraction) + " success after " +
str(maxtries) + " attempts.")
# # 控制机械臂回到初始化位置
# arm.set_named_target('home')
# arm.go()
# rospy.sleep(1)
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_pick_and_place_demo')
# 初始化场景对象
scene = PlanningSceneInterface()
# 创建一个发布场景变化信息的发布者
self.scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=10)
# 创建一个发布抓取姿态的发布者
self.gripper_pose_pub = rospy.Publisher('gripper_pose',
PoseStamped,
queue_size=10)
# 创建一个存储物体颜色的字典对象
self.colors = dict()
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander(GROUP_NAME_ARM)
# 初始化需要使用move group控制的机械臂中的gripper group
gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.05)
arm.set_goal_orientation_tolerance(0.1)
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 设置目标位置所使用的参考坐标系
arm.set_pose_reference_frame(REFERENCE_FRAME)
# 设置每次运动规划的时间限制：5s
arm.set_planning_time(5)
# 设置pick和place阶段的最大尝试次数
max_pick_attempts = 5
max_place_attempts = 5
rospy.sleep(2)
# 设置场景物体的名称
table_id = 'table'
target_id = 'target'
# 移除场景中之前运行残留的物体
scene.remove_world_object(table_id)
scene.remove_world_object(target_id)
# 移除场景中之前与机器臂绑定的物体
scene.remove_attached_object(GRIPPER_FRAME, target_id)
rospy.sleep(1)
# 控制机械臂和夹抓先回到初始化位置
arm.set_named_target('home')
arm.go()
rospy.sleep(1)
gripper.set_named_target('finger_home')
gripper.go()
rospy.sleep(3)
# 设置桌面的高度
table_ground = 0.1
# 设置table、box1和box2的三维尺寸[长, 宽, 高]
table_size = [1, 2, 0.01]
# 将三个物体加入场景当中
table_pose = PoseStamped()
table_pose.header.frame_id = REFERENCE_FRAME
table_pose.pose.position.x = 1.5
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = table_ground + table_size[2] / 2.0
table_pose.pose.orientation.w = 1.0
scene.add_box(table_id, table_pose, table_size)
# 将桌子设置成红色，两个box设置成橙色
self.setColor(table_id, 0.8, 0, 0, 1.0)
# 设置目标物体的尺寸
target_size = [0.5, 0.05, 0.2]
# 设置目标物体的位置，位于桌面之上两个盒子之间
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 1.5
target_pose.pose.position.y = 0.0
target_pose.pose.position.z = table_ground + table_size[
2] + target_size[2] / 2.0
target_pose.pose.orientation.w = 1.0
# 将抓取的目标物体加入场景中
scene.add_box(target_id, target_pose, target_size)
# 将目标物体设置为黄色
self.setColor(target_id, 0.9, 0.9, 0, 1.0)
# 将场景中的颜色设置发布
self.sendColors()
# 设置支持的外观
arm.set_support_surface_name(table_id)
# 设置一个place阶段需要放置物体的目标位置
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = 1.5
place_pose.pose.position.y = 0.3
place_pose.pose.position.z = table_ground + table_size[
2] + target_size[2] / 2.0
place_pose.pose.orientation.w = 1.0
# 将目标位置设置为机器人的抓取目标位置
grasp_pose = target_pose
# 生成抓取姿态
grasps = self.make_grasps(grasp_pose, [target_id])
# 将抓取姿态发布，可以在rviz中显示
for grasp in grasps:
self.gripper_pose_pub.publish(grasp.grasp_pose)
rospy.sleep(0.2)
# 追踪抓取成功与否，以及抓取的尝试次数
result = None
n_attempts = 0
# 重复尝试抓取，直道成功或者超多最大尝试次数
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
n_attempts += 1
rospy.loginfo("Pick attempt: " + str(n_attempts))
result = arm.pick(target_id, grasps)
rospy.sleep(0.2)
# 如果pick成功，则进入place阶段
if result == MoveItErrorCodes.SUCCESS:
result = None
n_attempts = 0
# 生成放置姿态
places = self.make_places(place_pose)
# 重复尝试放置，直道成功或者超多最大尝试次数
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts:
n_attempts += 1
rospy.loginfo("Place attempt: " + str(n_attempts))
for place in places:
result = arm.place(target_id, place)
if result == MoveItErrorCodes.SUCCESS:
break
rospy.sleep(0.2)
if result != MoveItErrorCodes.SUCCESS:
rospy.loginfo("Place operation failed after " +
str(n_attempts) + " attempts.")
else:
rospy.loginfo("Pick operation failed after " + str(n_attempts) +
" attempts.")
# 控制机械臂回到初始化位置
arm.set_named_target('home')
arm.go()
# 控制夹爪回到张开的状态
# gripper.set_joint_value_target(GRIPPER_OPEN)
gripper.set_named_target('finger_home')
gripper.go()
rospy.sleep(1)
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def stop_ros(reason):
rospy.signal_shutdown(reason)
roscpp_shutdown()
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
sim = False; # for RPi2
# sim = True;
rospy.init_node('moveit_demo')
# Initialize the move group for the right arm
arm1 = moveit_commander.MoveGroupCommander('arm1')
# Get the name of the end-effector link
# end_effector_link = arm1.get_end_effector_link()
arm1.set_end_effector_link('link6')
end_effector_link = arm1.get_end_effector_link()
# Set the reference frame for pose targets
reference_frame = 'base_link'
# reference_frame = 'link5'
# Set the right arm reference frame accordingly
arm1.set_pose_reference_frame(reference_frame)
# Allow replanning to increase the odds of a solution
arm1.allow_replanning(False)
# Allow some leeway in position (meters) and orientation (radians)
arm1.set_goal_position_tolerance(0.01)
arm1.set_goal_orientation_tolerance(0.05)
# Start the arm in the "init" pose stored in the SRDF file
arm1.set_named_target('init')
arm1.go()
if sim:
rospy.sleep(2)
# # Set the target pose. This particular pose has the gripper oriented horizontally
# # 0.85 meters above the ground, 0.10 meters to the right and 0.20 meters ahead of
# # the center of the robot base.
# target_pose = PoseStamped()
# target_pose.header.frame_id = reference_frame
# target_pose.header.stamp = rospy.Time.now()
# target_pose.pose.position.x = 0.20
# target_pose.pose.position.y = 0.3
# target_pose.pose.position.z = 0.55
# target_pose.pose.orientation.x = 0.0
# target_pose.pose.orientation.y = 0.0
# target_pose.pose.orientation.z = 0.0
# target_pose.pose.orientation.w = 1.0
# Set the start state to the current state
arm1.set_start_state_to_current_state()
# Set the goal pose of the end effector to the stored pose
# arm1.set_pose_target(target_pose, end_effector_link)
# Shift the end-effector to the right 5cm
arm1.shift_pose_target(0, -0.1, end_effector_link) # roll
# Plan the trajectory to the goal
traj = arm1.plan()
# Execute the planned trajectory
arm1.execute(traj)
if sim:
rospy.sleep(2)
# Shift (3/Roll) the end-effector to the left 5cm
arm1.shift_pose_target(0, 0.1, end_effector_link)
# Plan the trajectory to the goal
traj = arm1.plan()
# Execute the planned trajectory
arm1.execute(traj)
if sim:
rospy.sleep(2)
# # Shift the end-effector to the right 5cm
# arm1.shift_pose_target(1, -0.05, end_effector_link)
# arm1.go()
# if sim:
# rospy.sleep(2)
# # Rotate the end-effector 90 degrees
# arm1.shift_pose_target(3, -1.57, end_effector_link)
# arm1.go()
# rospy.sleep(1)
#
# # Store this pose as the new target_pose
# saved_target_pose = arm1.get_current_pose(end_effector_link)
#
# # Move to the named pose "vertical"
# arm1.set_named_target('vertical')
# arm1.go()
# rospy.sleep(1)
#
# # Go back to the stored target
# arm1.set_pose_target(saved_target_pose, end_effector_link)
# arm1.go()
# rospy.sleep(1)
# # Finish up in the init position
# arm1.set_named_target('init')
# arm1.go()
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit MoveIt
moveit_commander.os._exit(0)
def move_joints():
print "============ Starting tutorial setup"
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_group_python_interface_tutorial', anonymous=True)
# instantiate robot commander object
robot = moveit_commander.RobotCommander()
# instantiate planning scene interface object
scene = moveit_commander.PlanningSceneInterface()
# instantiate arm move group commanders
left = moveit_commander.MoveGroupCommander("left_arm")
right = moveit_commander.MoveGroupCommander("right_arm")
# publishes trajectory to visualize
display_trajectory_publisher = rospy.Publisher(
'/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
# Gather basic information
print "============ Reference frame left: %s" % left.get_planning_frame()
print "============ Reference frame right: %s" % right.get_planning_frame()
print "============ End effector left: %s" % left.get_end_effector_link()
print "============ End effector right: %s" % right.get_end_effector_link()
print "============ Robot Groups:"
print robot.get_group_names()
print "============ Printing robot state"
print robot.get_current_state()
print "============"
print "============ Printing robot variables"
print robot.get_current_variable_values()
print "============"
print "============ Printing left hand values"
print "%s" % left.get_end_effector_link()
# current set of left arm joint values
#group_variable_values = left.get_current_joint_values()
#print "============ Joint values left: %s" % group_variable_values
#group_variable_values[0] = 1.0
d = {
"right_s0": 0.0,
"right_s1": 0.0,
"right_e0": 0.0,
"right_e1": 0.0,
"right_w0": 0.0,
"right_w1": 0.0,
"right_w2": 0.0
}
right.set_joint_value_target(d)
plan = left.plan()
print "============ Waiting while RVIZ displays plan2..."
rospy.sleep(5)
print "============ Printing robot variables"
print robot.get_current_variable_values()
print "============"
#left.execute(plan2)
left.clear_pose_targets()
moveit_commander.roscpp_shutdown()
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
# We need a tf2 listener to convert poses into arm reference base
try:
self._tf2_buff = tf2_ros.Buffer()
self._tf2_list = tf2_ros.TransformListener(self._tf2_buff)
except rospy.ROSException as err:
rospy.logerr("MoveItDemo: could not start tf buffer client: " + str(err))
raise err
self.gripper_opened = [rospy.get_param(GRIPPER_PARAM + "/max_opening") - 0.01]
self.gripper_closed = [rospy.get_param(GRIPPER_PARAM + "/min_opening") + 0.01]
self.gripper_neutral = [rospy.get_param(GRIPPER_PARAM + "/neutral",
(self.gripper_opened[0] + self.gripper_closed[0])/2.0) ]
self.gripper_tighten = rospy.get_param(GRIPPER_PARAM + "/tighten", 0.0)
# Use the planning scene object to add or remove objects
self.scene = PlanningSceneInterface(REFERENCE_FRAME)
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('target_pose', PoseStamped, queue_size=10)
# Create a dictionary to hold object colors
self.colors = dict()
# Initialize the move group for the right arm
arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Get the name of the end-effector link
end_effector_link = arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
arm.set_goal_position_tolerance(0.05)
arm.set_goal_orientation_tolerance(0.1)
# Allow replanning to increase the odds of a solution
arm.allow_replanning(True)
# Set the right arm reference frame
arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow 5 seconds per planning attempt
arm.set_planning_time(15)
# Set a limit on the number of pick attempts before bailing
max_pick_attempts = 1
# Set a limit on the number of place attempts
max_place_attempts = 3
# Give the scene a chance to catch up
rospy.sleep(2)
rospy.loginfo("Connecting to basic_grasping_perception/find_objects...")
find_objects = actionlib.SimpleActionClient("basic_grasping_perception/find_objects", FindGraspableObjectsAction)
find_objects.wait_for_server()
rospy.loginfo("...connected")
rospy.sleep(1)
arm.set_named_target('right_up')
if arm.go() != True:
rospy.logwarn(" Go failed")
gripper.set_joint_value_target(self.gripper_opened)
if gripper.go() != True:
rospy.logwarn(" Go failed")
rospy.sleep(1)
# Initialize the grasping goal
goal = FindGraspableObjectsGoal()
goal.plan_grasps = False
find_objects.send_goal(goal)
find_objects.wait_for_result(rospy.Duration(5.0))
find_result = find_objects.get_result()
rospy.loginfo("Found %d objects" %len(find_result.objects))
for name in self.scene.getKnownCollisionObjects():
self.scene.removeCollisionObject(name, False)
for name in self.scene.getKnownAttachedObjects():
self.scene.removeAttachedObject(name, False)
self.scene.waitForSync()
self.scene._colors = dict()
# Use the nearest object on the table as the target
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_id = 'target'
target_size = None
the_object = None
the_object_dist = 0.30
the_object_dist_min = 0.10
count = -1
for obj in find_result.objects:
count +=1
dx = obj.object.primitive_poses[0].position.x
dy = obj.object.primitive_poses[0].position.y
d = math.sqrt((dx * dx) + (dy * dy))
if d < the_object_dist and d > the_object_dist_min:
#if dx > the_object_dist_xmin and dx < the_object_dist_xmax:
# if dy > the_object_dist_ymin and dy < the_object_dist_ymax:
rospy.loginfo("object is in the working zone")
the_object_dist = d
the_object = count
target_size = [0.02, 0.02, 0.05]
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = obj.object.primitive_poses[0].position.x + target_size[0] / 2.0
target_pose.pose.position.y = obj.object.primitive_poses[0].position.y
target_pose.pose.position.z = 0.04
target_pose.pose.orientation.x = 0.0
target_pose.pose.orientation.y = 0.0
target_pose.pose.orientation.z = 0.0
target_pose.pose.orientation.w = 1.0
# wait for the scene to sync
self.scene.waitForSync()
self.scene.setColor(target_id, 223.0/256.0, 90.0/256.0, 12.0/256.0)
self.scene.sendColors()
grasp_pose = target_pose
grasp_pose.pose.position.y -= target_size[1] / 2.0
grasp_pose.pose.position.x += target_size[0]
grasps = self.make_grasps(grasp_pose, [target_id], [target_size[1] - self.gripper_tighten])
# Track success/failure and number of attempts for pick operation
for grasp in grasps:
self.gripper_pose_pub.publish(grasp.grasp_pose)
rospy.sleep(0.2)
# Track success/failure and number of attempts for pick operation
result = None
n_attempts = 0
# Set the start state to the current state
arm.set_start_state_to_current_state()
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
result = arm.pick(target_id, grasps)
n_attempts += 1
rospy.loginfo("Pick attempt: " + str(n_attempts))
rospy.sleep(1.0)
else:
rospy.loginfo("object is not in the working zone")
rospy.sleep(1)
arm.set_named_target('right_up')
arm.go()
# Open the gripper to the neutral position
gripper.set_joint_value_target(self.gripper_neutral)
gripper.go()
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
def random_search():
global locations
global target
global flag
''' Default configuration for MoveIt'''
# Initializing the node representing the robot
moveit_commander.roscpp_initialize(sys.argv)
flag = False
rospy.init_node("random_search")
rospy.Subscriber("aruco_map/locations", Float32MultiArray,
handle_map_locations)
rospy.sleep(2)
# Provides information such as the robot's kinematic model and the robot's current joint states
robot = moveit_commander.RobotCommander()
# Provides a remote interface for getting, setting, and updating the robot's internal understanding of the
# surrounding world:
scene = moveit_commander.PlanningSceneInterface()
# Is an interface to a planning group (group of joints).Used to plan and execute motions
group_name = "fanuc_arm"
move_group = moveit_commander.MoveGroupCommander(group_name)
for i in range(0, 1):
if flag == True:
#val = random.randint(0,3) #print("Going to ArUco ID: %d"%randint(0,1))
#print val
#
target = geometry_msgs.msg.Pose()
target.position.x = locations[
0 + i] - 0.01 # Coordinate x of aruco maker ID 0
target.position.y = locations[
5 + i] + 0.01 # Coordinate y of aruco maker ID 0
target.position.z = locations[
10 + i] + 0.08 # Coordinate z of aruco maker ID 0
print(target.position.x)
print(target.position.y)
print(target.position.z)
target.orientation.x = 0.7071 # target_orientation.x
target.orientation.y = 0.7071 # target_orientation.y
target.orientation.z = 0 # target_orientation.z
target.orientation.w = 0 # target_orientation.w
#print target
move_group.set_pose_target(target)
print('\x1b[6;31;43m' + '>>> Fanuc ARC Mate 120iBe >>' +
'\x1b[0m' + '\x1b[6;30;43m' + " " +
'Approaching ArUco ID: %d ' % i + " " + '\x1b[0m')
plan = move_group.go(wait=True)
flag = False
joint_goal = move_group.get_current_joint_values()
joint_goal[0] = 0
joint_goal[1] = 0
joint_goal[2] = 0
joint_goal[3] = 0
joint_goal[4] = 0
joint_goal[5] = 0
# 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
plan2 = move_group.go(joint_goal, wait=True)
''' STAGE X : Shuting everything down'''
print('\x1b[6;31;43m' + '>>> Fanuc ARC Mate 120iBe >>' + '\x1b[0m' +
'\x1b[6;30;43m' + " " + 'Shuting Down...' + " " + '\x1b[0m')
move_group.stop()
flag = False
move_group.clear_pose_targets()
moveit_commander.roscpp_shutdown()
exit()
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_cartesian', anonymous=True)
cartesian = rospy.get_param('~cartesian', True)
#set cartesian parameters
ur5_manipulator = MoveGroupCommander('ur5_manipulator')
ur5_manipulator.allow_replanning(True)
ur5_manipulator.set_pose_reference_frame('base_link')
ur5_manipulator.set_goal_position_tolerance(0.01)
ur5_manipulator.set_goal_orientation_tolerance(0.1)
end_effector_link = ur5_manipulator.get_end_effector_link()
#set the ur5 initial pose
ur5_manipulator.set_named_target('ready')
ur5_manipulator.go()
#get the end effort information
start_pose = ur5_manipulator.get_current_pose(end_effector_link).pose
print("The first waypoint:")
print(start_pose)
#define waypoints
waypoints = []
waypoints.append(start_pose)
wpose = deepcopy(start_pose)
wpose.position.z -= 0.3
print("The second waypoint:")
print(wpose)
waypoints.append(deepcopy(wpose))
print(" ")
print(waypoints)
if cartesian:
fraction = 0.0
maxtries = 100
attempts = 0
while fraction < 1.0 and attempts < maxtries:
(plan, fraction) = ur5_manipulator.compute_cartesian_path(
waypoints, 0.01, 0.0, True)
attempts += 1
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) +
" attempts...")
if fraction == 1.0:
rospy.loginfo("Path computed successfully. Moving the arm.")
ur5_manipulator.execute(plan)
rospy.sleep(2)
rospy.loginfo("Path execution complete.")
else:
rospy.loginfo("Path planning failed with only " +
str(fraction) + " success after " +
str(maxtries) + " attempts.")
#ur5_manipulator.set_named_target('home')
#ur5_manipulator.go()
rospy.sleep(3)
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the ROS node
rospy.init_node('moveit_demo', anonymous=True)
GRIPPER_OPEN = [0.05]
GRIPPER_CLOSED = [-0.03]
GRIPPER_NEUTRAL = [0.01]
# Connect to the right_arm move group
right_arm = moveit_commander.MoveGroupCommander('right_arm')
# Connect to the right_gripper move group
right_gripper = moveit_commander.MoveGroupCommander('right_gripper')
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Display the name of the end_effector link
rospy.loginfo("The end effector link is: " + str(end_effector_link))
# Set a small tolerance on joint angles
right_arm.set_goal_joint_tolerance(0.001)
right_gripper.set_goal_joint_tolerance(0.001)
# Start the arm target in "resting" pose stored in the SRDF file
right_arm.set_named_target('resting')
# Plan a trajectory to the goal configuration
traj = right_arm.plan()
# Execute the planned trajectory
right_arm.execute(traj)
# Pause for a moment
rospy.sleep(1)
# Set the gripper target to neutal position using a joint value target
right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
# Plan and execute the gripper motion
right_gripper.go()
rospy.sleep(1)
# Set target joint values for the arm: joints are in the order they appear in
# the kinematic tree.
joint_positions = [-0.0867, -1.274, 0.02832, 0.0820, -1.273, -0.003]
# Set the arm's goal configuration to the be the joint positions
right_arm.set_joint_value_target(joint_positions)
# Plan and execute the motion
right_arm.go()
rospy.sleep(1)
# Save this configuration for later
right_arm.remember_joint_values('saved_config', joint_positions)
# Close the gripper as if picking something up
right_gripper.set_joint_value_target(GRIPPER_CLOSED)
right_gripper.go()
rospy.sleep(1)
# Set the arm target to the named "straight_out" pose stored in the SRDF file
right_arm.set_named_target('straight_forward')
# Plan and execute the motion
right_arm.go()
rospy.sleep(1)
# Set the goal configuration to the named configuration saved earlier
right_arm.set_named_target('saved_config')
# Plan and execute the motion
right_arm.go()
rospy.sleep(1)
# Open the gripper as if letting something go
right_gripper.set_joint_value_target(GRIPPER_OPEN)
right_gripper.go()
rospy.sleep(1)
# Return the arm to the named "resting" pose stored in the SRDF file
right_arm.set_named_target('resting')
right_arm.go()
rospy.sleep(1)
# Return the gripper target to neutral position
right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
right_gripper.go()
rospy.sleep(1)
# Cleanly shut down MoveIt
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
rosrun gazebo_ros spawn_model -file $(rospack find ur5_single_arm_tufts)/urdf/objects/block.urdf -urdf -x 0.5 -y -0.0 -z 0.77 -model block
"""
table_path = pack_path + os.sep + 'urdf' + os.sep + 'objects' + os.sep + 'table.urdf'
table_name = 'table'
table_pose = Pose(position=Point(
x=0.85, y=0.0,
z=0.70 + 0.03)) # increase z by 0.03 to make gripper reach block
table2_path = pack_path + os.sep + 'urdf' + os.sep + 'objects' + os.sep + 'table2.urdf'
table2 = 'table2'
table2_pose = Pose(position=Point(x=0.0, y=0.85, z=0.4 + 0.03))
block_path = pack_path + os.sep + 'urdf' + os.sep + 'objects' + os.sep + 'block.urdf'
block_name = 'block'
block_pose = Pose(position=Point(x=0.5, y=0.0, z=0.74 + 0.03))
dropbox_path = pack_path + os.sep + 'urdf' + os.sep + 'objects' + os.sep + 'dropbox.urdf'
dropbox = 'dropbox'
dropbox_pose = Pose(position=Point(x=0, y=-0.85, z=0.24 + 0.03))
delete_gazebo_model([table_name, table2, block_name, dropbox])
spawn_gazebo_model(table_path, table_name, table_pose)
spawn_gazebo_model(table2_path, table2, table2_pose)
spawn_gazebo_model(block_path, block_name, block_pose)
spawn_gazebo_model(dropbox_path, dropbox, dropbox_pose)
main()
roscpp_shutdown()
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the ROS node
rospy.init_node('moveit_demo', anonymous=True)
# Connect to the right_arm move group
right_arm = moveit_commander.MoveGroupCommander('arm')
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Display the name of the end_effector link
rospy.loginfo("The end effector link is: " + str(end_effector_link))
# Set a small tolerance on joint angles
right_arm.set_goal_tolerance(0.0001)
right_arm.set_named_target('right')
right_arm.go()
rospy.sleep(1)
start_pose = right_arm.get_current_pose().pose
start_pose.position.x = 0.4
start_pose.position.y = 0
#start_pose.position.z = 0.4
start_pose.orientation.x = 0.707106781186547
start_pose.orientation.y = 0
start_pose.orientation.z = 0
start_pose.orientation.w = 0.707106781186547
right_arm.set_pose_target(start_pose)
right_arm.go()
# Pause for a moment
rospy.sleep(1)
# joints_positions = [0.7, 0.2, 0.5, 1]
# right_arm.set_joint_value_target(joints_positions)
# right_arm.go()
# rospy.sleep(1)
star = [[-0.2853, 0.3925], [0.2853, 0.3925], [-0.1763, 0.0573],
[0, 0.6], [0.1763, 0.0573], [-0.2853, 0.3925]]
waypoints = []
wpose = deepcopy(start_pose)
waypoints.append(deepcopy(wpose))
end_pose = deepcopy(start_pose)
for i in range(6):
wpose.position.x = 0.4
wpose.position.y = star[i][0]
wpose.position.z = star[i][1]
waypoints.append(deepcopy(wpose))
'''
wpose.position.z -= 0.15
waypoints.append(deepcopy(wpose))
wpose.position.x = 0.4
wpose.position.y = -0.2853
wpose.position.z = 0.3925
waypoints.append(deepcopy(wpose))
'''
#waypoints.append(end_pose)
fraction = 0.0
maxtries = 100
attempts = 0
# Set the internal state to the current state
right_arm.set_start_state_to_current_state()
# Plan the Cartesian path connecting the waypoints
while fraction < 1.0 and attempts < maxtries:
(plan, fraction) = right_arm.compute_cartesian_path(
waypoints, # waypoint poses
0.01, # eef_step
0.0, # jump_threshold
True) # avoid_collisions
# Increment the number of attempts
attempts += 1
# Print out a progress message
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) +
" attempts...")
# If we have a complete plan, execute the trajectory
if fraction == 1.0:
rospy.loginfo("Path computed successfully. Moving the arm.")
right_arm.execute(plan)
rospy.loginfo("Path execution complete.")
else:
rospy.loginfo("Path planning failed with only " + str(fraction) +
" success after " + str(maxtries) + " attempts.")
'''
with open ("/home/jyk/Desktop/data.txt",'w') as f:
for point in plan.joint_trajectory.points:
f.write(str(point.positions).replace('(','').replace(')','').replace(',',' '))
f.write('\n')
'''
rospy.sleep(3)
# Return the arm to the named "resting" pose stored in the SRDF file
right_arm.set_named_target('right')
right_arm.go()
rospy.sleep(1)
# Cleanly shut down MoveIt
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
#!/usr/bin/env python
import sys, rospy, tf, moveit_commander, random
from geometry_msgs.msg import Pose, Point, Quaternion
orient = [Quaternion(*tf.transformations.quaternion_from_euler(3.14, -1.5, -1.57)),
Quaternion(*tf.transformations.quaternion_from_euler(3.14, -1.5, -1.57))]
pose = [Pose(Point( 0.5, -0.5, 1.3), orient[0]),
Pose(Point(-0.5, -0.5, 1.3), orient[1])]
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('r2_wave_arm',anonymous=True)
group = [moveit_commander.MoveGroupCommander("left_arm"),
moveit_commander.MoveGroupCommander("right_arm")]
# now, wave arms around randomly
while not rospy.is_shutdown():
pose[0].position.x = 0.5 + random.uniform(-0.1, 0.1)
pose[1].position.x = -0.5 + random.uniform(-0.1, 0.1)
for side in [0,1]:
pose[side].position.z = 1.5 + random.uniform(-0.1, 0.1)
group[side].set_pose_target(pose[side])
group[side].go(True)
moveit_commander.roscpp_shutdown()
def __init__(self):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('cw3_q1')
self.tf_buffer = tf2_ros.Buffer()
self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
# Robot contains the entire state of the robot (iiwa and shadow hand)
robot = moveit_commander.RobotCommander()
# We can get a list of all the groups in the robot
print('============ Robot Groups:')
print('{}\n\n'.format(robot.get_group_names()))
# Planning groups are used to control seperate aspects of the robot.
# iiwa_group controls the iiwa arm from the base link to the end effector
# hand group controls all of the joints of the hand.
self.iiwa_group = moveit_commander.MoveGroupCommander('hand_iiwa')
self.hand_group = moveit_commander.MoveGroupCommander('sr_hand')
# print('\n\nhand_iiwa Group Information')
# print('============ Reference frame: {}'.format(iiwa_group.get_planning_frame()))
# print('============ End effector: {}\n\n'.format(iiwa_group.get_end_effector_link()))
#
# print('sr_hand Group Information')
# print('============ Reference frame: {}'.format(hand_group.get_planning_frame()))
# print('============ End effector: {}'.format(hand_group.get_end_effector_link()))
rospy.sleep(1)
# The hand has a very strict controller and will occasionally say it as failed even though it has moved to the right
# position (just not in the right amount of time), ensure you check the achieved position.
# Objects in the scene can be found using the object tracker node which publishes on the topic
# '/recognized_objects_array' and to tf. The list of object names is found in the param '/object_list'
## =================================== Object Tracking =========================================================
objects = rospy.get_param('object_list')
print(objects)
all_object_pose = []
no_object = len(objects)
for i in range(0,no_object):
while True:
try:
object_pose = self.tf_buffer.lookup_transform('world', objects[i], rospy.Time.now())
rospy.sleep(1)
except (tf2_ros.ExtrapolationException,tf2_ros.LookupException):
continue
break
print('Found object {} [{}] at: \n{}'.format(i,objects[i],object_pose.transform))
# store the object pose to new message
all_object_pose.append(object_pose)
iiwa_pose = self.tf_buffer.lookup_transform('world','hand_iiwa_link_0',rospy.Time.now())
rospy.sleep(1)
print('Found iiwa start Pos at: \n{}'.format(iiwa_pose.transform))
## ===================================== end object tracking =================================================
# TF doesn't always return as transform even when the transform exists, try catching the execption, waiting a second
# and looking up the transform again.
# To grasp objects they must first be added to the allowed collision matrix so that the path planner knows to ignore
# the collision. You can do this using a service '/add_object_acm', they can also be removed from the acm after
# grasping to prevent any unintended collisions.
add_to_acm = rospy.ServiceProxy('/add_object_acm', ChangeCollisionObject)
remove_from_acm = rospy.ServiceProxy('/remove_object_acm', ChangeCollisionObject)
i = 1 # object no.
success = add_to_acm(objects[i])
print success
## open the hand
# units in radian
ff = [0,0,0,0]
mf = [0,0,0,0]
rf = [0,0,0,0]
# open [-0.6,1.2,0 ,0]
# grab [0.2 ,1.2,0.65,1]
th = [-0.6,1.2,0,0]
self.open_joint_values = ff+mf+rf+th
self.hand_group.set_joint_value_target(self.open_joint_values)
if self.hand_group.go() != True:
rospy.logwarn("Go failed")
# Retry
print self.hand_group.go()
checkPos = self.hand_group.get_current_joint_values()
print checkPos[-4:]
# move the iiwa just above object
pose = self.iiwa_group.get_current_pose(self.iiwa_group.get_end_effector_link())
obj_quat = (all_object_pose[i].transform.rotation.x,all_object_pose[i].transform.rotation.y,all_object_pose[i].transform.rotation.z,all_object_pose[i].transform.rotation.w)
euler = euler_from_quaternion(obj_quat)
new_quat = quaternion_from_euler(euler[0]+math.pi,euler[1],euler[2]+math.pi/2)
# print new_quat
pose.pose.position = all_object_pose[i].transform.translation
pose.pose.position.x -= 0.00 # 0.02 for object 0 and 2, 0 for object 1
pose.pose.position.y += 0.00 # 0.01 for object 0 and 2, 0 for object 1
pose.pose.position.z += 0.35 # 0.3 for object 0 and 2, 0.35 for object 1
pose.pose.orientation.x = new_quat[0]
pose.pose.orientation.y = new_quat[1]
pose.pose.orientation.z = new_quat[2]
pose.pose.orientation.w = new_quat[3]
self.iiwa_group.set_pose_target(pose)
if self.iiwa_group.go != True:
rospy.logwarn(" Arm failed")
print self.iiwa_group.go()
object_name = objects[i]+'__link_0'
ff = [0,0.6,0.5,0.35]#[0,0.55,0.4,0.35]
mf = [0,0.6,0.5,0.35]#[0,0.55,0.4,0.35]
rf = [0,0.6,0.5,0.35]#[0,0.55,0.4,0.35]
th = [0.22,1.20,0.65,1.1]
self.closed_joint_values = ff+mf+rf+th
target_pose_stamped = PoseStamped()
target_pose_stamped.header.frame_id = 'world'
target_pose_stamped.pose = pose.pose
grasp_pose = target_pose_stamped
grasps = self.make_grasps(grasp_pose, object_name)
print grasps
result = self.hand_group.pick(object_name,grasps)
print result
# # hand_group.set_named_target('open')
# # hand_group.set_named_target('fingers_pack_thumb_open')
#
# # plan = hand_group.plan()
#
# # hand_group.set_named_target('open_grasp')
# # hand_group.set_named_target('fingers_pack_thumb_open')
#
# ## grasping
#
# # move three fingers
# # units in radian
# ff = [0,0.6,0.5,0.35]#[0,0.55,0.4,0.35]
# mf = [0,0.6,0.5,0.35]#[0,0.55,0.4,0.35]
# rf = [0,0.6,0.5,0.35]#[0,0.55,0.4,0.35]
# th = [0.22,1.20,0.65,1.1]
# joint_values = ff+mf+rf+th
# # hand_group.pick(object_name,joint_values)
# plan = hand_group.plan(joint_values)
# # print('Plan result: {}\n\n'.format(plan))
# hand_group.execute(plan, wait=True)
# # # rospy.sleep(10)
# #
# # checkPos = hand_group.get_current_joint_values()
# # print checkPos[-4:]
#
# ## attach object
# object_name = objects[i]+'__link_0'
# hand_group.attach_object(object_name,'lh_palm')
#
# ## Lifting
# pose = iiwa_group.get_current_pose(iiwa_group.get_end_effector_link())
# pose.pose.position.z += 0.2
#
# # iiwa_group.clear_pose_targets()
# print('iiwa planning to pose: {}'.format(pose.pose))
# iiwa_group.set_pose_target(pose)
# result = iiwa_group.plan()
# # print('Plan result: {}'.format(result))
# # rospy.sleep(5)
# iiwa_group.execute(result, wait=True)
# rospy.sleep(10)
#
# # Put it back to original position
# pose = iiwa_group.get_current_pose(iiwa_group.get_end_effector_link())
# pose.pose.position.z -= 0.2
#
# # iiwa_group.clear_pose_targets()
# print('iiwa planning to pose: {}'.format(pose.pose))
# iiwa_group.set_pose_target(pose)
# result = iiwa_group.plan()
# # # print('Plan result: {}'.format(result))
# # # rospy.sleep(5)
# iiwa_group.execute(result, wait=True)
# # check the object position
# for i in range(0,no_object):
# while True:
# try:
# object_pose = tf_buffer.lookup_transform('world', objects[i], rospy.Time.now())
# rospy.sleep(1)
# except (tf2_ros.ExtrapolationException,tf2_ros.LookupException):
# continue
# break
# print('Found object {} [{}] at: \n{}'.format(i,objects[i],object_pose.transform))
# # store the object pose to new message
# all_object_pose.append(object_pose)
success = remove_from_acm(objects[i])
moveit_commander.roscpp_shutdown()
def release(self):
#release planner
rospy.logwarn("Commander Shutdown")
moveit_commander.roscpp_shutdown()
def picknplace():
# Define positions.
pos1 = {
'left_e0': -1.4580487388850858,
'left_e1': 1.627553615946424,
'left_s0': 0.07669903939427068,
'left_s1': -0.3539660668045592,
'left_w0': -1.9155585088719105,
'left_w1': -1.4124128104454947,
'left_w2': -0.6438884357149024,
'right_e0': 1.7238109084167481,
'right_e1': 1.7169079948791506,
'right_s0': 0.36930587426147465,
'right_s1': -0.33249033539428713,
'right_w0': -1.2160632682067871,
'right_w1': 1.668587600115967,
'right_w2': -1.810097327636719
}
pos2 = {
'left_e0': -0.949534106616211,
'left_e1': 1.4994662184448244,
'left_s0': -0.6036214393432617,
'left_s1': -0.7869321432861328,
'left_w0': -2.4735440176391603,
'left_w1': -1.212228316241455,
'left_w2': -0.8690001153442384,
'right_e0': 1.8342575250183106,
'right_e1': 1.8668546167236328,
'right_s0': -0.45674277907104494,
'right_s1': -0.21667478604125978,
'right_w0': -1.2712865765075685,
'right_w1': 1.7472041154052735,
'right_w2': -2.4582042097778323
}
lpos1 = {
'left_e0': -1.4580487388850858,
'left_e1': 1.627553615946424,
'left_s0': 0.07669903939427068,
'left_s1': -0.3539660668045592,
'left_w0': -1.9155585088719105,
'left_w1': -1.4124128104454947,
'left_w2': -0.6438884357149024
}
placegoal = geometry_msgs.msg.Pose()
placegoal.position.x = 0.55
placegoal.position.y = 0.28
placegoal.position.z = 0
placegoal.orientation.x = 1.0
placegoal.orientation.y = 0.0
placegoal.orientation.z = 0.0
placegoal.orientation.w = 0.0
# Define variables.
offset_zero_point = 0.903
table_size_x = 0.714655654394
table_size_y = 1.05043717328
table_size_z = 0.729766045265
center_x = 0.457327827197
center_y = 0.145765166941
center_z = -0.538116977368
# The distance from the zero point in Moveit to the ground is 0.903 m.
# The value is not allways the same. (look in Readme)
center_z_cube = -offset_zero_point + table_size_z + 0.0275 / 2
pressure_ok = 0
j = 0
k = 0
start = 1
locs_x = []
# Initialize a list for the objects and the stacked cubes.
objlist = [
'obj01', 'obj02', 'obj03', 'obj04', 'obj05', 'obj06', 'obj07', 'obj08',
'obj09', 'obj10', 'obj11'
]
boxlist = [
'box01', 'box02', 'box03', 'box04', 'box05', 'box06', 'box07', 'box08',
'box09', 'box10', 'box11'
]
# Clear planning scene.
p.clear()
# Add table as attached object.
p.attachBox('table',
table_size_x,
table_size_y,
table_size_z,
center_x,
center_y,
center_z,
'base',
touch_links=['pedestal'])
p.waitForSync()
# Move both arms to start state.
both_arms.set_joint_value_target(pos1)
both_arms.plan()
both_arms.go(wait=True)
time.sleep(0.5)
# cProfile to measure the performance (time) of the task.
pr = cProfile.Profile()
pr.enable()
# Loop to continue pick and place until all objects are cleared from table.
while locs_x or start:
# Only for the start.
if start:
start = 0
# Receive the data from all objects from the topic "detected_objects".
temp = rospy.wait_for_message("detected_objects", PoseArray)
locs = temp.poses
locs_x = []
locs_y = []
orien = []
size = []
# Adds the data from the objects.
for i in range(len(locs)):
locs_x.append(locs[i].position.x)
locs_y.append(locs[i].position.y)
orien.append(locs[i].position.z * pi / 180)
size.append(locs[i].orientation.x)
# Filter objects list to remove multiple detected locations for same objects.
ind_rmv = []
for i in range(0, len(locs)):
if (locs_y[i] > 0.24 or locs_x[i] > 0.75):
ind_rmv.append(i)
continue
for j in range(i, len(locs)):
if not (i == j):
if sqrt((locs_x[i] - locs_x[j])**2 +
(locs_y[i] - locs_y[j])**2) < 0.018:
ind_rmv.append(i)
locs_x = del_meth(locs_x, ind_rmv)
locs_y = del_meth(locs_y, ind_rmv)
orien = del_meth(orien, ind_rmv)
size = del_meth(size, ind_rmv)
# Do the task only if there are still objects on the table.
if locs_x:
# Clear planning scene.
p.clear()
# Add table as attached object.
p.attachBox('table',
table_size_x,
table_size_y,
table_size_z,
center_x,
center_y,
center_z,
'base',
touch_links=['pedestal'])
# Sort objects based on size (largest first to smallest last). This was done to enable stacking large cubes.
ig0 = itemgetter(0)
sorted_lists = zip(*sorted(
zip(size, locs_x, locs_y, orien), reverse=True, key=ig0))
locs_x = list(sorted_lists[1])
locs_y = list(sorted_lists[2])
orien = list(sorted_lists[3])
size = list(sorted_lists[0])
# Initialize the data of the biggest object on the table.
xn = locs_x[0]
yn = locs_y[0]
zn = -0.16
thn = orien[0]
sz = size[0]
if thn > pi / 4:
thn = -1 * (thn % (pi / 4))
# Add the detected objects into the planning scene.
#for i in range(1,len(locs_x)):
#p.addBox(objlist[i], 0.05, 0.05, 0.0275, locs_x[i], locs_y[i], center_z_cube)
# Add the stacked objects as collision objects into the planning scene to avoid moving against them.
#for e in range(0, k):
#p.attachBox(boxlist[e], 0.05, 0.05, 0.0275, placegoal.position.x, placegoal.position.y, center_z_cube+0.0275*(e-1), 'base', touch_links=['cubes'])
if k > 0:
p.attachBox(boxlist[0],
0.07,
0.07,
0.0275 * k,
placegoal.position.x,
placegoal.position.y,
center_z_cube,
'base',
touch_links=['cubes'])
p.waitForSync()
# Move both arms to the second position.
both_arms.set_joint_value_target(pos2)
both_arms.plan()
both_arms.go(wait=True)
# Initialize the approach pickgoal (5 cm to pickgoal).
approach_pickgoal = geometry_msgs.msg.Pose()
approach_pickgoal.position.x = xn
approach_pickgoal.position.y = yn
approach_pickgoal.position.z = zn + 0.05
approach_pickgoal_dummy = PoseStamped()
approach_pickgoal_dummy.header.frame_id = "base"
approach_pickgoal_dummy.header.stamp = rospy.Time.now()
approach_pickgoal_dummy.pose.position.x = xn
approach_pickgoal_dummy.pose.position.y = yn
approach_pickgoal_dummy.pose.position.z = zn + 0.05
approach_pickgoal_dummy.pose.orientation.x = 1.0
approach_pickgoal_dummy.pose.orientation.y = 0.0
approach_pickgoal_dummy.pose.orientation.z = 0.0
approach_pickgoal_dummy.pose.orientation.w = 0.0
# Orientate the gripper --> uses function from geometry.py (by Mike Ferguson) to 'rotate a pose' given rpy angles.
approach_pickgoal_dummy.pose = rotate_pose_msg_by_euler_angles(
approach_pickgoal_dummy.pose, 0.0, 0.0, thn)
approach_pickgoal.orientation.x = approach_pickgoal_dummy.pose.orientation.x
approach_pickgoal.orientation.y = approach_pickgoal_dummy.pose.orientation.y
approach_pickgoal.orientation.z = approach_pickgoal_dummy.pose.orientation.z
approach_pickgoal.orientation.w = approach_pickgoal_dummy.pose.orientation.w
# Move to the approach goal and the pickgoal.
pickgoal = deepcopy(approach_pickgoal)
pickgoal.position.z = zn
move("left", approach_pickgoal, pickgoal)
time.sleep(0.5)
# Read the force in z direction.
b = leftarm.endpoint_effort()
z_ = b['force']
z_force = z_.z
print("----->", z_force)
# Search again for objects, if the gripper isn't at the right position and presses on an object.
#print("----->force in z direction:", z_force)
if z_force > -4:
leftgripper.close()
attempts = 0
pressure_ok = 1
# If the gripper hadn't enough pressure after 2 seconds it opens and search again for objects.
while (leftgripper.force() < 25 and pressure_ok == 1):
time.sleep(0.04)
attempts += 1
if (attempts > 50):
leftgripper.open()
pressure_ok = 0
print("----->pressure is to low<-----")
else:
print("----->gripper presses on an object<-----")
# Move back to the approach pickgoal.
pickgoal.position.z = zn + 0.05
move("left", pickgoal)
# Move to the second position.
both_arms.set_joint_value_target(pos1)
both_arms.plan()
both_arms.go(wait=True)
if pressure_ok and z_force > -4:
# Define the approach placegoal.
# Increase the height of the tower every time by 2.75 cm.
approached_placegoal = deepcopy(placegoal)
approached_placegoal.position.z = -0.145 + (k * 0.0275) + 0.08
# Define the placegoal.
placegoal.position.z = -0.145 + (k * 0.0275)
move("left", approached_placegoal, placegoal)
leftgripper.open()
while (leftgripper.force() > 10):
time.sleep(0.01)
# Move to the approach placegoal.
move("left", approached_placegoal)
k += 1
# Move left arm to start position.
left_arm.set_joint_value_target(lpos1)
left_arm.plan()
left_arm.go(wait=True)
pr.disable()
sortby = 'cumulative'
ps = pstats.Stats(pr).sort_stats(sortby).print_stats(0.0)
p.clear()
moveit_commander.roscpp_shutdown()
# Exit MoveIt.
moveit_commander.os._exit(0)
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node(NODE_NAME, anonymous=True)
self.pub = rospy.Publisher(ON_OFF_SIGNAL_TOPIC, Bool, queue_size=2)
self.rate = rospy.Rate(2)
# 是否需要使用笛卡尔运动规划
self.cartesian = rospy.get_param('~cartesian', True)
# 初始化需要使用move group控制的机械臂中的self.arm group
self.arm = MoveGroupCommander(ARM_GROUP)
# 设置目标位置所使用的参考坐标系
reference_frame = 'base_link'
self.arm.set_pose_reference_frame(REFERENCE_FRAME)
# 获取终端link的名称
end_effector_link = 'link_6'
# 当运动规划失败后，允许重新规划
self.arm.allow_replanning(True)
# 设置位置（单位：米）和姿态（单位：弧度）的允许误差
self.arm.set_goal_position_tolerance(0.01)
self.arm.set_goal_orientation_tolerance(0.1)
# 环境建模
scene = PlanningSceneInterface()
self.scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=5)
self.colors = dict()
rospy.sleep(1)
# 场景物体设置
table_id = 'table'
scene.remove_world_object(table_id)
#
table_groud = 0.5 - HEIGHT_OF_END_EFFECTOR
table_size = [0.5, 1.5, 0.04]
table_pose = PoseStamped()
table_pose.header.frame_id = REFERENCE_FRAME
table_pose.pose.position.x = 0.6
table_pose.pose.position.y = -0.5
table_pose.pose.position.z = table_groud - table_size[2] / 2.0 - 0.02
table_pose.pose.orientation.w = 1.0
scene.add_box(table_id, table_pose, table_size)
self.setColor(table_id, 0, 0.8, 0, 1.0)
self.sendColors()
# 第五轴的关节角度旋转，从水平到竖直
joint_positions = [0, 0, 0, 0, 1.571, 0]
self.arm.set_joint_value_target(joint_positions)
self.arm.go()
rospy.sleep(1)
# 全局变量，对比目标点位置
global forward_pose
forward_pose = Twist()
# 全局变量，计数
global item_num
item_num = 0
while not rospy.is_shutdown():
# 开启web端action通讯，等待讯号
self.server = actionlib.SimpleActionServer('abb_grasp',
AbbGraspAction,
self.abb_execute, False)
self.server.start()
rospy.spin()
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def launch_script():
## BEGIN_TUTORIAL
##
## Setup
## ^^^^^
## CALL_SUB_TUTORIAL imports
##
## First initialize moveit_commander and rospy.
print"Starting the script"
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('simple_script',
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
## leg. This interface can be used to plan and execute motions on the left
## leg.
lleg_group_single = moveit_commander.MoveGroupCommander("l_leg_single")
#rleg_group = moveit_commander.MoveGroupCommander("r_leg")
## 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)
## Wait for RVIZ to initialize. This sleep is ONLY to allow Rviz to come up.
print "============ Waiting for RVIZ to startup for 10 seconds"
rospy.sleep(10)
print "============ Starting script "
## Getting Basic Information
## ^^^^^^^^^^^^^^^^^^^^^^^^^
##
## We can get the name of the reference frame for this robot
print "\n =========== Some basic informations of Move Commander=================="
#print "Get active Link %s" % lleg_group_single.get_active_joints()
#print "Current Joint Values %s" % lleg_group_single.get_current_joint_values()
print "Current Pose %s" % lleg_group_single.get_current_pose()
current_pose = lleg_group_single.get_current_pose()
#current_pose.z = current_pose.z - 0.1
#print "Current RPY %s" % lleg_group_single.get_current_rpy()
#print "End effector Link %s" % lleg_group_single.get_end_effector_link()
#print "Setting end effector link to pelvis"
#lleg_group_single.set_end_effector_link("pelvis")
#print "End effector Link %s" % lleg_group_single.get_end_effector_link()
#print "New Pose: %s" % current_pose
#print "Get Joints %s" % lleg_group_single.get_joints()
#print "known_constraints %s" % lleg_group_single.get_known_constraints()
#print "get_name %s" % lleg_group_single.get_name()
#print "path_constraints %s" % lleg_group_single.get_path_constraints()
#print "planning_frame() %s" % lleg_group_single.get_planning_frame()
#print "planning_time %s" % lleg_group_single.get_planning_time()
#print "pose_reference_frame %s" % lleg_group_single.get_pose_reference_frame()
#print "random_joint_values %s" % lleg_group_single.get_random_joint_values()
#print "random_pose %s" % lleg_group_single.get_random_pose()
#print "variable_count %s" % lleg_group_single.get_variable_count()
#print "\n =========== Some basic informations of Robot Commander===================="
#print "current_variable_values %s" % robot.get_current_variable_values()
#print "group_names %s" % robot.get_group_names()
#print "get_joint_names %s" % robot.get_joint_names()
#print "get_link_names %s" % robot.get_link_names()
#print "get_planning_frame %s" % robot.get_planning_frame()
#print "get_root_link %s" % robot.get_root_link()
## Planning to a Pose goal
## ^^^^^^^^^^^^^^^^^^^^^^^
## We can plan a motion for this group to a desired pose for the
## end-effector
print "Going to current pose"
#pos_target = geometry_msgs.msg.Point()
#pose_target.orientation.w = 1.0
#pos_target.position.x = 0
#pos_target.position.y = 0
#pos_target.position.z = -0.5
#pos_target=[0,0,0.33]
#group.set_position_target(pos_target)
#pos_target=lleg_group_single.get_random_pose(end_effector_link="pelvis")
#print "Pos Target: %s" %pos_target
#lleg_group_single.set_planner_id("RRTConnectkConfigDefault")
pos_target = lleg_group_single.get_random_pose()
#pos_target = lleg_group_single.get_current_pose()
print "Random Pose: %s" % pos_target
#print "TEST %s" %pos_target.pose.position.x
#print "TEST %s" %pos_target.pose.position.y
#print "TEST %s" %pos_target.pose.position.z
#print "TEST %s" %pos_target.pose.orientation.x
#print "TEST %s" %pos_target.pose.orientation.y
#print "TEST %s" %pos_target.pose.orientation.z
#print "TEST %s" %pos_target.pose.orientation.w
print "Goal Joint Tolerance %s" % lleg_group_single.get_goal_joint_tolerance()
print "Goal Orientation Tolerance %s" % lleg_group_single.get_goal_orientation_tolerance()
print "Goal Position Tolerance %s" % lleg_group_single.get_goal_position_tolerance()
#lleg_group_single.set_goal_position_tolerance(0.1)
#lleg_group_single.set_goal_joint_tolerance(0.1)
#lleg_group_single.set_goal_orientation_tolerance(0.1)
#print "Goal Joint Tolerance %s" % lleg_group_single.get_goal_joint_tolerance()
#print "Goal Orientation Tolerance %s" % lleg_group_single.get_goal_orientation_tolerance()
#print "Goal Position Tolerance %s" % lleg_group_single.get_goal_position_tolerance()
#x = pos_target.pose.position.x
#y = pos_target.pose.position.y
#z = pos_target.pose.position.z
#working_pose = [0.183792065459, 0.0754540293222, 0.3066920494, -0.085881893673, 0.0963357114615, 0.492868689835, 0.860479044263]
#lleg_group_single.set_position_target([0,0,0.33],end_effector_link = "pelvis")
#lleg_group_single.set_pose_target(lleg_group_single.get_current_pose(),end_effector_link = "pelvis")
lleg_group_single.set_pose_target(pos_target,end_effector_link = "pelvis")
#lleg_group_single.set_pose_target(working_pose,end_effector_link = "pelvis")
#lleg_group_single.set_position_target(current_pose)
## 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 = lleg_group_single.plan()
#print "============ Waiting while RVIZ displays plan1..."
## 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(5)
print "Moving real robot now"
### 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
lleg_group_single.go(wait=True)
print "============ DONE"
### 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()
rospy.spin()
### END_TUTORIAL
print "============ STOPPING"
def __init__(self, robot_name="panda_arm", frame="panda_link0"):
try:
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node(name="pick_place_test")
self.scene = PlanningSceneInterface()
self.scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=10)
# region Robot initial
self.robot = MoveGroupCommander(robot_name)
self.robot.set_goal_joint_tolerance(0.00001)
self.robot.set_goal_position_tolerance(0.00001)
self.robot.set_goal_orientation_tolerance(0.01)
self.robot.set_goal_tolerance(0.00001)
self.robot.allow_replanning(True)
self.robot.set_pose_reference_frame(frame)
self.robot.set_planning_time(3)
# endregion
self.gripper = MoveGroupCommander("hand")
self.gripper.set_joint_value_target(GRIPPER_CLOSED)
self.gripper.go()
self.gripper.set_joint_value_target(GRIPPER_OPEN)
self.gripper.go()
self.gripper.set_joint_value_target(GRIPPER_CLOSED)
self.gripper.go()
# Robot go home
self.robot.set_named_target("home")
self.robot.go()
# clear all object in world
self.clear_all_object()
table_pose = un.Pose(0, 0, -10, 0, 0, 0)
table_color = un.Color(255, 255, 0, 100)
self.add_object_box("table", table_pose, table_color, frame,
(2000, 2000, 10))
bearing_pose = un.Pose(250, 250, 500, -90, 45, -90)
bearing_color = un.Color(255, 0, 255, 255)
bearing_file_name = "../stl/bearing.stl"
self.add_object_mesh("bearing", bearing_pose, bearing_color, frame,
bearing_file_name)
obpose = self.scene.get_object_poses(["bearing"])
# self.robot.set_support_surface_name("table")
g = Grasp()
# Create gripper position open or close
g.pre_grasp_posture = self.open_gripper()
g.grasp_posture = self.close_gripper()
g.pre_grasp_approach = self.make_gripper_translation(
0.01, 0.1, [0, 1.0, 0])
g.post_grasp_retreat = self.make_gripper_translation(
0.01, 0.9, [0, 1.0, 0])
p = PoseStamped()
p.header.frame_id = "panda_link0"
p.pose.orientation = obpose["bearing"].orientation
p.pose.position = obpose["bearing"].position
g.grasp_pose = p
g.allowed_touch_objects = ["bearing"]
a = []
a.append(g)
result = self.robot.pick(object_name="bearing", grasp=a)
print(result)
except Exception as ex:
print(ex)
moveit_commander.roscpp_shutdown()
moveit_commander.roscpp_shutdown()
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the ROS node
rospy.init_node('moveit_constraints_demo', anonymous=True)
robot = RobotCommander()
# Connect to the arm move group
arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Increase the planning time since constraint planning can take a while
arm.set_planning_time(5)
# Allow replanning to increase the odds of a solution
arm.allow_replanning(True)
# Set the right arm reference frame
arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow some leeway in position(meters) and orientation (radians)
arm.set_goal_position_tolerance(0.05)
arm.set_goal_orientation_tolerance(0.1)
# Get the name of the end-effector link
end_effector_link = arm.get_end_effector_link()
# Start in the "resting" configuration stored in the SRDF file
arm.set_named_target('l_arm_init')
# Plan and execute a trajectory to the goal configuration
arm.go()
rospy.sleep(1)
# Open the gripper
gripper.set_joint_value_target(GRIPPER_NEUTRAL)
gripper.go()
rospy.sleep(1)
# Set an initial target pose with the arm up and to the right
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.263803774718
target_pose.pose.position.y = 0.295405791959
target_pose.pose.position.z = 0.690438884208
q = quaternion_from_euler(0, 0, -1.57079633)
target_pose.pose.orientation.x = q[0]
target_pose.pose.orientation.y = q[1]
target_pose.pose.orientation.z = q[2]
target_pose.pose.orientation.w = q[3]
# Set the start state and target pose, then plan and execute
arm.set_start_state(robot.get_current_state())
arm.set_pose_target(target_pose, end_effector_link)
arm.go()
rospy.sleep(2)
# Close the gripper
gripper.set_joint_value_target(GRIPPER_CLOSED)
gripper.go()
rospy.sleep(1)
# Store the current pose
start_pose = arm.get_current_pose(end_effector_link)
# Create a contraints list and give it a name
constraints = Constraints()
constraints.name = "Keep gripper horizontal"
# Create an orientation constraint for the right gripper
orientation_constraint = OrientationConstraint()
orientation_constraint.header = start_pose.header
orientation_constraint.link_name = arm.get_end_effector_link()
orientation_constraint.orientation.w = 1.0
orientation_constraint.absolute_x_axis_tolerance = 0.1
orientation_constraint.absolute_y_axis_tolerance = 0.1
orientation_constraint.absolute_z_axis_tolerance = 0.1
orientation_constraint.weight = 1.0
# q = quaternion_from_euler(0, 0, -1.57079633)
# orientation_constraint.orientation.x = q[0]
# orientation_constraint.orientation.y = q[1]
# orientation_constraint.orientation.z = q[2]
# orientation_constraint.orientation.w = q[3]
# Append the constraint to the list of contraints
constraints.orientation_constraints.append(orientation_constraint)
# Set the path constraints on the arm
arm.set_path_constraints(constraints)
# Set a target pose for the arm
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.39000848183
target_pose.pose.position.y = 0.185900663329
target_pose.pose.position.z = 0.732752341378
target_pose.pose.orientation.w = 1
# Set the start state and target pose, then plan and execute
arm.set_start_state_to_current_state()
arm.set_pose_target(target_pose, end_effector_link)
arm.go()
rospy.sleep(1)
# Clear all path constraints
arm.clear_path_constraints()
# Open the gripper
gripper.set_joint_value_target(GRIPPER_NEUTRAL)
gripper.go()
rospy.sleep(1)
# Return to the "resting" configuration stored in the SRDF file
arm.set_named_target('l_arm_init')
# Plan and execute a trajectory to the goal configuration
arm.go()
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit MoveIt
moveit_commander.os._exit(0)
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("left_arm")
## 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)
## Wait for RVIZ to initialize. This sleep is ONLY to allow Rviz to come up.
print "============ Waiting for RVIZ..."
rospy.sleep(10)
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"
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)
## 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(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(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
# 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"
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
self.gripper_opened = [rospy.get_param(GRIPPER_PARAM + "/max_opening")]
self.gripper_closed = [rospy.get_param(GRIPPER_PARAM + "/min_opening")]
self.gripper_neutral = [rospy.get_param(GRIPPER_PARAM + "/neutral")]
self.gripper_tighten = rospy.get_param(GRIPPER_PARAM + "/tighten")
# We need a tf listener to convert poses into arm reference base
self.tf_listener = tf.TransformListener()
# Use the planning scene object to add or remove objects
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene',
PlanningScene,
queue_size=10)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('target_pose',
PoseStamped,
queue_size=10)
# Create a dictionary to hold object colors
self.colors = dict()
# Initialize the move group for the right arm
arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Get the name of the end-effector link
end_effector_link = arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
arm.set_goal_position_tolerance(0.04)
arm.set_goal_orientation_tolerance(0.1)
# Allow replanning to increase the odds of a solution
arm.allow_replanning(True)
# Set the right arm reference frame
arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow 5 seconds per planning attempt
arm.set_planning_time(5)
# Set a limit on the number of pick attempts before bailing
max_pick_attempts = 1
# Set a limit on the number of place attempts
max_place_attempts = 3
rospy.loginfo("Scaling for MoveIt timeout=" + str(
rospy.get_param(
'/move_group/trajectory_execution/allowed_execution_duration_scaling'
)))
# Give the scene a chance to catch up
rospy.sleep(2)
# Give each of the scene objects a unique name
table_id = 'table'
box1_id = 'box1'
box2_id = 'box2'
target_id = 'target'
tool_id = 'tool'
# Remove leftover objects from a previous run
scene.remove_world_object(table_id)
scene.remove_world_object(box1_id)
scene.remove_world_object(box2_id)
scene.remove_world_object(target_id)
scene.remove_world_object(tool_id)
# Remove any attached objects from a previous session
scene.remove_attached_object(GRIPPER_FRAME, target_id)
# Give the scene a chance to catch up
rospy.sleep(1)
# Start the arm in the "arm_up" pose stored in the SRDF file
rospy.loginfo("Set Arm: right_up")
arm.set_named_target('right_up')
if arm.go() != True:
rospy.logwarn(" Go failed")
rospy.sleep(2)
# Move the gripper to the closed position
rospy.loginfo("Set Gripper: Close " + str(self.gripper_closed))
gripper.set_joint_value_target(self.gripper_closed)
if gripper.go() != True:
rospy.logwarn(" Go failed")
rospy.sleep(2)
# Move the gripper to the neutral position
rospy.loginfo("Set Gripper: Neutral " + str(self.gripper_neutral))
gripper.set_joint_value_target(self.gripper_neutral)
if gripper.go() != True:
rospy.logwarn(" Go failed")
rospy.sleep(2)
# Move the gripper to the open position
rospy.loginfo("Set Gripper: Open " + str(self.gripper_opened))
gripper.set_joint_value_target(self.gripper_opened)
if gripper.go() != True:
rospy.logwarn(" Go failed")
rospy.sleep(2)
# Set the height of the table off the ground
table_ground = 0.34
# Set the dimensions of the scene objects [l, w, h]
table_size = [0.2, 0.7, 0.01]
box1_size = [0.1, 0.05, 0.05]
box2_size = [0.05, 0.05, 0.15]
# Set the target size [l, w, h]
target_size = [0.018, 0.018, 0.018]
# Add a table top and two boxes to the scene
table_pose = PoseStamped()
table_pose.header.frame_id = REFERENCE_FRAME
table_pose.pose.position.x = 0.36
table_pose.pose.position.y = 0.0
table_pose.pose.position.z = table_ground + table_size[2] / 2.0
table_pose.pose.orientation.w = 1.0
scene.add_box(table_id, table_pose, table_size)
#box1_pose = PoseStamped()
#box1_pose.header.frame_id = REFERENCE_FRAME
#box1_pose.pose.position.x = table_pose.pose.position.x - 0.04
#box1_pose.pose.position.y = 0.0
#box1_pose.pose.position.z = table_ground + table_size[2] + box1_size[2] / 2.0
#box1_pose.pose.orientation.w = 1.0
#scene.add_box(box1_id, box1_pose, box1_size)
#box2_pose = PoseStamped()
#box2_pose.header.frame_id = REFERENCE_FRAME
#box2_pose.pose.position.x = table_pose.pose.position.x - 0.06
#box2_pose.pose.position.y = 0.2
#box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0
#box2_pose.pose.orientation.w = 1.0
#scene.add_box(box2_id, box2_pose, box2_size)
# Set the target pose in between the boxes and on the table
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = table_pose.pose.position.x - 0.03
target_pose.pose.position.y = -0.1
target_pose.pose.position.z = table_ground + table_size[
2] + target_size[2] / 2.0
target_pose.pose.orientation.w = 1.0
# Add the target object to the scene
scene.add_box(target_id, target_pose, target_size)
# Make the table red and the boxes orange
self.setColor(table_id, 0.8, 0, 0, 1.0)
self.setColor(box1_id, 0.8, 0.4, 0, 1.0)
self.setColor(box2_id, 0.8, 0.4, 0, 1.0)
# Make the target yellow
self.setColor(target_id, 0.9, 0.9, 0, 1.0)
# Send the colors to the planning scene
self.sendColors()
# Set the support surface name to the table object
arm.set_support_surface_name(table_id)
# Specify a pose to place the target after being picked up
place_pose = PoseStamped()
place_pose.header.frame_id = REFERENCE_FRAME
place_pose.pose.position.x = table_pose.pose.position.x - 0.03
place_pose.pose.position.y = -0.15
place_pose.pose.position.z = table_ground + table_size[
2] + target_size[2] / 2.0
place_pose.pose.orientation.w = 1.0
# Initialize the grasp pose to the target pose
grasp_pose = target_pose
# Shift the grasp pose by half the width of the target to center it
grasp_pose.pose.position.y -= target_size[1] / 2.0
# Generate a list of grasps
grasps = self.make_grasps(grasp_pose, [target_id],
[target_size[1] - self.gripper_tighten])
# Track success/failure and number of attempts for pick operation
result = MoveItErrorCodes.FAILURE
n_attempts = 0
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
rospy.loginfo("Pick attempt #" + str(n_attempts))
for grasp in grasps:
# Publish the grasp poses so they can be viewed in RViz
self.gripper_pose_pub.publish(grasp.grasp_pose)
rospy.sleep(0.2)
result = arm.pick(target_id, grasps)
#result = MoveItErrorCodes.SUCCESS
#if result == MoveItErrorCodes.SUCCESS:
break
n_attempts += 1
rospy.sleep(0.2)
print 'second'
result = MoveItErrorCodes.SUCCESS
# If the pick was successful, attempt the place operation
if result == MoveItErrorCodes.SUCCESS:
rospy.loginfo(" Pick: Done!")
# Generate valid place poses
places = self.make_places(place_pose)
success = False
n_attempts = 0
# Repeat until we succeed or run out of attempts
while not success and n_attempts < max_place_attempts:
rospy.loginfo("Place attempt #" + str(n_attempts))
for place in places:
# Publish the place poses so they can be viewed in RViz
self.gripper_pose_pub.publish(place)
rospy.sleep(0.2)
success = arm.place(target_id, place)
#if success:
break
n_attempts += 1
rospy.sleep(0.2)
if not success:
rospy.logerr("Place operation failed after " +
str(n_attempts) + " attempts.")
else:
rospy.loginfo(" Place: Done!")
else:
rospy.logerr("Pick operation failed after " + str(n_attempts) +
" attempts.")
# Return the arm to the "resting" pose stored in the SRDF file (passing through right_up)
arm.set_named_target('right_up')
arm.go()
arm.set_named_target('resting')
arm.go()
# Open the gripper to the neutral position
gripper.set_joint_value_target(self.gripper_neutral)
gripper.go()
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node("moveit_demo")
# Set three basic gripper openings
GRIPPER_OPEN = [0.075]
GRIPPER_CLOSED = [-0.075]
GRIPPER_NEUTRAL = [0.04]
# Define Positions
XVALUE = input("Defina la primer posiciOn para x, \n")
YVALUE = input("Defina la primer posiciOn para Y, \n")
ZVALUE = input("Defina la primer posiciOn para Z, \n")
# Initialize the move group for the right arm
right_arm = moveit_commander.MoveGroupCommander("right_arm")
right_gripper = moveit_commander.MoveGroupCommander("right_gripper")
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Set the reference frame for pose targets
reference_frame = "camera_link"
# Set the right arm reference frame accordingly
right_arm.set_pose_reference_frame(reference_frame)
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Allow some leeway in position (meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.01)
right_arm.set_goal_orientation_tolerance(0.05)
# Start the arm in the "resting" pose stored in the SRDF file
right_arm.set_named_target("resting")
right_arm.go()
rospy.sleep(0.5)
# Set the gripper to a neural position using a joint value target
right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
# Plan and execute a trajectory to the goal configuration
right_gripper.go()
rospy.sleep(1)
# Set the target pose. This particular pose has the gripper oriented horizontally
# 0.85 meters above the ground, 0.10 meters to the right and 0.20 meters ahead of
# the center of the robot base.
target_pose = PoseStamped()
target_pose.header.frame_id = reference_frame
target_pose.header.stamp = rospy.Time.now()
target_pose.pose.position.x = XVALUE
target_pose.pose.position.y = YVALUE
target_pose.pose.position.z = ZVALUE
target_pose.pose.orientation.x = 0.0
target_pose.pose.orientation.y = 0.0
target_pose.pose.orientation.z = 0.0
target_pose.pose.orientation.w = 1.0
# Set the start state to the current state
right_arm.set_start_state_to_current_state()
# Set the goal pose of the end effector to the stored pose
right_arm.set_pose_target(target_pose, end_effector_link)
# Plan the trajectory to the goal
traj = right_arm.plan()
# Execute the planned trajectory
right_arm.execute(traj)
# Pause for a second
rospy.sleep(1)
# Set the gripper target to a partially closed position
right_gripper.set_joint_value_target(GRIPPER_CLOSED)
# Plan and execute a trajectory to the goal configuration
right_gripper.go()
rospy.sleep(1)
DIRECTION = raw_input("Mover a la izquierda(I) o derecha(D)?")
DISTANCE = input("Defina, en metros, cuanto se movera el objeto \n")
if DIRECTION == "D":
DISTANCE = DISTANCE * (-1)
# Shift the end-effector to the right 5cm
# right_arm.shift_pose_target(1, -0.05, end_effector_link)
# right_arm.go()
# rospy.sleep(1)
# Rotate the end-effector 90 degrees
# right_arm.shift_pose_target(3, 1.57, end_effector_link)
# right_arm.go()
# rospy.sleep(0.5)
# Store this pose as the new target_pose
saved_target_pose = right_arm.get_current_pose(end_effector_link)
# Move to the named pose "wave"
# right_arm.set_named_target('wave')
# right_arm.go()
# rospy.sleep(1)
# Go back to the stored target
# right_arm.set_pose_target(saved_target_pose, end_effector_link)
# right_arm.go()
# rospy.sleep(1)
# Set the target pose. This particular pose has the gripper oriented horizontally
# 0.85 meters above the ground, 0.10 meters to the right and 0.20 meters ahead of
# the center of the robot base.
target_pose = PoseStamped()
target_pose.header.frame_id = reference_frame
target_pose.header.stamp = rospy.Time.now()
target_pose.pose.position.x = XVALUE
target_pose.pose.position.y = YVALUE + DISTANCE
target_pose.pose.position.z = ZVALUE
target_pose.pose.orientation.x = 0.0
target_pose.pose.orientation.y = 0.0
target_pose.pose.orientation.z = 0.0
target_pose.pose.orientation.w = 1.0
# Set the start state to the current state
right_arm.set_start_state_to_current_state()
# Set the goal pose of the end effector to the stored pose
right_arm.set_pose_target(target_pose, end_effector_link)
# Plan the trajectory to the goal
traj = right_arm.plan()
# Execute the planned trajectory
right_arm.execute(traj)
# Pause for a second
rospy.sleep(1)
# Set the gripper target to a partially open position
right_gripper.set_joint_value_target(GRIPPER_OPEN)
# Plan and execute a trajectory to the goal configuration
right_gripper.go()
rospy.sleep(1)
# Finish up in the resting position
right_arm.set_named_target("resting")
right_arm.go()
# Return the gripper target to neutral position
right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
right_gripper.go()
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit MoveIt
moveit_commander.os._exit(0)
def week4_assignment3():
## First initialize moveit_commander and rospy.
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('week4_assignment3', anonymous=True)
## Instantiate a MoveGroupCommander object. This object is an interface
## to one group of joints. In this case the group refers to the joints of
## robot2. This interface can be used to plan and execute motions on robot2.
robot2_group = moveit_commander.MoveGroupCommander("robot2")
## Action clients to the ExecuteTrajectory action server.
robot2_client = actionlib.SimpleActionClient(
'execute_trajectory', moveit_msgs.msg.ExecuteTrajectoryAction)
robot2_client.wait_for_server()
rospy.loginfo('Execute Trajectory server is available for robot2')
## Move robot2 to R2Home robot pose with the set_named_target API.
robot2_group.set_named_target("R2Home")
## Plan to the desired joint-space goal using the default planner (RRTConnect).
plan = robot2_group.plan()
## Create a goal message object for the action server.
robot2_goal = moveit_msgs.msg.ExecuteTrajectoryGoal()
## Update the trajectory in the goal message.
robot2_goal.trajectory = plan
## Print message
rospy.loginfo('Go to Home')
## Send the goal to the action server.
robot2_client.send_goal(robot2_goal)
robot2_client.wait_for_result()
## Move robot2 to R2PreGrasp pose
robot2_group.set_named_target("R2PreGrasp")
plan = robot2_group.plan()
robot2_goal = moveit_msgs.msg.ExecuteTrajectoryGoal()
robot2_goal.trajectory = plan
## Print message
rospy.loginfo('Go to Pre Grasp')
robot2_client.send_goal(robot2_goal)
robot2_client.wait_for_result()
# Pick motions with the compute_cartesian_path API.
waypoints = []
# start with the current pose
current_robot2_pose = robot2_group.get_current_pose()
rospy.sleep(0.5)
current_robot2_pose = robot2_group.get_current_pose()
## create linear offsets to the current pose
new_robot2_eef_pose = geometry_msgs.msg.Pose()
# Manual offsets because we don't have a camera to detect objects yet.
# Create an x-offset of -5cm to the current x position.
new_robot2_eef_pose.position.x = current_robot2_pose.pose.position.x + 0.5
# Create a y-offset of +10cm to the current y position.
new_robot2_eef_pose.position.y = current_robot2_pose.pose.position.y + 0.5
# Create a z-offset of -10cm to the current z position.
new_robot2_eef_pose.position.z = current_robot2_pose.pose.position.z - 0.5
# Retain orientation of the current pose.
new_robot2_eef_pose.orientation = copy.deepcopy(
current_robot2_pose.pose.orientation)
# Update the list of waypoints.
# First add the new desired pose of the end effector for robot2.
waypoints.append(new_robot2_eef_pose)
rospy.loginfo('Cartesian path - Waypoint 1:')
print(new_robot2_eef_pose.position)
# Then go back to the pose where we started the linear motion from.
waypoints.append(current_robot2_pose.pose)
rospy.loginfo('Cartesian path - Waypoint 2:')
print(current_robot2_pose.pose.position)
## 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.
fraction = 0.0
for count_cartesian_path in range(0, 3):
if fraction < 1.0:
(plan_cartesian, fraction) = robot2_group.compute_cartesian_path(
waypoints, # waypoints to follow
0.01, # eef_step
0.0) # jump_threshold
else:
break
robot2_goal = moveit_msgs.msg.ExecuteTrajectoryGoal()
robot2_goal.trajectory = plan_cartesian
## Print message
rospy.loginfo('Cartesian movement')
robot2_client.send_goal(robot2_goal)
robot2_client.wait_for_result()
rospy.sleep(5)
## After executing the linear motions, go to the R2Place robot pose using the set_named_target API.
robot2_group.set_named_target("R2Place")
plan = robot2_group.plan()
robot2_goal = moveit_msgs.msg.ExecuteTrajectoryGoal()
robot2_goal.trajectory = plan
rospy.loginfo('Go to Place')
robot2_client.send_goal(robot2_goal)
robot2_client.wait_for_result()
## When finished shut down moveit_commander.
moveit_commander.roscpp_shutdown()
def end():
rospy.signal_shutdown("Done")
os._exit(0)
roscpp_shutdown()
def close(self):
""" Closes the environment and shuts down the simulation. """
print("closing GazeboSmartBotPincherKinectEnv")
# When finished, shut down moveit_commander.
moveit_commander.roscpp_shutdown()
super(GazeboSmartBotPincherKinectEnv, self).close()
def __init__(self):
# rospy.sleep(10) # wait for other files to be launched successfully
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('cw3_q2')
self.tf_buffer = tf2_ros.Buffer()
self.tf_listener = tf2_ros.TransformListener(self.tf_buffer)
# self.touch = [0,0,0,0]
# rospy.Subscriber("/contacts/lh_ff/distal", ContactsState, self.call_ff)
# rospy.Subscriber("/contacts/lh_mf/distal", ContactsState, self.call_mf)
# rospy.Subscriber("/contacts/lh_rf/distal", ContactsState, self.call_rf)
# rospy.Subscriber("/contacts/lh_th/distal", ContactsState, self.call_th)
# # Robot contains the entire state of the robot (iiwa and shadow hand)
# robot = moveit_commander.RobotCommander()
# # We can get a list of all the groups in the robot
# print('============ Robot Groups:')
# print('{}\n\n'.format(robot.get_group_names()))
self.iiwa_group = moveit_commander.MoveGroupCommander('hand_iiwa')
self.hand_group = moveit_commander.MoveGroupCommander('sr_hand')
rospy.sleep(1)
N = 10 # amount of tracking to average
combine_obj_pos = np.empty((N,9))
combine_euler = np.empty((N,9))
# object tracking
for cN in range(0,N):
[all_object_name,all_object_pose] = self.get_object_position()
# loop through each object and store their respective position and orientation
for m in range(0,3):
combine_obj_pos[cN,3*m:3*m+3] = (all_object_pose[m].transform.translation.x,all_object_pose[m].transform.translation.y,all_object_pose[m].transform.translation.z)
obj_quat = (all_object_pose[m].transform.rotation.x,all_object_pose[m].transform.rotation.y,all_object_pose[m].transform.rotation.z,all_object_pose[m].transform.rotation.w)
combine_euler[cN,3*m:3*m+3] = euler_from_quaternion(obj_quat)
print combine_obj_pos
average_obj_pos = np.mean(combine_obj_pos,axis=0)
average_euler = np.mean(combine_euler,axis=0)
# update the object pose with the new averaged value
for p in range(0,3):
all_object_pose[p].transform.translation.x = average_obj_pos[3*p]
all_object_pose[p].transform.translation.y = average_obj_pos[3*p+1]
all_object_pose[p].transform.translation.z = average_obj_pos[3*p+2]
new_quat = quaternion_from_euler(average_euler[3*p], average_euler[3*p+1], average_euler[3*p+2])
all_object_pose[p].transform.rotation.x = new_quat[0]
all_object_pose[p].transform.rotation.y = new_quat[1]
all_object_pose[p].transform.rotation.z = new_quat[2]
all_object_pose[p].transform.rotation.w = new_quat[3]
print all_object_pose
# print average_obj_pos
# create a service proxy for add and remove objects to allowed collision matrix
add_to_acm = rospy.ServiceProxy('/add_object_acm', ChangeCollisionObject)
remove_from_acm = rospy.ServiceProxy('/remove_object_acm', ChangeCollisionObject)
# open the hand
self.hand_open()
for i in range(0,3):
# i = 2 # object no.
# move the iiwa to just above the object
self.move_iiwa_to_object(all_object_pose,i)
# add object to allowed collision matrix
add_to_acm(all_object_name[i])
# close the hand (grasping)
self.hand_close()
# hold the grasping position for 10 seconds
rospy.sleep(10)
# open the hand (releasing)
self.hand_open()
# remove object from allowed collision matrix
remove_from_acm(all_object_name[i])
moveit_commander.roscpp_shutdown()
rospy.loginfo("Task finished")
def measure_table():
# This function measure the size and the position of the table. Add the value for offset_zero_point.
offset_zero_point = 0.903
# Define positions.
pos1 = {
'left_e0': -1.69483279891317,
'left_e1': 1.8669726956453,
'left_s0': 0.472137005716569,
'left_s1': -0.38852045702393034,
'left_w0': -1.9770933862776057,
'left_w1': -1.5701993084642143,
'left_w2': -0.6339059781326424,
'right_e0': 1.7238109084167481,
'right_e1': 1.7169079948791506,
'right_s0': 0.36930587426147465,
'right_s1': -0.33249033539428713,
'right_w0': -1.2160632682067871,
'right_w1': 1.668587600115967,
'right_w2': -1.810097327636719
}
lpos1 = {
'left_e0': -1.69483279891317,
'left_e1': 1.8669726956453,
'left_s0': 0.472137005716569,
'left_s1': -0.38852045702393034,
'left_w0': -1.9770933862776057,
'left_w1': -1.5701993084642143,
'left_w2': -0.6339059781326424
}
rpos1 = {
'right_e0': 1.7238109084167481,
'right_e1': 1.7169079948791506,
'right_s0': 0.36930587426147465,
'right_s1': -0.33249033539428713,
'right_w0': -1.2160632682067871,
'right_w1': 1.668587600115967,
'right_w2': -1.810097327636719
}
m_z_start = geometry_msgs.msg.Pose()
m_z_start.position.x = 0.55
m_z_start.position.y = 0
m_z_start.position.z = 0.0
m_z_start.orientation.x = 1.0
m_z_start.orientation.y = 0.0
m_z_start.orientation.z = 0.0
m_z_start.orientation.w = 0.0
m_x_start = geometry_msgs.msg.Pose()
m_x_start.position.x = 0.824555206312
m_x_start.position.y = -0.101147200174
m_x_start.position.z = 0
m_x_start.orientation.x = 0.999078899126
m_x_start.orientation.y = -0.0370175672297
m_x_start.orientation.z = 0.0134534998151
m_x_start.orientation.w = 0.0170310416472
# For the right arm.
m_y_start_1 = geometry_msgs.msg.Pose()
m_y_start_1.position.x = 0.502944492492
m_y_start_1.position.y = -0.759184953936
m_y_start_1.position.z = 0
m_y_start_1.orientation.x = 0.695094141364
m_y_start_1.orientation.y = -0.718718693339
m_y_start_1.orientation.z = 0.00984382718873
m_y_start_1.orientation.w = 0.0138084595126
# For the left arm.
m_y_start_2 = geometry_msgs.msg.Pose()
m_y_start_2.position.x = 0.569637271212
m_y_start_2.position.y = 0.86081004269
m_y_start_2.position.z = 0
m_y_start_2.orientation.x = -0.752397350016
m_y_start_2.orientation.y = -0.657989479311
m_y_start_2.orientation.z = -0.0240898615418
m_y_start_2.orientation.w = 0.0191768447787
# The start values for the attached table in MoveIt.
table_size_x = 1.5
table_size_y = 2
table_size_z = 0.8
center_x = 0
center_y = 0
center_z = 0
both_arms.set_joint_value_target(pos1)
both_arms.plan()
both_arms.go(wait=True)
# cProfile to measure the performance (time) of the task.
pr = cProfile.Profile()
pr.enable()
# Start to measure the height of the table.
state = move("right", m_z_start)
right_ir_sensor = rospy.wait_for_message(
"/robot/range/right_hand_range/state", Range)
# At first move with 10 cm steps and if the range is smaller then 25 cm with 1 cm steps.
while (right_ir_sensor.range > 0.25):
if not state:
m_z_start.position.z -= 0.1
state = move("right", m_z_start)
right_ir_sensor = rospy.wait_for_message(
"/robot/range/right_hand_range/state", Range)
print "-----> The distance to table:", right_ir_sensor.range, "m"
while (right_ir_sensor.range > 0.12):
if not state:
m_z_start.position.z -= 0.01
state = move("right", m_z_start)
right_ir_sensor = rospy.wait_for_message(
"/robot/range/right_hand_range/state", Range)
print "-----> The distance to table:", right_ir_sensor.range, "m"
time.sleep(3)
right_ir_sensor = rospy.wait_for_message(
"/robot/range/right_hand_range/state", Range)
distance = right_ir_sensor.range
print "-----> The distance to table:", distance, "m"
a = right_arm.get_current_pose()
z_pos = a.pose.position.z
# From the ground to the zero point in MoveIt with the base frame are 0.903 m.
# The z position of the tip of the gripper and the distance between
# the tip and the table must be substract.
# (zpos is negative for table < 90 cm)
# The value 0.099 is the distance from the ir sensor to the gripper tip.
table_size_z = offset_zero_point + z_pos - (distance - 0.099)
print "-----> The table size in z direction is:", table_size_z, " m"
center_z = -offset_zero_point + table_size_z / 2
print "-----> The center of z is the position:", center_z, "m"
# Initialize the position z for the next measurements.
m_y_start_1.position.z = -offset_zero_point + table_size_z + 0.025
m_y_start_2.position.z = -offset_zero_point + table_size_z + 0.025
m_x_start.position.z = -offset_zero_point + table_size_z + 0.025
# Add table as attached object.
p.attachBox('table',
table_size_x,
table_size_y,
table_size_z,
center_x,
center_y,
center_z,
'base',
touch_links=['pedestal'])
p.waitForSync()
# Move to the start position.
both_arms.set_joint_value_target(pos1)
both_arms.plan()
both_arms.go(wait=True)
# Move to the right and left border point.
right_arm.set_pose_target(m_y_start_1)
right_arm.plan()
right_arm.go(wait=True)
left_arm.set_pose_target(m_y_start_2)
left_arm.plan()
left_arm.go(wait=True)
# Start to measure the size and position in y direction of the table. At first y1 endpoint.
right_ir_sensor = rospy.wait_for_message(
"/robot/range/right_hand_range/state", Range)
distance = right_ir_sensor.range
# If the table is at the border point under the gripper the default value -0.8 is used.
if distance < 0.15:
y1_endpoint = -0.8
else:
y1_endpoint = 0
while (right_ir_sensor.range > 0.2 and m_y_start_1.position.y < 0.2):
m_y_start_1.position.y += 0.1
move("right", m_y_start_1)
right_ir_sensor = rospy.wait_for_message(
"/robot/range/right_hand_range/state", Range)
while right_ir_sensor.range < 0.2 and y1_endpoint == 0:
m_y_start_1.position.y -= 0.01
move("right", m_y_start_1)
right_ir_sensor = rospy.wait_for_message(
"/robot/range/right_hand_range/state", Range)
time.sleep(3)
if y1_endpoint == 0:
a = right_arm.get_current_pose()
y_pos = a.pose.position.y
# The ir sensor is not in the middle of the hand and a small safety distance is necessary.
y1_endpoint = y_pos - 0.04
print "-----> y1 endpoint:", y1_endpoint, "m"
# Start to measure the second y2 endpoint.
left_ir_sensor = rospy.wait_for_message(
"/robot/range/left_hand_range/state", Range)
distance = left_ir_sensor.range
print "-----> The distance to table:", distance, "m"
# If the table is at the border point under the gripper the default value 1.2 is used.
if distance < 0.15:
y2_endpoint = 1.2
else:
y2_endpoint = 0
while (left_ir_sensor.range > 0.2 and m_y_start_2.position.y > 0.2):
m_y_start_2.position.y -= 0.1
move("left", m_y_start_2)
left_ir_sensor = rospy.wait_for_message(
"/robot/range/left_hand_range/state", Range)
while left_ir_sensor.range < 0.2 and y2_endpoint == 0:
m_y_start_2.position.y += 0.01
move("left", m_y_start_2)
left_ir_sensor = rospy.wait_for_message(
"/robot/range/left_hand_range/state", Range)
time.sleep(3)
if y2_endpoint == 0:
a = left_arm.get_current_pose()
y_pos = a.pose.position.y
# The ir sensor is not in the middle of the hand and a small safety distance is necessary.
y2_endpoint = y_pos + 0.04
print "-----> y2 endpoint:", y2_endpoint, "m"
# The y1 endpoint must be negative and the y2 endpoint positive.
table_size_y = abs(y1_endpoint) + abs(y2_endpoint)
print "-----> The table size in y direction is:", table_size_y, "m"
center_y = y1_endpoint + table_size_y / 2 - 0.04
print "-----> The center of y is the position:", center_y, "m"
# Remove the old table and add one with the new value.
p.removeAttachedObject('table')
p.attachBox('table',
table_size_x,
table_size_y,
table_size_z,
center_x,
center_y,
center_z,
'base',
touch_links=['pedestal'])
p.waitForSync()
# Move to the start position.
both_arms.set_joint_value_target(pos1)
both_arms.plan()
both_arms.go(wait=True)
# Move to the border point in the x direction.
right_arm.set_pose_target(m_x_start)
right_arm.plan()
right_arm.go(wait=True)
right_ir_sensor = rospy.wait_for_message(
"/robot/range/right_hand_range/state", Range)
distance = right_ir_sensor.range
# The x endpoint in the direction of the robot is allway 0.1.
x1_endpoint = 0.1
# If the table is at the border point under the gripper the default value 1.1 is used.
if distance < 0.15:
x2_endpoint = 1.1
else:
x2_endpoint = 0
while (right_ir_sensor.range > 0.2 and m_x_start.position.x > 0.2):
m_x_start.position.x -= 0.1
move("right", m_x_start)
right_ir_sensor = rospy.wait_for_message(
"/robot/range/right_hand_range/state", Range)
while right_ir_sensor.range < 0.2 and x2_endpoint == 0:
m_x_start.position.x += 0.01
move("right", m_x_start)
right_ir_sensor = rospy.wait_for_message(
"/robot/range/right_hand_range/state", Range)
time.sleep(3)
if x2_endpoint == 0:
a = right_arm.get_current_pose()
x_pos = a.pose.position.x
# The ir sensor is not in the middle of the hand and a small safety distance is necessary.
x2_endpoint = x_pos + 0.04
print "-----> x2 endpoint:", x2_endpoint, "m"
table_size_x = abs(x2_endpoint) - abs(x1_endpoint)
print "-----> The table size in x direction is:", table_size_x, "m"
center_x = x1_endpoint + table_size_x / 2
print "-----> The center of x is the position:", center_x, "m"
# Remove the old table and add one with the new value.
p.removeAttachedObject('table')
# Add table as attached object. It is most possible to approach up to approximately 0.5 cm.
# (Point on the table: -0.175)
# Size of the box in x-, y- and z-dimension.
# The x, y, z positions in link_name frame ('base') where the center of the box is.
# The touch_links are robot links that can touch this object.
# The value can be determined with right_arm.get_current_pose().
# The pose value of z must be in every space direct of the table the
# same height otherwise the pedestal must be configured.
# The zero point from MoveIt (with right_arm.get_current_pose())
# and the function from the baxter_interface
# (rightarm.endpoint_pose()) is different.
# The distance from the zero point from the link_name frame 'base' to
# the ground is for MoveIt 0.903 m.
p.attachBox('table',
table_size_x,
table_size_y,
table_size_z,
center_x,
center_y,
center_z,
'base',
touch_links=['pedestal'])
p.waitForSync()
# Move to the start position.
right_arm.set_joint_value_target(rpos1)
right_arm.plan()
right_arm.go(wait=True)
pr.disable()
sortby = 'cumulative'
ps = pstats.Stats(pr).sort_stats(sortby).print_stats(0.0)
print "-----> The table size in x direction is:", table_size_x, "m"
print "-----> The table size in y direction is:", table_size_y, "m"
print "-----> The table size in z direction is:", table_size_z, "m"
print "-----> The center of x is the position:", center_x, "m"
print "-----> The center of y is the position:", center_y, "m"
print "-----> The center of z is the position:", center_z, "m"
time.sleep(100)
moveit_commander.roscpp_shutdown()
# Exit MoveIt.
moveit_commander.os._exit(0)
image_num += 1
pass
pass
print "Scanning ended!"
pass
def RotateLeftArm(self):
pass
def parse_args():
parser = argparse.ArgumentParser()
parser.add_argument('-n', '--name', dest='name', type=str, help='Object name', required = True)
args = parser.parse_args()
return args
if __name__ == '__main__':
args = parse_args()
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('object_scanning_node', anonymous=True)
image_sub = image_subscriber(args.name)
mover = robot_mover(image_sub)
mover.gotoStartPoses()
mover.Scan()
try:
rospy.spin()
except KeyboardInterrupt:
print("Shutting down")
moveit_commander.roscpp_shutdown()
print "scanning done"
pass
def callback(data):
if (data.ctrl == 6):
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
# Initialize the move group for the right arm
right_arm = moveit_commander.MoveGroupCommander('right_arm')
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Set the reference frame for pose targets
reference_frame = 'base_link'
# Set the right arm reference frame accordingly
right_arm.set_pose_reference_frame(reference_frame)
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Allow some leeway in position (meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.01)
right_arm.set_goal_orientation_tolerance(0.05)
right_arm.set_planner_id("ESTkConfigDefault");
# Start the arm in the "resting" pose stored in the SRDF file
right_arm.set_named_target('right_start')
right_arm.go()
rospy.sleep(2)
# Set the target pose. This particular pose has the gripper oriented horizontally
# 0.85 meters above the ground, 0.10 meters to the right and 0.20 meters ahead of
# the center of the robot base.
target_pose = PoseStamped()
target_pose.header.frame_id = reference_frame
target_pose.header.stamp = rospy.Time.now()
target_pose.pose.position.x = 0.50
target_pose.pose.position.y = -0.1
target_pose.pose.position.z = 0.85
target_pose.pose.orientation.x = 0.0
target_pose.pose.orientation.y = 0.0
target_pose.pose.orientation.z = 0.0
target_pose.pose.orientation.w = 1.0
# Set the start state to the current state
right_arm.set_start_state_to_current_state()
# Set the goal pose of the end effector to the stored pose
right_arm.set_pose_target(target_pose, end_effector_link)
# Plan the trajectory to the goal
traj = right_arm.plan()
# Execute the planned trajectory
right_arm.execute(traj)
# Pause for a second
rospy.sleep(1)
# Shift the end-effector to the right 5cm
right_arm.shift_pose_target(1, -0.05, end_effector_link)
right_arm.go()
rospy.sleep(1)
# Rotate the end-effector 90 degrees
right_arm.shift_pose_target(3, -1.57, end_effector_link)
right_arm.go()
rospy.sleep(1)
# Store this pose as the new target_pose
saved_target_pose = right_arm.get_current_pose(end_effector_link)
# Go back to the stored target
right_arm.set_pose_target(saved_target_pose, end_effector_link)
right_arm.go()
rospy.sleep(1)
# Finish up in the resting position
right_arm.set_named_target('right_start')
right_arm.go()
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit MoveIt
moveit_commander.os._exit(0)
def __init__(self):
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_drawing', anonymous=True)
#订阅stop话题
rospy.Subscriber("/probot_controller_ctrl", ControllerCtrl, callback)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('manipulator')
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 设置目标位置所使用的参考坐标系
reference_frame = 'base_link'
arm.set_pose_reference_frame('base_link')
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.01)
arm.set_goal_orientation_tolerance(0.01)
# 设置允许的最大速度和加速度
arm.set_max_acceleration_scaling_factor(0.5)
arm.set_max_velocity_scaling_factor(0.5)
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
# 控制机械臂先回到初始化位置
arm.set_named_target('home')
arm.go()
rospy.sleep(1)
star_pose = PoseStamped()
star_pose.header.frame_id = reference_frame
star_pose.header.stamp = rospy.Time.now()
star_pose.pose.position.x = 0.30
star_pose.pose.position.y = 0.0
star_pose.pose.position.z = 0.30
star_pose.pose.orientation.w = 1.0
# 设置机械臂终端运动的目标位姿
arm.set_pose_target(star_pose, end_effector_link)
arm.go()
radius = 0.05
centerY = 0.0
centerX = 0.40 - radius
# 初始化路点列表
waypoints = []
starPoints = []
pose_temp = star_pose
for th in numpy.arange(0, 6.2831855, 1.2566371):
pose_temp.pose.position.y = -(centerY + radius * math.sin(th))
pose_temp.pose.position.x = centerX + radius * math.cos(th)
pose_temp.pose.position.z = 0.30
wpose = deepcopy(pose_temp.pose)
starPoints.append(deepcopy(wpose))
# 将圆弧上的路径点加入列表
waypoints.append(starPoints[0])
waypoints.append(starPoints[2])
waypoints.append(starPoints[4])
waypoints.append(starPoints[1])
waypoints.append(starPoints[3])
waypoints.append(starPoints[0])
fraction = 0.0 #路径规划覆盖率
maxtries = 100 #最大尝试规划次数
attempts = 0 #已经尝试规划次数
# 设置机器臂当前的状态作为运动初始状态
arm.set_start_state_to_current_state()
# 尝试规划一条笛卡尔空间下的路径，依次通过所有路点，完成圆弧轨迹
while fraction < 1.0 and attempts < maxtries:
(plan, fraction) = arm.compute_cartesian_path(
waypoints, # waypoint poses，路点列表
0.01, # eef_step，终端步进值
0.0, # jump_threshold，跳跃阈值
True) # avoid_collisions，避障规划
# 尝试次数累加
attempts += 1
# 打印运动规划进程
if attempts % 10 == 0:
rospy.loginfo("Still trying after " + str(attempts) +
" attempts...")
# 如果路径规划成功（覆盖率100%）,则开始控制机械臂运动
if fraction == 1.0:
rospy.loginfo("Path computed successfully. Moving the arm.")
arm.execute(plan)
rospy.loginfo("Path execution complete.")
# 如果路径规划失败，则打印失败信息
else:
rospy.loginfo("Path planning failed with only " + str(fraction) +
" success after " + str(maxtries) + " attempts.")
rospy.sleep(1)
# 控制机械臂先回到初始化位置
arm.set_named_target('home')
arm.go()
rospy.sleep(1)
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def __del__(self):
moveit_commander.roscpp_shutdown()
rospy.loginfo('\033[94m' + "Object of class Ur5Moveit Deleted." +
'\033[0m')
def shutdown():
"""Shutdown moveit!
"""
moveit_commander.roscpp_shutdown()
