def test_plan_to_waypoints_target(self):
waypoints = []
waypoints.append(conversions.list_to_pose([0.71, 0.17, 0.34, 0, 0, 0, 1]))
waypoints.append(conversions.list_to_pose([0.71, 0.15, 0.34, 0, 0, 0, 1]))
waypoints.append(conversions.list_to_pose([0.69, 0.15, 0.34, 0, 0, 0, 1]))
waypoints.append(conversions.list_to_pose([0.71, 0.17, 0.34, 0, 0, 0, 1]))
(plan, f) = self.robot_commander.plan_to_waypoints_target(waypoints)
self.assertIsInstance(plan, RobotTrajectory)
def move_to_pose(self, pose, vel=1.0, accel=1.0, wait=True):
"""
Move to pose
:param pose: Array of positions and orientations [x, y, z, qx, qy, qz, qw]
:param vel: Velocity (fraction of max) [0.0, 1.0]
:param wait: to wait for completition of motion or not
:
:type pose:
:type vel:
:type accel:
:type wait: bool
:
:return: success
:rtype: bool
"""
if type(pose) is list:
pose = list_to_pose(pose)
self.moveit_arm_group.set_max_velocity_scaling_factor(vel)
self.moveit_arm_group.set_max_acceleration_scaling_factor(accel)
self.moveit_arm_group.set_pose_target(pose)
success = self.moveit_arm_group.go(wait=wait)
self.moveit_arm_group.stop()
self.moveit_arm_group.clear_pose_targets()
return success
def ik_joint_and_gripper_plan_execution(self,
waypoint_list,
gripper_last=True):
# executes a trajectory based on ik computation with baxter interface API
# if poses not as list of poses, will convert them into poses.
# MUST BE PASSED A ZIPPED LIST OF JOINTS AND GRIPPERS
# Example:
#-- pose_action = [Pose1, Pose2, Pose3, ..., PoseN]
# -- gripper_action = [0] * len(pose_action)
# -- waypoint_list = zip(pose_action, gripper_action)
# -- self.ik_planner.ik_joint_plan_execution(waypoint_list)
for waypoint in waypoint_list:
pose = waypoint[0]
gripper_pos = waypoint[1]
if type(pose) is list:
pose = conversions.list_to_pose(pose)
goal_joints = self._ik_request(pose)
if not goal_joints:
print "no ik solution found, terminate further execution and return False"
return 1
else:
if gripper_last:
self._limb.move_to_joint_positions(goal_joints,
timeout=5.0)
self.gripper.command_position(gripper_pos)
rospy.sleep(0.4)
else:
self.gripper.command_position(gripper_pos)
rospy.sleep(0.8)
self._limb.move_to_joint_positions(goal_joints,
timeout=5.0)
return 0
def any_plan(self, goal, **kwargs):
assert len(goal) == 7, 'goal should be a list describing pose'
pose_target = copy(goal)
pose_target = conversions.list_to_pose(pose_target)
self.group.set_planning_time(self._pt)
plan = self.group.plan(pose_target)
return plan
def move_to_pose_cartesian(self, pose, wait=True): #no support for vel, accel scaling for cartesian https://answers.ros.org/question/291887/moveit-acceleration-scaling-factor-not-working/
"""
Move to pose following a cartesian path
:param pose: geometry_msgs Pose
:param wait: to wait for completition of motion or not
:
:type pose:
:type wait: bool
:
:return: success
:rtype: bool
"""
if type(pose) is list:
pose = list_to_pose(pose)
(plan, fraction) = self.moveit_arm_group.compute_cartesian_path(
[pose], # waypoints to follow
0.005, # eef_step
0.0) # jump_threshold
if fraction != 1.0:
raise ValueError('Unable to plan entire path!')
success = self.moveit_arm_group.execute(plan, wait=wait)
self.moveit_arm_group.stop()
self.moveit_arm_group.clear_pose_targets()
return success
def goto_pose(self, pose, velocity=None, group_name=None, wait=True):
if group_name:
self.set_group(group_name)
if not self.active_group:
raise ValueError('No active Planning Group')
if type(pose) is list:
pose = list_to_pose(pose)
self.active_group.set_max_velocity_scaling_factor(
velocity if velocity else 0.5)
self.active_group.set_pose_target(pose)
self.result = None
self.active_group.go(wait=False)
if not wait:
return True
while not self.result:
rospy.sleep(0.01)
self.active_group.stop()
self.active_group.clear_pose_targets()
return self.result.result.error_code.val == 1
def goto_pose_cartesian(self, pose, velocity=1.0, group_name=None, wait=True):
"""
Move to pose following a cartesian trajectory.
:param pose: Array position & orientation [x, y, z, qx, qy, qz, qw]
:param velocity: Velocity (fraction of max) [0.0, 1.0]
:param group_name: Move group (use current if None)
:param wait: Wait for completion if True
:return: Bool success
"""
if group_name:
self.set_group(group_name)
if not self.active_group:
raise ValueError('No active Planning Group')
if type(pose) is list:
pose = list_to_pose(pose)
self.active_group.set_max_velocity_scaling_factor(velocity)
(plan, fraction) = self.active_group.compute_cartesian_path(
[pose], # waypoints to follow
0.005, # eef_step
0.0) # jump_threshold
if fraction != 1.0:
raise ValueError('Unable to plan entire path!')
success = self.active_group.execute(plan, wait=wait)
self.active_group.stop()
self.active_group.clear_pose_targets()
return success
def move_pose_relative_world(self, dpose, velocity):
if velocity is None:
self.moveg.set_max_velocity_scaling_factor(self.velocity)
else:
self.moveg.set_max_velocity_scaling_factor(velocity)
pose = pose_to_list(self.moveg.get_current_pose().pose)
pose = np.append(pose[0:3], tr.euler_from_quaternion(pose[3:]))
pose = np.add(pose, dpose)
wp = [list_to_pose(pose)]
self.moveg.set_start_state(self.robot.get_current_state())
(plan,
fraction) = self.moveg.compute_cartesian_path(wp,
eef_step=0.01,
jump_threshold=0.0)
if fraction < 1.0:
self.last_error_msg = "No motion plan found."
return False
v = self.velocity if velocity is None else velocity
plan = self.moveg.retime_trajectory(self.robot.get_current_state(),
plan, v)
try:
self.moveg.execute(plan, wait=True)
self.moveg.stop()
except MoveItCommanderException as e:
self.last_error_msg = str(e)
return False
return True
def cartesian_plan(self,
waypoint_list,
interpolation=0.01,
jump_threshold=0.0):
waypoints = [self.group.get_current_pose().pose]
if type(waypoint_list[0]) is list:
waypoints = waypoints + [
conversions.list_to_pose(copy(waypoint))
for waypoint in waypoint_list
]
else:
waypoints = waypoints + conversions.list_to_pose(
copy(waypoint_list))
self.group.set_planning_time(self._pt)
plan, fraction = self.group.compute_cartesian_path(
waypoints, interpolation, jump_threshold)
rospy.sleep(self._pt)
return plan, fraction
def affine2pose(T):
"""Convert affine transformation to Pose.
Example:
>>> T = np.array([[-1,0,0,1],[0,1,0,2],[0,0,-1,3],[0,0,0,1]])
>>> conversions.pose_to_list(affine2pose(T))
[1.0, 2.0, 3.0, 0.0, 1.0, 0.0, 0.0]
"""
q = mat2q(T[0:3][:, 0:3])
t = T[np.ix_([0, 1, 2], [3])]
return conversions.list_to_pose(np.append(t, q))
def change_end_pose(group, input_pose):
end_goal = pose_to_list(group.get_current_pose().pose)
for i, ele in enumerate(input_pose):
if ele != None:
end_goal[i] = ele
# print "\nOUT: ", input_pose, end_goal
group.set_pose_target(list_to_pose(end_goal))
if group.go(wait=True):
# if group.plan():
print "\nSolution found for ", input_pose
print "\nGroup moved to ", end_goal
else:
print "\nNo solution found for ", input_pose
def add_pose(pose_a, pose_b):
"""
Add two poses
"""
position_a, orientation_a = pose_to_lists(pose_a, 'euler')
position_b, orientation_b = pose_to_lists(pose_b, 'euler')
position_c = [a + b for a, b in zip(position_a, position_b)]
orientation_c = [a + b for a, b in zip(orientation_a, orientation_b)]
pose_c = list_to_pose(position_c + orientation_c)
return pose_c
def change_end_pose(group, input_pose):
pose_goal = geometry_msgs.msg.Pose()
end_goal = pose_to_list(pose_goal)
for i, ele in enumerate(input_pose):
if ele != None:
end_goal[i] = ele
group.set_pose_target(list_to_pose(end_goal))
if group.go(wait=True):
print "Solution found for ", input_pose
print "Group moved to ", end_goal
else:
print "No solution found for", input_pose
group.stop()
group.clear_pose_targets()
def get_TF_to_pose(a,b):
end_flag = 0
listener = tf.TransformListener()
while end_flag ==0:
try:
(trans,rot) = listener.lookupTransform(a,b, rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
continue
end_flag = 1
euler = euler_from_quaternion([rot[0],rot[1],rot[2],rot[3]])
pose_target_euler = [trans[0],trans[1],trans[2],euler[0]-1.57,euler[1],euler[2]+3.14]
pose_target = list_to_pose(pose_target_euler)
return pose_target
def add_box_object(self, id_name, pose, size = (1, 1, 1), rotation = (0, 0, 0), frame='/world'):
'''
Adds the particular BOX TYPE objects to the moveit planning scene
:param id_name: unique id that object should be labeled with
:param pose: pose of the object
:param size: size of the object
:param rotation: rotation offset in r, p, y
:param frame: frame in which the object pose is passed
:type id_name: string
:type pose: list of double of length 7 (x, y, z, q_x, q_y, q_z, q_w)
:type size: tuple of length 3
:type frame: string
'''
assert type(size) is tuple, 'size should be tuple'
assert len(size)==3, 'size should be of length 3'
assert not id_name in self.scene_objects, \
'Object with the same name already exists!'
self.scene_objects.append(id_name)
object_pose = PoseStamped()
object_pose.header.frame_id = frame
if type(pose) is list:
pose = list_to_pose(pose)
(r, p, y) = rotation
q_object_rotation = quaternion_from_euler(math.radians(r), math.radians(p), math.radians(y))
q_object = [pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w]
q_object_final = quaternion_multiply(q_object, q_object_rotation)
x = pose.position.x
y = pose.position.y
z = pose.position.z
object_pose.pose = Pose(Point(x, y, z), Quaternion(*q_object_final))
#Add object description in scene
self.clear_octomap() #need to sleep before adding part!! https://answers.ros.org/question/209030/moveit-planningsceneinterface-addbox-not-showing-in-rviz/
self.planning_scene_interface.add_box(id_name, object_pose, size)
self.wait_for_planning_scene_object(id_name)
def goto_pose(self, pose, velocity=1.0, group_name=None, wait=True):
"""
Move to pose
:param pose: Array position & orientation [x, y, z, qx, qy, qz, qw]
:param velocity: Velocity (fraction of max) [0.0, 1.0]
:param group_name: Move group (use current if None)
:param wait: Wait for completion if True
:return: Bool success
"""
if group_name:
self.set_group(group_name)
if not self.active_group:
raise ValueError('No active Planning Group')
if type(pose) is list:
pose = list_to_pose(pose)
self.active_group.set_max_velocity_scaling_factor(velocity)
self.active_group.set_pose_target(pose)
success = self.active_group.go(wait=wait)
self.active_group.stop()
self.active_group.clear_pose_targets()
return success
def move_pose_relative(self, dpose, velocity):
if velocity is None:
self.moveg.set_max_velocity_scaling_factor(self.velocity)
else:
self.moveg.set_max_velocity_scaling_factor(velocity)
pose = pose_to_list(self.moveg.get_current_pose().pose)
t_xform = np.dot(tr.translation_matrix(pose[0:3]),
tr.quaternion_matrix(pose[3:]))
s_xform = np.dot(tr.translation_matrix(dpose[0:3]),
tr.euler_matrix(*dpose[3:]))
xform = np.dot(t_xform, s_xform)
pose = tr.translation_from_matrix(xform).tolist() + list(
tr.euler_from_matrix(xform))
wp = [list_to_pose(pose)] # waypoints
self.moveg.set_start_state(self.robot.get_current_state())
(plan,
fraction) = self.moveg.compute_cartesian_path(wp,
eef_step=0.01,
jump_threshold=0.0)
if fraction < 1.0:
self.last_error_msg = "No motion plan found."
return False
v = self.velocity if velocity is None else velocity
plan = self.moveg.retime_trajectory(self.robot.get_current_state(),
plan, v)
try:
self.moveg.execute(plan, wait=True)
self.moveg.stop()
except MoveItCommanderException as e:
self.last_error_msg = str(e)
return False
return True
def add_world_object(self, id_name, pose, size, frame='/base'):
'''
Adds the particular object to the moveit planning scene
:param id_name: unique id that object should be labeled with
:param pose: pose of the object
:param size: size of the object
:param frame: frame in which the object pose is passed
:type id_name: string
:type pose: list of double of length 7 (x,y,z, q_x, q_y, q_z, q_w)
:type size: tuple of length 3
:type frame: string
'''
assert type(size) is tuple, 'size should be tuple'
assert len(size) == 3, 'size should be of length 3'
pose = conversions.list_to_pose(pose)
pose_stamped = PoseStamped()
pose_stamped.header.frame_id = frame
pose_stamped.pose = pose
self.scene.add_box(id_name, pose_stamped, size)
rospy.sleep(1.0)
self.scene.add_box(id_name, pose_stamped, size)
def goto_pose_cartesian(self,
pose,
velocity=None,
group_name=None,
wait=True):
if group_name:
self.set_group(group_name)
if not self.active_group:
raise ValueError('No active Planning Group')
if type(pose) is list:
pose = list_to_pose(pose)
(plan, fraction) = self.active_group.compute_cartesian_path(
[pose], # waypoints to follow
0.005, # eef_step
0.0) # jump_threshold
plan = self.active_group.retime_trajectory(
self.robot.get_current_state(), plan,
velocity if velocity else 0.5)
if fraction != 1.0:
raise ValueError('Unable to plan entire path!')
return False
self.result = None
self.active_group.execute(plan, wait=False)
if not wait:
return True
while not self.result:
rospy.sleep(0.01)
self.active_group.stop()
self.active_group.clear_pose_targets()
return self.result.result.error_code.val == 1
def attach_arm_object(self, link_name, id_name, pose, size):
'''
Attaches the specified object to the robot
:param link_name: name of the link to which the bject
should be attached
:param id_name: unique id associated with the object
:param pose: pose of the object
:parma size: size of the object
:type link_name: string
:type id_name: string
:type pose: list of double of length 7 (x,y,z, q_x, q_y, q_z, q_w)
:type size: tuple of length 3
'''
assert type(size) is tuple, 'size should be tuple'
assert len(size) == 3, 'size should be of length 3'
pose = conversions.list_to_pose(pose)
pose_stamped = PoseStamped()
pose_stamped.header.frame_id = link_name
pose_stamped.pose = pose
self.scene.attach_box(link_name, id_name, pose_stamped, size)
rospy.sleep(1.0)
self.scene.attach_box(link_name, id_name, pose_stamped, size)
# ar마커 변수 선언
ar_x = 0
ar_y = 0
ar_z = 0
#move_Joint(1.57,-2.27,1.93,-1.19,-1.57,0) #down pose
move_Joint(0, 0, 0, 0, 0, 0) #down pose
rospy.sleep(3)
husky_tr, husky_rot = get_TF('odom', 'base_link')
husky_x = husky_tr[0]
husky_y = husky_tr[1]
# 사용자입력 : 목표 eef 위치 지정 (x,y,z,r,p,yaw)
goal_eef = [0, 0, 0, 0, 0, 0, 1]
goal_eef_quat = list_to_pose(goal_eef)
# 현재 eef pose 받아오기
eef_pose = Pose()
eef_tr, eef_rot = get_TF('base_link', 'rh_p12_rn_base')
#vector_x = ar_tr[0] - eef_tr[0]
#vector_y = ar_tr[1] - eef_tr[1]
#angle_to_goal = atan2(vector_y,vector_x)
#distance_to_goal_eef = sqrt(vector_x**2 + vector_y**2)
# ar 마커 및 eef pose data 선언.
ar_pose = Pose()
eef_pose = Pose()
ar_tr, ar_rot = get_TF('base_link', 'ar_marker_0')
def test_get_ik(self):
pose = PoseStamped()
pose.header.stamp = rospy.get_rostime()
pose.pose = conversions.list_to_pose([0.71, 0.17, 0.34, 0, 0, 0, 1])
joint_state_from_ik = self.robot_commander.get_ik(pose)
self.assertIsInstance(joint_state_from_ik, JointState)
def get_pose(self):
print("Current tool pose is: ", self.rob.getl())
return list_to_pose(self.rob.getl())
from moveit_commander.conversions import list_to_pose
import copy
approach_height = 0.08
approach_offset = 0.05
positions = {}
positions["back"] = list_to_pose([0, 0, 0, 0, 0, 0])
positions["back_up"] = list_to_pose([0, 0, approach_height, 0, 0, 0])
positions["back_out"] = list_to_pose([-approach_offset, 0, 0, 0, 0, 0])
positions["back_out_up"] = list_to_pose(
[-approach_offset, 0, approach_height, 0, 0, 0])
positions["left"] = list_to_pose([0, 0, 0, 0, 0, 1.5707])
positions["left_up"] = list_to_pose([0, 0, approach_height, 0, 0, 1.5707])
positions["left_out"] = list_to_pose([0, -approach_offset, 0, 0, 0, 1.5707])
positions["left_out_up"] = list_to_pose(
[0, -approach_offset, approach_height, 0, 0, 1.5707])
positions["right"] = list_to_pose([0, 0, 0, 0, 0, -1.5707])
positions["right_up"] = list_to_pose([0, 0, approach_height, 0, 0, -1.5707])
positions["right_out"] = list_to_pose([0, approach_offset, 0, 0, 0, -1.5707])
positions["right_out_up"] = list_to_pose(
[0, approach_offset, approach_height, 0, 0, -1.5707])
positions["back_turned"] = list_to_pose([0, 0, 0, 1.5707, 0, 0])
positions["back_turned_up"] = list_to_pose(
[0, 0, approach_height, 1.5707, 0, 0])
positions["back_turned_out"] = list_to_pose(
[-approach_offset, 0, 0, 1.5707, 0, 0])
positions["back_turned_out_up"] = list_to_pose(
[-approach_offset, 0, approach_height, 1.5707, 0, 0])
def add_dynamic_obj(self,
ref_frame,
obj_name,
pos,
ori,
size,
touch_links=None):
"""
Add object to the ref_frame, the object will move with the ref_frame.
Only box is supported for now.
Args:
ref_frame (str): which link are you adding object to.
obj_name (str): object name.
pos (list): position of the object with respect to the ref_frame.
ori (list): orientation of the object with
respect to the ref_frame.
size (float or list or tuple): size can be a
float, which means the edge
length of a cube. size can also be a list or tuple of length 3,
the it specifies the 3 edge lengths of the cuboid.
Returns:
bool: if the object is successfully added.
"""
if not isinstance(pos, list):
raise TypeError('pos should be a list')
if not isinstance(ori, list):
raise TypeError('ori should be a list')
pose = pos + ori
pose = conversions.list_to_pose(pose)
pose_stamped = PoseStamped()
pose_stamped.header.frame_id = ref_frame
pose_stamped.pose = pose
if isinstance(size, float):
size = (size, size, size)
elif isinstance(size, list) or isinstance(size, tuple):
if len(size) != 3:
raise ValueError('If size is a list or tuple, its length'
' should be 3 for a box')
else:
raise TypeError('size should be a float number, a 3-element list '
'or a 3-element tuple for a box')
if isinstance(size, list):
size = tuple(size)
if touch_links is None:
self.scene.attach_box(ref_frame, obj_name, pose_stamped, size)
else:
# moveit ignores collisions between box and links in touch_links
self.scene.attach_box(ref_frame,
obj_name,
pose_stamped,
size,
touch_links=touch_links)
obj_dict, obj_adict = self.get_objects()
success = False
if obj_name in obj_adict.keys():
success = True
return success
def main():
try:
pnp = PickNPlaceTutorial()
pnp.jmove_to_joint_goal([1.57, -2.27, 1.93, -1.19, -1.57,
0]) #home pose
table = geometry_msgs.msg.PoseStamped()
table.header.frame_id = "odom"
table.pose.position.x = 0.75
table.pose.position.y = 0.1
table.pose.position.z = 0.7
table.pose.orientation.w = 1
pnp.add_box(name='table', pose_stamp=table, size=(0.1, 0.1, 0.3))
i = 0
while (i < 10):
test_pose = list_to_pose([0.75, 0.1, 0.7, 0, 9 * pi / 180 * i, 0])
table.pose.position.x = test_pose.position.x
table.pose.position.y = test_pose.position.y
table.pose.position.z = test_pose.position.z
table.pose.orientation.x = test_pose.orientation.x
table.pose.orientation.y = test_pose.orientation.y
table.pose.orientation.z = test_pose.orientation.z
table.pose.orientation.w = test_pose.orientation.w
table.pose.position.x = table.pose.position.x - 0.1
table.pose.position.y = table.pose.position.y - 0.1
table.pose.position.z = table.pose.position.z - 0.1
pnp.add_box(name='table', pose_stamp=table, size=(0.1, 0.1, 0.1))
rospy.sleep(0.1)
print(table.pose)
pnp.jmove_to_pose_goal(table.pose)
rospy.sleep(0.1)
i += 1
i = 0
while (i < 10):
test_pose = list_to_pose(
[0.75, 0.1, 0.7, 0, pi / 2 - i * 9 * pi / 180, 0])
table.pose.position.x = test_pose.position.x
table.pose.position.y = test_pose.position.y
table.pose.position.z = test_pose.position.z
table.pose.orientation.x = test_pose.orientation.x
table.pose.orientation.y = test_pose.orientation.y
table.pose.orientation.z = test_pose.orientation.z
table.pose.orientation.w = test_pose.orientation.w
table.pose.position.x = table.pose.position.x - 0.1
table.pose.position.y = table.pose.position.y - 0.1
table.pose.position.z = table.pose.position.z - 0.1
pnp.add_box(name='table', pose_stamp=table, size=(0.1, 0.1, 0.1))
rospy.sleep(0.1)
print(table.pose)
pnp.jmove_to_pose_goal(table.pose)
rospy.sleep(0.1)
i += 1
i = 0
while (i < 5):
test_pose = list_to_pose([0.75 + 0.01 * i, 0.1, 0.7, 0, 0, 0])
table.pose.position.x = test_pose.position.x
table.pose.position.y = test_pose.position.y
table.pose.position.z = test_pose.position.z
table.pose.orientation.x = test_pose.orientation.x
table.pose.orientation.y = test_pose.orientation.y
table.pose.orientation.z = test_pose.orientation.z
table.pose.orientation.w = test_pose.orientation.w
table.pose.position.x = table.pose.position.x - 0.1
table.pose.position.y = table.pose.position.y - 0.1
table.pose.position.z = table.pose.position.z - 0.1
pnp.add_box(name='table', pose_stamp=table, size=(0.1, 0.1, 0.1))
rospy.sleep(0.1)
print(table.pose)
pnp.jmove_to_pose_goal(table.pose)
rospy.sleep(0.1)
i += 1
i = 0
while (i < 5):
test_pose = list_to_pose([0.85 - 0.01 * i, 0.1, 0.7, 0, 0, 0])
table.pose.position.x = test_pose.position.x
table.pose.position.y = test_pose.position.y
table.pose.position.z = test_pose.position.z
table.pose.orientation.x = test_pose.orientation.x
table.pose.orientation.y = test_pose.orientation.y
table.pose.orientation.z = test_pose.orientation.z
table.pose.orientation.w = test_pose.orientation.w
table.pose.position.x = table.pose.position.x - 0.1
table.pose.position.y = table.pose.position.y - 0.1
table.pose.position.z = table.pose.position.z - 0.1
pnp.add_box(name='table', pose_stamp=table, size=(0.1, 0.1, 0.1))
rospy.sleep(0.1)
print(table.pose)
pnp.jmove_to_pose_goal(table.pose)
rospy.sleep(0.1)
i += 1
i = 0
while (i < 5):
test_pose = list_to_pose([0.75, 0.1 + 0.01 * i, 0.7, 0, 0, 0])
table.pose.position.x = test_pose.position.x
table.pose.position.y = test_pose.position.y
table.pose.position.z = test_pose.position.z
table.pose.orientation.x = test_pose.orientation.x
table.pose.orientation.y = test_pose.orientation.y
table.pose.orientation.z = test_pose.orientation.z
table.pose.orientation.w = test_pose.orientation.w
table.pose.position.x = table.pose.position.x - 0.1
table.pose.position.y = table.pose.position.y - 0.1
table.pose.position.z = table.pose.position.z - 0.1
pnp.add_box(name='table', pose_stamp=table, size=(0.1, 0.1, 0.1))
rospy.sleep(0.1)
print(table.pose)
pnp.jmove_to_pose_goal(table.pose)
rospy.sleep(0.1)
i += 1
i = 0
while (i < 10):
test_pose = list_to_pose([0.75, 0.15 - 0.01 * i, 0.7, 0, 0, 0])
table.pose.position.x = test_pose.position.x
table.pose.position.y = test_pose.position.y
table.pose.position.z = test_pose.position.z
table.pose.orientation.x = test_pose.orientation.x
table.pose.orientation.y = test_pose.orientation.y
table.pose.orientation.z = test_pose.orientation.z
table.pose.orientation.w = test_pose.orientation.w
table.pose.position.x = table.pose.position.x - 0.1
table.pose.position.y = table.pose.position.y - 0.1
table.pose.position.z = table.pose.position.z - 0.1
pnp.add_box(name='table', pose_stamp=table, size=(0.1, 0.1, 0.1))
rospy.sleep(0.1)
print(table.pose)
pnp.jmove_to_pose_goal(table.pose)
rospy.sleep(0.1)
i += 1
i = 0
while (i < 5):
test_pose = list_to_pose([0.75, 0.05 + 0.01 * i, 0.7, 0, 0, 0])
table.pose.position.x = test_pose.position.x
table.pose.position.y = test_pose.position.y
table.pose.position.z = test_pose.position.z
table.pose.orientation.x = test_pose.orientation.x
table.pose.orientation.y = test_pose.orientation.y
table.pose.orientation.z = test_pose.orientation.z
table.pose.orientation.w = test_pose.orientation.w
table.pose.position.x = table.pose.position.x - 0.1
table.pose.position.y = table.pose.position.y - 0.1
table.pose.position.z = table.pose.position.z - 0.1
pnp.add_box(name='table', pose_stamp=table, size=(0.1, 0.1, 0.1))
rospy.sleep(0.1)
print(table.pose)
pnp.jmove_to_pose_goal(table.pose)
rospy.sleep(0.1)
i += 1
pnp.jmove_to_joint_goal([1.57, -2.27, 1.93, -1.19, -1.57,
0]) #home pose
#
#pnp.jmove_to_pose_goal(list_to_pose([0.35, -0.10, 0.48, 0, -pi, -pi/2]))
#pnp.tmove_to_pose_goal(list_to_pose([0.35, -0.10, 0.18, 0, -pi, -pi/2]))
#pnp.tmove_to_pose_goal(list_to_pose([0.35, -0.10, 0.28, 0, -pi, -pi/2]))
#
#
#pnp.jmove_to_pose_goal(list_to_pose([0.65, -0.10, 0.48, 0, -pi, -pi/2]))
#pnp.tmove_to_pose_goal(list_to_pose([0.65, -0.10, 0.18, 0, -pi, -pi/2]))
#pnp.tmove_to_pose_goal(list_to_pose([0.65, -0.10, 0.28, 0, -pi, -pi/2]))
#
#
#pnp.jmove_to_pose_goal(list_to_pose([0.35, 0.10, 0.48, 0, -pi, -pi/2]))
#pnp.tmove_to_pose_goal(list_to_pose([0.35, 0.10, 0.18, 0, -pi, -pi/2]))
#pnp.tmove_to_pose_goal(list_to_pose([0.35, 0.10, 0.28, 0, -pi, -pi/2]))
#
#
#pnp.jmove_to_pose_goal(list_to_pose([0.65, 0.10, 0.48, 0, -pi, -pi/2]))
#pnp.tmove_to_pose_goal(list_to_pose([0.65, 0.10, 0.18, 0, -pi, -pi/2]))
#pnp.tmove_to_pose_goal(list_to_pose([0.65, 0.10, 0.28, 0, -pi, -pi/2]))
#
#pnp.jmove_to_joint_goal([0, 0, -pi/2, 0, -pi/2, 0])
except rospy.ROSInterruptException:
return
except KeyboardInterrupt:
return
else:
print("============ Python pick place demo complete!")
def main():
try:
pnp = PickNPlaceTutorial()
raw_input()
table = geometry_msgs.msg.PoseStamped()
table.header.frame_id = pnp.robot_group.get_planning_frame()
table.pose.position.x =0.5
table.pose.position.y =0
table.pose.position.z =0.05
table.pose.orientation.w = 1.0
pnp.add_box(name='table', pose_stamp=table, size=(0.5, 0.5, 0.1))
wall = geometry_msgs.msg.PoseStamped()
wall.header.frame_id = pnp.robot_group.get_planning_frame()
wall.pose.position.x =0.5
wall.pose.position.y =0
wall.pose.position.z =0.2
wall.pose.orientation.w = 1.0
pnp.add_box(name='wall', pose_stamp=wall, size=(0.01, 0.5, 0.25))
cube_0 = geometry_msgs.msg.PoseStamped()
cube_0.header.frame_id = pnp.robot_group.get_planning_frame()
cube_0.pose.position.x =0.35
cube_0.pose.position.y =-0.10
cube_0.pose.position.z =0.13
cube_0.pose.orientation.w = 1.0
pnp.add_box(name='cube_0', pose_stamp=cube_0)
cube_1 = geometry_msgs.msg.PoseStamped()
cube_1.header.frame_id = pnp.robot_group.get_planning_frame()
cube_1.pose.position.x =0.35
cube_1.pose.position.y =0.10
cube_1.pose.position.z =0.13
cube_1.pose.orientation.w = 1.0
pnp.add_box(name='cube_1', pose_stamp=cube_1)
rospy.sleep(0.5)
pnp.jmove_to_joint_goal([0, 0, -pi/2, 0, -pi/2, 0])
pnp.jmove_to_pose_goal(list_to_pose([0.35, -0.10, 0.48, 0, -pi, -pi/2]))
pnp.tmove_to_pose_goal(list_to_pose([0.35, -0.10, 0.18, 0, -pi, -pi/2]))
pnp.hold_hand('cube_0')
pnp.tmove_to_pose_goal(list_to_pose([0.35, -0.10, 0.28, 0, -pi, -pi/2]))
pnp.jmove_to_pose_goal(list_to_pose([0.65, -0.10, 0.48, 0, -pi, -pi/2]))
pnp.tmove_to_pose_goal(list_to_pose([0.65, -0.10, 0.18, 0, -pi, -pi/2]))
pnp.release_hand('cube_0')
pnp.tmove_to_pose_goal(list_to_pose([0.65, -0.10, 0.28, 0, -pi, -pi/2]))
pnp.jmove_to_pose_goal(list_to_pose([0.35, 0.10, 0.48, 0, -pi, -pi/2]))
pnp.tmove_to_pose_goal(list_to_pose([0.35, 0.10, 0.18, 0, -pi, -pi/2]))
pnp.hold_hand('cube_1')
pnp.tmove_to_pose_goal(list_to_pose([0.35, 0.10, 0.28, 0, -pi, -pi/2]))
pnp.jmove_to_pose_goal(list_to_pose([0.65, 0.10, 0.48, 0, -pi, -pi/2]))
pnp.tmove_to_pose_goal(list_to_pose([0.65, 0.10, 0.18, 0, -pi, -pi/2]))
pnp.release_hand('cube_1')
pnp.tmove_to_pose_goal(list_to_pose([0.65, 0.10, 0.28, 0, -pi, -pi/2]))
pnp.jmove_to_joint_goal([0, 0, -pi/2, 0, -pi/2, 0])
except rospy.ROSInterruptException:
return
except KeyboardInterrupt:
return
else:
print("============ Python pick place demo complete!")
def add_static_obj(self,
obj_name,
pos,
ori,
size=None,
obj_type='box',
ref_frame='/base_link',
normal=None):
"""
Add static object to the planning scene.
Args:
obj_name (str): object name.
pos (list): position.
ori (list): orientation. It can be quaternion ([qx, qy, qz, qw])
or euler angles ([roll, pitch, yaw]).
size (float or list or tuple): size of the object.
if the object is a plane, size should be None
if the object is a box, size can be a float
(which means the edge length of a cube.)
size can also be a list or tuple of length 3,
that specifies the 3 edge lengths of the cuboid
if the object is a sphere, size is a float,
(which means the radius).
obj_type (str): one of [`sphere`, `box`, `plane`].
ref_frame (str): reference frame on which
the pos and ori are specified.
normal (list or tuple): only used if the
object is a plane. It means the
normal direction of the plane.
Returns:
bool: if the object is successfully added.
"""
if not isinstance(pos, list):
raise TypeError('pos should be a list')
if not isinstance(ori, list):
raise TypeError('ori should be a list')
if obj_type not in ['sphere', 'box', 'plane']:
raise ValueError('Unsupported object type. Only [sphere],'
' [box], [plane] are supported for now.')
pose = pos + ori
pose = conversions.list_to_pose(pose)
pose_stamped = PoseStamped()
pose_stamped.header.frame_id = ref_frame
pose_stamped.pose = pose
if obj_type == 'plane':
if size is not None:
raise ValueError('No size needed for plane')
if normal is None:
normal = (0, 0, 1)
if len(normal) != 3:
raise ValueError('Length of the normal should be 3')
if isinstance(normal, list):
normal = tuple(normal)
self.scene.add_plane(obj_name, pose_stamped, normal)
elif obj_type == 'box':
if isinstance(size, float):
size = (size, size, size)
elif isinstance(size, list) or isinstance(size, tuple):
if len(size) != 3:
raise ValueError('If size is a list or tuple, its length'
' should be 3 for a box')
else:
raise TypeError('size should be a float number, '
'a 3-element list '
'or a 3-element tuple for a box')
if isinstance(size, list):
size = tuple(size)
self.scene.add_box(obj_name, pose_stamped, size)
else:
if not isinstance(size, float):
raise ValueError('Size should a float number for sphere')
self.scene.add_sphere(obj_name, pose_stamped, radius=size)
obj_dict, obj_adict = self.get_objects()
success = False
if obj_name in obj_dict.keys():
success = True
return success
