def move_action(x):
#Create the function used to call the service
compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
step = 0
pose = [x]
if step == 0:
s = pose[step]
request = GetPositionIKRequest()
request.ik_request.group_name = "left_arm"
request.ik_request.ik_link_name = "left_gripper"
request.ik_request.attempts = 20
request.ik_request.pose_stamped.header.frame_id = "ar_marker_0"
request.ik_request.pose_stamped.pose.position.x = s[0]
request.ik_request.pose_stamped.pose.position.y = s[1]
request.ik_request.pose_stamped.pose.position.z = s[2]
request.ik_request.pose_stamped.pose.orientation.x = s[3]
request.ik_request.pose_stamped.pose.orientation.y = s[4]
request.ik_request.pose_stamped.pose.orientation.z = s[5]
request.ik_request.pose_stamped.pose.orientation.w = s[6]
response = compute_ik(request)
group = MoveGroupCommander("left_arm")
group.set_pose_target(request.ik_request.pose_stamped)
group.go()
step = step + 1
def requestIK(link_name, group_name, p, o, compute_ik):
px, py, pz = p
ox, oy, oz, ow = o
request = GetPositionIKRequest()
request.ik_request.group_name = group_name
request.ik_request.ik_link_name = link_name
request.ik_request.attempts = 10000
request.ik_request.pose_stamped.header.frame_id = "head_camera"
request.ik_request.pose_stamped.pose.position.x = px
request.ik_request.pose_stamped.pose.position.y = py
request.ik_request.pose_stamped.pose.position.z = pz
request.ik_request.pose_stamped.pose.orientation.x = ox
request.ik_request.pose_stamped.pose.orientation.y = oy
request.ik_request.pose_stamped.pose.orientation.z = oz
request.ik_request.pose_stamped.pose.orientation.w = ow
try:
# print request
print 'finding ik....'
response = compute_ik(request)
print 'moveIt succeeded'
# print(response)
group = MoveGroupCommander(group_name)
group.set_pose_target(request.ik_request.pose_stamped)
group.go()
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def main():
init_node()
# Preparing Left Arm
rospy.loginfo("Preparing Left Arm...")
larmg = MoveGroupCommander("left_arm")
larmg.set_pose_target([0.325, 0.182, 0.067, 0.0, -math.pi / 2, 0.0])
larmg.go()
# Right Arm
group = MoveGroupCommander("right_arm")
# Frame ID Definitoins
planning_frame_id = group.get_planning_frame()
tgt_frame_id = '/LARM_JOINT5_Link'
# Get a target pose
pose_target = get_current_target_pose(tgt_frame_id, planning_frame_id)
# Move to a point above target
if pose_target:
pose_target.pose.position.z += 0.4
rospy.loginfo("Set Target To: \n{}".format(pose_target))
group.set_pose_target(pose_target)
ret = group.go()
rospy.loginfo("Executed ... {}".format(ret))
else:
rospy.logwarn("Pose Error: {}".format(pose_target))
def __init__(self):
group = MoveGroupCommander("arm")
#group.set_orientation_tolerance([0.3,0.3,0,3])
p = PoseStamped()
p.header.frame_id = "/katana_base_link"
p.pose.position.x = 0.4287
#p.pose.position.y = -0.0847
p.pose.position.y = 0.0
p.pose.position.z = 0.4492
q = quaternion_from_euler(2, 0, 0)
p.pose.orientation.x = q[0]
p.pose.orientation.y = q[1]
p.pose.orientation.z = q[2]
p.pose.orientation.w = q[3]
print "Planning frame: ", group.get_planning_frame()
print "Pose reference frame: ", group.get_pose_reference_frame()
#group.set_pose_reference_frame("katana_base_link")
print "RPy target: 0,0,0"
#group.set_rpy_target([0, 0, 0],"katana_gripper_tool_frame")
#group.set_position_target([0.16,0,0.40], "katana_gripper_tool_frame")
group.set_pose_target(p, "katana_gripper_tool_frame")
group.go()
print "Current rpy: ", group.get_current_rpy(
"katana_gripper_tool_frame")
class GenericDualArmClient():
_safe_kinematicsolver = "RRTConnectkConfigDefault"
def __init__(self, *args, **kwargs):
larm_name = args[0]
rarm_name = args[1] # "arm_right" for Motoman SDA10F
self.r_arm = MoveGroupCommander(rarm_name)
self.r_arm.set_planner_id(self._safe_kinematicsolver)
def move_rarm_shift_forward(self, axis, value):
'''
Ref. http://docs.ros.org/indigo/api/moveit_commander/html/classmoveit__commander_1_1move__group_1_1MoveGroupCommander.html#a22f9ec1477c3d61e728660850d50ce1f
'''
self.r_arm.shift_pose_target(axis, value)
self.r_arm.plan()
self.r_arm.go()
def move_rarm_fixedpose_forward(self):
tpose = Pose()
#TODO Currently the following position may work with SDA10F but not with NXO
tpose.position.x = 0.599
tpose.position.y = -0.379
tpose.position.z = 1.11
self.r_arm.set_pose_target(tpose)
self.r_arm.plan()
self.r_arm.go()
def make_single_move(pose):
print "ok"
s = pose
print s
#Construct the request
request = GetPositionIKRequest()
request.ik_request.group_name = "left_arm"
request.ik_request.ik_link_name = "left_gripper"
request.ik_request.attempts = 20
request.ik_request.pose_stamped.header.frame_id = "ar_marker_3"
#Set the desired orientation for the end effector HERE
request.ik_request.pose_stamped.pose.position.x = s[0]
request.ik_request.pose_stamped.pose.position.y = s[1]
request.ik_request.pose_stamped.pose.position.z = s[2]
request.ik_request.pose_stamped.pose.orientation.x = s[3]
request.ik_request.pose_stamped.pose.orientation.y = s[4]
request.ik_request.pose_stamped.pose.orientation.z = s[5]
request.ik_request.pose_stamped.pose.orientation.w = s[6]
try:
print "ok2"
response = compute_ik(request)
group = MoveGroupCommander("left_arm")
print "ok3"
group.set_pose_target(request.ik_request.pose_stamped)
print "ok4"
group.go()
print "in"
print x
#print "out"
except rospy.ServiceException, e:
print "Service call failed: %s"%e
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('start_pos')
# 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=5)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('gripper_pose',
PoseStamped,
queue_size=5)
# Create a dictionary to hold object colors
self.colors = dict()
# Initialize the move group for the right arm
right_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.05)
right_arm.set_goal_orientation_tolerance(0.1)
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Set the right arm reference frame
right_arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow 10 seconds per planning attempt
right_arm.set_planning_time(10)
# Start the arm in the "resting" pose stored in the SRDF file
right_arm.set_named_target('right_start')
right_arm.go()
# Open the gripper to the neutral position
right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
right_gripper.go()
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
class HeadMover():
""" Moves head to specified joint values """
def __init__(self):
self.group_head = MoveGroupCommander('head')
def move_head(self, name, joint_values):
rospy.loginfo('Moving head to specified position')
self.group_head.set_joint_value_target(joint_values)
self.group_head.go()
def main():
init_node()
# Preparing Head
rospy.loginfo("Preparing Head...")
headg = MoveGroupCommander("head")
headg.set_joint_value_target([0.0, 60.0 / 180.0 * math.pi])
headg.go()
# Preparing Both Arms
rospy.loginfo("Preparing Left Arm...")
barmg = MoveGroupCommander("botharms")
barmg.set_pose_target([0.325, 0.482, 0.167, 0.0, -math.pi / 2, 0.0],
'LARM_JOINT5_Link')
barmg.set_pose_target([0.325, -0.482, 0.167, 0.0, -math.pi / 2, 0.0],
'RARM_JOINT5_Link')
barmg.go()
rospy.sleep(2.0)
# Right Arm
group = MoveGroupCommander("right_arm")
# Frame ID Definitoins
planning_frame_id = group.get_planning_frame()
tgt_frame_id = '/ar_marker_4'
# Get a target pose
pose_target = get_current_target_pose(tgt_frame_id, planning_frame_id, 5.0)
# Move to a point above target
if pose_target:
# Rotate Pose for Right Hand
quat = []
quat.append(pose_target.pose.orientation.x)
quat.append(pose_target.pose.orientation.y)
quat.append(pose_target.pose.orientation.z)
quat.append(pose_target.pose.orientation.w)
quat = quaternion_multiply(
quat, quaternion_about_axis(math.pi / 2, (1, 0, 0)))
quat = quaternion_multiply(
quat, quaternion_about_axis(math.pi / 2, (0, 0, 1)))
pose_target.pose.orientation.x = quat[0]
pose_target.pose.orientation.y = quat[1]
pose_target.pose.orientation.z = quat[2]
pose_target.pose.orientation.w = quat[3]
pose_target.pose.position.z += 0.4
rospy.loginfo("Set Target To: \n{}".format(pose_target))
group.set_pose_target(pose_target)
ret = group.go()
rospy.loginfo("Executed ... {}".format(ret))
else:
rospy.logwarn("Pose Error: {}".format(pose_target))
def grasp_object(self, object_pose):
rospy.loginfo("Removing any previous 'part' object")
self.scene.remove_attached_object("arm_tool_link")
self.scene.remove_world_object("part")
self.scene.remove_world_object("table")
rospy.loginfo("Clearing octomap")
self.clear_octomap_srv.call(EmptyRequest())
rospy.sleep(2.0)
#Add object description in scene
rospy.loginfo("Adding new 'part' object")
object_pose.pose.position.z = object_pose.pose.position.z + 0.06
object_pose.pose.position.x = object_pose.pose.position.x - 0.03
# rospy.loginfo("Object pose: %s", object_pose.pose)
self.scene.add_box("part", object_pose, (self.object_depth, self.object_width, self.object_height))
#define a virtual table below the object
table_pose = copy.deepcopy(object_pose)
table_height = 0.8
table_width = 0.8
table_depth = 0.8
table_pose.pose.position.z += -(self.object_height)/2 - table_height/2
table_pose.pose.position.x += 0.25
# # We need to wait for the object part to appear
self.scene.add_box("table", table_pose, (table_depth, table_width, table_height))
self.wait_for_planning_scene_object()
self.wait_for_planning_scene_object("table")
# compute grasps
possible_grasps = self.sg.create_grasps_from_object_pose(object_pose)
goal = createPickupGoal("arm_torso", "part", object_pose, possible_grasps, self.links_to_allow_contact)
rospy.loginfo("Sending goal")
self.pickup_ac.send_goal(goal)
rospy.loginfo("Waiting for result")
self.pickup_ac.wait_for_result()
result = self.pickup_ac.get_result()
rospy.logdebug("Using torso result: " + str(result))
rospy.loginfo("Pick result: " + str(moveit_error_dict[result.error_code.val]))
# Arm back to safe pose before moving on
rospy.loginfo("Moving arm to safe pose")
pick_final_points = rospy.get_param("/play_motion/motions/pick_final_pose/points")
final_pick_goal = pick_final_points[0]['positions']
move_group = MoveGroupCommander("arm_torso")
move_group.go(final_pick_goal, wait=True)
move_group.stop()
# Remove table from world
self.scene.remove_world_object("table")
return result.error_code.val
def main(robo):
#Wait for the IK service to become available
rospy.wait_for_service('compute_ik')
rospy.init_node('service_query')
arm = 'left'
#Create the function used to call the service
compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
if robo == 'sawyer':
arm = 'right'
while not rospy.is_shutdown():
raw_input('Press [ Enter ]: ')
#Construct the request
request = GetPositionIKRequest()
request.ik_request.group_name = arm + "_arm"
#Alan does not have a gripper so replace link with 'right_wrist' instead
link = arm + "_gripper"
if robo == 'sawyer':
link += '_tip'
request.ik_request.ik_link_name = link
request.ik_request.attempts = 20
request.ik_request.pose_stamped.header.frame_id = "base"
#Set the desired orientation for the end effector HERE
request.ik_request.pose_stamped.pose.position.x = 0.5
request.ik_request.pose_stamped.pose.position.y = 0.5
request.ik_request.pose_stamped.pose.position.z = 0.0
request.ik_request.pose_stamped.pose.orientation.x = 0.0
request.ik_request.pose_stamped.pose.orientation.y = 1.0
request.ik_request.pose_stamped.pose.orientation.z = 0.0
request.ik_request.pose_stamped.pose.orientation.w = 0.0
try:
#Send the request to the service
response = compute_ik(request)
#Print the response HERE
print(response)
group = MoveGroupCommander(arm + "_arm")
# Setting position and orientation target
group.set_pose_target(request.ik_request.pose_stamped)
# TRY THIS
# Setting just the position without specifying the orientation
###group.set_position_target([0.5, 0.5, 0.0])
# Plan IK and execute
group.go()
except rospy.ServiceException, e:
print "Service call failed: %s" % e
class TestHironxMoveit(unittest.TestCase):
@classmethod
def setUpClass(self):
rospy.init_node("test_hironx_moveit")
self.rarm = MoveGroupCommander("right_arm")
self.larm = MoveGroupCommander("left_arm")
self.rarm_current_pose = self.rarm.get_current_pose().pose
self.larm_current_pose = self.larm.get_current_pose().pose
def _set_target_random(self):
'''
@type self: moveit_commander.MoveGroupCommander
@param self: In this particular test script, the argument "self" is either
'rarm' or 'larm'.
'''
global current, current2, target
current = self.get_current_pose()
print "*current*", current
target = self.get_random_pose()
print "*target*", target
self.set_pose_target(target)
self.go()
current2 = self.get_current_pose()
print "*current2*", current2
# Associate a method to MoveGroupCommander class. This enables users to use
# the method on interpreter.
# Although not sure if this is the smartest Python code, this works fine from
# Python interpreter.
##MoveGroupCommander._set_target_random = _set_target_random
# ****** Usage ******
#
# See wiki tutorial
# https://code.google.com/p/rtm-ros-robotics/wiki/hironx_ros_bridge_en#Manipulate_Hiro_with_Moveit_via_Python
#
def test_set_pose_target_rpy(self):
# #rpy ver
target=[0.2035, -0.5399, 0.0709, 0,-1.6,0]
self.rarm.set_pose_target(target)
self.rarm.go()
time.sleep(_SEC_WAIT_BETWEEN_TESTCASES)
def test_set_pose_target_quarternion(self):
# #Quaternion ver
target2=[0.2035, -0.5399, 0.0709, 0.000427, 0.000317, -0.000384, 0.999999]
self.rarm.set_pose_target(target2)
self.rarm.go()
time.sleep(_SEC_WAIT_BETWEEN_TESTCASES)
def run(self, cycle, verbose=2):
#enable robot if not already done
enableProcess = subprocess.Popen(
["rosrun", "baxter_tools", "enable_robot.py", "-e"])
enableProcess.wait()
#start moveit servers. Since we do not know how long it will take them
#to start, pause for 15 seconds
jointServer = subprocess.Popen([
"rosrun", "baxter_interface", "joint_trajectory_action_server.py"
])
planServer = subprocess.Popen(
["roslaunch", "baxter_moveit_config", "move_group.launch"])
time.sleep(15)
raw_input("press enter")
try:
#left = MoveGroupCommander("left_arm")
right = MoveGroupCommander("right_arm")
upRight = geometry_msgs.msg.Pose(
position=geometry_msgs.msg.Point(x=0.9667369015076861,
y=-0.1190874231664102,
z=-0.07489146157788866))
#upLeft = geometry_msgs.msg.Pose(position = geometry_msgs.msg.Point(
#x = 0.7, y = 0.35, z = 0.8))
#downRight = geometry_msgs.msg.Pose(position = geometry_msgs.msg.Point(
#x = 0.7, y = -0.45, z = 0.3))
#downLeft = geometry_msgs.msg.Pose(position = geometry_msgs.msg.Point(
#x = 0.7, y = 0.45, z = 0.3))
right.set_pose_target(upRight)
#left.set_pose_target(upLeft)
right.go()
#left.go()
#right.set_pose_target(downRight)
#left.set_pose_target(downLeft)
right.go()
#left.go()
finally:
#clean up - kill servers
jointServer.kill()
planServer.kill()
sys.exit()
def main():
#Wait for the IK service to become available
rospy.wait_for_service('compute_ik')
rospy.init_node('service_query')
#Create the function used to call the service
compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
while not rospy.is_shutdown():
raw_input('Press [ Enter ]: ')
#Construct the request
request = GetPositionIKRequest()
request.ik_request.group_name = "left_arm"
request.ik_request.ik_link_name = "left_gripper"
request.ik_request.attempts = 20
#request.ik_request.pose_stamped.header.frame_id = "base"
request.ik_request.pose_stamped.header.frame_id = "base"
#Set the desired orientation for the end effector HERE
euler_angle = [-np.pi, 0, 0]
traslation = [0.591, 0.261, 0.327 - 0.1]
quaternion = tf.transformations.quaternion_from_euler(
euler_angle[0], euler_angle[1], euler_angle[2])
request.ik_request.pose_stamped.pose.position.x = traslation[0]
request.ik_request.pose_stamped.pose.position.y = traslation[1]
request.ik_request.pose_stamped.pose.position.z = traslation[2]
request.ik_request.pose_stamped.pose.orientation.x = quaternion[0]
request.ik_request.pose_stamped.pose.orientation.y = quaternion[1]
request.ik_request.pose_stamped.pose.orientation.z = quaternion[2]
request.ik_request.pose_stamped.pose.orientation.w = quaternion[3]
try:
#Send the request to the service
response = compute_ik(request)
#Print the response HERE
#print(response)
group = MoveGroupCommander("left_arm")
# Setting position and orientation target
group.set_pose_target(request.ik_request.pose_stamped)
# TRY THIS
# Setting just the position without specifying the orientation
###group.set_position_target([0.5, 0.5, 0.0])
# Plan IK and execute
group.go()
except rospy.ServiceException, e:
print "Service call failed: %s" % e
class TestMove():
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('ur3_move', anonymous=True)
self.scene = PlanningSceneInterface()
self.robot_cmd = RobotCommander()
self.Pose_goal = Pose()
self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM)
# robot_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
self.robot_arm.set_goal_orientation_tolerance(0.005)
self.robot_arm.set_planning_time(5)
self.robot_arm.set_num_planning_attempts(5)
rospy.sleep(2)
# Allow replanning to increase the odds of a solution
self.robot_arm.allow_replanning(True)
def move_code(self):
# self.robot_arm.set_named_target("up") #go to goal state.
# self.robot_arm.go()
# print("====== move plan go to home 1 ======")
# rospy.sleep(2)
# print("====== move plan go to up ======")
# self.robot_arm.set_named_target("zeros") #go to goal state.
# self.robot_arm.go(wait=True)
# print("====== move plan go to zeros ======")
# rospy.sleep(1)
# robot_arm.set_named_target("up")
# robot_arm.go(wait=True)
self.robot_arm.set_pose_target(self.Pose_goal) # go to goal state.
self.robot_arm.go(True)
print("====== move plan go to Pose_goal ======")
#rospy.sleep(2)
robot_state = self.robot_arm.get_current_pose();
robot_angle = self.robot_arm.get_current_joint_values();
print(robot_state)
def callback(self, data):
rospy.loginfo(rospy.get_caller_id() + "I heard %s", data)
self.Pose_goal = data
self.move_code()
def listener(self):
rospy.Subscriber("chatter", Pose, self.callback)
class TestMove():
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('ur3_move', anonymous=True)
self.scene = PlanningSceneInterface()
self.robot_cmd = RobotCommander()
self.Pose_goal = Pose()
self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM)
# robot_gripper = MoveGroupComm # self.robot_arm.set_named_target("up") #go to goal state.
# self.robot_arm.go()
# print("====== move plan go to home 1 ======")
# rospy.sleep(2)
# print("====== move plan go to home 1 ======")
# rospy.sleep(2)
# print("====== move plan go to up ======")
# self.robot_arm.set_named_target("zeros") #go to goal state.
# self.robot_arm.go(wait=True)
# print("====== move plan go to zeros ======")
# rospy.sleep(1)
# robot_arm.set_named_target("up")
# robot_arm.go(wait=True)
Pose_goal.header.frame_id = 'rightbase_link'
Pose_goal.pose.position.x = -0.1955793462195291 # red line 0.2 0.2
Pose_goal.pose.position.y = 0.3456909607161672 # green line 0.15 0.15
Pose_goal.pose.position.z = 0.16049011785234568 # blue line # 0.35 0.6
# Pose_goal.pose.orientation = start_pose.orientation
Pose_goal.pose.orientation.x = 0.28520761755123414
Pose_goal.pose.orientation.y = 0.24468120052517786
Pose_goal.pose.orientation.z = 0.6034841927127607
Pose_goal.pose.orientation.w = 0.7032741671255489
self.robot_arm.set_pose_target(self.Pose_goal) # go to goal state.
self.robot_arm.go(True)
print("====== move plan go to Pose_goal ======")
#rospy.sleep(2)
robot_state = self.robot_arm.get_current_pose()
robot_angle = self.robot_arm.get_current_joint_values()
print(robot_state)
def callback(self, data):
rospy.loginfo(rospy.get_caller_id() + "I heard %s", data)
self.Pose_goal = data
self.move_code()
def listener(self):
rospy.Subscriber("chatter", Pose, self.callback)
def move_to(self,
pos_x,
pos_y,
pos_z,
orien_x,
orien_y,
orien_z,
orien_w,
ik,
orien_const=None):
#Construct the request
request = GetPositionIKRequest()
request.ik_request.group_name = "right_arm"
request.ik_request.ik_link_name = "right_gripper"
request.ik_request.attempts = 20
request.ik_request.pose_stamped.header.frame_id = "base"
#Set the desired orientation for the end effector HERE
request.ik_request.pose_stamped.pose.position.x = pos_x
request.ik_request.pose_stamped.pose.position.y = pos_y
request.ik_request.pose_stamped.pose.position.z = pos_z
request.ik_request.pose_stamped.pose.orientation.x = orien_x
request.ik_request.pose_stamped.pose.orientation.y = orien_y
request.ik_request.pose_stamped.pose.orientation.z = orien_z
request.ik_request.pose_stamped.pose.orientation.w = orien_w
try:
#Send the request to the service
response = ik(request)
#Print the response HERE
print(response)
group = MoveGroupCommander("right_arm")
if orien_const is not None:
constraints = Constraints()
constraints.orientation_constraints = orien_const
group.set_path_constraints(constraints)
# Setting position and orientation target
group.set_pose_target(request.ik_request.pose_stamped)
# TRY THIS
# Setting just the position without specifying the orientation
# group.set_position_target([0.5, 0.5, 0.0])
# Plan IK and execute
group.go()
except rospy.ServiceException, e:
print("Service call failed: %s")
def run(self, cycle, verbose = 2):
#enable robot if not already done
enableProcess = subprocess.Popen(["rosrun", "baxter_tools",
"enable_robot.py", "-e"])
enableProcess.wait()
#start moveit servers. Since we do not know how long it will take them
#to start, pause for 15 seconds
jointServer = subprocess.Popen(["rosrun", "baxter_interface",
"joint_trajectory_action_server.py"])
planServer = subprocess.Popen(["roslaunch", "baxter_moveit_config",
"move_group.launch"])
time.sleep(15)
raw_input("press enter")
try:
#left = MoveGroupCommander("left_arm")
right = MoveGroupCommander("right_arm")
upRight = geometry_msgs.msg.Pose(position = geometry_msgs.msg.Point(
x = 0.9667369015076861, y = -0.1190874231664102, z = -0.07489146157788866 ))
#upLeft = geometry_msgs.msg.Pose(position = geometry_msgs.msg.Point(
#x = 0.7, y = 0.35, z = 0.8))
#downRight = geometry_msgs.msg.Pose(position = geometry_msgs.msg.Point(
#x = 0.7, y = -0.45, z = 0.3))
#downLeft = geometry_msgs.msg.Pose(position = geometry_msgs.msg.Point(
#x = 0.7, y = 0.45, z = 0.3))
right.set_pose_target(upRight)
#left.set_pose_target(upLeft)
right.go()
#left.go()
#right.set_pose_target(downRight)
#left.set_pose_target(downLeft)
right.go()
#left.go()
finally:
#clean up - kill servers
jointServer.kill()
planServer.kill()
sys.exit()
class TestMove():
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('ur3_move', anonymous=True)
self.scene = PlanningSceneInterface()
self.robot_cmd = RobotCommander()
self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM)
#robot_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
self.robot_arm.set_goal_orientation_tolerance(0.005)
self.robot_arm.set_planning_time(5)
self.robot_arm.set_num_planning_attempts(5)
self.pose_goal = Pose()
rospy.sleep(2)
# Allow replanning to increase the odds of a solution
self.robot_arm.allow_replanning(True)
def move_code(self):
self.robot_arm.set_named_target("home") #go to goal state.
self.robot_arm.go(wait=True)
print("====== move plan go to home 1 ======")
rospy.sleep(0.5)
# print("====== move plan go to up ======")
self.robot_arm.set_named_target("up") #go to goal state.
self.robot_arm.go(wait=True)
print("====== move plan go to up ======")
rospy.sleep(0.5)
self.robot_arm.set_named_target("home") #go to goal state.
self.robot_arm.go(wait=True)
print("====== move plan go to home 1 ======")
rospy.sleep(0.5)
# robot_arm.set_named_target("up")
# robot_arm.go(wait=True)
robot_state = self.robot_arm.get_current_pose()
robot_angle = self.robot_arm.get_current_joint_values()
print(robot_state)
def move_TF(self):
self.ee_TF_list = [-2.046, 1.121, 0.575, 0.453, 0.561, -0.435, 0.538]
self.pose_goal.position.x = self.ee_TF_list[0]
self.pose_goal.position.y = self.ee_TF_list[1]
self.pose_goal.position.z = self.ee_TF_list[2]
self.pose_goal.orientation.x = self.ee_TF_list[3]
self.pose_goal.orientation.y = self.ee_TF_list[4]
self.pose_goal.orientation.z = self.ee_TF_list[5]
self.pose_goal.orientation.w = self.ee_TF_list[6]
self.robot_arm.set_pose_target(self.pose_goal)
self.robot_arm.go(True)
class TestDensoMoveit(unittest.TestCase):
arm = None
def setUp(self):
self.arm = MoveGroupCommander("manipulator")
def test_moveit(self):
p = [0.35, -0.35, 0.4]
pose = PoseStamped(
header=rospy.Header(stamp=rospy.Time.now(), frame_id='/BASE'),
pose=Pose(position=Point(*p),
orientation=Quaternion(
*quaternion_from_euler(1.57, 0, 1.57, 'sxyz'))))
self.arm.set_pose_target(pose)
self.assertTrue(self.arm.go() or self.arm.go())
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('rest')
right_arm.go()
# Move to the named "straight_forward" pose stored in the SRDF file
right_arm.set_named_target('wave')
right_arm.go()
rospy.sleep(2)
# Move back to the resting position at roughly 1/4 speed
right_arm.set_named_target('rest')
# Get back the planned trajectory
traj = right_arm.plan()
# Set the trajectory speed
new_traj = create_tracking_trajectory(traj, 1.0)
rospy.loginfo(new_traj)
# 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 movement(des_coor, i):
#Wait for the IK service to become available
rospy.wait_for_service('compute_ik')
received_info = True
#Create the function used to call the service
compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
while received_info == True:
#Construct the requests
request = GetPositionIKRequest()
request.ik_request.group_name = "left_arm"
request.ik_request.ik_link_name = "left_gripper"
request.ik_request.attempts = 30
request.ik_request.pose_stamped.header.frame_id = "base"
#Set the desired orientation for the end effector HERE
print(des_coor)
request.ik_request.pose_stamped.pose.position.x = des_coor[0]
request.ik_request.pose_stamped.pose.position.y = des_coor[1]
request.ik_request.pose_stamped.pose.position.z = 0.25 + initial_xyz[
2] # this should be a found constant
# Should be constant Determined by Experiment
request.ik_request.pose_stamped.pose.orientation.x = 0.0
request.ik_request.pose_stamped.pose.orientation.y = 1.0
request.ik_request.pose_stamped.pose.orientation.z = 0.0
request.ik_request.pose_stamped.pose.orientation.w = 0.0
try:
#Send the request to the service
response = compute_ik(request)
#Print the response HERE
print(response)
if response.error_code.val == 1:
received_info = False
else:
received_info = False
# received_info = False
group = MoveGroupCommander("left_arm")
# Setting position and orientation target
group.set_pose_target(request.ik_request.pose_stamped)
# Plan IK and execute
group.go()
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def __init__(self, pose):
self.check_collision = False
# 初始化move_group的API
moveit_commander.roscpp_initialize(sys.argv)
# 初始化ROS节点
rospy.init_node('moveit_obstacles_demo')
# 初始化场景对象
scene = PlanningSceneInterface()
# 创建一个发布场景变化信息的发布者
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5)
# 创建一个存储物体颜色的字典对象
self.colors = dict()
# 等待场景准备就绪
rospy.sleep(1)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('arm')
# 获取终端link的名称
end_effector_link = arm.get_end_effector_link()
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.01)
arm.set_goal_orientation_tolerance(0.05)
# 当运动规划失败后，允许重新规划
arm.allow_replanning(True)
# 设置目标位置所使用的参考坐标系
reference_frame = 'base_link'
arm.set_pose_reference_frame(reference_frame)
# 设置每次运动规划的时间限制：5s
arm.set_planning_time(5)
# 将场景中的颜色设置发布
self.sendColors()
rospy.sleep(5)
joint_positions = pose
arm.set_joint_value_target(joint_positions)
# 控制机械臂完成运动
if arm.go():
pass
else:
self.check_collision = True
rospy.sleep(1)
# 关闭并退出moveit
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
def talker_by13():
#init
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_fk_demo')
#cartesian = rospy.get_param('~cartesian', True)
arm = MoveGroupCommander('manipulator')
arm.set_pose_reference_frame('base_link')
arm.allow_replanning(True)
arm.set_goal_position_tolerance(0.001)
arm.set_goal_orientation_tolerance(0.001)
# arm.set_max_acceleration_scaling_factor(0.5)
#arm.set_max_velocity_scaling_factor(0.5)
end_effector_link = arm.get_end_effector_link()
#arm.set_named_target('home')
arm.set_named_target('up')
arm.go()
rospy.sleep(2)
target_pose = PoseStamped()
target_pose.header.frame_id = 'base_link'
target_pose.header.stamp = rospy.Time.now()
target_pose.pose.position.x = 0.86
target_pose.pose.position.y = 0.25
target_pose.pose.position.z = 0.02832
target_pose.pose.orientation.x = 0
target_pose.pose.orientation.y = 0
target_pose.pose.orientation.z = 0
target_pose.pose.orientation.w = 1
#next: find workspace
arm.set_start_state_to_current_state()
arm.set_pose_target(target_pose, end_effector_link)
traj = arm.plan()
arm.execute(traj)
#arm.shift_pose_target(2,-0.05,end_effector_link)
#arm.go()
rospy.sleep(2)
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):
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_test')
arm = MoveGroupCommander('arm')
end_effector_link = arm.get_end_effector_link()
arm.allow_replanning(True)
arm.set_planning_time(5)
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_init_orientation = Quaternion()
target_init_orientation = quaternion_from_euler(0.0, 1.57, 0.0)
self.setPose(target_pose, [0.5, 0.2, 0.5],list(target_init_orientation))
current_pose = arm.get_current_pose(end_effector_link)
self.setPose(current_pose, [current_pose.pose.position.x, current_pose.pose.position.y, current_pose.pose.position.z], list(target_init_orientation))
arm.set_pose_target(current_pose)
arm.go()
rospy.sleep(2)
constraints = Constraints()
orientation_constraint = OrientationConstraint()
constraints.name = 'gripper constraint'
orientation_constraint.header = target_pose.header
orientation_constraint.link_name = end_effector_link
orientation_constraint.orientation.x = target_init_orientation[0]
orientation_constraint.orientation.y = target_init_orientation[1]
orientation_constraint.orientation.z = target_init_orientation[2]
orientation_constraint.orientation.w = target_init_orientation[3]
orientation_constraint.absolute_x_axis_tolerance = 0.03
orientation_constraint.absolute_y_axis_tolerance = 0.03
orientation_constraint.absolute_z_axis_tolerance = 3.14
orientation_constraint.weight = 1.0
constraints.orientation_constraints.append(orientation_constraint)
arm.set_path_constraints(constraints)
arm.set_pose_target(target_pose, end_effector_link)
arm.go()
rospy.sleep(1)
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 = rospy.get_param('~cartesian', True)
# 初始化需要使用move group控制的机械臂中的arm group
arm = MoveGroupCommander('arm')
# 当运动规划失败后，允许重新规划
arm.allow_replanning(False)
# 设置目标位置所使用的参考坐标系
reference_frame = 'base_link'
arm.set_pose_reference_frame(reference_frame)
# 设置位置(单位：米)和姿态（单位：弧度）的允许误差
arm.set_goal_position_tolerance(0.0001)
arm.set_goal_orientation_tolerance(0.0001)
arm.set_max_velocity_scaling_factor(0.5)
# 获取终端link的名称
eef_link = arm.get_end_effector_link()
scene = moveit_commander.PlanningSceneInterface()
print "[INFO] Current pose:", arm.get_current_pose(eef_link).pose
# 控制机械臂运动到之前设置的姿态
# arm.set_named_target('pick_6')
arm.set_named_target('home')
arm.go()
self.box_name = ''
self.scene = scene
self.group = arm
self.eef_link = eef_link
self.reference_frame = reference_frame
self.moveit_commander = moveit_commander
self.move_distance = 0.1
self.back_distance = 0.15
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 move_action(x):
#Create the function used to call the service
compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
step = 0
pose = [x]
if step == 0:
s = pose[step]
request = GetPositionIKRequest()
request.ik_request.group_name = "left_arm"
request.ik_request.ik_link_name = "left_gripper"
request.ik_request.attempts = 20
request.ik_request.pose_stamped.header.frame_id = "ar_marker_0"
request.ik_request.pose_stamped.pose.position.x = s[0]
request.ik_request.pose_stamped.pose.position.y = s[1]
request.ik_request.pose_stamped.pose.position.z = s[2]
request.ik_request.pose_stamped.pose.orientation.x = s[3]
request.ik_request.pose_stamped.pose.orientation.y = s[4]
request.ik_request.pose_stamped.pose.orientation.z = s[5]
request.ik_request.pose_stamped.pose.orientation.w = s[6]
response = compute_ik(request)
group = MoveGroupCommander("left_arm")
group.set_pose_target(request.ik_request.pose_stamped)
# orien_const = OrientationConstraint()
# orien_const.link_name = "left_arm"
# orien_const.header.frame_id = "base"
# orien_const.orientation.x = 0.0
# orien_const.orientation.y = 1.0
# orien_const.orientation.z = 0.0
# orien_const.absolute_x_axis_tolerance = 0.1
# orien_const.absolute_y_axis_tolerance = 0.1
# orien_const.absolute_z_axis_tolerance = 0.1
# orien_const.weight = 1.0
# consts = Constraints()
# consts.orientation_constraints = [orien_const]
# group.set_path_constraints(consts)
group.go()
step = step + 1
class Robot:
def __init__(self, config, debug=0):
print "[INFO] Initialise Robot"
self.DEBUG = debug
# configuration
self.config = config
# initialise move groups
self.arm = MoveGroupCommander(ARM_GROUP)
self.gripper = MoveGroupCommander(GRIPPER_GROUP)
# initialise pick and place manager
if self.DEBUG is 1:
# verbose mode
self.pick_and_place = PickPlaceInterface(ARM_GROUP, GRIPPER_GROUP,
False, True)
else:
# non verbose mode
self.pick_and_place = PickPlaceInterface(ARM_GROUP, GRIPPER_GROUP,
False, False)
# tolerance: position (in m), orientation (in rad)
self.arm.set_goal_position_tolerance(0.01)
self.arm.set_goal_orientation_tolerance(0.1)
self.place_manager = None
self.pick_manager = None
self.initialise_move_actions()
self.start_position()
"""Initialise the place and pick manager.
"""
def initialise_move_actions(self):
self.place_manager = PlaceManager(self.arm, self.pick_and_place,
self.config, self.DEBUG)
self.pick_manager = PickManager(self.arm, self.pick_and_place,
self.DEBUG)
"""Move robot arm to "start position".
"""
def start_position(self):
self.arm.set_named_target(START_POSITION)
self.arm.go()
rospy.sleep(2)
def main():
#Wait for the IK service to become available
rospy.wait_for_service('compute_ik')
rospy.init_node('service_query')
#Create the function used to call the service
compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
while not rospy.is_shutdown():
raw_input('Press [ Enter ]: ')
#Construct the request
request = GetPositionIKRequest()
request.ik_request.group_name = "left_arm"
request.ik_request.ik_link_name = "left_hand"
request.ik_request.attempts = 20
request.ik_request.pose_stamped.header.frame_id = "base"
#Set the desired orientation for the end effector HERE
request.ik_request.pose_stamped.pose.position.x = 0.5
request.ik_request.pose_stamped.pose.position.y = 0.5
request.ik_request.pose_stamped.pose.position.z = 0.3
request.ik_request.pose_stamped.pose.orientation.x = 0.0
request.ik_request.pose_stamped.pose.orientation.y = 1.0
request.ik_request.pose_stamped.pose.orientation.z = 0.0
request.ik_request.pose_stamped.pose.orientation.w = 0.0
try:
#Send the request to the service
response = compute_ik(request)
#Print the response HERE
print(response)
group = MoveGroupCommander("left_arm")
#this command tries to achieve a pose: which is position, orientation
group.set_pose_target(request.ik_request.pose_stamped)
#then, this command tries to achieve a position only. orientation is airbitrary.
group.set_position_target([0.5,0.5,0.3])
group.go()
except rospy.ServiceException, e:
print "Service call failed: %s"%e
class Tiago():
def __init__(self, group_name='arm_torso'):
self.rate = rospy.Rate(10)
self.robot = RobotCommander()
rospy.sleep(1)
self.move_group = MoveGroupCommander(group_name)
self.velocity = Twist()
self.velocity_publisher = rospy.Publisher(
'/mobile_base_controller/cmd_vel', Twist, queue_size=10)
def move(self, linear=(0, 0, 0), angular=(0, 0, 0)):
self.velocity.linear.x = linear[
0] # Forward or Backward with in m/sec.
self.velocity.linear.y = linear[1]
self.velocity.linear.z = linear[2]
self.velocity.angular.x = angular[0]
self.velocity.angular.y = angular[1]
self.velocity.angular.z = angular[
2] # Anti-clockwise/clockwise in radians per sec
self.velocity_publisher.publish(self.velocity)
def send_arm_goal(self, frame_id='base_footprint'):
# SET EEF GOAL
goal_pose = PoseStamped()
goal_pose.header.frame_id = 'base_footprint'
goal_pose.pose.position.x = 0.55
goal_pose.pose.position.y = -0.30
goal_pose.pose.position.z = 0.76
goal_pose.pose.orientation = Quaternion(
*quaternion_from_euler(PI / 2, 0.0, 0.0))
# SEND GOAL
self.move_group.set_pose_reference_frame(frame_id)
self.move_group.set_pose_target(goal_pose)
# EXECUTE EEF GOAL
# print(dir(self.move_group))
self.move_group.go()
def main():
#Wait for the IK service to become available
rospy.wait_for_service('compute_ik')
rospy.init_node('service_query')
#Create the function used to call the service
compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
while not rospy.is_shutdown():
raw_input('Press [ Enter ]: ')
#Construct the request
request = moveit_msgs.srv.GetPositionIKRequest() #asdf
request.ik_request.group_name = "left_arm"
request.ik_request.ik_link_name = "left_gripper"
request.ik_request.attempts = 20
request.ik_request.pose_stamped.header.frame_id = "base"
#Set the desired orientation for the end effector HERE
request.ik_request.pose_stamped.pose.position.x = 0.5
request.ik_request.pose_stamped.pose.position.y = 0.5
request.ik_request.pose_stamped.pose.position.z = 0.0
request.ik_request.pose_stamped.pose.orientation.x = 0.0
request.ik_request.pose_stamped.pose.orientation.y = 1.0
request.ik_request.pose_stamped.pose.orientation.z = 0.0
request.ik_request.pose_stamped.pose.orientation.w = 0.0
try:
#Send the request to the service
response = compute_ik(request)
#Print the response HERE
print(response)
group = MoveGroupCommander("left_arm")
# Setting position and orientation target
group.set_pose_target(request.ik_request.pose_stamped)
# TRY THIS
# Setting just the position without specifying the orientation
###group.set_position_target([0.5, 0.5, 0.0])
# Plan IK and execute
group.go()
except rospy.ServiceException, e:
print "Service call failed: %s"%e
def talker_by13():
#init
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_fk_demo')
cartesian = rospy.get_param('~cartesian', True)
arm = MoveGroupCommander('manipulator')
#arm.set_pose_reference_frame('base_link')
#arm.set_goal_position_tolerance(0.001)
# arm.set_goal_orientation_tolerance(0.001)
# arm.set_max_acceleration_scaling_factor(0.5)
#arm.set_max_velocity_scaling_factor(0.5)
end_effector_link = arm.get_end_effector_link()
arm.set_named_target('home')
arm.go()
rospy.sleep(1)
start_pose = arm.get_current_pose(end_effector_link).pose
waypoints = []
wpose = deepcopy(start_pose)
wpose.position.x -= 0.2
wpose.position.y -= 0.2
#wpose.position.z += 0.6
#wpose.position.x = 0.8
#wpose.position.y = 0.9
#wpose.position.z = 0.9
arm.set_pose_target(wpose)
arm.go()
rospy.sleep(1)
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
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 main():
rospy.init_node('arm_start_up', anonymous=True)
scene = PlanningSceneInterface()
robot = RobotCommander()
right_arm = MoveGroupCommander("right_arm")
left_arm = MoveGroupCommander("left_arm")
right_arm.set_planner_id("RRTConnect:");
left_arm.set_planner_id("RRTConnect:")
# move to a random target
#group.set_named_target("ahead")
#group.go()
#rospy.sleep(1)
right_arm.set_named_target("right_start")
right_arm.go()
rospy.sleep(1)
left_arm.set_named_target("left_start")
left_arm.go()
rospy.sleep(1)
class Robot:
def __init__(self, config, debug=0):
print "[INFO] Initialise Robot"
self.DEBUG = debug
# configuration
self.config = config
# initialise move groups
self.arm = MoveGroupCommander(ARM_GROUP)
self.gripper = MoveGroupCommander(GRIPPER_GROUP)
# initialise pick and place manager
if self.DEBUG is 1:
# verbose mode
self.pick_and_place = PickPlaceInterface(ARM_GROUP, GRIPPER_GROUP, False, True)
else:
# non verbose mode
self.pick_and_place = PickPlaceInterface(ARM_GROUP, GRIPPER_GROUP, False, False)
# tolerance: position (in m), orientation (in rad)
self.arm.set_goal_position_tolerance(0.01)
self.arm.set_goal_orientation_tolerance(0.1)
self.place_manager = None
self.pick_manager = None
self.initialise_move_actions()
self.start_position()
"""Initialise the place and pick manager.
"""
def initialise_move_actions(self):
self.place_manager = PlaceManager(self.arm, self.pick_and_place, self.config, self.DEBUG)
self.pick_manager = PickManager(self.arm, self.pick_and_place, self.DEBUG)
"""Move robot arm to "start position".
"""
def start_position(self):
self.arm.set_named_target(START_POSITION)
self.arm.go()
rospy.sleep(2)
def __init__(self):
group = MoveGroupCommander("arm")
#group.set_orientation_tolerance([0.3,0.3,0,3])
p = PoseStamped()
p.header.frame_id = "/katana_base_link"
p.pose.position.x = 0.4287
#p.pose.position.y = -0.0847
p.pose.position.y = 0.0
p.pose.position.z = 0.4492
q = quaternion_from_euler(2, 0, 0)
p.pose.orientation.x = q[0]
p.pose.orientation.y = q[1]
p.pose.orientation.z = q[2]
p.pose.orientation.w = q[3]
print "Planning frame: " ,group.get_planning_frame()
print "Pose reference frame: ",group.get_pose_reference_frame()
#group.set_pose_reference_frame("katana_base_link")
print "RPy target: 0,0,0"
#group.set_rpy_target([0, 0, 0],"katana_gripper_tool_frame")
#group.set_position_target([0.16,0,0.40], "katana_gripper_tool_frame")
group.set_pose_target(p, "katana_gripper_tool_frame")
group.go()
print "Current rpy: " , group.get_current_rpy("katana_gripper_tool_frame")
class MoveitCommanderMoveGroupState(EventState):
"""
Uses moveit commander to plan to the given joint configuration and execute the resulting trajctory.
"""
def __init__(self, planning_group, joint_names):
"""Constructor"""
super(MoveitCommanderMoveGroupState, self).__init__(outcomes=['reached', 'failed'],
input_keys=['target_joint_config'])
self._planning_group = planning_group
self._joint_names = joint_names
Logger.loginfo("About to make mgc in init with group %s" % self._planning_group)
self.group_to_move = MoveGroupCommander(self._planning_group)
Logger.loginfo("finished making mgc in init.")
self._done = False
def execute(self, userdata):
"""Execute this state"""
if self._done is not False:
return self._done
def on_enter(self, userdata):
# create the motion goal
Logger.loginfo("Entering MoveIt Commander code!")
if len(self._joint_names) != len(userdata.target_joint_config):
Logger.logwarn("Number of joint names (%d) does not match number of joint values (%d)"
% (len(self._joint_names), len(userdata.target_joint_config)))
self.group_to_move.set_joint_value_target(dict(zip(self._joint_names, userdata.target_joint_config)))
# execute the motion
try:
Logger.loginfo("Moving %s to: %s" % (self._planning_group, ", ".join(map(str, userdata.target_joint_config))))
result = self.group_to_move.go()
except Exception as e:
Logger.logwarn('Was unable to execute move group request:\n%s' % str(e))
self._done = "failed"
if result:
self._done = "reached"
else:
self._done = "failed"
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)
robot = RobotCommander()
# Connect to the right_arm move group
right_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Increase the planning time since contraint planning can take a while
right_arm.set_planning_time(15)
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Set the right arm reference frame
right_arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow some leeway in position(meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.05)
right_arm.set_goal_orientation_tolerance(0.1)
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Start in the "resting" configuration stored in the SRDF file
right_arm.set_named_target('resting')
# Plan and execute a trajectory to the goal configuration
right_arm.go()
rospy.sleep(1)
# Open the gripper
right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
right_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.237012590198
target_pose.pose.position.y = -0.0747191267505
target_pose.pose.position.z = 0.901578401949
target_pose.pose.orientation.w = 1.0
# Set the start state and target pose, then plan and execute
right_arm.set_start_state(robot.get_current_state())
right_arm.set_pose_target(target_pose, end_effector_link)
right_arm.go()
rospy.sleep(2)
# Close the gripper
right_gripper.set_joint_value_target(GRIPPER_CLOSED)
right_gripper.go()
rospy.sleep(1)
# Store the current pose
start_pose = right_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 = right_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 = 3.14
orientation_constraint.weight = 1.0
# Append the constraint to the list of contraints
constraints.orientation_constraints.append(orientation_constraint)
# Set the path constraints on the right_arm
right_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.173187824708
target_pose.pose.position.y = -0.0159929871606
target_pose.pose.position.z = 0.692596608605
target_pose.pose.orientation.w = 1.0
# Set the start state and target pose, then plan and execute
right_arm.set_start_state_to_current_state()
right_arm.set_pose_target(target_pose, end_effector_link)
right_arm.go()
rospy.sleep(1)
# Clear all path constraints
right_arm.clear_path_constraints()
# Open the gripper
right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
right_gripper.go()
rospy.sleep(1)
# Return to the "resting" configuration stored in the SRDF file
right_arm.set_named_target('resting')
# Plan and execute a trajectory to the goal configuration
right_arm.go()
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit MoveIt
moveit_commander.os._exit(0)
# right_arm.set_position_target([.75,-0.3, 1])
# right_arm.go()
# rospy.sleep(1)
grasps = []
g = Grasp()
g.id = "test"
grasp_pose = PoseStamped()
grasp_pose.header.frame_id = "base_link"
grasp_pose.pose.position.x = 0.47636
grasp_pose.pose.position.y = -0.21886
grasp_pose.pose.position.z = 0.7164
grasp_pose.pose.orientation.x = 0.00080331
grasp_pose.pose.orientation.y = 0.001589
grasp_pose.pose.orientation.z = -2.4165e-06
grasp_pose.pose.orientation.w = 1
rospy.logwarn("moving to arm")
right_arm.set_pose_target(grasp_pose)
right_arm.go()
rospy.sleep(3)
rospy.sleep(2)
rospy.logwarn("pick part")
# pick the object
robot.right_arm.pick("part")
rospy.spin()
roscpp_shutdown()
from std_msgs.msg import Header
moveit_error_dict = {}
for name in MoveItErrorCodes.__dict__.keys():
if not name[:1] == '_':
code = MoveItErrorCodes.__dict__[name]
moveit_error_dict[code] = name
if __name__=='__main__':
rospy.init_node("pose_goal_test11")
rospy.loginfo("Starting up move group commander for right arm")
right_arm_mgc = MoveGroupCommander("right_arm_torso")
goal_point = Point(0.4, -0.2, 1.1)
goal_ori = Quaternion(0.0,0.0,0.0,1.0)
right_arm_mgc.set_pose_reference_frame('base_link')
list_goals = []
for i in range(1):
goal_point.z += 0.05
rospy.loginfo(str(i) + ": Setting new goal:\n " + str(goal_point))
list_goals.append(Pose(goal_point, goal_ori))
rospy.loginfo("list of goals:\n" + str(list_goals))
right_arm_mgc.set_pose_targets(list_goals)
rospy.loginfo("go()")
right_arm_mgc.go()
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_obstacles_demo')
# Construct the initial scene object
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5)
# Create a dictionary to hold object colors
self.colors = dict()
# Pause for the scene to get ready
rospy.sleep(1)
# Initialize the move group for the right arm
arm = MoveGroupCommander(GROUP_NAME_ARM)
# 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.01)
arm.set_goal_orientation_tolerance(0.05)
# Allow replanning to increase the odds of a solution
arm.allow_replanning(True)
# Set the right arm reference frame accordingly
arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow 5 seconds per planning attempt
arm.set_planning_time(5)
# Give each of the scene objects a unique name
table_id = 'table'
box1_id = 'box1'
box2_id = 'box2'
# 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)
# Give the scene a chance to catch up
rospy.sleep(1)
# Start the arm in the "resting" pose stored in the SRDF file
arm.set_named_target('l_arm_init')
arm.go()
rospy.sleep(2)
# Set the height of the table off the ground
table_ground = 0.65
# Set the length, width and height of the table and boxes
table_size = [0.2, 0.7, 0.01]
box1_size = [0.1, 0.05, 0.05]
box2_size = [0.05, 0.05, 0.15]
# 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.35
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 = 0.3
box1_pose.pose.position.y = 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 = 0.3
box2_pose.pose.position.y = 0.25
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)
# 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)
# Send the colors to the planning scene
self.sendColors()
# Set the target pose in between the boxes and above the table
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.22
target_pose.pose.position.y = 0.14
target_pose.pose.position.z = table_pose.pose.position.z + table_size[2] + 0.05
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 target pose for the arm
arm.set_pose_target(target_pose, end_effector_link)
# Move the arm to the target pose (if possible)
arm.go()
# Pause for a moment...
rospy.sleep(2)
# Return the arm to the "resting" pose stored in the SRDF file
arm.set_named_target('l_arm_init')
arm.go()
# Exit MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
temp = set_compliance_slope(sl)
except rospy.ServiceException, e:
print "Service call failed: %s"%e
if __name__ == '__main__':
group = MoveGroupCommander("whole_arm")
rospy.init_node("temp_arm_demo")
target_position = [0.05, 0.0, 0.24] # x, y, z それぞれの位置
target_orientation = Quaternion(*quaternion_from_euler(0.0, 1.571, 0.0, 'sxyz'))
# 位置と傾きを用いて姿勢を生成
target_pose = Pose(Point(*target_position), target_orientation)
group.set_pose_target(target_pose)
# 姿勢への移動指示を実行
group.go()
time.sleep(1)
# 新たな姿勢を指定
target_position = [0.12, 0.12, 0.02]
target_pose = Pose(Point(*target_position), target_orientation)
group.set_pose_target(target_pose)
group.go()
time.sleep(1)
target_position = [0.05, 0.0, 0.24]
target_pose = Pose(Point(*target_position), target_orientation)
group.set_pose_target(target_pose)
group.go()
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)
cartesian = rospy.get_param('~cartesian', True)
# Connect to the right_arm move group
right_arm = MoveGroupCommander('right_arm')
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Set the right arm reference frame
right_arm.set_pose_reference_frame('base_footprint')
# Allow some leeway in position(meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.01)
right_arm.set_goal_orientation_tolerance(0.1)
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Start in the "straight_forward" configuration stored in the SRDF file
right_arm.set_named_target('straight_forward')
# Plan and execute a trajectory to the goal configuration
right_arm.go()
# Get the current pose so we can add it as a waypoint
start_pose = right_arm.get_current_pose(end_effector_link).pose
# Initialize the waypoints list
waypoints = []
# Set the first waypoint to be the starting pose
if cartesian:
# Append the pose to the waypoints list
waypoints.append(start_pose)
wpose = deepcopy(start_pose)
# Set the next waypoint back 0.2 meters and right 0.2 meters
wpose.position.x -= 0.2
wpose.position.y -= 0.2
if cartesian:
# Append the pose to the waypoints list
waypoints.append(deepcopy(wpose))
else:
right_arm.set_pose_target(wpose)
right_arm.go()
rospy.sleep(1)
# Set the next waypoint to the right 0.15 meters
wpose.position.x += 0.05
wpose.position.y += 0.15
wpose.position.z -= 0.15
if cartesian:
# Append the pose to the waypoints list
waypoints.append(deepcopy(wpose))
else:
right_arm.set_pose_target(wpose)
right_arm.go()
rospy.sleep(1)
if cartesian:
# Append the pose to the waypoints list
waypoints.append(deepcopy(start_pose))
else:
right_arm.set_pose_target(start_pose)
right_arm.go()
rospy.sleep(1)
if cartesian:
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.")
# Move normally back to the 'resting' position
right_arm.set_named_target('resting')
right_arm.go()
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit MoveIt
moveit_commander.os._exit(0)
while rospy.get_time() == 0.0: pass
### Create a handle for the Planning Scene Interface
psi = PlanningSceneInterface()
rospy.sleep(1.0)
### Create a handle for the Move Group Commander
mgc = MoveGroupCommander("arm")
rospy.sleep(1.0)
### Add virtual obstacle
pose = gen_pose(pos=[-0.2, -0.1, 1.2])
psi.add_box("box", pose, size=(0.15, 0.15, 0.6))
rospy.sleep(1.0)
### Move to stored joint position
mgc.set_named_target("left")
mgc.go()
### Move to Cartesian position
goal_pose = gen_pose(pos=[0.123, -0.417, 1.361], euler=[3.1415, 0.0, 1.5707])
mgc.go(goal_pose.pose)
### Move Cartesian linear
goal_pose.pose.position.z -= 0.1
(traj,frac) = mgc.compute_cartesian_path([goal_pose.pose], 0.01, 4, True)
mgc.execute(traj)
print "Done"
# Action Clients for Place
rospy.loginfo("Connecting to place AS")
place_ac = actionlib.SimpleActionClient('/place', PlaceAction)
place_ac.wait_for_server()
rospy.loginfo("Successfully connected")
rospy.sleep(1)
# take the part to specific pose
pose = generate_rand_position()
# IKFast
arm.set_position_target([pose.pose.position.x, pose.pose.position.y, pose.pose.position.z],
arm.get_end_effector_link())
# KDL
# arm.set_joint_value_target(pose, True)
arm.go()
rospy.sleep(5)
# ===== place start ==== #
place_result = place(PLANNING_GROUP, PICK_OBJECT, generate_place_pose())
rospy.loginfo("Place Result is:")
rospy.loginfo("Human readable error: " + str(moveit_error_dict[place_result.error_code.val]))
rospy.sleep(5)
# end
moveit_commander.roscpp_shutdown()
moveit_commander.os._exit(0)
class RazerControl():
def __init__(self):
self.pub_right_hand_pose = rospy.Publisher(RIGHT_HAND_POSESTAMPED_TOPIC, PoseStamped, latch=True)
self.pub_right_hand_pose_reference = rospy.Publisher(RIGHT_HAND_REFERENCE_POSESTAMPED_TOPIC, PoseStamped, latch=True)
self.pub_left_hand_pose = rospy.Publisher(LEFT_HAND_POSESTAMPED_TOPIC, PoseStamped, latch=True)
self.pub_left_hand_pose_reference = rospy.Publisher(LEFT_HAND_REFERENCE_POSESTAMPED_TOPIC, PoseStamped, latch=True)
self.hydra_data_subs = rospy.Subscriber(HYDRA_DATA_TOPIC, Hydra, self.hydraDataCallback)
self.pub_move_base = rospy.Publisher(MOVE_BASE_TOPIC, Twist)
self.subs = rospy.Subscriber('/joint_states', JointState, self.getJointStates)
self.current_joint_states = None
rospy.loginfo("Getting first joint_states")
while self.current_joint_states == None:
rospy.sleep(0.1)
rospy.loginfo("Gotten!")
rospy.loginfo("Connecting with right hand AS")
self.right_hand_as = actionlib.SimpleActionClient(HAND_RIGHT_AS, FollowJointTrajectoryAction)
self.right_hand_as.wait_for_server()
rospy.loginfo("Connecting with left hand AS")
self.left_hand_as = actionlib.SimpleActionClient(HAND_LEFT_AS, FollowJointTrajectoryAction)
self.left_hand_as.wait_for_server()
rospy.loginfo("Starting up move group commander for right, left, torso and head... (slow)")
self.right_arm_mgc = MoveGroupCommander("right_arm")
self.right_arm_mgc.set_pose_reference_frame('base_link')
self.left_arm_mgc = MoveGroupCommander("left_arm")
self.left_arm_mgc.set_pose_reference_frame('base_link')
self.torso_mgc = MoveGroupCommander("right_arm_torso")
self.torso_mgc.set_pose_reference_frame('base_link')
self.head_mgc = MoveGroupCommander("head")
self.head_mgc.set_pose_reference_frame('base_link')
self.last_hydra_message = None
self.tmp_pose_right = PoseStamped()
self.tmp_pose_left = PoseStamped()
self.read_message = False
def getJointStates(self, data):
self.current_joint_states = data
def create_hand_goal(self, hand_side="right", hand_pose="closed", values=0.0):
"""Returns the hand goal to send
possible poses: closed, open, intermediate"""
hand_goal = FollowJointTrajectoryGoal()
hand_goal.trajectory.joint_names.append('hand_'+ hand_side +'_thumb_joint')
hand_goal.trajectory.joint_names.append('hand_'+ hand_side +'_middle_joint')
hand_goal.trajectory.joint_names.append('hand_'+ hand_side +'_index_joint')
jtp = JointTrajectoryPoint()
joint_list = ['hand_'+ hand_side +'_thumb_joint',
'hand_'+ hand_side +'_middle_joint',
'hand_'+ hand_side +'_index_joint']
ids_list = []
values_list = []
rospy.loginfo("current_joint_state is:\n" + str(self.current_joint_states))
for joint in joint_list:
idx_in_message = self.current_joint_states.name.index(joint)
ids_list.append(idx_in_message)
values_list.append(self.current_joint_states.position[idx_in_message])
if hand_pose == "closed":
jtp.positions.append(2.0)
jtp.positions.append(values_list[1]) # TODO: read values and keep them
jtp.positions.append(values_list[2]) # TODO: read values and keep them
elif hand_pose == "open":
jtp.positions.append(0.0)
jtp.positions.append(values_list[1]) # TODO: read values and keep them
jtp.positions.append(values_list[2]) # TODO: read values and keep them
elif hand_pose == "intermediate":
jtp.positions.append(values_list[0]) # TODO: read values and keep them
jtp.positions.append(values)
jtp.positions.append(values)
jtp.velocities.append(0.0)
jtp.velocities.append(0.0)
jtp.velocities.append(0.0)
jtp.time_from_start.secs = 2
hand_goal.trajectory.points.append(jtp)
return hand_goal
def hydraDataCallback(self, data):
#rospy.loginfo("Received data from " + HYDRA_DATA_TOPIC)
self.last_hydra_message = data
self.tmp_pose_right = PoseStamped()
self.tmp_pose_right.header.frame_id = 'base_link'
self.tmp_pose_right.header.stamp = rospy.Time.now()
self.tmp_pose_right.pose.position.x = self.last_hydra_message.paddles[1].transform.translation.x
self.tmp_pose_right.pose.position.y = self.last_hydra_message.paddles[1].transform.translation.y
self.tmp_pose_right.pose.position.z = self.last_hydra_message.paddles[1].transform.translation.z
self.tmp_pose_right.pose.position.x += RIGHT_HAND_INITIAL_POINT.x
self.tmp_pose_right.pose.position.y += RIGHT_HAND_INITIAL_POINT.y
self.tmp_pose_right.pose.position.z += RIGHT_HAND_INITIAL_POINT.z
self.tmp_pose_right.pose.orientation = self.last_hydra_message.paddles[1].transform.rotation
self.tmp_pose_left = PoseStamped()
self.tmp_pose_left.header.frame_id = 'base_link'
self.tmp_pose_left.header.stamp = rospy.Time.now()
self.tmp_pose_left.pose.position.x = self.last_hydra_message.paddles[0].transform.translation.x
self.tmp_pose_left.pose.position.y = self.last_hydra_message.paddles[0].transform.translation.y
self.tmp_pose_left.pose.position.z = self.last_hydra_message.paddles[0].transform.translation.z
self.tmp_pose_left.pose.position.x += LEFT_HAND_INITIAL_POINT.x
self.tmp_pose_left.pose.position.y += LEFT_HAND_INITIAL_POINT.y
self.tmp_pose_left.pose.position.z += LEFT_HAND_INITIAL_POINT.z
self.tmp_pose_left.pose.orientation = self.last_hydra_message.paddles[0].transform.rotation
if self.last_hydra_message.paddles[1].buttons[0] == True:
self.pub_right_hand_pose.publish(self.tmp_pose_right)
if self.last_hydra_message.paddles[0].buttons[0] == True:
self.pub_left_hand_pose.publish(self.tmp_pose_left)
self.pub_right_hand_pose_reference.publish(self.tmp_pose_right)
self.pub_left_hand_pose_reference.publish(self.tmp_pose_left)
self.read_message = False
def run(self):
rospy.loginfo("Press LB / RB to send the current pose")
while self.last_hydra_message == None:
rospy.sleep(0.1)
rospy.loginfo("Got the first data of the razer... Now we can do stuff")
sleep_rate=0.05 # check at 20Hz
counter = 0
while True:
counter += 1
rospy.loginfo("Loop #" + str(counter))
if not self.read_message:
self.read_message = True
if self.last_hydra_message.paddles[1].buttons[0] == True:
# send curr left paddle pos to move_group right
rospy.loginfo("sending curr right hand")
self.right_arm_mgc.set_pose_target(self.tmp_pose_right)
self.right_arm_mgc.go(wait=False)
if self.last_hydra_message.paddles[0].buttons[0] == True:
# send curr right paddle pos to move_group left
rospy.loginfo("sending curr left hand")
self.left_arm_mgc.set_pose_target(self.tmp_pose_left)
self.left_arm_mgc.go(wait=False)
if self.last_hydra_message.paddles[1].trigger > 0.0:
# send goal right hand close proportional to trigger value (2.0 max?)
rospy.loginfo("Closing right hand to value: " + str(self.last_hydra_message.paddles[1].trigger * 2.0))
right_hand_goal = self.create_hand_goal(hand_side="right", hand_pose="intermediate", values=self.last_hydra_message.paddles[1].trigger * 2.0)
self.right_hand_as.send_goal(right_hand_goal)
if self.last_hydra_message.paddles[0].trigger > 0.0:
# send goal left hand close proportional to trigger value (2.0 max?)
rospy.loginfo("Closing left hand to value: " + str(self.last_hydra_message.paddles[0].trigger * 2.0))
left_hand_goal = self.create_hand_goal(hand_side="left", hand_pose="intermediate", values=self.last_hydra_message.paddles[0].trigger * 2.0)
self.left_hand_as.send_goal(left_hand_goal)
if self.last_hydra_message.paddles[1].joy[0] != 0.0:
# send torso rotation left(neg)/right (pos)
rospy.loginfo("Rotation torso")
curr_joint_val = self.torso_mgc.get_current_joint_values()
self.torso_mgc.set_joint_value_target("torso_1_joint", curr_joint_val[0] + (self.last_hydra_message.paddles[1].joy[0] * 0.1 * -1))
self.torso_mgc.go(wait=True)
rospy.loginfo("Rotation torso sent!")
if self.last_hydra_message.paddles[1].joy[1] != 0.0:
# send torso inclination front(pos)/back(neg)
rospy.loginfo("Inclination torso")
curr_joint_val = self.torso_mgc.get_current_joint_values()
self.torso_mgc.set_joint_value_target("torso_2_joint", curr_joint_val[1] + (self.last_hydra_message.paddles[1].joy[1] * 0.1))
self.torso_mgc.go(wait=True)
rospy.loginfo("Inclination torso sent!")
if self.last_hydra_message.paddles[0].joy[0] != 0.0 or self.last_hydra_message.paddles[0].joy[1] != 0.0:
twist_goal = Twist()
twist_goal.linear.x = 1.0 * self.last_hydra_message.paddles[0].joy[1]
twist_goal.angular.z = 1.0 * self.last_hydra_message.paddles[0].joy[0] * -1.0
self.pub_move_base.publish(twist_goal)
# move base rotate left (neg)/ right(pos)
rospy.loginfo("Move base")
if self.last_hydra_message.paddles[1].buttons[3] == True:
# thumb up
rospy.loginfo("Right thumb up")
right_thumb_up = self.create_hand_goal(hand_side="right", hand_pose="open")
self.right_hand_as.send_goal(right_thumb_up)
if self.last_hydra_message.paddles[0].buttons[3] == True:
# thumb up
rospy.loginfo("Left thumb up")
left_thumb_up = self.create_hand_goal(hand_side="left", hand_pose="open")
self.left_hand_as.send_goal(left_thumb_up)
if self.last_hydra_message.paddles[1].buttons[1] == True:
# thumb down
rospy.loginfo("Right thumb down")
right_thumb_up = self.create_hand_goal(hand_side="right", hand_pose="closed")
self.right_hand_as.send_goal(right_thumb_up)
if self.last_hydra_message.paddles[0].buttons[1] == True:
# thumb down
rospy.loginfo("Left thumb down")
left_thumb_up = self.create_hand_goal(hand_side="left", hand_pose="closed")
self.left_hand_as.send_goal(left_thumb_up)
rospy.sleep(sleep_rate)
def __init__(self):
rospy.init_node('arm_tracker')
rospy.on_shutdown(self.shutdown)
# Maximum distance of the target before the arm will lower
self.max_target_dist = 1.2
# Arm length to center of gripper frame
self.arm_length = 0.4
# Distance between the last target and the new target before we move the arm
self.last_target_threshold = 0.01
# Distance between target and end-effector before we move the arm
self.target_ee_threshold = 0.025
# Initialize the move group for the right arm
self.right_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Set the reference frame for pose targets
self.reference_frame = REFERENCE_FRAME
# Keep track of the last target pose
self.last_target_pose = PoseStamped()
# Set the right arm reference frame accordingly
self.right_arm.set_pose_reference_frame(self.reference_frame)
# Allow replanning to increase the chances of a solution
self.right_arm.allow_replanning(False)
# Set a position tolerance in meters
self.right_arm.set_goal_position_tolerance(0.05)
# Set an orientation tolerance in radians
self.right_arm.set_goal_orientation_tolerance(0.2)
# What is the end effector link?
self.ee_link = self.right_arm.get_end_effector_link()
# Create the transform listener
self.listener = tf.TransformListener()
# Queue up some tf data...
rospy.sleep(3)
# Set the gripper target to closed position using a joint value target
right_gripper.set_joint_value_target(GRIPPER_CLOSED)
# Plan and execute the gripper motion
right_gripper.go()
rospy.sleep(1)
# Subscribe to the target topic
rospy.wait_for_message('/target_pose', PoseStamped)
# Use queue_size=1 so we don't pile up outdated target messages
self.target_subscriber = rospy.Subscriber('/target_pose', PoseStamped, self.update_target_pose, queue_size=1)
rospy.loginfo("Ready for action!")
while not rospy.is_shutdown():
try:
target = self.target
except:
rospy.sleep(0.5)
continue
# Timestamp the target with the current time
target.header.stamp = rospy.Time()
# Get the target pose in the right_arm shoulder lift frame
#target_arm = self.listener.transformPose('right_arm_shoulder_pan_link', target)
target_arm = self.listener.transformPose('right_arm_base_link', target)
# Convert the position values to a Python list
p0 = [target_arm.pose.position.x, target_arm.pose.position.y, target_arm.pose.position.z]
# Compute the distance between the target and the shoulder link
dist_target_shoulder = euclidean(p0, [0, 0, 0])
# If the target is too far away, then lower the arm
if dist_target_shoulder > self.max_target_dist:
rospy.loginfo("Target is too far away")
self.right_arm.set_named_target('resting')
self.right_arm.go()
rospy.sleep(1)
continue
# Transform the pose to the base reference frame
target_base = self.listener.transformPose(self.reference_frame, target)
# Compute the distance between the current target and the last target
p1 = [target_base.pose.position.x, target_base.pose.position.y, target_base.pose.position.z]
p2 = [self.last_target_pose.pose.position.x, self.last_target_pose.pose.position.y, self.last_target_pose.pose.position.z]
dist_last_target = euclidean(p1, p2)
# Move the arm only if we are far enough away from the previous target
if dist_last_target < self.last_target_threshold:
rospy.loginfo("Still close to last target")
rospy.sleep(0.5)
continue
# Get the pose of the end effector in the base reference frame
ee_pose = self.right_arm.get_current_pose(self.ee_link)
# Convert the position values to a Python list
p3 = [ee_pose.pose.position.x, ee_pose.pose.position.y, ee_pose.pose.position.z]
# Compute the distance between the target and the end-effector
dist_target = euclidean(p1, p3)
# Only move the arm if we are far enough away from the target
if dist_target < self.target_ee_threshold:
rospy.loginfo("Already close enough to target")
rospy.sleep(1)
continue
# We want the gripper somewhere on the line connecting the shoulder and the target.
# Using a parametric form of the line, the parameter ranges from 0 to the
# minimum of the arm length and the distance to the target.
t_max = min(self.arm_length, dist_target_shoulder)
# Bring it back 10% so we don't collide with the target
t = 0.9 * t_max
# Now compute the target positions from the parameter
try:
target_arm.pose.position.x *= (t / dist_target_shoulder)
target_arm.pose.position.y *= (t / dist_target_shoulder)
target_arm.pose.position.z *= (t / dist_target_shoulder)
except:
rospy.sleep(1)
rospy.loginfo("Exception!")
continue
# Transform to the base_footprint frame
target_ee = self.listener.transformPose(self.reference_frame, target_arm)
# Set the target gripper orientation to be horizontal
target_ee.pose.orientation.x = 0
target_ee.pose.orientation.y = 0
target_ee.pose.orientation.z = 0
target_ee.pose.orientation.w = 1
# Update the current start state
self.right_arm.set_start_state_to_current_state()
# Set the target pose for the end-effector
self.right_arm.set_pose_target(target_ee, self.ee_link)
# Plan and execute the trajectory
success = self.right_arm.go()
if success:
# Store the current target as the last target
self.last_target_pose = target
# Pause a bit between motions to keep from locking up
rospy.sleep(0.5)
def __init__(self, script_path):
rospy.init_node('show_time')
rospy.on_shutdown(self.cleanup)
# Initialize a number of parameters and variables for nav locations
# 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)
# Initialize the move group for the arms
right_arm = MoveGroupCommander('right_arm')
left_arm = MoveGroupCommander('left_arm')
right_gripper = MoveGroupCommander('right_gripper')
left_gripper = MoveGroupCommander('left_gripper')
# Set the default TTS voice to use
self.voice = rospy.get_param("~voice", "voice_don_diphone")
self.robot = rospy.get_param("~robot", "robbie")
self.start =(Pose(Point(1.7, 0, 0.0), Quaternion(0.0, 0.0, 1.000, 0.018)))
self.left =(Pose(Point(1.960, -1.854, 0.0), Quaternion(0.0, 0.0, 0.916, 0.401)))
self.right =(Pose(Point(2.123, 1.592, 0.0), Quaternion(0.0, 0.0, 0.877, -0.480)))
self.auto_dock =(Pose(Point(0.5, 0, 0.0), Quaternion(0.0, 0.0, 0.002, 1.000)))
self.client = SimpleActionClient("move_base", MoveBaseAction)
self.client.wait_for_server()
# Create the sound client object
self.soundhandle = SoundClient()
# Wait a moment to let the client connect to the
# sound_play server
rospy.sleep(2)
# Make sure any lingering sound_play processes are stopped.
self.soundhandle.stopAll()
# Set the wave file path if used
self.wavepath = rospy.get_param("~wavepath", script_path + "/../sounds")
#define afternoon and morning
self.noon = strftime("%p:", localtime())
if self.noon == "AM:":
self.noon1 = "Goood Morning "
else:
self.noon1 ="Good After Noon "
self.local = strftime("%H:%M:", localtime())
#local time
self.local = strftime("%H:%M:", localtime())
self.soundhandle.say(self.noon1 + self.robot +" is on line" + " the time is " + self.local, self.voice)
right_arm.set_named_target('r_travel')
right_arm.go()
left_arm.set_named_target('l_travel')
left_arm.go()
rospy.sleep(5)
self.move_to(self.start)
self.soundhandle.say("Welcome to SHOW time. This is where I get to demonstrate my capabilities" , self.voice)
rospy.sleep(6)
self.soundhandle.say("I can move to my left", self.voice)
rospy.sleep(2)
self.move_to(self.left)
self.soundhandle.say("now back to the start position", self.voice)
rospy.sleep(2)
self.move_to(self.start)
self.soundhandle.say("I can move to my right", self.voice)
rospy.sleep(2)
self.move_to(self.right)
self.soundhandle.say("And again back to the start position", self.voice)
rospy.sleep(2)
self.move_to(self.start)
self.soundhandle.say("I once caught a fish this big", self.voice)
right_arm.set_named_target('r_fish')
right_arm.go()
left_arm.set_named_target('l_fish')
left_arm.go()
#rospy.sleep(2)
self.soundhandle.say("Thank you for your time ", self.voice)
right_arm.set_named_target('right_start')
right_arm.go()
left_arm.set_named_target('left_start')
left_arm.go()
#rospy.sleep(2)
#rospy.sleep(2)
self.move_to(self.auto_dock)
# add auto dock sequence here
# 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')
# Construct the initial scene object
scene = PlanningSceneInterface()
# Create a scene publisher to push changes to the scene
self.scene_pub = rospy.Publisher('planning_scene', PlanningScene)
# Create a dictionary to hold object colors
self.colors = dict()
# Pause for the scene to get ready
rospy.sleep(1)
# Initialize the move group for the right arm
right_arm = MoveGroupCommander('right_arm')
# Initialize the move group for the left arm
left_arm = MoveGroupCommander('left_arm')
right_arm.set_planner_id("KPIECEkConfigDefault");
left_arm.set_planner_id("KPIECEkConfigDefault");
rospy.sleep(1)
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# 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)
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# 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 5 seconds per planning attempt
right_arm.set_planning_time(5)
# Give each of the scene objects a unique name
table_id = 'table'
box1_id = 'box1'
box2_id = 'box2'
# 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)
# Give the scene a chance to catch up
rospy.sleep(1)
# Start the arm in the "resting" pose stored in the SRDF file
#left_arm.set_named_target('left_start')
#left_arm.go()
# 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 height of the table off the ground
table_ground = 0.75
# Set the length, width and height of the table and boxes
table_size = [0.2, 0.7, 0.01]
box1_size = [0.1, 0.05, 0.05]
box2_size = [0.05, 0.05, 0.15]
# 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.56
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 = 0.51
box1_pose.pose.position.y = -0.1
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 = 0.49
box2_pose.pose.position.y = 0.15
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)
# 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)
# Send the colors to the planning scene
self.sendColors()
# Set the target pose in between the boxes and above the table
target_pose = PoseStamped()
target_pose.header.frame_id = reference_frame
target_pose.pose.position.x = 0.4
target_pose.pose.position.y = 0.0
target_pose.pose.position.z = table_pose.pose.position.z + table_size[2] + 0.05
target_pose.pose.orientation.w = 1.0
# Set the target pose for the arm
right_arm.set_pose_target(target_pose, end_effector_link)
# Move the arm to the target pose (if possible)
right_arm.go()
# Pause for a moment...
rospy.sleep(2)
# Return the arm to the "resting" pose stored in the SRDF file
right_arm.set_named_target('right_start')
right_arm.go()
# Exit MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
if pose_st_target:
pub.publish(pose_st_target)
rospy.loginfo(trans)
rate.sleep()
if raw_input() == 'q':
sys.exit(1)
# move to a way point
pose_st_target.pose.position.z += 0.1
rospy.loginfo("set target to {}".format(pose_st_target.pose))
group.set_pose_target(pose_st_target.pose)
plan = group.plan()
rospy.loginfo("plan is {}".format(plan))
ret = group.go()
rospy.loginfo("executed ... {}".format(ret))
pose_st_target.pose.position.z -= 0.1
rospy.loginfo("set target to {}".format(pose_st_target.pose))
group.set_pose_target(pose_st_target.pose)
plan = group.plan()
rospy.loginfo("plan is {}".format(plan))
ret = group.go()
rospy.loginfo("executed ... {}".format(ret))
gripper_close()
pose_st_target.pose.position.y -= 0.2
pose_st_target.pose.position.z += 0.2
pub.publish(pose_st_target)
rospy.loginfo("set target to {}".format(pose_st_target.pose))
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)
p2.pose.orientation.w = b2[3]
#detach/remove current scene objects
print 'removing objects...'
robot.detach_object("bowl")
rospy.sleep(1)
scene.remove_world_object("bowl")
scene.remove_world_object("punch")
scene.remove_world_object("glovebox")
rospy.sleep(2)
#reset the gripper and arm position to home
robot.set_start_state_to_current_state()
robot.set_named_target("start_glove")
robot.go()
gripper.set_start_state_to_current_state()
gripper.go()
#add scene objects
print 'adding scene objects'
scene.add_mesh("bowl", p, "8inhemi.STL")
scene.add_mesh("punch", p1, "punch.STL")
scene.add_mesh("glovebox", p2, "GloveBox.stl")
print 'attaching bowl...'
robot.attach_object("bowl")
rospy.sleep(2)
currentbowlpose = p;
gripper.set_named_target("pinch")
gripper.go()
'ur5_arm_shoulder_lift_joint',
'ur5_arm_elbow_joint',
'ur5_arm_wrist_1_joint',
'ur5_arm_wrist_2_joint',
'ur5_arm_wrist_3_joint']
msg.trajectory.points.append(JointTrajectoryPoint(positions=[-1.57, -0.1745, -2.79, -1.57, 0, 0],
time_from_start=rospy.Duration(2)))
client.send_goal(msg)
client.wait_for_result()
# open
open_hand()
# reach
rospy.loginfo("reach")
arm.set_pose_target([0.90, 0.16, 0.255, 0, 0, 0])
arm.plan() and arm.go()
arm.plan() and arm.go()
# approach
rospy.loginfo("approach")
arm.set_pose_target([1.13, 0.16, 0.255, 0, 0, 0])
arm.plan() and arm.go()
# rotate
for i in range(4):
# close
rospy.loginfo("close")
close_hand()
# rotate
angles = arm.get_current_joint_values()
import numpy
start_angle = angles[5]
print("current angles=", start_angle)
print robot.get_current_state()
print ">>>>> Printing robot pose"
print group.get_current_pose()
## Planning to a Pose goal
print ">>>>> Generating plan"
pose_target = geometry_msgs.msg.Pose()
#pose_target.orientation.w = 1.0
pose_target.position.x = 0.5 #0.4
pose_target.position.y = 0.2 #0.2
pose_target.position.z = 0.2 #0.2
group.set_pose_target(pose_target)
plan = group.plan()
#print "============ Waiting while RVIZ displays plan..."
rospy.sleep(1)
print ">>>>> Go for plan"
group.go(wait=True)
## Adding/Removing Objects and Attaching/Detaching Objects
collision_object = moveit_msgs.msg.CollisionObject()
moveit_commander.roscpp_shutdown()
rospy.spin()
roscpp_shutdown()
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_demo')
#Initialize robot
robot = moveit_commander.RobotCommander()
# 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('gripper_pose', PoseStamped, queue_size=10)
# Create a publisher for displaying object frames
self.object_frames_pub = rospy.Publisher('object_frames', PoseStamped, queue_size=10)
## Create a publisher for visualizing direction ###
self.p_pub = rospy.Publisher('target', PoseStamped, latch=True, queue_size = 10)
# Create a dictionary to hold object colors
self.colors = dict()
# Initialize the MoveIt! commander for the arm
right_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the MoveIt! commander for the gripper
right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Allow 5 seconds per planning attempt
right_arm.set_planning_time(5)
# Prepare Gripper and open it
self.ac = actionlib.SimpleActionClient('r_gripper_controller/gripper_action',pr2c.Pr2GripperCommandAction)
self.ac.wait_for_server()
g_open = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.088, 100))
g_close = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.0, 1))
self.ac.send_goal(g_open)
# Give the scene a chance to catch up
rospy.sleep(2)
# Prepare Gazebo Subscriber
self.pwh = None
self.pwh_copy = None
self.idx_targ = None
self.gazebo_subscriber = rospy.Subscriber("/gazebo/model_states", ModelStates, self.model_state_callback)
rospy.sleep(2)
# PREPARE THE SCENE
while self.pwh is None:
rospy.sleep(0.05)
target_id = 'target'
self.taid = self.pwh.name.index('wood_cube_5cm')
table_id = 'table'
self.tid = self.pwh.name.index('table')
#obstacle1_id = 'obstacle1'
#self.o1id = self.pwh.name.index('wood_block_10_2_1cm')
# Remove leftover objects from a previous run
scene.remove_world_object(target_id)
scene.remove_world_object(table_id)
#scene.remove_world_object(obstacle1_id)
# Remove any attached objects from a previous session
scene.remove_attached_object(GRIPPER_FRAME, target_id)
# Set the target size [l, w, h]
target_size = [0.05, 0.05, 0.05]
table_size = [1.5, 0.8, 0.03]
#obstacle1_size = [0.1, 0.025, 0.01]
## Set the target pose on the table
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose = self.pwh.pose[self.taid]
target_pose.pose.position.z += 0.025
# Add the target object to the scene
scene.add_box(target_id, target_pose, target_size)
table_pose = PoseStamped()
table_pose.header.frame_id = REFERENCE_FRAME
table_pose.pose = self.pwh.pose[self.tid]
table_pose.pose.position.z += 1
scene.add_box(table_id, table_pose, table_size)
#obstacle1_pose = PoseStamped()
#obstacle1_pose.header.frame_id = REFERENCE_FRAME
#obstacle1_pose.pose = self.pwh.pose[self.o1id]
## Add the target object to the scene
#scene.add_box(obstacle1_id, obstacle1_pose, obstacle1_size)
# 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 = 0.50
place_pose.pose.position.y = -0.30
place_pose.pose.orientation.w = 1.0
# Add the target object to the scene
scene.add_box(target_id, target_pose, target_size)
### Make the target purple ###
self.setColor(target_id, 0.6, 0, 1, 1.0)
# Send the colors to the planning scene
self.sendColors()
#print target_pose
self.object_frames_pub.publish(target_pose)
rospy.sleep(2)
print "==================== Generating Transformations ==========================="
M1 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w])
M1[0,3] = target_pose.pose.position.x
M1[1,3] = target_pose.pose.position.y
M1[2,3] = target_pose.pose.position.z
M2 = transformations.euler_matrix(0, 1.57, 0)
M2[0,3] = 0.0 # offset about x
M2[1,3] = 0.0 # about y
M2[2,3] = 0.25 # about z
T = np.dot(M1, M2)
pre_grasping = deepcopy(target_pose)
pre_grasping.pose.position.x = T[0,3]
pre_grasping.pose.position.y = T[1,3]
pre_grasping.pose.position.z = T[2,3]
quat = transformations.quaternion_from_matrix(T)
pre_grasping.pose.orientation.x = quat[0]
pre_grasping.pose.orientation.y = quat[1]
pre_grasping.pose.orientation.z = quat[2]
pre_grasping.pose.orientation.w = quat[3]
pre_grasping.header.frame_id = 'gazebo_world'
# # Initialize the grasp object
# g = Grasp()
# grasps = []
# # Set the first grasp pose to the input pose
# g.grasp_pose = pre_grasping
# g.allowed_touch_objects = [target_id]
# grasps.append(deepcopy(g))
# right_arm.pick(target_id, grasps)
#Change the frame_id for the planning to take place!
#target_pose.header.frame_id = 'gazebo_world'
self.p_pub.publish(pre_grasping)
right_arm.set_pose_target(pre_grasping, GRIPPER_FRAME)
plan = right_arm.plan()
rospy.sleep(2)
right_arm.go(wait=True)
# 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')
# 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)
# Create a publisher for displaying gripper poses
self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped)
# Create a dictionary to hold object colors
self.colors = dict()
# Initialize the move group for the right arm
right_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Get the name of the end-effector link
end_effector_link = right_arm.get_end_effector_link()
# Allow some leeway in position (meters) and orientation (radians)
right_arm.set_goal_position_tolerance(0.05)
right_arm.set_goal_orientation_tolerance(0.1)
# Allow replanning to increase the odds of a solution
right_arm.allow_replanning(True)
# Set the right arm reference frame
right_arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow 10 seconds per planning attempt
right_arm.set_planning_time(10)
# Set a limit on the number of pick attempts before bailing
max_pick_attempts = 10
# Set a limit on the number of place attempts
max_place_attempts = 5
# 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 "resting" pose stored in the SRDF file
right_arm.set_named_target('right_start')
right_arm.go()
# Open the gripper to the neutral position
right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
right_gripper.go()
rospy.sleep(1)
# Set the height of the table off the ground
table_ground = 0.65
# 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.02, 0.01, 0.12]
# 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.55
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 = 0.55
box1_pose.pose.position.y = -0.1
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 = 0.54
box2_pose.pose.position.y = 0.13
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 = 0.60
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
# 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
right_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 = 0.50
place_pose.pose.position.y = -0.25
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])
# Publish the grasp poses so they can be viewed in RViz
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
# Repeat until we succeed or run out of attempts
while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts:
n_attempts += 1
rospy.loginfo("Pick attempt: " + str(n_attempts))
result = right_arm.pick(target_id, grasps)
rospy.sleep(0.2)
# If the pick was successful, attempt the place operation
if result == MoveItErrorCodes.SUCCESS:
result = None
n_attempts = 0
#_------------------------now we move to the other table__________-------------------------------------------
#_------------------------now we move to the other table__________-------------------------------------------
# Generate valid place poses
places = self.make_places(place_pose)
# Repeat until we succeed or run out of attempts
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 = right_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.")
# Return the arm to the "resting" pose stored in the SRDF file
right_arm.set_named_target('right_start')
right_arm.go()
# Open the gripper to the neutral position
right_gripper.set_joint_value_target(GRIPPER_NEUTRAL)
right_gripper.go()
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit the script
moveit_commander.os._exit(0)
