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 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 __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")
def callback(states):
scene = PlanningSceneInterface()
robot = RobotCommander()
arm = MoveGroupCommander("manipulator")
arm.set_planner_id("RRTConnectkConfigDefault")
arm.set_num_planning_attempts(20)
arm.allow_replanning(True)
pose = copy.deepcopy(states.pose[-1])
temp = tf.transformations.euler_from_quaternion(
(pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w))
quaternion = tf.transformations.quaternion_from_euler(
math.pi, temp[1], temp[2])
pose.position.z += 0.22
pose.orientation.x = quaternion[0]
pose.orientation.y = quaternion[1]
pose.orientation.z = quaternion[2]
pose.orientation.w = quaternion[3]
print pose
arm.set_pose_target(pose)
move_plan = arm.plan()
i = 0
while (not move_plan.joint_trajectory.joint_names):
move_plan = arm.plan()
i += 1
if (i == 5): break
state = arm.execute(move_plan)
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
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)
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)
def execute(self, userdata):
group = resolve_planning_group(userdata.ik_link)
if not group:
return 'aborted'
(goal, tip_frame) = transform_to_tip(group, userdata.ik_link, userdata.goal)
mc = MoveGroupCommander(group)
mc.set_pose_target(goal, tip_frame)
plan = mc.plan()
userdata.trajectory = plan.joint_trajectory
if not plan.joint_trajectory.points:
return 'aborted'
return 'succeeded'
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 execute(self, userdata):
group = 'arms'
mc = MoveGroupCommander(group)
group_1 = resolve_planning_group(userdata.ik_link_1)
(goal_1, tip_frame_1) = transform_to_tip(group_1, userdata.ik_link_1, userdata.goal_1)
group_2 = resolve_planning_group(userdata.ik_link_2)
(goal_2, tip_frame_2) = transform_to_tip(group_2, userdata.ik_link_2, userdata.goal_2)
mc.set_pose_target(goal_1, tip_frame_1)
mc.set_pose_target(goal_2, tip_frame_2)
plan = mc.plan()
userdata.trajectory = plan.joint_trajectory
if not plan.joint_trajectory.points:
return 'aborted'
return 'succeeded'
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 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 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 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 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
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 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
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
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 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 movement(des_coor):
#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():
#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 = des_coor[0]
request.ik_request.pose_stamped.pose.position.y = des_coor[1]
request.ik_request.pose_stamped.pose.position.z = 0.0 # 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)
group = MoveGroupCommander("right_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 Baxter_Go(target_translation, reference_frame, compute_ik):
#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 = reference_frame
#Set the desired orientation for the end effector HERE
euler_angle = [-np.pi, 0, 0]
quaternion = tf.transformations.quaternion_from_euler(
euler_angle[0], euler_angle[1], euler_angle[2])
request.ik_request.pose_stamped.pose.position.x = target_translation[0]
request.ik_request.pose_stamped.pose.position.y = target_translation[1]
request.ik_request.pose_stamped.pose.position.z = target_translation[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
def move_hand(robot_hand, human_hand):
#Initiaize other variables
right_gripper = robot_gripper.Gripper('right')
left_gripper = robot_gripper.Gripper('left')
#right_gripper.calibrate()
right_gripper.close()
#Move to outside position
#Move right hand to outside position
raw_input('Press [Enter]:')
#Construct the request for right hand
request = GetPositionIKRequest()
request.ik_request.group_name = "right_arm"
request.ik_request.ik_link_name = "right_gripper"
request.ik_request.attempts = 500
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.35
request.ik_request.pose_stamped.pose.position.z = 0.4
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:
group = MoveGroupCommander("right_arm")
# Setting position and orientation target
group.set_pose_target(request.ik_request.pose_stamped)
print("Trying to move the right arm out of the way!")
# Plan IK and execute
group.go()
except rospy.ServiceException, e:
print "Service call failed: %s"%e
def move_to(x, y, z):
#Create the function used to call the service
compute_ik = rospy.ServiceProxy('compute_ik', GetPositionIK)
#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"
#Set the desired orientation for the end effector HERE
request.ik_request.pose_stamped.pose.position.x = x
request.ik_request.pose_stamped.pose.position.y = y
request.ik_request.pose_stamped.pose.position.z = z
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 __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")
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))
class ArmTracker:
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 update_target_pose(self, target):
self.target = target
def relax_all_servos(self):
command = 'rosrun donaxi_dynamixels arbotix_relax_all_servos.py'
args = shlex.split(command)
subprocess.Popen(args)
def shutdown(self):
# Stop any further target messages from being processed
self.target_subscriber.unregister()
# Stop any current arm movement
self.right_arm.stop()
# Move to the resting position
self.right_arm.set_named_target('resting')
self.right_arm.go()
# Relax the servos
self.relax_all_servos()
os._exit(0)
class MoveItDemo:
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
self.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
self.right_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the MoveIt! commander for the gripper
self.right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Allow 5 seconds per planning attempt
self.right_arm.set_planning_time(5)
# Prepare Action Controller for gripper
self.ac = actionlib.SimpleActionClient('r_gripper_controller/gripper_action',pr2c.Pr2GripperCommandAction)
self.ac.wait_for_server()
# 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)
### OPEN THE GRIPPER ###
self.open_gripper()
self.obj_att = None
# PREPARE THE SCENE
while self.pwh is None:
rospy.sleep(0.05)
############## CLEAR THE SCENE ################
# planning_scene.world.collision_objects.remove('target')
# Remove leftover objects from a previous run
self.scene.remove_world_object('target')
self.scene.remove_world_object('table')
# self.scene.remove_world_object(obstacle1_id)
# Remove any attached objects from a previous session
self.scene.remove_attached_object(GRIPPER_FRAME, 'target')
# Run and keep in the BG the scene generator also add the ability to kill the code with ctrl^c
timerThread = threading.Thread(target=self.scene_generator)
timerThread.daemon = True
timerThread.start()
initial_pose = target_pose
initial_pose.header.frame_id = 'gazebo_world'
print "==================== Generating Transformations ==========================="
#################### PRE GRASPING POSE #########################
# 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'
# self.plan_exec(pre_grasping)
################# GENERATE GRASPS ###################
# Set a limit on the number of pick attempts before bailing
max_pick_attempts = 5
# Track success/failure and number of attempts for pick operation
success = False
n_attempts = 0
grasps = self.grasp_generator(initial_pose)
possible_grasps = []
for grasp in grasps:
self.gripper_pose_pub.publish(grasp)
possible_grasps.append(grasp.pose)
rospy.sleep(0.2)
#print possible_grasps
self.right_arm.pick(target_id, grasps)
# target_name = target_id
# group_name = GROUP_NAME_ARM
# end_effector = GROUP_NAME_GRIPPER
# attached_object_touch_links = ['r_forearm_link']
# #print (target_name, group_name, end_effector)
# PickupGoal(target_name, group_name ,end_effector, possible_grasps )
# # Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# # Exit the script
moveit_commander.os._exit(0)
################################################################# FUNCTIONS #################################################################################
def model_state_callback(self,msg):
self.pwh = ModelStates()
self.pwh = msg
def grasp_generator(self, initial_pose):
# Pitch angles to try
pitch_vals = [1, 1.57,0, 1,2 ]
# Yaw angles to try
yaw_vals = [0]#, 1.57, -1.57]
# A list to hold the grasps
grasps = []
g = PoseStamped()
g.header.frame_id = REFERENCE_FRAME
g.pose = initial_pose.pose
#g.pose.position.z += 0.18
# Generate a grasp for each pitch and yaw angle
for y in yaw_vals:
for p in pitch_vals:
# Create a quaternion from the Euler angles
q = transformations.quaternion_from_euler(0, p, y)
# Set the grasp pose orientation accordingly
g.pose.orientation.x = q[0]
g.pose.orientation.y = q[1]
g.pose.orientation.z = q[2]
g.pose.orientation.w = q[3]
# Append the grasp to the list
grasps.append(deepcopy(g))
# Return the list
return grasps
def plan_exec(self, pose):
self.right_arm.clear_pose_targets()
self.right_arm.set_pose_target(pose, GRIPPER_FRAME)
self.right_arm.plan()
rospy.sleep(5)
self.right_arm.go(wait=True)
def close_gripper(self):
g_close = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.035, 100))
self.ac.send_goal(g_close)
self.ac.wait_for_result()
rospy.sleep(15) # Gazebo requires up to 15 seconds to attach object
def open_gripper(self):
g_open = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.088, 100))
self.ac.send_goal(g_open)
self.ac.wait_for_result()
rospy.sleep(5) # And up to 20 to detach it
def scene_generator(self):
# print obj_att
global target_pose
global target_id
next_call = time.time()
while True:
next_call = next_call+1
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')
# 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
if self.obj_att is None:
# Add the target object to the scene
self.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
self.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)
### Make the target purple ###
self.setColor(target_id, 0.6, 0, 1, 1.0)
# Send the colors to the planning scene
self.sendColors()
else:
self.scene.remove_world_object('target')
time.sleep(next_call - time.time())
#threading.Timer(0.5, self.scene_generator).start()
# Set the color of an object
def setColor(self, name, r, g, b, a = 0.9):
# Initialize a MoveIt color object
color = ObjectColor()
# Set the id to the name given as an argument
color.id = name
# Set the rgb and alpha values given as input
color.color.r = r
color.color.g = g
color.color.b = b
color.color.a = a
# Update the global color dictionary
self.colors[name] = color
# Actually send the colors to MoveIt!
def sendColors(self):
# Initialize a planning scene object
p = PlanningScene()
# Need to publish a planning scene diff
p.is_diff = True
# Append the colors from the global color dictionary
for color in self.colors.values():
p.object_colors.append(color)
# Publish the scene diff
self.scene_pub.publish(p)
class Planner(object):
move_group = None
goals = None
jspub = None
namespace = None
# These will eventually go to model objects
robot_data = {
'group_name': 'right_arm_and_torso',
'eef_link': 'r_wrist_joint_link'
}
# Current state of the 'session' (right now, only one)
current_scene = None
status = None
link_poses = None
def __init__(self):
rospy.init_node('moveit_web',disable_signals=True)
self.jspub = rospy.Publisher('/update_joint_states',JointState)
self.psw_pub = rospy.Publisher('/planning_scene_world', PlanningSceneWorld)
# Give time for subscribers to connect to the publisher
rospy.sleep(1)
self.goals = []
# HACK: Synthesize a valid initial joint configuration for PR2
initial_joint_state = JointState()
initial_joint_state.name = ['r_elbow_flex_joint']
initial_joint_state.position = [-0.1]
self.jspub.publish(initial_joint_state)
# Create group we'll use all along this demo
# self.move_group = MoveGroupCommander('right_arm_and_torso')
self.move_group = MoveGroupCommander(self.robot_data['group_name'])
self._move_group = self.move_group._g
self.ps = PlanningSceneInterface()
self.status = {'text':'ready to plan','ready':True}
def get_scene(self):
return self.current_scene
def set_scene(self, scene):
self.current_scene = scene
psw = PlanningSceneWorld()
for co_json in scene['objects']:
# TODO: Fix orientation by using proper quaternions on the client
pose = self._make_pose(co_json['pose'])
# TODO: Decide what to do with STL vs. Collada. The client has a Collada
# loader but the PlanningSceneInterface can only deal with STL.
# TODO: Proper mapping between filenames and URLs
# filename = '/home/julian/aaad/moveit/src/moveit_web/django%s' % co_json['meshUrl']
filename = '/home/julian/aaad/moveit/src/moveit_web/django/static/meshes/table_4legs.stl'
co = self.ps.make_mesh(co_json['name'], pose, filename)
psw.collision_objects.append(co)
self.psw_pub.publish(psw)
def get_link_poses(self):
if self.link_poses is None:
self.link_poses = self._move_group.get_link_poses_compressed()
return self.link_poses
# Create link back to socket.io namespace to allow emitting information
def set_socket(self, namespace):
self.namespace = namespace
def emit(self, event, data=None):
if self.namespace:
self.namespace.emit(event, data)
def emit_new_goal(self, pose):
self.emit('target_pose', message_converter.convert_ros_message_to_dictionary(pose)['pose'])
def set_random_goal(self):
goal_pose = self.move_group.get_random_pose()
# goal_pose = self.move_group.get_random_pose('base_footprint')
self.emit_new_goal(goal_pose)
def _make_pose(self, json_pose):
pose = PoseStamped()
pose.header.frame_id = "odom_combined"
pp = json_pose['position']
pose.pose.position.x = pp['x']
pose.pose.position.y = pp['y']
pose.pose.position.z = pp['z']
# TODO: Orientation is not working. See about
# properly using Quaternions everywhere
pp = json_pose['orientation']
pose.pose.orientation.x = pp['x']
pose.pose.orientation.y = pp['y']
pose.pose.orientation.z = pp['z']
pose.pose.orientation.w = pp['w']
return pose
def plan_to_poses(self, poses):
goal_pose = self._make_pose(poses[0])
self.move_group.set_pose_target(goal_pose)
# self.move_group.set_pose_target(goal_pose,'base_footprint')
self.emit('status',{'text':'Starting to plan'})
trajectory = self.move_group.plan()
if trajectory is None or len(trajectory.joint_trajectory.joint_names) == 0:
self.status = {'reachable':False,'text':'Ready to plan','ready':True}
self.emit('status', self.status)
else:
self.status = {'reachable':True,'text':'Rendering trajectory'}
self.emit('status', self.status)
self.publish_trajectory(trajectory)
def publish_goal_position(self, trajectory):
self.publish_position(self, trajectory, -1)
def publish_position(self, trajectory, step):
jsmsg = JointState()
jsmsg.name = trajectory.joint_trajectory.joint_names
jsmsg.position = trajectory.joint_trajectory.points[step].positions
self.jspub.publish(jsmsg)
def publish_trajectory(self, trajectory):
cur_time = 0.0
acceleration = 4.0
for i in range(len(trajectory.joint_trajectory.points)):
point = trajectory.joint_trajectory.points[i]
gevent.sleep((point.time_from_start - cur_time)/acceleration)
cur_time = point.time_from_start
# self.publish_position(trajectory, i)
# TODO: Only say "True" to update state on the last step of the trajectory
new_poses = self._move_group.update_robot_state(trajectory.joint_trajectory.joint_names,
trajectory.joint_trajectory.points[i].positions, True)
self.link_poses = new_poses
self.emit('link_poses', new_poses)
self.status = {'text':'Ready to plan','ready':True}
self.emit('status', self.status)
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('moveit_py_demo', anonymous=True)
scene = PlanningSceneInterface()
robot = RobotCommander()
right_arm = MoveGroupCommander("right_arm")
right_gripper = MoveGroupCommander("right_gripper")
eef = right_arm.get_end_effector_link()
rospy.sleep(2)
scene.remove_attached_object("right_gripper_link", "part")
# clean the scene
scene.remove_world_object("table")
scene.remove_world_object("part")
right_arm.set_named_target("start1")
right_arm.go()
right_gripper.set_named_target("open")
right_gripper.go()
rospy.sleep(1)
# publish a demo scene
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
# add a table
#p.pose.position.x = 0.42
#p.pose.position.y = -0.2
#p.pose.position.z = 0.3
#scene.add_box("table", p, (0.5, 1.5, 0.6))
# add an object to be grasped
p.pose.position.x = 0.15
p.pose.position.y = -0.12
p.pose.position.z = 0.7
scene.add_box("part", p, (0.07, 0.01, 0.2))
rospy.sleep(1)
g = Grasp()
g.id = "test"
start_pose = PoseStamped()
start_pose.header.frame_id = FIXED_FRAME
# start the gripper in a neutral pose part way to the target
start_pose.pose.position.x = 0.0130178
start_pose.pose.position.y = -0.125155
start_pose.pose.position.z = 0.597653
start_pose.pose.orientation.x = 0.0
start_pose.pose.orientation.y = 0.388109
start_pose.pose.orientation.z = 0.0
start_pose.pose.orientation.w = 0.921613
right_arm.set_pose_target(start_pose)
right_arm.go()
rospy.sleep(2)
# generate a list of grasps
#grasps = self.make_grasps(start_pose)
#result = False
#n_attempts = 0
# repeat until will succeed
#while result == False:
#result = robot.right_arm.pick("part", grasps)
#n_attempts += 1
#print "Attempts: ", n_attempts
#rospy.sleep(0.2)
rospy.spin()
roscpp_shutdown()
class MoveItDemo:
def __init__(self):
global obj_att
# 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
self.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
self.right_arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the MoveIt! commander for the gripper
self.right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Allow 5 seconds per planning attempt
self.right_arm.set_planning_time(5)
# Prepare Action Controller for gripper
self.ac = actionlib.SimpleActionClient('r_gripper_controller/gripper_action',pr2c.Pr2GripperCommandAction)
self.ac.wait_for_server()
# 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)
### OPEN THE GRIPPER ###
self.open_gripper()
# PREPARE THE SCENE
while self.pwh is None:
rospy.sleep(0.05)
############## CLEAR THE SCENE ################
# planning_scene.world.collision_objects.remove('target')
# Remove leftover objects from a previous run
self.scene.remove_world_object('target')
self.scene.remove_world_object('table')
# self.scene.remove_world_object(obstacle1_id)
# Remove any attached objects from a previous session
self.scene.remove_attached_object(GRIPPER_FRAME, 'target')
# Run and keep in the BG the scene generator also add the ability to kill the code with ctrl^c
timerThread = threading.Thread(target=self.scene_generator)
timerThread.daemon = True
timerThread.start()
initial_pose = PoseStamped()
initial_pose.header.frame_id = 'gazebo_world'
initial_pose.pose = target_pose.pose
print "==================== Generating Transformations ==========================="
#################### PRE GRASPING POSE #########################
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'
self.plan_exec(pre_grasping)
#################### GRASPING POSE #########################
M3 = transformations.quaternion_matrix([target_pose.pose.orientation.x, target_pose.pose.orientation.y, target_pose.pose.orientation.z, target_pose.pose.orientation.w])
M3[0,3] = target_pose.pose.position.x
M3[1,3] = target_pose.pose.position.y
M3[2,3] = target_pose.pose.position.z
M4 = transformations.euler_matrix(0, 1.57, 0)
M4[0,3] = 0.0 # offset about x
M4[1,3] = 0.0 # about y
M4[2,3] = 0.18 # about z
T2 = np.dot(M3, M4)
grasping = deepcopy(target_pose)
grasping.pose.position.x = T2[0,3]
grasping.pose.position.y = T2[1,3]
grasping.pose.position.z = T2[2,3]
quat2 = transformations.quaternion_from_matrix(T2)
grasping.pose.orientation.x = quat2[0]
grasping.pose.orientation.y = quat2[1]
grasping.pose.orientation.z = quat2[2]
grasping.pose.orientation.w = quat2[3]
grasping.header.frame_id = 'gazebo_world'
self.plan_exec(grasping)
#Close the gripper
print "========== Waiting for gazebo to catch up =========="
self.close_gripper()
#################### ATTACH OBJECT ######################
touch_links = [GRIPPER_FRAME, 'r_gripper_l_finger_tip_link','r_gripper_r_finger_tip_link', 'r_gripper_r_finger_link', 'r_gripper_l_finger_link']
#print touch_links
self.scene.attach_box(GRIPPER_FRAME, target_id, target_pose, target_size, touch_links)
# counter to let the planning scene know when to remove the object
obj_att = 1
#self.scene.remove_world_object(target_id)
#################### POST-GRASP RETREAT #########################
M5 = transformations.quaternion_matrix([initial_pose.pose.orientation.x, initial_pose.pose.orientation.y, initial_pose.pose.orientation.z, initial_pose.pose.orientation.w])
M5[0,3] = initial_pose.pose.position.x
M5[1,3] = initial_pose.pose.position.y
M5[2,3] = initial_pose.pose.position.z
M6 = transformations.euler_matrix(0, 1.57, 0)
M6[0,3] = 0.0 # offset about x
M6[1,3] = 0.0 # about y
M6[2,3] = 0.3 # about z
T3 = np.dot(M5, M6)
post_grasping = deepcopy(initial_pose)
post_grasping.pose.position.x = T3[0,3]
post_grasping.pose.position.y = T3[1,3]
post_grasping.pose.position.z = T3[2,3]
quat3 = transformations.quaternion_from_matrix(T3)
post_grasping.pose.orientation.x = quat3[0]
post_grasping.pose.orientation.y = quat3[1]
post_grasping.pose.orientation.z = quat3[2]
post_grasping.pose.orientation.w = quat3[3]
post_grasping.header.frame_id = 'gazebo_world'
self.plan_exec(post_grasping)
# 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.52
place_pose.pose.position.y = -0.48
place_pose.pose.position.z = 0.48
place_pose.pose.orientation.w = 1.0
n_attempts = 0
max_place_attempts = 2
# Generate valid place poses
places = self.make_places(place_pose)
success = False
# Repeat until we succeed or run out of attempts
while success == False and n_attempts < max_place_attempts:
for place in places:
success = self.right_arm.place(target_id, place)
if success:
break
n_attempts += 1
rospy.loginfo("Place attempt: " + str(n_attempts))
rospy.sleep(0.2)
self.open_gripper()
obj_att = None
rospy.sleep(3)
## # 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'
# # Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# # Exit the script
moveit_commander.os._exit(0)
##################################################################################################################
#Get pose from Gazebo
def model_state_callback(self,msg):
self.pwh = ModelStates()
self.pwh = msg
# Generate a list of possible place poses
def make_places(self, init_pose):
# Initialize the place location as a PoseStamped message
place = PoseStamped()
# Start with the input place pose
place = init_pose
# A list of x shifts (meters) to try
x_vals = [0, 0.005, 0.01, 0.015, -0.005, -0.01, -0.015]
# A list of y shifts (meters) to try
y_vals = [0, 0.005, 0.01, 0.015, -0.005, -0.01, -0.015]
# A list of pitch angles to try
#pitch_vals = [0, 0.005, -0.005, 0.01, -0.01, 0.02, -0.02]
pitch_vals = [0]
# A list of yaw angles to try
yaw_vals = [0]
# A list to hold the places
places = []
# Generate a place pose for each angle and translation
for y in yaw_vals:
for p in pitch_vals:
for y in y_vals:
for x in x_vals:
place.pose.position.x = init_pose.pose.position.x + x
place.pose.position.y = init_pose.pose.position.y + y
# Create a quaternion from the Euler angles
q = quaternion_from_euler(0, p, y)
# Set the place pose orientation accordingly
place.pose.orientation.x = q[0]
place.pose.orientation.y = q[1]
place.pose.orientation.z = q[2]
place.pose.orientation.w = q[3]
# Append this place pose to the list
places.append(deepcopy(place))
# Return the list
return places
def plan_exec(self, pose):
self.right_arm.clear_pose_targets()
self.right_arm.set_pose_target(pose, GRIPPER_FRAME)
self.right_arm.plan()
rospy.sleep(5)
self.right_arm.go(wait=True)
def close_gripper(self):
g_close = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.035, 100))
self.ac.send_goal(g_close)
self.ac.wait_for_result()
rospy.sleep(15) # Gazebo requires up to 15 seconds to attach object
def open_gripper(self):
g_open = pr2c.Pr2GripperCommandGoal(pr2c.Pr2GripperCommand(0.088, 100))
self.ac.send_goal(g_open)
self.ac.wait_for_result()
rospy.sleep(5) # And up to 20 to detach it
def scene_generator(self):
# print obj_att
global target_pose
global target_id
global target_size
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')
# 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
if obj_att is None:
self.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
self.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
self.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()
else:
self.scene.remove_world_object('target')
# Publish targe's frame
#self.object_frames_pub.publish(target_pose)
threading.Timer(0.5, self.scene_generator).start()
# Set the color of an object
def setColor(self, name, r, g, b, a = 0.9):
# Initialize a MoveIt color object
color = ObjectColor()
# Set the id to the name given as an argument
color.id = name
# Set the rgb and alpha values given as input
color.color.r = r
color.color.g = g
color.color.b = b
color.color.a = a
# Update the global color dictionary
self.colors[name] = color
# Actually send the colors to MoveIt!
def sendColors(self):
# Initialize a planning scene object
p = PlanningScene()
# Need to publish a planning scene diff
p.is_diff = True
# Append the colors from the global color dictionary
for color in self.colors.values():
p.object_colors.append(color)
# Publish the scene diff
self.scene_pub.publish(p)
def __init__(self):
# Initialize the move_group API
moveit_commander.roscpp_initialize(sys.argv)
# Initialize the ROS node
rospy.init_node('moveit_constraints_demo', anonymous=True)
robot = RobotCommander()
# Connect to the arm move group
arm = MoveGroupCommander(GROUP_NAME_ARM)
# Initialize the move group for the right gripper
gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
# Increase the planning time since constraint planning can take a while
arm.set_planning_time(5)
# Allow replanning to increase the odds of a solution
arm.allow_replanning(True)
# Set the right arm reference frame
arm.set_pose_reference_frame(REFERENCE_FRAME)
# Allow some leeway in position(meters) and orientation (radians)
arm.set_goal_position_tolerance(0.05)
arm.set_goal_orientation_tolerance(0.1)
# Get the name of the end-effector link
end_effector_link = arm.get_end_effector_link()
# Start in the "resting" configuration stored in the SRDF file
arm.set_named_target('l_arm_init')
# Plan and execute a trajectory to the goal configuration
arm.go()
rospy.sleep(1)
# Open the gripper
gripper.set_joint_value_target(GRIPPER_NEUTRAL)
gripper.go()
rospy.sleep(1)
# Set an initial target pose with the arm up and to the right
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.263803774718
target_pose.pose.position.y = 0.295405791959
target_pose.pose.position.z = 0.690438884208
q = quaternion_from_euler(0, 0, -1.57079633)
target_pose.pose.orientation.x = q[0]
target_pose.pose.orientation.y = q[1]
target_pose.pose.orientation.z = q[2]
target_pose.pose.orientation.w = q[3]
# Set the start state and target pose, then plan and execute
arm.set_start_state(robot.get_current_state())
arm.set_pose_target(target_pose, end_effector_link)
arm.go()
rospy.sleep(2)
# Close the gripper
gripper.set_joint_value_target(GRIPPER_CLOSED)
gripper.go()
rospy.sleep(1)
# Store the current pose
start_pose = arm.get_current_pose(end_effector_link)
# Create a contraints list and give it a name
constraints = Constraints()
constraints.name = "Keep gripper horizontal"
# Create an orientation constraint for the right gripper
orientation_constraint = OrientationConstraint()
orientation_constraint.header = start_pose.header
orientation_constraint.link_name = arm.get_end_effector_link()
orientation_constraint.orientation.w = 1.0
orientation_constraint.absolute_x_axis_tolerance = 0.1
orientation_constraint.absolute_y_axis_tolerance = 0.1
orientation_constraint.absolute_z_axis_tolerance = 0.1
orientation_constraint.weight = 1.0
# q = quaternion_from_euler(0, 0, -1.57079633)
# orientation_constraint.orientation.x = q[0]
# orientation_constraint.orientation.y = q[1]
# orientation_constraint.orientation.z = q[2]
# orientation_constraint.orientation.w = q[3]
# Append the constraint to the list of contraints
constraints.orientation_constraints.append(orientation_constraint)
# Set the path constraints on the arm
arm.set_path_constraints(constraints)
# Set a target pose for the arm
target_pose = PoseStamped()
target_pose.header.frame_id = REFERENCE_FRAME
target_pose.pose.position.x = 0.39000848183
target_pose.pose.position.y = 0.185900663329
target_pose.pose.position.z = 0.732752341378
target_pose.pose.orientation.w = 1
# Set the start state and target pose, then plan and execute
arm.set_start_state_to_current_state()
arm.set_pose_target(target_pose, end_effector_link)
arm.go()
rospy.sleep(1)
# Clear all path constraints
arm.clear_path_constraints()
# Open the gripper
gripper.set_joint_value_target(GRIPPER_NEUTRAL)
gripper.go()
rospy.sleep(1)
# Return to the "resting" configuration stored in the SRDF file
arm.set_named_target('l_arm_init')
# Plan and execute a trajectory to the goal configuration
arm.go()
rospy.sleep(1)
# Shut down MoveIt cleanly
moveit_commander.roscpp_shutdown()
# Exit MoveIt
moveit_commander.os._exit(0)
def move_arm():
#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)
global step
grip_left = baxter_interface.Gripper('left', CHECK_VERSION)
while not rospy.is_shutdown():
print("I am on step " + str(step))
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"
cmd = positions[step]
#Set the desired orientation for the end effector HERE
request.ik_request.pose_stamped.pose.position.x = cmd[0]
request.ik_request.pose_stamped.pose.position.y = cmd[1]
request.ik_request.pose_stamped.pose.position.z = cmd[2]
request.ik_request.pose_stamped.pose.orientation.x = cmd[3]
request.ik_request.pose_stamped.pose.orientation.y = cmd[4]
request.ik_request.pose_stamped.pose.orientation.z = cmd[5]
request.ik_request.pose_stamped.pose.orientation.w = cmd[6]
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.go()
if (step == 1) or (step == 6):
print("closing")
grip_left.close()
elif(step == 3) or (step == 8):
print("OPENING")
grip_left.open()
except rospy.ServiceException, e:
print "Service call failed: %s"%e
step = (step+1) % len(positions)
start_pose = geometry_msgs.msg.Pose()
start_pose.position.x = -0.0988490064784
start_pose.position.y = 0.272349904278
start_pose.position.z = 1.18864499931
start_pose.orientation.x = 0.393751611087
start_pose.orientation.y = 0.918424640162
start_pose.orientation.z = -0.0150455838492
start_pose.orientation.w = 0.0350639347048
# start_pose.orientation.w = 0
# start_pose.orientation.x = 0
# start_pose.orientation.y = 1
# start_pose.orientation.z = 0
# start_pose.position.y = 0.0256415233819
# start_pose.position.z = 1.25871460368
# start_pose.position.x = 0.243500142238
right_arm.set_pose_target(start_pose)
plan_start = right_arm.plan()
print "============ Waiting while RVIZ displays plan_start..."
rospy.sleep(5)
right_arm.execute(plan_start)
print "============ Waiting while RVIZ executes plan_start..."
rospy.sleep(5)
waypoints = []
waypoints.append(right_arm.get_current_pose().pose)
gain = 0.2
points = 5
for i in xrange(points):
wpose = geometry_msgs.msg.Pose()
wpose.orientation.w = waypoints[i-1].orientation.w
####################### INIT POSITION RIGHT ARM ###############################
pose.pose.position.x = 0.5410
pose.pose.position.y = -0.6719
pose.pose.position.z = 1.3042
pose.pose.orientation.w = 0.68025
pose.pose.orientation.x = -0.2169
pose.pose.orientation.y = 0.65613
pose.pose.orientation.z = 0.24435
pose.header.frame_id = robot.get_planning_frame()
pose.header.stamp = rospy.Time.now()
group_right.set_pose_target(pose, "r_eef")
result = group_right.go(None,1)
if result == 0:
print "ERROR init pos right"
sys.exit()
####################### INIT POSITION LEFT ARM ###############################
pose.pose.position.x = 0.5636
pose.pose.position.y = 0.6398
pose.pose.position.z = 1.1455
pose.pose.orientation.w = 0.15506
pose.pose.orientation.x = 0.8216
pose.pose.orientation.y = -0.32951
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)
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException) as e:
rospy.logwarn(e)
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
# 各コントローラ毎に，ゲインを指示するための service proxy (ROS の service を送るためのクライアント) を作成
set_compliance_slope = rospy.ServiceProxy(controller + '/set_compliance_slope', SetComplianceSlope)
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)
def __init__(self):
roscpp_initialize(sys.argv)
rospy.init_node('moveit_py_demo', anonymous=True)
scene = PlanningSceneInterface()
robot = RobotCommander()
right_arm = MoveGroupCommander(GROUP_NAME_ARM)
#right_arm.set_planner_id("KPIECEkConfigDefault");
right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
eef = right_arm.get_end_effector_link()
rospy.sleep(2)
scene.remove_attached_object(GRIPPER_FRAME, "part")
# clean the scene
scene.remove_world_object("table")
scene.remove_world_object("part")
#right_arm.set_named_target("r_start")
#right_arm.go()
#right_gripper.set_named_target("right_gripper_open")
#right_gripper.go()
rospy.sleep(1)
# publish a demo scene
p = PoseStamped()
p.header.frame_id = robot.get_planning_frame()
# add a table
#p.pose.position.x = 0.42
#p.pose.position.y = -0.2
#p.pose.position.z = 0.3
#scene.add_box("table", p, (0.5, 1.5, 0.6))
# add an object to be grasped
p.pose.position.x = 0.7
p.pose.position.y = -0.2
p.pose.position.z = 0.85
scene.add_box("part", p, (0.07, 0.01, 0.2))
rospy.sleep(1)
g = Grasp()
g.id = "test"
start_pose = PoseStamped()
start_pose.header.frame_id = FIXED_FRAME
# start the gripper in a neutral pose part way to the target
start_pose.pose.position.x = 0.47636
start_pose.pose.position.y = -0.21886
start_pose.pose.position.z = 0.9
start_pose.pose.orientation.x = 0.00080331
start_pose.pose.orientation.y = 0.001589
start_pose.pose.orientation.z = -2.4165e-06
start_pose.pose.orientation.w = 1
right_arm.set_pose_target(start_pose)
right_arm.go()
rospy.sleep(2)
# generate a list of grasps
grasps = self.make_grasps(start_pose)
result = False
n_attempts = 0
# repeat until will succeed
while result == False:
result = robot.right_arm.pick("part", grasps)
n_attempts += 1
print "Attempts: ", n_attempts
rospy.sleep(0.3)
rospy.spin()
roscpp_shutdown()
mg = MoveGroupCommander('right_arm_and_torso')
p = mg.get_current_pose()
print "Start pose:"
print p
p.pose.position.x += 0.3
#ps = PlanningSceneInterface()
#psw_pub = rospy.Publisher('/planning_scene_world', PlanningSceneWorld)
#rospy.sleep(1)
#co = ps.make_sphere("test_co", p, 0.02)
#psw = PlanningSceneWorld()
#psw.collision_objects.append(co)
#psw_pub.publish(psw)
# ps.remove_world_object("test_sphere")
# ps.add_sphere("test_sphere", p, 0.1)
# rospy.sleep(1)
# ps.add_sphere("test_sphere", p, 0.1)
#p.pose.position.x += 0.3
print "End pose:"
print p
mg.set_pose_target(mg.get_random_pose())
traj = mg.plan()
print traj
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)
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)
marker2.pose.position.x = eef_pose.pose.position.x marker2.pose.position.y = eef_pose.pose.position.y marker2.pose.position.z = eef_pose.pose.position.z # We add the new marker to the MarkerArray, removing the oldest # marker from it when necessary if(count > MARKERS_MAX): markerArray.markers.pop(0) markerArray.markers.append(marker) markerArray.markers.append(marker2) print 'how far away: ', (goal_pose.pose.position.x-eef_pose.pose.position.x), ' ',(goal_pose.pose.position.y-eef_pose.pose.position.y), ' ',(goal_pose.pose.position.z-eef_pose.pose.position.z) # Renumber the marker IDs id = 0 for m in markerArray.markers: m.id = id id += 1 # while not rospy.is_shutdown(): # Publish the MarkerArray publisher.publish(markerArray) robot.set_pose_target(eef_pose) robot.go() rospy.sleep(1) rospy.spin()
msg.trajectory.joint_names = ['ur5_arm_shoulder_pan_joint',
'ur5_arm_shoulder_lift_joint',
'ur5_arm_elbow_joint',
'ur5_arm_wrist_1_joint',
'ur5_arm_wrist_2_joint',
'ur5_arm_wrist_3_joint']
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]
roscpp_initialize(sys.argv)
rospy.init_node('pumpkin_planning', anonymous=True)
right_arm = MoveGroupCommander("right_arm")
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=1)
print right_arm.get_current_pose().pose
wpose = geometry_msgs.msg.Pose()
wpose.orientation.w = 0.0176318609849
wpose.orientation.x = 0.392651866792
wpose.orientation.y = 0.918465607415
wpose.orientation.z = 0.0439835989663
wpose.position.y = 0.227115629827
wpose.position.z = 1.32344046934
wpose.position.x = -0.358178766726
right_arm.set_pose_target(wpose)
plan1 = right_arm.plan()
right_arm.execute(plan1)
print "============ Waiting while RVIZ executes plan1..."
rospy.sleep(5)
waypoints = []
waypoints.append(right_arm.get_current_pose().pose)
print right_arm.get_current_pose().pose
points = 20
for i in xrange(points):
wpose = geometry_msgs.msg.Pose()
wpose.orientation.w = waypoints[i-1].orientation.w + 0.5285
wpose.orientation.x = waypoints[i-1].orientation.x - 0.6736
wpose.orientation.y = waypoints[i-1].orientation.y - 1.638
## We can get the name of the reference frame for this robot
print ">>>>> Reference frame: %s" % group.get_planning_frame()
print ">>>>> Printing robot state"
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()
class Pick_Place:
def __init__(self, planArm="left_arm"):
# Initialize the move_group API
#moveit_commander.roscpp_initialize(sys.argv)
# Initialize the ROS node
rospy.init_node('pick_place', anonymous=True)
# Connect to the arm move group
if planArm == "right_arm":
self.arm = MoveGroupCommander('right_arm')
self.hand = Gripper(1)
else:
self.arm = MoveGroupCommander('left_arm')
self.hand = Gripper(0)
rospy.sleep(1)
self.hand.release()
# Allow replanning to increase the odds of a solution
self.arm.allow_replanning(True)
# Allow some leeway in position(meters) and orientation (radians)
#self.arm.set_goal_position_tolerance(0.001)
#self.arm.set_goal_orientation_tolerance(0.01)
def pick(self):
self.graspFromSide()
def graspFromSide(self):
targetPose=Pose()
targetPose.position.x = 0.96
targetPose.position.y = 0.10
targetPose.position.z = -0.0
targetPose.orientation.x = 0.044
targetPose.orientation.y = 0.702
targetPose.orientation.z = 0.045
targetPose.orientation.w = 0.708
# right side grasp
# Position
#x = 0.636
#y = 0.832
#z = 0.192
# Orientation
#x = -0.382
#y = 0.922
#z = 0.021
#w= 0.049
#xyz 0.871 0.428 0.038
#orientation 0.119 0.698 -0.083 0.700
self.moveToTargetPose(targetPose)
while self.hand.range.state() == 65535.0:
targetPose.position.x += 0.05
self.moveToTargetPose(targetPose)
while self.hand.range.state() > 70:
targetPose.position.x += 0.02
self.moveToTargetPose(targetPose)
self.hand.grasp()
rospy.sleep(1)
targetPose.position.x -= 0.05
targetPose.position.y -= 0.05
targetPose.position.z += 0.1
self.moveToTargetPose(targetPose)
self.hand.release()
rospy.sleep(1)
def moveToTargetPose(self, targetPose):
# move to the defined pose
self.arm.set_start_state_to_current_state()
self.arm.set_pose_target(targetPose)
self.arm.go()
rospy.sleep(0.5)
# 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()
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_test")
rospy.loginfo("Starting up move group commander for right arm")
right_arm_mgc = MoveGroupCommander("right_arm_torso")
rospy.loginfo("Creating first goal")
goal_point = Point(0.4, -0.2, 1.1)
goal_ori = Quaternion(0.0,0.0,0.0,1.0)
goal_pose = Pose(goal_point, goal_ori)
right_arm_mgc.set_pose_target(goal_pose)
right_arm_mgc.set_pose_reference_frame('base_link')
while True:
rospy.loginfo("go()")
right_arm_mgc.go()
# goal_point.x += (random() - 0.5) / 10
# goal_point.y += (random() - 0.5) / 10
# goal_point.z += (random() - 0.5) / 10
goal_point.z += 0.05
rospy.loginfo("Setting new goal:\n " + str(goal_point))
goal_pose = Pose(goal_point, goal_ori)
right_arm_mgc.set_pose_target(goal_pose)
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)
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)
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)
