class Arm:
def __init__(self):
self.moveGroup = MoveGroupInterface("arm_with_torso", "base_link")
self.planning_scene = PlanningSceneInterface("base_link")
self.planning_scene.addCube("my_front_ground", 2, 1.4, 0.0, -0.48)
self.planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -0.6)
self.planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -0.6)
self.planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -0.6)
def MoveToPose(self, X, Y, Z, Roll, Pitch, Yaw):
orientation = tf.transformations.quaternion_from_euler(
Roll, Pitch, Yaw)
self.poseStamped = PoseStamped()
self.poseStamped.header.frame_id = 'base_link'
self.poseStamped.header.stamp = rospy.Time.now()
self.poseStamped.pose.position.x = X
self.poseStamped.pose.position.y = Y
self.poseStamped.pose.position.z = Z
self.poseStamped.pose.orientation.x = orientation[0]
self.poseStamped.pose.orientation.y = orientation[1]
self.poseStamped.pose.orientation.z = orientation[2]
self.poseStamped.pose.orientation.w = orientation[3]
self.moveGroup.moveToPose(self.poseStamped,
'gripper_link',
0.005,
max_velocity_scaling_factor=0.2)
self.result = self.moveGroup.get_move_action().get_result()
def Tuck(self):
joints = [
"torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint",
"upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint",
"wrist_flex_joint", "wrist_roll_joint"
]
pose = [0.05, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
self.moveGroup.moveToJointPosition(joints,
pose,
0.01,
max_velocity_scaling_factor=0.2)
while not rospy.is_shutdown():
self.result = self.moveGroup.get_move_action().get_result()
if self.result.error_code.val == MoveItErrorCodes.SUCCESS:
break
def AddDice(self, name, X, Y, Z):
self.planning_scene.addCube(name, 0.1, X, Y, Z)
def RemoveDice(self, name):
self.planning_scene.removeCollisionObject(name)
def setup_scene():
scene = PlanningSceneInterface('base_link')
scene.removeCollisionObject('box')
scene.addCube('box', 0.25, -0.45, 0.1, 0.125)
pick_place = PickPlaceInterface('xarm5', 'gripper', verbose=True)
grasp = Grasp()
# fill in g
# setup object named object_name using PlanningSceneInterface
pick_place.pickup('box', [grasp], support_name='supporting_surface')
place_loc = PlaceLocation()
# fill in l
pick_place.place('box'[place_loc],
goal_is_eef=True,
support_name='supporting_surface')
def arm_move(self):
move_group = MoveGroupInterface("arm_with_torso", "base_link")
# Define ground plane
# This creates objects in the planning scene that mimic the ground
# If these were not in place gripper could hit the ground
planning_scene = PlanningSceneInterface("base_link")
planning_scene.removeCollisionObject("my_front_ground")
planning_scene.removeCollisionObject("my_back_ground")
planning_scene.removeCollisionObject("my_right_ground")
planning_scene.removeCollisionObject("my_left_ground")
planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
#planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
planning_scene.addCube("my_left_ground", 1, 1.5,
self.translation[2][0], self.translation[2][1],
self.translation[2][2])
planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
# TF joint names
joint_names = [
"torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint",
"upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint",
"wrist_flex_joint", "wrist_roll_joint"
]
# Lists of joint angles in the same order as in joint_names
disco_poses = [[0, 2.5, -0.1, 3.0, 1.5, 3.0, 1.0, 3.0]]
for pose in disco_poses:
if rospy.is_shutdown():
break
# Plans the joints in joint_names to angles in pose
move_group.moveToJointPosition(joint_names, pose, wait=False)
# Since we passed in wait=False above we need to wait here
move_group.get_move_action().wait_for_result()
result = move_group.get_move_action().get_result()
if result:
# Checking the MoveItErrorCode
if result.error_code.val == MoveItErrorCodes.SUCCESS:
rospy.loginfo("Disco!")
else:
# If you get to this point please search for:
# moveit_msgs/MoveItErrorCodes.msg
rospy.logerr("Arm goal in state: %s",
move_group.get_move_action().get_state())
else:
rospy.logerr("MoveIt! failure no result returned.")
# This stops all arm movement goals
# It should be called when a program is exiting so movement stops
move_group.get_move_action().cancel_all_goals()
class ArmController(object):
def __init__(self):
# Create move group interface for a fetch robot
self._move_group = MoveGroupInterface("arm_with_torso", "base_link")
# Define ground plane
# This creates objects in the planning scene that mimic the ground
# If these were not in place gripper could hit the ground
self._planning_scene = PlanningSceneInterface("base_link")
self._planning_scene.removeCollisionObject("my_front_ground")
self._planning_scene.removeCollisionObject("my_back_ground")
self._planning_scene.removeCollisionObject("my_right_ground")
self._planning_scene.removeCollisionObject("my_left_ground")
self._planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
self._planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
self._planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
self._planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
def SetPose(self, target_pose):
# Plans the joints in joint_names to angles in pose
self._move_group.moveToJointPosition(target_pose.keys(),
target_pose.values(),
wait=False)
# Since we passed in wait=False above we need to wait here
self._move_group.get_move_action().wait_for_result()
result = self._move_group.get_move_action().get_result()
return_result = False
if result:
# Checking the MoveItErrorCode
if result.error_code.val == MoveItErrorCodes.SUCCESS:
rospy.loginfo("MoveIt pose reached.")
return_result = True
else:
# If you get to this point please search for:
# moveit_msgs/MoveItErrorCodes.msg
rospy.logerr("Arm goal in state: %s",
self._move_group.get_move_action().get_state())
else:
rospy.logerr("MoveIt! failure no result returned.")
# This stops all arm movement goals
# It should be called when a program is exiting so movement stops
self._move_group.get_move_action().cancel_all_goals()
return return_result
def SetPoseWithRetry(self, target_pose, max_time=3):
for _ in range(max_time):
if self.SetPose(target_pose):
return True
return False
def __init__(self):
self.moving_mode = False
self.plan_only = False
self.prev_joy_buttons = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
self.joint_names = [
"shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint",
"wrist_roll_joint"
]
self.joy_subscriber = rospy.Subscriber("/fetch_joy", Joy,
self.joy_callback)
self.joints_subscriber = rospy.Subscriber("/joint_states", JointState,
self.joints_states_callback)
self.rgbimage_subscriber = rospy.Subscriber("/head_camera/depth/image",
Image,
self.depthimage_callback)
self.depthimage_subscriber = rospy.Subscriber(
"/head_camera/depth/image", Image, self.rgbimage_callback)
self.reset_subscriber = rospy.Subscriber("/fetch_ai_arm_reset_pos",
Empty,
self.arm_reset_callback)
self.reset_subscriber = rospy.Subscriber("/fetch_ai_arm_start_pos",
Empty,
self.arm_start_callback)
self.arm_effort_pub = rospy.Publisher(
"/arm_controller/weightless_torque/command",
JointTrajectory,
queue_size=2)
self.gripper_client = actionlib.SimpleActionClient(
"gripper_controller/gripper_action", GripperCommandAction)
self.arm_cartesian_twist_pub = rospy.Publisher(
"/arm_controller/cartesian_twist/command", Twist, queue_size=2)
self.head_point_client = actionlib.SimpleActionClient(
"head_controller/point_head", PointHeadAction)
self.arm_move_group = MoveGroupInterface("arm",
"base_link",
plan_only=self.plan_only)
planning_scene = PlanningSceneInterface("base_link")
planning_scene.removeCollisionObject("my_front_ground")
planning_scene.removeCollisionObject("my_back_ground")
planning_scene.removeCollisionObject("my_right_ground")
planning_scene.removeCollisionObject("my_left_ground")
planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
def setBoundaries():
'''
This is a fix for the FETCH colliding with itself
Define ground plane
This creates objects in the planning scene that mimic the ground
If these were not in place gripper could hit the ground
'''
planning_scene = PlanningSceneInterface("base_link")
planning_scene.removeCollisionObject("my_front_ground")
planning_scene.removeCollisionObject("my_back_ground")
planning_scene.removeCollisionObject("my_right_ground")
planning_scene.removeCollisionObject("my_left_ground")
planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
class GraspingClient(object):
def __init__(self):
self.scene = PlanningSceneInterface("base_link")
self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True)
self.move_group = MoveGroupInterface("arm", "base_link")
find_topic = "basic_grasping_perception/find_objects"
rospy.loginfo("Waiting for %s..." % find_topic)
self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction)
self.find_client.wait_for_server()
def updateScene(self):
# find objects
rospy.loginfo("Add collision objects")
self.scene.removeCollisionObject("my_front_ground")
self.scene.removeCollisionObject("my_back_ground")
self.scene.removeCollisionObject("my_right_ground")
self.scene.removeCollisionObject("my_left_ground")
self.scene.removeCollisionObject("box_1")
self.scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
self.scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
self.scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
self.scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
self.scene.addBox("box_1", 0.44, 0.43, 0.68, 0.7, -0.4, .34)
self.scene.waitForSync()
def tuck(self):
joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def zero(self):
joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def move():
rospy.init_node("move")
move_group = MoveGroupInterface("arm_with_torso", "base_link")
#planning scene setup
planning_scene = PlanningSceneInterface("base_link")
planning_scene.removeCollisionObject("my_front_ground")
planning_scene.removeCollisionObject("my_back_ground")
planning_scene.removeCollisionObject("my_right_ground")
planning_scene.removeCollisionObject("my_left_ground")
planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
#move head camera
client = actionlib.SimpleActionClient("head_controller/point_head", PointHeadAction)
rospy.loginfo("Waiting for head_controller...")
client.wait_for_server()
goal = PointHeadGoal()
goal.target.header.stamp = rospy.Time.now()
goal.target.header.frame_id = "base_link"
goal.target.point.x = 1.2
goal.target.point.y = 0.0
goal.target.point.z = 0.0
goal.min_duration = rospy.Duration(1.0)
client.send_goal(goal)
client.wait_for_result()
#arm setup
move_group = MoveGroupInterface("arm", "base_link")
#set arm initial position
joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
pose = [1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0]
while not rospy.is_shutdown():
result = move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
class RobotArm:
def __init__(self):
self.moveGroup = MoveGroupInterface("arm_with_torso", "base_link")
self.planning_scene = PlanningSceneInterface("base_link")
self.planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
self.planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
self.planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
self.planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
self.planning_scene.removeCollisionObject("my_front_ground")
self.planning_scene.removeCollisionObject("my_back_ground")
self.planning_scene.removeCollisionObject("my_right_ground")
self.planning_scene.removeCollisionObject("my_left_ground")
self.planning_scene.removeCollisionObject("table1")
def MoveToPose(self,X,Y,Z,x,y,z,w):
self.poseStamped = PoseStamped()
self.poseStamped.header.frame_id = 'base_link'
self.poseStamped.header.stamp = rospy.Time.now()
self.poseStamped.pose.position.x = X
self.poseStamped.pose.position.y = Y
self.poseStamped.pose.position.z = Z
self.poseStamped.pose.orientation.x = x
self.poseStamped.pose.orientation.y = y
self.poseStamped.pose.orientation.z = z
self.poseStamped.pose.orientation.w = w
self.moveGroup.moveToPose(self.poseStamped, 'gripper_link')
self.result = self.moveGroup.get_move_action().get_result()
def OriginReturn(self):
#Move to the origin
joints = ["torso_lift_joint","shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
pose = [0.0, 1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0]
self.moveGroup.moveToJointPosition(joints, pose, 0.02)
while 1:
self.result = self.moveGroup.get_move_action().get_result()
if self.result.error_code.val == MoveItErrorCodes.SUCCESS:
break
class PlanningSceneClient(object):
'''Define ground plane
This creates objects in the planning scene that mimic the ground
If these were not in place gripper could hit the ground'''
def __init__(self):
self.planning_scene = PlanningSceneInterface("base_link")
self.planning_scene.removeCollisionObject("my_front_ground")
self.planning_scene.removeCollisionObject("my_back_ground")
self.planning_scene.removeCollisionObject("my_right_ground")
self.planning_scene.removeCollisionObject("my_left_ground")
self.planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
self.planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
self.planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
self.planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
def close(self):
self.planning_scene.removeCollisionObject("my_front_ground")
self.planning_scene.removeCollisionObject("my_back_ground")
self.planning_scene.removeCollisionObject("my_right_ground")
self.planning_scene.removeCollisionObject("my_left_ground")
def picknplace():
# Define initial parameters
p = PlanningSceneInterface("base")
g = MoveGroupInterface("both_arms", "base")
gr = MoveGroupInterface("right_arm", "base")
gl = MoveGroupInterface("left_arm", "base")
leftgripper = baxter_interface.Gripper('left')
leftgripper.calibrate()
leftgripper.open()
jts_both = ['left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1', 'left_w2', 'right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2']
jts_right = ['right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2']
jts_left = ['left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1', 'left_w2']
pos1 = [-1.441426162661994, 0.8389151064712133, 0.14240920034028015, -0.14501001475655606, -1.7630090377446503, -1.5706376573674472, 0.09225918246029519,1.7238109084167481, 1.7169079948791506, 0.36930587426147465, -0.33249033539428713, -1.2160632682067871, 1.668587600115967, -1.810097327636719]
pos2 = [-0.949534106616211, 1.4994662184448244, -0.6036214393432617, -0.7869321432861328, -2.4735440176391603, -1.212228316241455, -0.8690001153442384, 1.8342575250183106, 1.8668546167236328, -0.45674277907104494, -0.21667478604125978, -1.2712865765075685, 1.7472041154052735, -2.4582042097778323]
lpos1 = [-1.441426162661994, 0.8389151064712133, 0.14240920034028015, -0.14501001475655606, -1.7630090377446503, -1.5706376573674472, 0.09225918246029519]
lpos2 = [-0.949534106616211, 1.4994662184448244, -0.6036214393432617, -0.7869321432861328, -2.4735440176391603, -1.212228316241455, -0.8690001153442384]
rpos1 = [1.7238109084167481, 1.7169079948791506, 0.36930587426147465, -0.33249033539428713, -1.2160632682067871, 1.668587600115967, -1.810097327636719]
rpos2 = [1.8342575250183106, 1.8668546167236328, -0.45674277907104494, -0.21667478604125978, -1.2712865765075685, 1.7472041154052735, -2.4582042097778323]
# Clear planning scene
p.clear()
# Add table as attached object
p.attachBox('table', 0.7, 1.27, 0.54, 0.65, -0.2, -0.38, 'base', touch_links=['pedestal'])
# Move both arms to start state
g.moveToJointPosition(jts_both, pos1, plan_only=False)
# Get obj locations
temp = rospy.wait_for_message("Dpos", PoseArray)
locs = temp.poses
locs_x = []
locs_y = []
orien = []
size = []
for i in range(len(locs)):
locs_x.append(0.57 + locs[i].position.x)
locs_y.append(-0.011 + locs[i].position.y)
orien.append(locs[i].position.z*pi/180)
size.append(locs[i].orientation.x)
# Filter objects list to remove multiple detected lcoations for same objects --> required, as adding objects to collision scene
ind_rmv = []
for i in range(0,len(locs)):
if (locs_y[i] > 0.42):
ind_rmv.append(i)
continue
for j in range(i,len(locs)):
if not (i == j):
if sqrt((locs_x[i] - locs_x[j])**2 + (locs_y[i] - locs_y[j])**2)<0.018:
ind_rmv.append(i)
locs_x = del_meth(locs_x, ind_rmv)
locs_y = del_meth(locs_y, ind_rmv)
orien = del_meth(orien, ind_rmv)
size = del_meth(size, ind_rmv)
# Sort objects based on size (largest first to smallest last) This was done to enable stacking large cubes
if locs_x:
ig0 = itemgetter(0)
sorted_lists = zip(*sorted(zip(size,locs_x,locs_y,orien), reverse=True, key=ig0))
locs_x = list(sorted_lists[1])
locs_y = list(sorted_lists[2])
orien = list(sorted_lists[3])
size = list(sorted_lists[0])
# Loop to continue pick and place until all objects are cleared from table
j=0
k=0
while locs_x:
p.clear() # CLear planning scene of all collision objects
# Attach table as attached collision object to base of baxter
p.attachBox('table', 0.7, 1.27, 0.54, 0.65, -0.2, -0.38, 'base', touch_links=['pedestal'])
xn = locs_x[0]
yn = locs_y[0]
zn = -0.06
thn = orien[0]
sz = size[0]
if thn > pi/4:
thn = -1*(thn%(pi/4))
# Add collision objects into planning scene
objlist = ['obj01', 'obj02', 'obj03', 'obj04', 'obj05', 'obj06', 'obj07', 'obj08', 'obj09', 'obj10', 'obj11']
for i in range(1,len(locs_x)):
p.addCube(objlist[i], 0.05, locs_x[i], locs_y[i], -0.05)
p.waitForSync()
# Move both arms to pos2 (Right arm away and left arm on table)
g.moveToJointPosition(jts_both, pos2, plan_only=False)
# Move left arm to pick object and pick object
pickgoal = PoseStamped()
pickgoal.header.frame_id = "base"
pickgoal.header.stamp = rospy.Time.now()
pickgoal.pose.position.x = xn
pickgoal.pose.position.y = yn
pickgoal.pose.position.z = zn+0.1
pickgoal.pose.orientation.x = 1.0
pickgoal.pose.orientation.y = 0.0
pickgoal.pose.orientation.z = 0.0
pickgoal.pose.orientation.w = 0.0
gl.moveToPose(pickgoal, "left_gripper", plan_only=False)
# Orientate the gripper --> uses function from geometry.py (by Mike Ferguson) to 'rotate a pose' given rpy angles
pickgoal.pose = rotate_pose_msg_by_euler_angles(pickgoal.pose, 0.0, 0.0, thn)
gl.moveToPose(pickgoal, "left_gripper", plan_only=False)
pickgoal.pose.position.z = zn
gl.moveToPose(pickgoal, "left_gripper", max_velocity_scaling_factor = 0.14, plan_only=False)
leftgripper.close()
pickgoal.pose.position.z = zn+0.1
gl.moveToPose(pickgoal, "left_gripper", plan_only=False)
# Move both arms to pos1
g.moveToJointPosition(jts_both, pos1, plan_only=False)
# Get obj locations
temp = rospy.wait_for_message("Dpos", PoseArray)
locs = temp.poses
locs_x = []
locs_y = []
orien = []
size = []
for i in range(len(locs)):
locs_x.append(0.57 + locs[i].position.x)
locs_y.append(-0.011 + locs[i].position.y)
orien.append(locs[i].position.z*pi/180)
size.append(locs[i].orientation.x)
# Filter objects list to remove multiple detected lcoations for same objects --> required, as adding objects to collision scene
ind_rmv = []
for i in range(0,len(locs)):
if (locs_y[i] > 0.42):
ind_rmv.append(i)
continue
for j in range(i,len(locs)):
if not (i == j):
if sqrt((locs_x[i] - locs_x[j])**2 + (locs_y[i] - locs_y[j])**2)<0.018:
ind_rmv.append(i)
locs_x = del_meth(locs_x, ind_rmv)
locs_y = del_meth(locs_y, ind_rmv)
orien = del_meth(orien, ind_rmv)
size = del_meth(size, ind_rmv)
# Sort objects based on size (largest first to smallest last) This was done to enable stacking large cubes
if locs_x:
ig0 = itemgetter(0)
sorted_lists = zip(*sorted(zip(size,locs_x,locs_y,orien), reverse=True, key=ig0))
locs_x = list(sorted_lists[1])
locs_y = list(sorted_lists[2])
orien = list(sorted_lists[3])
size = list(sorted_lists[0])
# Place object
stleft = PoseStamped()
stleft.header.frame_id = "base"
stleft.header.stamp = rospy.Time.now()
stleft.pose.position.x = 0.65
stleft.pose.position.y = 0.55
stleft.pose.position.z = 0.1
stleft.pose.orientation.x = 1.0
stleft.pose.orientation.y = 0.0
stleft.pose.orientation.z = 0.0
stleft.pose.orientation.w = 0.0
# Stack objects (large cubes) if size if greater than some threshold
if sz > 16.:
stleft.pose.position.x = 0.65
stleft.pose.position.y = 0.7
stleft.pose.position.z = -0.04+(k*0.05)
k += 1
gl.moveToPose(stleft, "left_gripper", plan_only=False)
leftgripper.open()
stleft.pose.position.z = 0.3
gl.moveToPose(stleft, "left_gripper", plan_only=False)
class Reaching:
def __init__(self):
rospy.init_node('reaching', anonymous=True)
self.robot = moveit_commander.RobotCommander()
self.model_pose = [0.0, 0.0, 0.0]
self.obstacle_pose = [0.0, 0.0, 0.0]
self.set_forbidden()
self.set_init_pose()
def set_forbidden(self):
#set forbidden erea
self.planning_scene = PlanningSceneInterface("base_link")
self.planning_scene.removeCollisionObject("my_front_ground")
self.planning_scene.removeCollisionObject("my_back_ground")
self.planning_scene.removeCollisionObject("my_right_ground")
self.planning_scene.removeCollisionObject("my_left_ground")
self.planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
self.planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
self.planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
self.planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
self.planning_scene.removeCollisionObject("demo_cube")
self.planning_scene.removeCollisionObject("obstacle")
print dir(self.planning_scene)
import inspect
print "addBox's variable is ", inspect.getargspec(
self.planning_scene.addBox)
print "attachBox's variable is ", inspect.getargspec(
self.planning_scene.attachBox)
print "addCube's variable is ", inspect.getargspec(
self.planning_scene.addCube)
print "setColor's variable is ", inspect.getargspec(
self.planning_scene.setColor)
def set_init_pose(sefl):
#set init pose
move_group = MoveGroupInterface("manipulator", "base_link")
planning_scene = PlanningSceneInterface("base_link")
joint_names = [
"shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint",
"wrist_1_joint", "wrist_2_joint", "wrist_3_joint"
]
pose = [0.0, -1.98, 1.0, -0.64, -1.57, 0.0]
move_group.moveToJointPosition(joint_names, pose, wait=False)
move_group.get_move_action().wait_for_result()
result = move_group.get_move_action().get_result()
move_group.get_move_action().cancel_all_goals()
def callback(self, data):
x = data.pose[1].position.x
y = data.pose[1].position.y
z = data.pose[1].position.z
self.model_pose = [x, y, z]
x = data.pose[2].position.x
y = data.pose[2].position.y
z = data.pose[2].position.z
self.obstacle_pose = [x, y, z]
def start_subscriber(self):
rospy.Subscriber("/gazebo/model_states", ModelStates, self.callback)
def target(self):
rate = rospy.Rate(1)
rospy.sleep(1)
while not rospy.is_shutdown():
self.planning_scene.removeCollisionObject("demo_cube")
self.planning_scene.removeCollisionObject("obstacle")
self.planning_scene.addCube("demo_cube", 0.06, self.model_pose[0],
self.model_pose[1], self.model_pose[2])
self.planning_scene.addBox("obstacle", 0.540614, 0.083603, 0.54373,
self.obstacle_pose[0],
self.obstacle_pose[1],
self.obstacle_pose[2])
print self.model_pose
group = moveit_commander.MoveGroupCommander("manipulator")
#print group.get_current_pose().pose
pose_target = geometry_msgs.msg.Pose()
pose = group.get_current_pose().pose
pose_target = geometry_msgs.msg.Pose()
pose_target.orientation.x = pose.orientation.x
pose_target.orientation.y = pose.orientation.y
pose_target.orientation.z = pose.orientation.z
pose_target.orientation.w = pose.orientation.w
#pose_target.position.x = pose.position.x
#pose_target.position.y = pose.position.y
#pose_target.position.z = pose.position.z
pose_target.position.x = self.model_pose[0]
pose_target.position.y = self.model_pose[1]
pose_target.position.z = self.model_pose[2] + 0.05
group.set_pose_target(pose_target)
group.go()
rospy.sleep(1)
class Reaching:
def __init__(self):
rospy.init_node('reaching', anonymous=True) #ノードの初期化 reaching ていう名前
self.robot = moveit_commander.RobotCommander() # インスタンスの作成
self.model_pose = [0.0, 0.0, 0.0] #モデルの姿勢
self.obstacle_pose = [0.0, 0.0, 0.0] #障害物の位置
self.subsc_pose = [0.0, 0.0, 0.0] #購読した座標
self.subsc_orientation = [0.0, 0.0, 0.0, 1.0] #購読した四元数
self.set_forbidden() #禁止エリアの初期化
self.set_init_pose() #位置姿勢の初期化
def set_forbidden(self):
#set forbidden erea
self.planning_scene = PlanningSceneInterface(
"base_link") #PlanningSceneInterface のインスタンスの作成
self.planning_scene.removeCollisionObject("my_front_ground")
self.planning_scene.removeCollisionObject("my_back_ground")
self.planning_scene.removeCollisionObject("my_right_ground")
self.planning_scene.removeCollisionObject("my_left_ground") #オブジェクトの削除
self.planning_scene.addCube("my_front_ground", 2, 1.1, 0, -1.0)
self.planning_scene.addCube("my_back_ground", 2, -1.2, 0, -1.0)
self.planning_scene.addCube("my_left_ground", 2, 0.0, 1.0, -1.0)
self.planning_scene.addCube("my_right_ground", 2, 0.0, -1.0,
-1.0) #座標に立方体を挿入
self.planning_scene.removeCollisionObject("demo_cube")
self.planning_scene.removeCollisionObject("obstacle") #オブジェクトの削除
print dir(self.planning_scene) #インスタンス内のあらゆるオブジェクトの属性とメソッドのリストを表示
import inspect
print "addBox's variable is ", inspect.getargspec(
self.planning_scene.addBox)
print "attachBox's variable is ", inspect.getargspec(
self.planning_scene.attachBox)
print "addCube's variable is ", inspect.getargspec(
self.planning_scene.addCube)
print "setColor's variable is ", inspect.getargspec(
self.planning_scene.setColor) #python関数のパラメータの名前とデフォルト値を取得
def set_init_pose(self):
#set init pose
move_group = MoveGroupInterface(
"manipulator", "base_link") #MoveGroupInterface のインスタンスの作成
planning_scene = PlanningSceneInterface(
"base_link") #PlanningSceneInterface のインスタンスの作成
joint_names = [
"shoulder_pan_joint",
"shoulder_lift_joint", #ジョイントの名前を定義
"elbow_joint",
"wrist_1_joint",
"wrist_2_joint",
"wrist_3_joint"
]
pose = [0.0, -1.98, 1.0, -0.64, -1.57, 0.0]
move_group.moveToJointPosition(joint_names, pose,
wait=False) #joint_names を pose に動かす
move_group.get_move_action().wait_for_result() #wait result
result = move_group.get_move_action().get_result() #result を代入
move_group.get_move_action().cancel_all_goals() #すべてのゴールをキャンセル
def callback(self, data): #トピックにデータが追加されるたびに呼ばれる
x = data.pose[1].position.x
y = data.pose[1].position.y
z = data.pose[1].position.z
self.model_pose = [x, y, z] #modelの姿勢を代入
# x = data.pose[2].position.x
# y = data.pose[2].position.y
# z = data.pose[2].position.z
# self.obstacle_pose = [x,y,z] #障害物の姿勢を代入
def callbacksub(self, data):
x = data.x
y = data.y
z = data.z
self.subsc_pose = [x, y, z] #購読した座標を代入
def callbackunity(self, data):
x = data.pose.position.y
y = -data.pose.position.x
z = data.pose.position.z
self.subsc_pose = [x, y, z] #unity からの座標 座標変換も行っている
x = data.pose.orientation.x
y = data.pose.orientation.y
z = data.pose.orientation.z
w = data.pose.orientation.w
self.subsc_orientation = [x, y, z, w] #unity からの四元数
def start_subscriber(self): #購読をスタート
rospy.Subscriber("/gazebo/model_states", ModelStates, self.callback)
# /gazebo/model_states から gazebo_msgs/ModelStates 型のトピックを購読 トピックが更新されるたびに self.callback を呼ぶ
rospy.Subscriber("/UR/reaching/pose", Vector3, self.callbacksub)
rospy.Subscriber("/unity/target", PoseStamped,
self.callbackunity) #unityから配信しているトピックを購読
def target(self):
rate = rospy.Rate(1) #Rateクラスのインスタンス rateを１で
rospy.sleep(1) #1秒スリープ
while not rospy.is_shutdown(): #シャットダウンフラグが立っていなければ、
self.planning_scene.removeCollisionObject("demo_cube") #オブジェクトの削除
self.planning_scene.removeCollisionObject("obstacle") #オブジェクトの削除
self.planning_scene.addCube(
"demo_cube", 0.06, self.model_pose[0], self.model_pose[1],
self.model_pose[2]) #一辺が0.06の正方形をmodel_pose(座標)に追加
#self.planning_scene.addBox("obstacle", 0.5,0.5,0.5,0.5,0.5,0)#---の大きさの箱をobstacle_pose(座標)に追加
print self.model_pose #model_poseを表示
group = moveit_commander.MoveGroupCommander(
"manipulator") #MoveGroupCommander クラスのインスタンス
#print group.get_current_pose().pose
pose_target = geometry_msgs.msg.Pose()
pose = group.get_current_pose().pose #エンドエフェクタの位置姿勢
pose_target = geometry_msgs.msg.Pose(
) #geometry_msgs.pose のインスタンス これに入れるとトピックに反映される？
pose_target.orientation.x = self.subsc_orientation[0]
pose_target.orientation.y = self.subsc_orientation[1]
pose_target.orientation.z = self.subsc_orientation[2]
pose_target.orientation.w = self.subsc_orientation[
3] #トピックに 四元数を代入
#pose_target.position.x = pose.position.x
#pose_target.position.y = pose.position.y
#pose_target.position.z = pose.position.z
#pose_target.position.x = self.model_pose[0] #
#pose_target.position.y = self.model_pose[1] #
#pose_target.position.z = self.model_pose[2]+0.05 #トピックに 座標を代入
pose_target.position.x = self.subsc_pose[0] #
pose_target.position.y = self.subsc_pose[1] #
pose_target.position.z = self.subsc_pose[2] #トピックに 座標を代入
group.set_pose_target(pose_target) #ターゲットをセット
group.go() #移動
rospy.sleep(1) #1秒スリープ
# of are itself & the floor.
if __name__ == '__main__':
rospy.init_node("simple_disco")
# Create move group interface for a fetch robot
move_group = MoveGroupInterface("arm_with_torso", "base_link")
# Define ground plane
# This creates objects in the planning scene that mimic the ground
# If these were not in place gripper could hit the ground
planning_scene = PlanningSceneInterface("base_link")
planning_scene.removeCollisionObject("my_front_ground")
planning_scene.removeCollisionObject("my_back_ground")
planning_scene.removeCollisionObject("my_right_ground")
planning_scene.removeCollisionObject("my_left_ground")
planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
# TF joint names
joint_names = [
"torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint",
"upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint",
"wrist_flex_joint", "wrist_roll_joint"
]
# Lists of joint angles in the same order as in joint_names
disco_poses = [[0.0, 1.5, -0.6, 3.0, 1.0, 3.0, 1.0, 3.0]]
disco_poses1 = [[0.0, 1.5, -0.6, 3.0, 1.0, 3.0, 1.0, 3.0],
[0.133, 0.8, 0.75, 0.0, -2.0, 0.0, 2.0, 0.0],
[0.266, -0.8, 0.0, 0.0, 2.0, 0.0, -2.0, 0.0],
class Reaching:
def __init__(self):
rospy.init_node('reaching', anonymous=True)
self.model_pose = [0.0, 0.0, 0.0]
self.set_init_pose()
#self.set_forbidden()
def set_forbidden(self):
#set forbidden erea
self.planning_scene = PlanningSceneInterface("base_link")
self.planning_scene.removeCollisionObject("my_front_ground")
self.planning_scene.removeCollisionObject("my_back_ground")
self.planning_scene.removeCollisionObject("my_right_ground")
self.planning_scene.removeCollisionObject("my_left_ground")
self.planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
self.planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
self.planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
self.planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
self.planning_scene.removeCollisionObject("demo_cube")
def set_init_pose(self):
#set init pose
move_group = MoveGroupInterface("arm_with_torso", "base_link")
#set arm initial position
joints = [
"torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint",
"upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint",
"wrist_flex_joint", "wrist_roll_joint"
]
#pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
pose = [0.385, -1.0, -1.0, 0.0, 1.0, 0.0, 1.5, 0.0]
#pose = [0.385, 1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0]
result = move_group.moveToJointPosition(joints, pose, 0.02)
def target(self):
rate = rospy.Rate(1)
rospy.sleep(1)
ls = tf.TransformListener()
while not rospy.is_shutdown():
print self.model_pose
try:
(trans, rot) = ls.lookupTransform('/base_link', '/object',
rospy.Time(0))
print "tf base_link to test", trans
group = moveit_commander.MoveGroupCommander("arm_with_torso")
#print group.get_current_pose().pose
pose_target = geometry_msgs.msg.Pose()
pose = group.get_current_pose().pose
pose_target = geometry_msgs.msg.Pose()
pose_target.orientation.x = pose.orientation.x
pose_target.orientation.y = pose.orientation.y
pose_target.orientation.z = pose.orientation.z
pose_target.orientation.w = pose.orientation.w
pose_target.position.x = trans[0]
pose_target.position.y = trans[1]
pose_target.position.z = trans[2] + 0.3
group.set_pose_target(pose_target)
group.go()
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
print "e"
rospy.sleep(1)
class Reaching:
def __init__(self):
rospy.init_node('reaching', anonymous=True)
self.robot = moveit_commander.RobotCommander()
self.model_pose = [0.0,0.0,0.0]
self.set_forbidden()
self.set_init_pose()
def set_forbidden(self):
#set forbidden erea
self.planning_scene = PlanningSceneInterface("base_link")
self.planning_scene.removeCollisionObject("my_front_ground")
self.planning_scene.removeCollisionObject("my_back_ground")
self.planning_scene.removeCollisionObject("my_right_ground")
self.planning_scene.removeCollisionObject("my_left_ground")
self.planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
self.planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
self.planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
self.planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
def set_init_pose(sefl):
#set init pose
move_group = MoveGroupInterface("manipulator","base_link")
planning_scene = PlanningSceneInterface("base_link")
joint_names = ["shoulder_pan_joint", "shoulder_lift_joint",
"elbow_joint", "wrist_1_joint",
"wrist_2_joint", "wrist_3_joint"]
pose = [0.0, -1.98, 1.0, -0.64, -1.57, 0.0]
move_group.moveToJointPosition(joint_names, pose, wait=False)
move_group.get_move_action().wait_for_result()
result = move_group.get_move_action().get_result()
move_group.get_move_action().cancel_all_goals()
def callback(self,data):
#print data.markers[0].pose.position
x = data.markers[0].pose.position.x
y = data.markers[0].pose.position.y
z = data.markers[0].pose.position.z
self.model_pose = [x,y,z]
def start_subscriber(self):
rospy.Subscriber("/visualization_marker_array", MarkerArray, self.callback)
def publish_tf(self):
br = tf.TransformBroadcaster()
br.sendTransform((self.model_pose[0], self.model_pose[1], self.model_pose[2]), (0.0, 0.0, 0.0, 1.0), rospy.Time.now(), "object", "depth_camera_frame")
def target(self):
rate = rospy.Rate(1)
rospy.sleep(1)
ls = tf.TransformListener()
while not rospy.is_shutdown():
self.publish_tf()
try:
(trans,rot) = ls.lookupTransform('/base_link', '/object', rospy.Time(0))
print "tf base_link to test",trans
group = moveit_commander.MoveGroupCommander("manipulator")
#print group.get_current_pose().pose
pose_target = geometry_msgs.msg.Pose()
pose = group.get_current_pose().pose
pose_target = geometry_msgs.msg.Pose()
pose_target.orientation.x = pose.orientation.x
pose_target.orientation.y = pose.orientation.y
pose_target.orientation.z = pose.orientation.z
pose_target.orientation.w = pose.orientation.w
pose_target.position.x = trans[0]
pose_target.position.y = trans[1]
pose_target.position.z = trans[2]+0.2
group.set_pose_target(pose_target)
group.go()
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
print "e"
rospy.sleep(1)
self.set_init_pose()
def main():
rospy.init_node('move_group_python_path_follow', anonymous=True)
planning_scene = PlanningSceneInterface("base_link")
planning_scene.removeCollisionObject("my_front_ground")
planning_scene.removeCollisionObject("my_back_ground")
planning_scene.removeCollisionObject("my_right_ground")
planning_scene.removeCollisionObject("my_left_ground")
planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
moveit_commander.roscpp_initialize(sys.argv)
move_group = MoveGroupInterface("arm", "base_link")
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
group = moveit_commander.MoveGroupCommander("arm")
print "============ Robot Groups:"
print robot.get_group_names()
print "============ Printing robot state"
print robot.get_current_state()
print "============"
gripper_frame = 'wrist_roll_link'
gripper_poses = list()
radius = 0.2
for theta in np.arange(-pi, pi, 0.5 * pi):
gripper_poses.append(
Pose(
Point(0.75, radius * np.sin(theta),
0.6 + radius * np.cos(theta)),
Quaternion(*quaternion_from_euler(0.0, 0.0, 0.0))))
gripper_pose_stamped = PoseStamped()
gripper_pose_stamped.header.frame_id = 'base_link'
gripper_pose_stamped.header.stamp = rospy.Time.now()
gripper_pose_stamped.pose = gripper_poses[0]
move_group.moveToPose(gripper_pose_stamped, gripper_frame)
result = move_group.get_move_action().get_result()
if result:
if result.error_code.val == MoveItErrorCodes.SUCCESS:
rospy.loginfo("Success!")
else:
# If you get to this point please search for:
# moveit_msgs/MoveItErrorCodes.msg
rospy.logerr("Arm goal in state: %s",
move_group.get_move_action().get_state())
trajectory = RobotTrajectory()
(plan, fraction) = group.compute_cartesian_path(gripper_poses, 0.01, 0.0)
group.execute(plan)
move_group.get_move_action().cancel_all_goals()
def move_corner(self, x, y):
position = self.cal_position.get_base_position_from_pix(x, y)
position[0] = position[0] - 0.20
move_group = MoveGroupInterface("arm_with_torso", "base_link")
planning_scene = PlanningSceneInterface("base_link")
planning_scene.removeCollisionObject("my_front_ground")
planning_scene.removeCollisionObject("my_back_ground")
planning_scene.removeCollisionObject("my_right_ground")
planning_scene.removeCollisionObject("my_left_ground")
planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
# This is the wrist link not the gripper itself
gripper_frame = 'wrist_roll_link'
pose = Pose(Point(position[0], position[1], position[2]),
Quaternion(0, 0, 0, 1))
# Construct a "pose_stamped" message as required by moveToPose
gripper_pose_stamped = PoseStamped()
gripper_pose_stamped.header.frame_id = 'base_link'
# Finish building the Pose_stamped message
# If the message stamp is not current it could be ignored
gripper_pose_stamped.header.stamp = rospy.Time.now()
# Set the message pose
gripper_pose_stamped.pose = pose
# Move gripper frame to the pose specified
move_group.moveToPose(gripper_pose_stamped, gripper_frame)
result = move_group.get_move_action().get_result()
if result:
# Checking the MoveItErrorCode
if result.error_code.val == MoveItErrorCodes.SUCCESS:
rospy.loginfo("Hello there!")
else:
# If you get to this point please search for:
# moveit_msgs/MoveItErrorCodes.msg
rospy.logerr("Arm goal in state: %s",
move_group.get_move_action().get_state())
else:
rospy.logerr("MoveIt! failure no result returned.")
time.sleep(1)
joint_names = [
"torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint",
"upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint",
"wrist_flex_joint", "wrist_roll_joint"
]
joints_value = [0.3, 1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0]
move_group.moveToJointPosition(joint_names, joints_value, wait=False)
# Since we passed in wait=False above we need to wait here
move_group.get_move_action().wait_for_result()
result = move_group.get_move_action().get_result()
if result:
# Checking the MoveItErrorCode
if result.error_code.val == MoveItErrorCodes.SUCCESS:
rospy.loginfo("pose Success!")
else:
# If you get to this point please search for:
# moveit_msgs/MoveItErrorCodes.msg
rospy.logerr("Arm goal in state: %s",
self.move_group.get_move_action().get_state())
else:
rospy.logerr("MoveIt! failure no result returned.")
if (rospy.Time.now() - t).to_sec() > 0.2:
rospy.sleep(0.1)
continue
item_translation, item_orientation = tf_listener.lookupTransform(
'/base_link', item_frame, t)
except (tf.Exception, tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
gripper_goal.command.position = 0.15
gripper.send_goal(gripper_goal)
gripper.wait_for_result(rospy.Duration(1.0))
print('Item: ' + str(item_translation))
scene.addCube('item', 0.05, item_translation[0], item_translation[1],
item_translation[2])
p.position.x = item_translation[0] - 0.01 - 0.06
p.position.y = item_translation[1]
p.position.z = item_translation[2] + 0.04 + 0.14
p.orientation = Quaternion(*quaternion_from_euler(0, 1.2, 0))
arm.set_pose_target(p)
arm.go(True)
gripper_goal.command.position = 0
gripper.send_goal(gripper_goal)
gripper.wait_for_result(rospy.Duration(2.0))
scene.removeAttachedObject('item')
clear_octomap()
class GraspingClient(object):
def __init__(self):
self.scene = PlanningSceneInterface("base_link")
self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True)
self.move_group = MoveGroupInterface("arm", "base_link")
find_topic = "basic_grasping_perception/find_objects"
rospy.loginfo("Waiting for %s..." % find_topic)
self.find_client = actionlib.SimpleActionClient(
find_topic, FindGraspableObjectsAction)
self.find_client.wait_for_server()
def updateScene(self):
# find objects
rospy.loginfo("get new grasps")
goal = FindGraspableObjectsGoal()
goal.plan_grasps = True
self.find_client.send_goal(goal)
self.find_client.wait_for_result(rospy.Duration(5.0))
find_result = self.find_client.get_result()
# remove previous objects
for name in self.scene.getKnownCollisionObjects():
self.scene.removeCollisionObject(name, False)
for name in self.scene.getKnownAttachedObjects():
self.scene.removeAttachedObject(name, False)
self.scene.removeCollisionObject("my_front_ground")
self.scene.removeCollisionObject("my_back_ground")
self.scene.removeCollisionObject("my_right_ground")
self.scene.removeCollisionObject("my_left_ground")
self.scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
self.scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
self.scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
self.scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
# TODO: ADD BOX
self.scene.waitForSync()
# insert objects to scene
idx = -1
for obj in find_result.objects:
idx += 1
obj.object.name = "object%d" % idx
self.scene.addSolidPrimitive(obj.object.name,
obj.object.primitives[0],
obj.object.primitive_poses[0],
wait=False)
for obj in find_result.support_surfaces:
# extend surface to floor, and make wider since we have narrow field of view
height = obj.primitive_poses[0].position.z
# added + .1
obj.primitives[0].dimensions = [
obj.primitives[0].dimensions[0],
1.5, # wider
obj.primitives[0].dimensions[2] + height
]
obj.primitive_poses[0].position.z += -height / 2.0
# add to scene
self.scene.addSolidPrimitive(obj.name,
obj.primitives[0],
obj.primitive_poses[0],
wait=False)
self.scene.waitForSync()
# store for grasping
self.objects = find_result.objects
self.surfaces = find_result.support_surfaces
def getSupportSurface(self, name):
for surface in self.support_surfaces:
if surface.name == name:
return surface
return None
def tuck(self):
joints = [
"shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint",
"wrist_roll_joint"
]
pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def zero(self):
joints = [
"shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint",
"wrist_roll_joint"
]
pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
X, Q = get_pose_from_msg(object_pose)
print(X)
print(Q)
# Define ground plane
# This creates objects in the planning scene that mimic the ground
# If these were not in place gripper could hit the ground
planning_scene = PlanningSceneInterface("base_link")
planning_scene.removeCollisionObject("my_front_ground")
planning_scene.removeCollisionObject("my_back_ground")
planning_scene.removeCollisionObject("my_right_ground")
planning_scene.removeCollisionObject("my_left_ground")
planning_scene.removeCollisionObject("table")
planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
table_height = 0.7
planning_scene.addCube("table", table_height, X[0] + 0.3, X[1],
0.5 * table_height)
print('created secene')
# Create move group interface for a fetch robot
move_group = MoveGroupInterface("arm_with_torso", "base_link")
move_group_gripper = MoveGroupInterface("gripper_custom", "gripper_link")
gripper_frame = 'wrist_roll_link'
class FetchTrajectoryRecorderNode:
def __init__(self, node_name='fetch_trajectory_recorder_node'):
rospy.init_node(node_name)
# Create move group interface for a fetch robot
self.move_group = MoveGroupInterface("arm_with_torso", "base_link")
self.gripper_names = [
'l_gripper_finger_joint', 'r_gripper_finger_joint'
]
self.ee_name = 'wrist_roll_link'
self.joint_names = [
'shoulder_pan_joint', 'shoulder_lift_joint', 'upperarm_roll_joint',
'elbow_flex_joint', 'forearm_roll_joint', 'wrist_flex_joint',
'wrist_roll_joint'
]
self.tf_listener = tf.TransformListener()
#set home as the initial state
self.home_gripper_values = [0.08, 0.08]
self.home_joint_values = [
-1.54, -0.036, -1.055, -1.53, -0.535, -1.42, 2.398
]
self.gripper_values = []
self.joint_values = []
self.received_joint_poses = []
self.temp_joint_values = home_joint_values
self.temp_gripper_values = home_gripper_values
self.gripper_values.append(home_gripper_values)
self.joint_values.append(home_joint_values)
rospy.Subscriber('joint_states', JointState,
self.arm_trajectory_record_cb)
#TODO: change this topic name as gripper related topics
# rospy.Subscriber('joint_states', JointState, self.gripper_record_cb)
# Define ground plane
# This creates objects in the planning scene that mimic the ground
# If these were not in place gripper could hit the ground
self.planning_scene = PlanningSceneInterface("base_link")
self.planning_scene.removeCollisionObject("my_front_ground")
self.planning_scene.removeCollisionObject("my_back_ground")
self.planning_scene.removeCollisionObject("my_right_ground")
self.planning_scene.removeCollisionObject("my_left_ground")
self.planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
self.planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
self.planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
self.planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
#record the arm joint values
def arm_trajectory_record_cb(self, msg):
import IPython
IPython.embed()
for name, position in zip(msg.name, msg.position):
if name in self.joint_names:
#if the new position is different from the previous one
if position != temp_joint_values:
self.joint_values.append(position)
self.temp_joint_values = position
#go back to initial position as the last state
self.joint_values.append(home_joint_values)
# for i, joint_name in enumerate(msg.name):
# if joint_name in self.joint_names:
# idx = self.joint_names.index(joint_name)
# self.received_joint_poses[idx] = msg.position[i]
# else:
# self.joint_names.append(joint_name)
# self.received_joint_poses.append(msg.position[i])
#record the gripper values
def gripper_record_cb(self, msg):
for name, position in zip(msg.name, msg.position):
if name in self.gripper_names:
if position != temp_gripper_values:
self.gripper_values.append(position)
self.temp_gripper_values = position
#go back to initial position as the last state
self.gripper_values.append(home_gripper_values)
#play back the trajetory just recorded
#TODO: write a gui to trigger this function to play back
#Otherwise, we will keep recording the joint values and do not move
def play_back(self, joint_values, gripper_values):
for joint in joint_values:
if rospy.is_shutdown():
break
# Plans the joints in joint_names to angles in pose
self.move_group.moveToJointPosition(self.joint_names,
joint,
wait=False)
# Since we passed in wait=False above we need to wait here
move_group.get_move_action().wait_for_result()
result = move_group.get_move_action().get_result()
if result:
# Checking the MoveItErrorCode
if result.error_code.val == MoveItErrorCodes.SUCCESS:
rospy.loginfo("Play Back ...")
else:
# If you get to this point please search for:
# moveit_msgs/MoveItErrorCodes.msg
rospy.logerr("Arm goal in state: %s",
move_group.get_move_action().get_state())
else:
rospy.logerr("MoveIt! failure no result returned.")
class PoseController(object):
def __init__(self):
# Create move group interface for a fetch robot
self.move_group_ = MoveGroupInterface("arm_with_torso", "base_link")
# Define ground plane
# This creates objects in the planning scene that mimic the ground
# If these were not in place gripper could hit the ground
self.planning_scene_ = PlanningSceneInterface("base_link")
self.planning_scene_.removeCollisionObject("my_front_ground")
self.planning_scene_.removeCollisionObject("my_back_ground")
self.planning_scene_.removeCollisionObject("my_right_ground")
self.planning_scene_.removeCollisionObject("my_left_ground")
self.planning_scene_.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
self.planning_scene_.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
self.planning_scene_.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
self.planning_scene_.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
self.angle = np.zeros(FILTER_LENGTH)
self.last_trigger_time = rospy.Time.now()
rospy.Subscriber(AUDIO_TOPIC, SoundSourceAngle, self.SetPoseWithRetry)
rospy.spin()
def SetPoseWithRetry(self, msg, max_time=3):
if msg.is_valid:
direction = 0
(direction, self.angle) = TemporalMedian(msg.angle, self.angle)
soulder_pan = round((direction / 180.0) * np.pi, 2)
target_pose = {
'torso_lift_joint': 0.05287206172943115,
'shoulder_pan_joint': soulder_pan,
'shoulder_lift_joint': 0.0,
'upperarm_roll_joint': -np.pi,
'elbow_flex_joint': 0.0,
'forearm_roll_joint': np.pi,
'wrist_flex_joint': -1.5,
'wrist_roll_joint': 0.0,
}
if rospy.Time.now() - self.last_trigger_time > rospy.Duration(2):
rospy.loginfo("Shoulder pan")
rospy.loginfo(soulder_pan)
for _ in range(max_time):
if self.SetPose(target_pose):
return True
return False
def SetPose(self, target_pose):
# Plans the joints in joint_names to angles in pose
self.move_group_.moveToJointPosition(target_pose.keys(),
target_pose.values(),
wait=False)
# Since we passed in wait=False above we need to wait here
self.move_group_.get_move_action().wait_for_result()
result = self.move_group_.get_move_action().get_result()
return_result = False
if result:
# Checking the MoveItErrorCode
if result.error_code.val == MoveItErrorCodes.SUCCESS:
rospy.loginfo("Done!")
return_result = True
else:
# If you get to this point please search for:
# moveit_msgs/MoveItErrorCodes.msg
rospy.logerr("Arm goal in state: %s",
self.move_group_.get_move_action().get_state())
else:
rospy.logerr("MoveIt! failure no result returned.")
# This stops all arm movement goals
# It should be called when a program is exiting so movement stops
self.move_group_.get_move_action().cancel_all_goals()
self.last_trigger_time = rospy.Time.now()
return return_result
class GraspingClient(object):
def __init__(self):
self.scene = PlanningSceneInterface("base_link")
self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True)
self.move_group = MoveGroupInterface("arm", "base_link")
find_topic = "basic_grasping_perception/find_objects"
rospy.loginfo("Waiting for %s..." % find_topic)
self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction)
self.find_client.wait_for_server()
def updateScene(self):
# find objects
rospy.loginfo("get new grasps")
goal = FindGraspableObjectsGoal()
goal.plan_grasps = True
self.find_client.send_goal(goal)
self.find_client.wait_for_result(rospy.Duration(5.0))
find_result = self.find_client.get_result()
# remove previous objects
for name in self.scene.getKnownCollisionObjects():
self.scene.removeCollisionObject(name, False)
for name in self.scene.getKnownAttachedObjects():
self.scene.removeAttachedObject(name, False)
self.scene.removeCollisionObject("my_front_ground")
self.scene.removeCollisionObject("my_back_ground")
self.scene.removeCollisionObject("my_right_ground")
self.scene.removeCollisionObject("my_left_ground")
self.scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
self.scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
self.scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
self.scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
# TODO: ADD BOX
self.scene.waitForSync()
# insert objects to scene
idx = -1
for obj in find_result.objects:
idx += 1
obj.object.name = "object%d"%idx
self.scene.addSolidPrimitive(obj.object.name,
obj.object.primitives[0],
obj.object.primitive_poses[0],
wait = False)
for obj in find_result.support_surfaces:
# extend surface to floor, and make wider since we have narrow field of view
height = obj.primitive_poses[0].position.z
# added + .1
obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0],
1.5, # wider
obj.primitives[0].dimensions[2] + height]
obj.primitive_poses[0].position.z += -height/2.0
# add to scene
self.scene.addSolidPrimitive(obj.name,
obj.primitives[0],
obj.primitive_poses[0],
wait = False)
self.scene.waitForSync()
# store for grasping
self.objects = find_result.objects
self.surfaces = find_result.support_surfaces
def getSupportSurface(self, name):
for surface in self.support_surfaces:
if surface.name == name:
return surface
return None
def tuck(self):
joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0]
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def zero(self):
joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint",
"elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"]
pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
while not rospy.is_shutdown():
result = self.move_group.moveToJointPosition(joints, pose, 0.02)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
return
def main():
rospy.init_node("robo_highfive")
### Move it Init stuff ------------------------------------------------------------
# Create move group interface for a fetch robot
move_group = MoveGroupInterface("arm_with_torso", "base_link")
# Define ground plane
# This creates objects in the planning scene that mimic the ground
# If these were not in place gripper could hit the ground
planning_scene = PlanningSceneInterface("base_link")
planning_scene.removeCollisionObject("my_front_ground")
planning_scene.removeCollisionObject("my_back_ground")
planning_scene.removeCollisionObject("my_right_ground")
planning_scene.removeCollisionObject("my_left_ground")
planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0)
planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0)
planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0)
planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
# TF joint names
joint_names = [
"torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint",
"upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint",
"wrist_flex_joint", "wrist_roll_joint"
]
protoFile = "hand/pose_deploy.prototxt"
weightsFile = "hand/pose_iter_102000.caffemodel"
nPoints = 22
POSE_PAIRS = [[0, 1], [1, 2], [2, 3], [3, 4], [0, 5], [5, 6], [6,
7], [7, 8],
[0, 9], [9, 10], [10, 11], [11, 12], [0, 13], [13, 14],
[14, 15], [15, 16], [0, 17], [17, 18], [18, 19], [19, 20]]
threshold = 0.2
### Hand Detection init stuff ---------------------------------------------------------------
input_source = "asl.mp4"
cap = cv2.VideoCapture(input_source)
hasFrame, frame = cap.read()
frameWidth = frame.shape[1]
frameHeight = frame.shape[0]
aspect_ratio = frameWidth / frameHeight
inHeight = 368
inWidth = int(((aspect_ratio * inHeight) * 8) // 8)
vid_writer = cv2.VideoWriter('output.avi',
cv2.VideoWriter_fourcc('M', 'J', 'P', 'G'),
15, (frame.shape[1], frame.shape[0]))
net = cv2.dnn.readNetFromCaffe(protoFile, weightsFile)
k = 0
highFive_Threshhold = [30, 30, 30, 30, 30]
### High Five Detection ------------------------------------------------------------------------
while 1:
k += 1
t = time.time()
hasFrame, frame = cap.read()
frameCopy = np.copy(frame)
if not hasFrame:
cv2.waitKey()
break
inpBlob = cv2.dnn.blobFromImage(frame,
1.0 / 255, (inWidth, inHeight),
(0, 0, 0),
swapRB=False,
crop=False)
net.setInput(inpBlob)
output = net.forward()
print("forward = {}".format(time.time() - t))
# Empty list to store the detected keypoints
points = []
finger1 = None
finger2 = None
highFive = True
for i in range(nPoints):
# confidence map of corresponding body's part.
probMap = output[0, i, :, :]
probMap = cv2.resize(probMap, (frameWidth, frameHeight))
# Find global maxima of the probMap.
minVal, prob, minLoc, point = cv2.minMaxLoc(probMap)
if prob > threshold:
cv2.circle(frameCopy, (int(point[0]), int(point[1])),
6, (0, 255, 255),
thickness=-1,
lineType=cv2.FILLED)
cv2.putText(frameCopy,
"{}".format(i), (int(point[0]), int(point[1])),
cv2.FONT_HERSHEY_SIMPLEX,
.8, (0, 0, 255),
2,
lineType=cv2.LINE_AA)
# if i == 8:
# finger1 = [int(point[0]), int(point[1])]
# if i == 12:
# finger2 = [int(point[0]), int(point[1])]
# #distance = np.sqrt((finger1[0] - finger2[0]) ** 2 + (finger1[1] - finger2[1]) ** 2)
#print(distance)
# Add the point to the list if the probability is greater than the threshold
points.append((int(point[0]), int(point[1])))
else:
points.append(None)
if (i != 21):
highFive = False
print("highFive??", highFive)
if highFive:
pinky = np.sqrt((points[20][0] - points[18][0])**2 +
(points[20][1] - points[18][1])**2)
ring = np.sqrt((points[16][0] - points[14][0])**2 +
(points[16][1] - points[14][1])**2)
bird = np.sqrt((points[12][0] - points[10][0])**2 +
(points[12][1] - points[10][1])**2)
index = np.sqrt((points[8][0] - points[6][0])**2 +
(points[8][1] - points[6][1])**2)
thumb = np.sqrt((points[4][0] - points[2][0])**2 +
(points[4][1] - points[2][1])**2)
# print("pinky: ", pinky)
# print("ring: ", ring)
# print("bird: ", bird)
# print("index: ", index)
# print("thumb: ", thumb)
finger_dists = [pinky, ring, bird, index, thumb]
print("Finger Distances: ", finger_dists)
for j in range(5):
if finger_dists[j] < highFive_Threshhold[j]:
print("False Positive!", j)
break
print()
# Draw Skeleton
for pair in POSE_PAIRS:
partA = pair[0]
partB = pair[1]
if points[partA] and points[partB]:
cv2.line(frame,
points[partA],
points[partB], (0, 255, 255),
2,
lineType=cv2.LINE_AA)
cv2.circle(frame,
points[partA],
5, (0, 0, 255),
thickness=-1,
lineType=cv2.FILLED)
cv2.circle(frame,
points[partB],
5, (0, 0, 255),
thickness=-1,
lineType=cv2.FILLED)
print("Time Taken for frame = {}".format(time.time() - t))
# cv2.putText(frame, "time taken = {:.2f} sec".format(time.time() - t), (50, 50), cv2.FONT_HERSHEY_COMPLEX, .8, (255, 50, 0), 2, lineType=cv2.LINE_AA)
# cv2.putText(frame, "Hand Pose using OpenCV", (50, 50), cv2.FONT_HERSHEY_COMPLEX, 1, (255, 50, 0), 2, lineType=cv2.LINE_AA)
cv2.imshow('Output-Skeleton', frame)
# cv2.imwrite("video_output/{:03d}.jpg".format(k), frame)
key = cv2.waitKey(1)
if key == 27:
break
print("total = {}".format(time.time() - t))
vid_writer.write(frame)
vid_writer.release()
class Reaching:
def __init__(self):
rospy.init_node('reaching', anonymous=True)
self.robot = moveit_commander.RobotCommander()
# self.model_pose = [0.0,0.2,0.2 , 0,0,0,1]
self.model_pose = [0.0, 0.3, 0.1, 0, 0, 0, 1]
self.obstacle_pose = [0.0, 0.0, 0.0]
self.set_forbidden()
self.set_init_pose()
def set_forbidden(self):
#set forbidden erea
self.planning_scene = PlanningSceneInterface("base_link")
self.planning_scene.removeCollisionObject("my_ground")
self.planning_scene.addCube("my_ground", 2, 0, 0, -1.04)
self.planning_scene.addBox("obstacle", 0.1, 0.5, 1, -0.3, 0, 0.5)
self.planning_scene.addBox("obstacle2", 0.5, 0.1, 1, 0, -0.3, 0.5)
#self.planning_scene.addCube("demo_cube", 0.06,0,0.3,0)
print dir(self.planning_scene)
import inspect
print "addBox's variable is ", inspect.getargspec(
self.planning_scene.addBox)
print "attachBox's variable is ", inspect.getargspec(
self.planning_scene.attachBox)
print "addCube's variable is ", inspect.getargspec(
self.planning_scene.addCube)
print "setColor's variable is ", inspect.getargspec(
self.planning_scene.setColor)
def set_init_pose(sefl):
#set init pose
move_group = MoveGroupInterface("manipulator", "base_link")
planning_scene = PlanningSceneInterface("base_link")
joint_names = [
"shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint",
"wrist_1_joint", "wrist_2_joint", "wrist_3_joint"
]
pose = [-1.26, -0.64, -2.44, -0.66, 1.56, 0.007]
move_group.moveToJointPosition(joint_names, pose, wait=False)
move_group.get_move_action().wait_for_result()
result = move_group.get_move_action().get_result()
move_group.get_move_action().cancel_all_goals()
def callback(self, data):
x = data.pose[1].position.x
y = data.pose[1].position.y
z = data.pose[1].position.z
def start_subscriber(self):
rospy.Subscriber("/gazebo/model_states", ModelStates, self.callback)
def reaching_pose(self):
print self.model_pose
group = moveit_commander.MoveGroupCommander("manipulator")
#print group.get_current_pose().pose
pose_target = geometry_msgs.msg.Pose()
pose = group.get_current_pose().pose
pose_target = geometry_msgs.msg.Pose()
pose_target.orientation.x = self.model_pose[3]
pose_target.orientation.y = self.model_pose[4]
pose_target.orientation.z = self.model_pose[5]
pose_target.orientation.w = self.model_pose[6]
pose_target.position.x = self.model_pose[0]
pose_target.position.y = self.model_pose[1]
pose_target.position.z = self.model_pose[2]
group.set_pose_target(pose_target)
group.go()
#raw_input("Enter -->")
def gripper_close(self):
print("Gripper_Close")
gripper_pose = Char()
gripper_pose = 255
pub.publish(gripper_pose)
#raw_input("Enter -->")
def gripper_open(self):
print("Gripper_Open")
gripper_pose = Char()
gripper_pose = 0
pub.publish(gripper_pose)
#raw_input("Enter -->")
def move_45(self):
rate = rospy.Rate(1)
rospy.sleep(1)
a = 0.065
b = 0.040
c = 0.012
pattern_45 = [
[-0.10, 0.31, 0.015, 0, 0, 0, 1], #B
'c',
[-0.10, 0.31, 0.1, 0, 0, 0, 1], #B
#[0.0 ,0.31 ,0.15 , 0,0,0,1], #A90
[0, 0.5, 0.081, 0.3826834, 0, 0, 0.9238795],
[0, 0.5, 0.0365, 0.3826834, 0, 0, 0.9238795],
'o',
[0, 0.5, 0.081, 0.3826834, 0, 0, 0.9238795],
#[0.0 ,0.31 ,0.1 , 0,0,0,1], #A90
[-0.10, 0.401, 0.041, 0, 0, 0, 1],
'c',
[-0.10, 0.401, 0.100, 0, 0, 0, 1],
[0, 0.5 - a, 0.037 + a, 0.3826834, 0, 0, 0.9238795],
[0, 0.5 - b, 0.037 + b, 0.3826834, 0, 0, 0.9238795],
[0, 0.5 - c, 0.037 + c, 0.3826834, 0, 0, 0.9238795],
'o',
'c',
[0, 0.5 - a, 0.037 + a, 0.3826834, 0, 0, 0.9238795],
[-0.10, 0.401, 0.100, 0, 0, 0, 1],
[-0.10, 0.401, 0.050, 0, 0, 0, 1],
'o',
[-0.10, 0.401, 0.100, 0, 0, 0, 1],
[0, 0.5, 0.081, 0.3826834, 0, 0, 0.9238795],
[0, 0.5, 0.0365, 0.3826834, 0, 0, 0.9238795],
'c',
[0, 0.5, 0.081, 0.3826834, 0, 0, 0.9238795],
[-0.10, 0.31, 0.1, 0, 0, 0, 1], #B
[-0.10, 0.31, 0.015, 0, 0, 0, 1], #B
'o',
[-0.10, 0.31, 0.1, 0, 0, 0, 1], #B
]
while not rospy.is_shutdown():
for pose in pattern_45:
if pose == 'c':
self.gripper_close()
rospy.sleep(1)
elif pose == 'o':
self.gripper_open()
rospy.sleep(1)
else:
self.model_pose = pose
self.reaching_pose()
if rospy.is_shutdown():
return 0
def move_90(self):
rate = rospy.Rate(1)
rospy.sleep(1)
a = 0.065
b = 0.040
c = 0.012
pattern_45 = [
[-0.10, 0.31, 0.015, 0, 0, 0, 1], #B
'c',
[-0.10, 0.31, 0.1, 0, 0, 0, 1], #B
[0.0, 0.300, 0.1, 0, 0, 0, 1], #A90
[0.0, 0.315, 0.0358, 0, 0, 0, 1], #A90
'o',
[0.0, 0.315, 0.1, 0, 0, 0, 1], #A90
[-0.10, 0.40, 0.041, 0, 0, 0, 1], #negi
'c',
[-0.10, 0.40, 0.1, 0, 0, 0, 1],
[-0.001, 0.316, 0.1, 0, 0, 0, 1], #A90
[-0.001, 0.316, 0.053, 0, 0, 0, 1], #A90
'o',
'c',
[0.0, 0.314, 0.1, 0, 0, 0, 1], #A90
[-0.10, 0.40, 0.1, 0, 0, 0, 1],
[-0.10, 0.40, 0.05, 0, 0, 0, 1], #negi
'o',
#[-0.10,0.40 ,0.1 , 0,0,0,1],
[0.0, 0.300, 0.1, 0, 0, 0, 1], #A90
[0.0, 0.315, 0.036, 0, 0, 0, 1], #A90
'c',
[0.0, 0.300, 0.1, 0, 0, 0, 1], #A90
[-0.10, 0.320, 0.1, 0, 0, 0, 1], #B
[-0.10, 0.31, 0.0153, 0, 0, 0, 1], #B
'o',
[-0.10, 0.31, 0.1, 0, 0, 0, 1], #B
]
while not rospy.is_shutdown():
for pose in pattern_45:
if pose == 'c':
self.gripper_close()
rospy.sleep(1)
elif pose == 'o':
self.gripper_open()
rospy.sleep(1)
else:
self.model_pose = pose
self.reaching_pose()
if rospy.is_shutdown():
return 0
class Reaching:
def __init__(self):
rospy.init_node('reaching', anonymous=True) #ノードの初期化 reaching ていう名前
self.robot = moveit_commander.RobotCommander() # インスタンスの作成
self.model_pose = [0.0, 0.0, 0.0] #モデルの姿勢
self.obstacle_pose = [0.0, 0.0, 0.0] #障害物の位置
self.subsc_pose = [0.0, 0.0, 0.0] #購読した座標
self.subsc_orientation = [0.0, 0.0, 0.0, 1.0] #購読した四元数
self.set_forbidden() #禁止エリアの初期化
self.set_init_pose() #位置姿勢の初期化
self.before_pose = [0, 0, 0] #前回の場所
self.pcarray = 0 #pointcloud を np array にしたものを入れる
self.executeFlag = False #unity コントローラ B ボタンと対応
self.before_executeFlag = False #前フレームの executeFlag
self.waypoints = []
self.way_flag = False #unity コントローラの 中指のボタンに対応
self.before_wayflag = False #前フレームのway_flag
self.state = {
'default': 0,
'hold': 1,
'way_standby': 2,
'plan_standby': 3
}
# default : 何もしていない target のセットか hold button を待つ
# hold : waypoint のセットを受け付ける hold button が離れたら終了
#way_standby : execute button が押されるのを待つ 押されたら default へ waypoint の設置をした場合
#plan_standby : execute button が押されるのを待つ 押されたら default へ waypoint を使わなかった場合
self.now_state = self.state['default'] #現在フレームの状態
def set_forbidden(self):
#set forbidden erea
self.planning_scene = PlanningSceneInterface(
"base_link") #PlanningSceneInterface のインスタンスの作成
self.planning_scene.removeCollisionObject("my_front_ground")
self.planning_scene.removeCollisionObject("my_back_ground")
self.planning_scene.removeCollisionObject("my_right_ground")
self.planning_scene.removeCollisionObject("my_left_ground") #オブジェクトの削除
self.planning_scene.addCube("my_front_ground", 2, 1.1, 0, -1.0)
self.planning_scene.addCube("my_back_ground", 2, -1.2, 0, -1.0)
self.planning_scene.addCube("my_left_ground", 2, 0.0, 1.0, -1.0)
self.planning_scene.addCube("my_right_ground", 2, 0.0, -1.0,
-1.0) #座標に立方体を挿入
self.planning_scene.addCube("targetOBJ", 0.5, 0.0, 1.0, 2.0)
self.planning_scene.removeCollisionObject("demo_cube")
self.planning_scene.removeCollisionObject("obstacle") #オブジェクトの削除
print dir(self.planning_scene) #インスタンス内のあらゆるオブジェクトの属性とメソッドのリストを表示
import inspect
print "addBox's variable is ", inspect.getargspec(
self.planning_scene.addBox)
print "attachBox's variable is ", inspect.getargspec(
self.planning_scene.attachBox)
print "addCube's variable is ", inspect.getargspec(
self.planning_scene.addCube)
print "setColor's variable is ", inspect.getargspec(
self.planning_scene.setColor) #python関数のパラメータの名前とデフォルト値を取得
def set_init_pose(self):
#set init pose
move_group = MoveGroupInterface(
"manipulator", "base_link") #MoveGroupInterface のインスタンスの作成
planning_scene = PlanningSceneInterface(
"base_link") #PlanningSceneInterface のインスタンスの作成
joint_names = [
"shoulder_pan_joint",
"shoulder_lift_joint", #ジョイントの名前を定義
"elbow_joint",
"wrist_1_joint",
"wrist_2_joint",
"wrist_3_joint"
]
pose = [0.0, -1.98, 1.0, -0.64, -1.57, 0.0]
move_group.moveToJointPosition(joint_names, pose,
wait=False) #joint_names を pose に動かす
move_group.get_move_action().wait_for_result() #wait result
result = move_group.get_move_action().get_result() #result を代入
move_group.get_move_action().cancel_all_goals() #すべてのゴールをキャンセル
def callback(self, data): #トピックにデータが追加されるたびに呼ばれる
x = data.pose[1].position.x
y = data.pose[1].position.y
z = data.pose[1].position.z
self.model_pose = [x, y, z] #modelの姿勢を代入
x = data.pose[2].position.x
y = data.pose[2].position.y
z = data.pose[2].position.z
self.obstacle_pose = [x, y, z] #障害物の姿勢を代入
def callbacksub(self, data):
x = data.x
y = data.y
z = data.z
self.subsc_pose = [x, y, z] #購読した座標を代入
def callbackunity(self, data):
x = data.pose.position.x
y = data.pose.position.y
z = data.pose.position.z
self.subsc_pose = [y, -x, z] #unity からの座標 座標変換も行っている
x = data.pose.orientation.x
y = data.pose.orientation.y
z = data.pose.orientation.z
w = data.pose.orientation.w
self.subsc_orientation = [y, -x, z, w] #unity からの四元数
def rtabcallback(self, data):
dtype_list = [(f.name, np.float32) for f in data.fields]
cloud_arr = np.fromstring(data.data, dtype_list)
self.pcarray = np.reshape(cloud_arr, (data.height, data.width))
def excallback(self, data):
self.executeFlag = data.data
def waycallback(self, data):
self.way_flag = data.data
def start_subscriber(self): #購読をスタート
rospy.Subscriber("/gazebo/model_states", ModelStates, self.callback)
# /gazebo/model_states から gazebo_msgs/ModelStates 型のトピックを購読 トピックが更新されるたびに self.callback を呼ぶ
#rospy.Subscriber("/UR/reaching/pose",Vector3,self.callbacksub)
rospy.Subscriber("/unity/target", PoseStamped,
self.callbackunity) #unityから配信しているトピックを購読
rospy.Subscriber('/rtabmap/cloud_map', PointCloud2,
self.rtabcallback) #zed で slam したpointcloud を購読
rospy.Subscriber('/unity/execute', Bool, self.excallback)
rospy.Subscriber('/unity/wayflag', Bool, self.waycallback)
def change_state(self, target):
#self.state={'default':0,'hold':1,'way_standby':2,'plan_standby':3}
# default : 何もしていない target のセットか hold button を待つ
# hold : waypoint のセットを受け付ける hold button が離れたら終了
#way_standby : execute button が押されるのを待つ 押されたら default へ waypoint の設置をした場合
#plan_standby : execute button が押されるのを待つ 押されたら default へ waypoint を使わなかった場合
set_flag = (self.before_pose != target.position
) # target の位置が変化している = トリガーが押された
hold_flag = self.way_flag # hold button が押されている
exe_flag = (self.before_executeFlag != self.executeFlag
) # executeFlag が前フレームと違う = execute button が押された
print('set ' + str(set_flag))
print('hold ' + str(hold_flag))
print('exe ' + str(exe_flag))
if self.now_state == self.state['default']:
if set_flag:
self.now_state = self.state['plan_standby']
elif hold_flag:
self.now_state = self.state['hold']
elif self.now_state == self.state['hold']:
if not hold_flag:
self.now_state = self.state['way_standby']
elif self.now_state == self.state['way_standby']:
if exe_flag:
self.now_state = self.state['default']
elif self.now_state == self.state['plan_standby']:
if exe_flag:
self.now_state = self.state['default']
def target(self):
rate = rospy.Rate(1) #Rateクラスのインスタンス rateを１で
rospy.sleep(1) #1秒スリープ
useway = False
while not rospy.is_shutdown(): #シャットダウンフラグが立っていなければ、
self.planning_scene.removeCollisionObject("demo_cube") #オブジェクトの削除
self.planning_scene.removeCollisionObject("obstacle") #オブジェクトの削除
self.planning_scene.addCube(
"demo_cube", 0.06, self.model_pose[0], self.model_pose[1],
self.model_pose[2]) #一辺が0.06の正方形をmodel_pose(座標)に追加
self.planning_scene.addBox(
"obstacle", 0.540614, 0.083603, 0.54373, self.obstacle_pose[0],
self.obstacle_pose[1],
self.obstacle_pose[2]) #---の大きさの箱をobstacle_pose(座標)に追加
print self.model_pose #model_poseを表示
group = moveit_commander.MoveGroupCommander(
"manipulator") #MoveGroupCommander クラスのインスタンス
#print group.get_current_pose().pose
pose_target = geometry_msgs.msg.Pose()
pose = group.get_current_pose().pose #エンドエフェクタの位置姿勢
pose_target = geometry_msgs.msg.Pose(
) #geometry_msgs.pose のインスタンス ここにターゲット設定
pose_target.orientation.x = self.subsc_orientation[0]
pose_target.orientation.y = self.subsc_orientation[1]
pose_target.orientation.z = self.subsc_orientation[2]
pose_target.orientation.w = self.subsc_orientation[
3] #トピックから 四元数を代入
pose_target.position.x = self.subsc_pose[0] #
pose_target.position.y = self.subsc_pose[1] #
pose_target.position.z = self.subsc_pose[2] #トピックから 座標を代入
#self.planning_scene.removeCollisionObject("targetOBJ")
#self.planning_scene.addCube("targetOBJ", 0.005,self.subsc_pose[0] ,self.subsc_pose[1] ,self.subsc_pose[2] )
self.way_flag = False
#self.state={'default':0,'hold':1,'way_standby':2,'plan_standby':3}
# default : 何もしていない target のセットか hold button を待つ
# hold : waypoint のセットを受け付ける hold button が離れたら終了
#way_standby : execute button が押されるのを待つ 押されたら default へ waypoint の設置をした場合
#plan_standby : execute button が押されるのを待つ 押されたら default へ waypoint を使わなかった場合
self.change_state(pose_target)
exe_flag = (self.before_executeFlag != self.executeFlag
) # executeFlag が前フレームと違う = execute button が押された
if self.now_state == self.state['default']:
if self.before_pose != pose_target.position:
group.set_pose_target(pose_target)
group.plan()
if self.now_state == self.state['hold']:
if self.before_pose != pose_target.position:
self.waypoints.append(copy.deepcopy(pose_target))
if self.now_state == self.state['way_standby']:
plan, fraction = group.compute_cartesian_path(
self.waypoints, 0.01, 0.0)
if exe_flag:
group.execute(plan)
self.waypoints = []
if self.now_state == self.state['plan_standby']:
if exe_flag:
group.go()
self.before_executeFlag = self.executeFlag
if self.before_pose != pose_target.position: #前フレームと座標が変化した
if self.way_flag: #waypoint を使用したティーチング(グリップボタン押してる)
self.waypoints.append(copy.deepcopy(pose_target))
useway = True
else:
group.set_pose_target(pose_target) #ターゲットをセット
useway = False
# group.set_pose_target(pose_target)#毎フレーム必要っぽい！！！！！！！！！！！！！
if not useway and self.before_pose != pose_target.position and not self.before_wayflag:
group.plan()
if self.before_wayflag != self.way_flag and not self.way_flag:
#前のフレームが true で false に変わったフレーム
print(self.waypoints)
plan, fraction = group.compute_cartesian_path(
self.waypoints, 0.01, 0.0)
self.before_pose = pose_target.position
self.before_wayflag = self.way_flag
if self.executeFlag != self.before_executeFlag:
if useway:
group.execute(plan)
self.before_executeFlag = self.executeFlag
self.waypoints = []
else:
group.go()
self.before_executeFlag = self.executeFlag
rospy.sleep(1) #1秒スリープ
class FetchRobotApi:
def __init__(self):
self.moving_mode = False
self.waldo_mode = False
self.prev_joy_buttons = 0
# See http://docs.fetchrobotics.com/robot_hardware.html#naming-conventions
self.joint_names = [
#'torso_lift_joint',
#'bellows_joint',
#'head_pan_joint',
#'head_tilt_joint',
'shoulder_pan_joint',
'shoulder_lift_joint',
'upperarm_roll_joint',
'elbow_flex_joint',
'forearm_roll_joint',
'wrist_flex_joint',
'wrist_roll_joint',
'l_gripper_finger_joint',
#'r_gripper_finger_joint',
]
self.joint_name_map = dict([
(name, index) for index, name in enumerate(self.joint_names)
])
self.cur_joint_pos = np.zeros([len(self.joint_names)])
self.cur_joint_vel = np.zeros([len(self.joint_names)])
self.cur_joint_effort = np.zeros([len(self.joint_names)])
self.cur_base_scan = np.zeros([662], dtype=np.float32)
self.cur_head_camera_depth_image = np.zeros([480, 640],
dtype=np.float32)
self.cur_head_camera_rgb_image = np.zeros([480, 640, 3],
dtype=np.uint8)
self.timeseq = None
self.timeseq_mutex = threading.Lock()
self.image_queue = Queue.Queue()
self.image_compress_thread = threading.Thread(
target=self.image_compress_main)
self.image_compress_thread.daemon = True
self.image_compress_thread.start()
logger.info('Subscribing...')
self.subs = []
if 1:
self.subs.append(
rospy.Subscriber('/joint_states', JointState,
self.joint_states_cb))
if 0:
self.subs.append(
rospy.Subscriber('/base_scan', LaserScan, self.base_scan_cb))
if 1:
self.subs.append(
rospy.Subscriber('/head_camera/depth/image', numpy_msg(Image),
self.head_camera_depth_image_cb))
if 1:
self.subs.append(
rospy.Subscriber('/head_camera/rgb/image_raw',
numpy_msg(Image),
self.head_camera_rgb_image_raw_cb))
if 1:
self.subs.append(
rospy.Subscriber('/spacenav/joy', Joy, self.spacenav_joy_cb))
logger.info('Subscribed')
self.arm_effort_pub = rospy.Publisher(
'/arm_controller/weightless_torque/command',
JointTrajectory,
queue_size=2)
#self.head_goal_pub = rospy.Publisher('/head_controller/point_head/goal', PointHeadActionGoal, queue_size=2)
self.gripper_client = actionlib.SimpleActionClient(
'gripper_controller/gripper_action', GripperCommandAction)
self.arm_cartesian_twist_pub = rospy.Publisher(
'/arm_controller/cartesian_twist/command', Twist, queue_size=2)
self.head_point_client = actionlib.SimpleActionClient(
'head_controller/point_head', PointHeadAction)
self.arm_move_group = MoveGroupInterface("arm",
"base_link",
plan_only=True)
self.arm_trajectory_client = actionlib.SimpleActionClient(
"arm_controller/follow_joint_trajectory",
FollowJointTrajectoryAction)
self.arm_trajectory_client.wait_for_server()
if 0:
logger.info('Creating MoveGroupInterface...')
self.move_group = MoveGroupInterface('arm_with_torso',
'base_link',
plan_only=True)
logger.info('Created MoveGroupInterface')
if 0:
logger.info('Creating PlanningSceneInterface...')
self.planning_scene = PlanningSceneInterface('base_link')
self.planning_scene.removeCollisionObject('my_front_ground')
self.planning_scene.removeCollisionObject('my_back_ground')
self.planning_scene.removeCollisionObject('my_right_ground')
self.planning_scene.removeCollisionObject('my_left_ground')
self.planning_scene.addCube('my_front_ground', 2, 1.1, 0.0,
-1.0)
self.planning_scene.addCube('my_back_ground', 2, -1.2, 0.0,
-1.0)
self.planning_scene.addCube('my_left_ground', 2, 0.0, 1.2,
-1.0)
self.planning_scene.addCube('my_right_ground', 2, 0.0, -1.2,
-1.0)
logger.warn('FetchRobotGymEnv ROS node running')
self.head_point_client.wait_for_server()
logger.warn('FetchRobotGymEnv ROS node connected')
def start_timeseq(self, script):
timeseq = TimeseqWriter.create(script)
timeseq.add_channel('robot_joints', 'FetchRobotJoints')
timeseq.add_channel('gripper_joints', 'FetchGripperJoints')
timeseq.add_schema(
'FetchRobotJoints',
('torso_lift_joint', 'JointState'),
('head_pan_joint', 'JointState'),
('head_tilt_joint', 'JointState'),
('shoulder_pan_joint', 'JointState'),
('shoulder_lift_joint', 'JointState'),
('upperarm_roll_joint', 'JointState'),
('elbow_flex_joint', 'JointState'),
('forearm_roll_joint', 'JointState'),
('wrist_flex_joint', 'JointState'),
('wrist_roll_joint', 'JointState'),
)
timeseq.add_schema(
'FetchGripperJoints',
('l_gripper_finger_joint', 'JointState'),
)
timeseq.add_schema(
'JointState',
('pos', 'double'),
('vel', 'double'),
('effort', 'double'),
)
timeseq.add_channel('arm_action', 'FetchArmWeightlessTorqueAction')
timeseq.add_schema(
'FetchArmWeightlessTorqueAction',
('shoulder_pan_joint', 'WeightlessTorqueAction'),
('shoulder_lift_joint', 'WeightlessTorqueAction'),
('upperarm_roll_joint', 'WeightlessTorqueAction'),
('elbow_flex_joint', 'WeightlessTorqueAction'),
('forearm_roll_joint', 'WeightlessTorqueAction'),
('wrist_flex_joint', 'WeightlessTorqueAction'),
('wrist_roll_joint', 'WeightlessTorqueAction'),
)
timeseq.add_schema(
'WeightlessTorqueAction',
('action', 'double'),
('effort', 'double'),
)
timeseq.add_channel('gripper_action', 'FetchGripperAction')
timeseq.add_schema(
'FetchGripperAction',
('action', 'double'),
('effort', 'double'),
('pos', 'double'),
)
timeseq.add_channel('head_camera_rgb', 'VideoFrame')
timeseq.add_schema(
'VideoFrame',
('url', 'string'),
)
with self.timeseq_mutex:
if self.timeseq:
self.timeseq.close()
self.timeseq = timeseq
def close_timeseq(self):
with self.timeseq_mutex:
if self.timeseq is not None:
self.timeseq.close()
threading.Thread(target=self.timeseq.upload_s3).start()
self.timeseq = None
def start_axis_video(self):
cameras = rospy.get_param('/axis_cameras')
if cameras and len(cameras):
with self.timeseq_mutex:
if self.timeseq:
for name, info in cameras.items():
logging.info('Camera %s: %s', name, repr(info))
self.timeseq.start_axis_video(
timeseq_name=name,
auth_header=info.get('auth_header'),
daemon_endpoint=info.get('daemon_endpoint'),
ipaddr=info.get('ipaddr'),
local_link_prefix=info.get('local_link_prefix'),
remote_traces_dir=info.get('remote_traces_dir'),
resolution=info.get('resolution'))
def stop_axis_video(self):
with self.timeseq_mutex:
if self.timeseq:
self.timeseq.stop_axis_video()
def base_scan_cb(self, data):
# fmin replaces nans with 15
self.cur_base_scan = np.fmin(np.array(data.ranges), 15.0)
def head_camera_depth_image_cb(self, data):
shape = [data.height, data.width]
dtype = np.dtype(np.float32)
npdata = np.fromstring(data.data, dtype=dtype).reshape(shape)
npdata.strides = (data.step, dtype.itemsize)
self.cur_head_camera_depth_image = np.fmin(npdata, 5.0)
def head_camera_rgb_image_raw_cb(self, data):
shape = [data.height, data.width, 3]
dtype = np.dtype(np.uint8)
npdata = np.fromstring(data.data, dtype=dtype).reshape(shape)
npdata.strides = (data.step, dtype.itemsize * 3, 1)
self.cur_head_camera_rgb_image = npdata
if self.timeseq:
self.image_queue.put(('head_camera_rgb', data))
def joint_states_cb(self, data):
"""
Handle a joint_states message. Messages can have subsets of the joints, hopefully non-overlapping.
"""
t0 = time.time()
for i in range(len(data.name)):
name = data.name[i]
jni = self.joint_name_map.get(name, -1)
if jni >= 0:
self.cur_joint_pos[jni] = data.position[i]
self.cur_joint_vel[jni] = data.velocity[i]
self.cur_joint_effort[jni] = data.effort[i]
with self.timeseq_mutex:
if self.timeseq:
channel, channel_type = (
(data.name[0] == 'l_wheel_joint' and
('robot_joints', 'FetchRobotJoints'))
or (data.name[0] == 'l_gripper_finger_joint' and
('gripper_joints', 'FetchGripperJoints'))
or (None, None))
if channel:
state = dict([(jn, {
'__type': 'JointState',
'pos': data.position[i],
'vel': data.velocity[i],
'effort': data.effort[i],
}) for i, jn in enumerate(data.name)])
state['__type'] = channel_type
state['rxtime'] = t0
self.timeseq.add(data.header.stamp.to_sec(), channel,
state)
if 0:
t1 = time.time()
print '%0.6f+%0.6f stamp=%0.6f (age %0.6f) from %s' % (
t0, t1 - t0, data.header.stamp.to_sec(),
data.header.stamp.to_sec() - t0, channel)
def image_compress_main(self):
while True:
channel, data = self.image_queue.get()
if 0:
print 'len(data.data)=%d data.width=%d data.height=%d' % (len(
data.data), data.width, data.height)
im = PIL.Image.frombytes('RGB', (data.width, data.height),
data.data, 'raw')
jpeg_writer = StringIO()
im.save(jpeg_writer, 'jpeg')
im_url = 'data:image/jpeg;base64,' + base64.b64encode(
jpeg_writer.getvalue())
with self.timeseq_mutex:
if self.timeseq:
self.timeseq.add(data.header.stamp.to_sec(), channel, {
'__type': 'VideoFrame',
'url': im_url,
})
def move_to_start(self):
self.moving_mode = True
# Look down
goal = PointHeadGoal()
goal.target.header.stamp = rospy.Time.now()
goal.target.header.frame_id = '/base_link'
goal.target.point.x = 1.5
goal.target.point.y = 0.0
goal.target.point.z = -0.2
goal.min_duration = rospy.Duration(0.5)
if 0: logger.info('Point head to %s...', goal)
self.head_point_client.send_goal(goal)
if 0: logger.info('Point head sent')
goal = GripperCommandGoal()
goal.command.max_effort = 60
goal.command.position = 0.1
self.gripper_client.send_goal(goal)
joints = [
'shoulder_pan_joint',
'shoulder_lift_joint',
'upperarm_roll_joint',
'elbow_flex_joint',
'forearm_roll_joint',
'wrist_flex_joint',
'wrist_roll_joint',
]
pose = [0.0, +0.8, 0.0, -0.8, 0.0, 0.0, 0.0]
result = self.arm_move_group.moveToJointPosition(joints,
pose,
plan_only=True)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
if 0: logger.info('Got trajectory %s', result.planned_trajectory)
follow_goal = FollowJointTrajectoryGoal()
follow_goal.trajectory = result.planned_trajectory.joint_trajectory
logger.info('sending trajectory to arm...')
self.arm_trajectory_client.send_goal(follow_goal)
result = self.arm_trajectory_client.wait_for_result()
logger.info('arm followed trajectory %s', result)
else:
logger.warn('moveToJointPosition returned %s', result)
return
result = self.head_point_client.wait_for_result()
logger.info('head followed trajectory %s', result)
logger.info('sending empty arm goal')
empty_goal = FollowJointTrajectoryGoal()
self.arm_trajectory_client.send_goal(empty_goal)
logger.info('Point head done')
self.moving_mode = False
def set_arm_action(self, action):
arm_joints = [
('shoulder_pan_joint', 1.57, 33.82),
('shoulder_lift_joint', 1.57, 131.76),
('upperarm_roll_joint', 1.57, 76.94),
('elbow_flex_joint', 1.57, 66.18),
('forearm_roll_joint', 1.57, 29.35),
('wrist_flex_joint', 2.26, 25.70),
('wrist_roll_joint', 2.26, 7.36),
]
arm_efforts = [
min(1.0, max(-1.0, action[self.joint_name_map.get(name)])) *
torque_scale * 0.75 for name, vel_scale, torque_scale in arm_joints
]
arm_joint_names = [
name for name, vel_scale, torque_scale in arm_joints
]
if 1:
arm_joint_names.append('gravity_compensation')
arm_efforts.append(1.0)
arm_msg = JointTrajectory(
joint_names=arm_joint_names,
points=[JointTrajectoryPoint(effort=arm_efforts)])
self.arm_effort_pub.publish(arm_msg)
with self.timeseq_mutex:
if self.timeseq:
state = dict([(jn, {
'__type': 'WeightlessTorqueAction',
'action': action[self.joint_name_map.get(jn)],
'effort': arm_efforts[i],
}) for i, jn in enumerate(arm_joint_names)])
state['__type'] = 'FetchArmWeightlessTorqueAction'
self.timeseq.add(time.time(), 'arm_action', state)
def set_gripper_action(self, action):
grip = action[self.joint_name_map.get('l_gripper_finger_joint')]
goal = GripperCommandGoal()
if grip > 0:
goal.command.max_effort = 60 * min(1.0, grip)
goal.command.position = 0.0
else:
goal.command.max_effort = 60
goal.command.position = 0.1
self.gripper_client.send_goal(goal)
with self.timeseq_mutex:
if self.timeseq:
state = {
'__type': 'FetchGripperAction',
'action': grip,
'effort': goal.command.max_effort,
'pos': goal.command.position,
}
self.timeseq.add(time.time(), 'gripper_action', state)
def set_waldo_action(self, joy):
twist = Twist()
twist.linear.x = joy.axes[0]
twist.linear.y = joy.axes[1]
twist.linear.z = joy.axes[2]
twist.angular.x = +3.0 * joy.axes[3]
twist.angular.y = +3.0 * joy.axes[4]
twist.angular.z = +2.0 * joy.axes[5]
self.arm_cartesian_twist_pub.publish(twist)
if joy.buttons[1] and not self.prev_joy_buttons[1]:
goal = GripperCommandGoal()
goal.command.max_effort = 60
goal.command.position = 0.0
self.gripper_client.send_goal(goal)
elif not joy.buttons[1] and self.prev_joy_buttons[1]:
goal = GripperCommandGoal()
goal.command.max_effort = 60
goal.command.position = 0.1
self.gripper_client.send_goal(goal)
def spacenav_joy_cb(self, joy):
if 0: logger.info('joy %s', str(joy))
if joy.buttons[0] and not self.prev_joy_buttons[0]:
if self.waldo_mode:
self.stop_waldo_mode()
elif not self.moving_mode:
self.start_waldo_mode()
if self.waldo_mode and not self.moving_mode:
self.set_waldo_action(joy)
self.prev_joy_buttons = joy.buttons
def start_waldo_mode(self):
logger.info('Start waldo mode')
self.waldo_mode = True
self.start_timeseq('fetchwaldo')
self.start_axis_video()
def stop_waldo_mode(self):
logger.info('Stop waldo mode')
self.waldo_mode = False
timeseq_url = None
if self.timeseq:
timeseq_url = self.timeseq.get_url()
logger.info('save_logs url=%s', timeseq_url)
self.stop_axis_video()
self.close_timeseq()
mts_thread = threading.Thread(target=self.move_to_start)
mts_thread.daemon = True
mts_thread.start()
class FetchRobotApi:
def __init__(self):
self.moving_mode = False
self.waldo_mode = False
self.prev_joy_buttons = 0
# See http://docs.fetchrobotics.com/robot_hardware.html#naming-conventions
self.joint_names = [
#'torso_lift_joint',
#'bellows_joint',
#'head_pan_joint',
#'head_tilt_joint',
'shoulder_pan_joint',
'shoulder_lift_joint',
'upperarm_roll_joint',
'elbow_flex_joint',
'forearm_roll_joint',
'wrist_flex_joint',
'wrist_roll_joint',
'l_gripper_finger_joint',
#'r_gripper_finger_joint',
]
self.joint_name_map = dict([(name, index) for index, name in enumerate(self.joint_names)])
self.cur_joint_pos = np.zeros([len(self.joint_names)])
self.cur_joint_vel = np.zeros([len(self.joint_names)])
self.cur_joint_effort = np.zeros([len(self.joint_names)])
self.cur_base_scan = np.zeros([662], dtype=np.float32)
self.cur_head_camera_depth_image = np.zeros([480,640], dtype=np.float32)
self.cur_head_camera_rgb_image = np.zeros([480,640,3], dtype=np.uint8)
self.timeseq = None
self.timeseq_mutex = threading.Lock()
self.image_queue = Queue.Queue()
self.image_compress_thread = threading.Thread(target=self.image_compress_main)
self.image_compress_thread.daemon = True
self.image_compress_thread.start()
logger.info('Subscribing...')
self.subs = []
if 1: self.subs.append(rospy.Subscriber('/joint_states', JointState, self.joint_states_cb))
if 0: self.subs.append(rospy.Subscriber('/base_scan', LaserScan, self.base_scan_cb))
if 1: self.subs.append(rospy.Subscriber('/head_camera/depth/image', numpy_msg(Image), self.head_camera_depth_image_cb))
if 1: self.subs.append(rospy.Subscriber('/head_camera/rgb/image_raw', numpy_msg(Image), self.head_camera_rgb_image_raw_cb))
if 1: self.subs.append(rospy.Subscriber('/spacenav/joy', Joy, self.spacenav_joy_cb))
logger.info('Subscribed')
self.arm_effort_pub = rospy.Publisher('/arm_controller/weightless_torque/command', JointTrajectory, queue_size=2)
#self.head_goal_pub = rospy.Publisher('/head_controller/point_head/goal', PointHeadActionGoal, queue_size=2)
self.gripper_client = actionlib.SimpleActionClient('gripper_controller/gripper_action', GripperCommandAction)
self.arm_cartesian_twist_pub = rospy.Publisher('/arm_controller/cartesian_twist/command', Twist, queue_size=2)
self.head_point_client = actionlib.SimpleActionClient('head_controller/point_head', PointHeadAction)
self.arm_move_group = MoveGroupInterface("arm", "base_link", plan_only = True)
self.arm_trajectory_client = actionlib.SimpleActionClient("arm_controller/follow_joint_trajectory", FollowJointTrajectoryAction)
self.arm_trajectory_client.wait_for_server()
if 0:
logger.info('Creating MoveGroupInterface...')
self.move_group = MoveGroupInterface('arm_with_torso', 'base_link', plan_only = True)
logger.info('Created MoveGroupInterface')
if 0:
logger.info('Creating PlanningSceneInterface...')
self.planning_scene = PlanningSceneInterface('base_link')
self.planning_scene.removeCollisionObject('my_front_ground')
self.planning_scene.removeCollisionObject('my_back_ground')
self.planning_scene.removeCollisionObject('my_right_ground')
self.planning_scene.removeCollisionObject('my_left_ground')
self.planning_scene.addCube('my_front_ground', 2, 1.1, 0.0, -1.0)
self.planning_scene.addCube('my_back_ground', 2, -1.2, 0.0, -1.0)
self.planning_scene.addCube('my_left_ground', 2, 0.0, 1.2, -1.0)
self.planning_scene.addCube('my_right_ground', 2, 0.0, -1.2, -1.0)
logger.warn('FetchRobotGymEnv ROS node running')
self.head_point_client.wait_for_server()
logger.warn('FetchRobotGymEnv ROS node connected')
def start_timeseq(self, script):
timeseq = TimeseqWriter.create(script)
timeseq.add_channel('robot_joints', 'FetchRobotJoints')
timeseq.add_channel('gripper_joints', 'FetchGripperJoints')
timeseq.add_schema('FetchRobotJoints',
('torso_lift_joint', 'JointState'),
('head_pan_joint', 'JointState'),
('head_tilt_joint', 'JointState'),
('shoulder_pan_joint', 'JointState'),
('shoulder_lift_joint', 'JointState'),
('upperarm_roll_joint', 'JointState'),
('elbow_flex_joint', 'JointState'),
('forearm_roll_joint', 'JointState'),
('wrist_flex_joint', 'JointState'),
('wrist_roll_joint', 'JointState'),
)
timeseq.add_schema('FetchGripperJoints',
('l_gripper_finger_joint', 'JointState'),
)
timeseq.add_schema('JointState',
('pos', 'double'),
('vel', 'double'),
('effort', 'double'),
)
timeseq.add_channel('arm_action', 'FetchArmWeightlessTorqueAction')
timeseq.add_schema('FetchArmWeightlessTorqueAction',
('shoulder_pan_joint', 'WeightlessTorqueAction'),
('shoulder_lift_joint', 'WeightlessTorqueAction'),
('upperarm_roll_joint', 'WeightlessTorqueAction'),
('elbow_flex_joint', 'WeightlessTorqueAction'),
('forearm_roll_joint', 'WeightlessTorqueAction'),
('wrist_flex_joint', 'WeightlessTorqueAction'),
('wrist_roll_joint', 'WeightlessTorqueAction'),
)
timeseq.add_schema('WeightlessTorqueAction',
('action', 'double'),
('effort', 'double'),
)
timeseq.add_channel('gripper_action', 'FetchGripperAction')
timeseq.add_schema('FetchGripperAction',
('action', 'double'),
('effort', 'double'),
('pos', 'double'),
)
timeseq.add_channel('head_camera_rgb', 'VideoFrame')
timeseq.add_schema('VideoFrame',
('url', 'string'),
)
with self.timeseq_mutex:
if self.timeseq:
self.timeseq.close()
self.timeseq = timeseq
def close_timeseq(self):
with self.timeseq_mutex:
if self.timeseq is not None:
self.timeseq.close()
threading.Thread(target=self.timeseq.upload_s3).start()
self.timeseq = None
def start_axis_video(self):
cameras = rospy.get_param('/axis_cameras')
if cameras and len(cameras):
with self.timeseq_mutex:
if self.timeseq:
for name, info in cameras.items():
logging.info('Camera %s: %s', name, repr(info))
self.timeseq.start_axis_video(timeseq_name = name,
auth_header=info.get('auth_header'),
daemon_endpoint=info.get('daemon_endpoint'),
ipaddr=info.get('ipaddr'),
local_link_prefix=info.get('local_link_prefix'),
remote_traces_dir=info.get('remote_traces_dir'),
resolution=info.get('resolution'))
def stop_axis_video(self):
with self.timeseq_mutex:
if self.timeseq:
self.timeseq.stop_axis_video()
def base_scan_cb(self, data):
# fmin replaces nans with 15
self.cur_base_scan = np.fmin(np.array(data.ranges), 15.0)
def head_camera_depth_image_cb(self, data):
shape = [data.height, data.width]
dtype = np.dtype(np.float32)
npdata = np.fromstring(data.data, dtype=dtype).reshape(shape)
npdata.strides = (data.step, dtype.itemsize)
self.cur_head_camera_depth_image = np.fmin(npdata, 5.0)
def head_camera_rgb_image_raw_cb(self, data):
shape = [data.height, data.width, 3]
dtype = np.dtype(np.uint8)
npdata = np.fromstring(data.data, dtype=dtype).reshape(shape)
npdata.strides = (data.step, dtype.itemsize*3, 1)
self.cur_head_camera_rgb_image = npdata
if self.timeseq:
self.image_queue.put(('head_camera_rgb', data))
def joint_states_cb(self, data):
"""
Handle a joint_states message. Messages can have subsets of the joints, hopefully non-overlapping.
"""
t0 = time.time()
for i in range(len(data.name)):
name = data.name[i]
jni = self.joint_name_map.get(name, -1)
if jni >= 0:
self.cur_joint_pos[jni] = data.position[i]
self.cur_joint_vel[jni] = data.velocity[i]
self.cur_joint_effort[jni] = data.effort[i]
with self.timeseq_mutex:
if self.timeseq:
channel, channel_type = ((data.name[0] == 'l_wheel_joint' and ('robot_joints', 'FetchRobotJoints')) or
(data.name[0] == 'l_gripper_finger_joint' and ('gripper_joints', 'FetchGripperJoints')) or
(None, None))
if channel:
state = dict([(jn, {
'__type': 'JointState',
'pos': data.position[i],
'vel': data.velocity[i],
'effort': data.effort[i],
}) for i, jn in enumerate(data.name)])
state['__type'] = channel_type
state['rxtime'] = t0
self.timeseq.add(data.header.stamp.to_sec(), channel, state)
if 0:
t1 = time.time()
print '%0.6f+%0.6f stamp=%0.6f (age %0.6f) from %s' % (t0, t1-t0, data.header.stamp.to_sec(), data.header.stamp.to_sec()-t0, channel)
def image_compress_main(self):
while True:
channel, data = self.image_queue.get()
if 0: print 'len(data.data)=%d data.width=%d data.height=%d' % (len(data.data), data.width, data.height)
im = PIL.Image.frombytes('RGB', (data.width, data.height), data.data, 'raw')
jpeg_writer = StringIO()
im.save(jpeg_writer, 'jpeg')
im_url = 'data:image/jpeg;base64,' + base64.b64encode(jpeg_writer.getvalue())
with self.timeseq_mutex:
if self.timeseq:
self.timeseq.add(data.header.stamp.to_sec(), channel, {
'__type': 'VideoFrame',
'url': im_url,
})
def move_to_start(self):
self.moving_mode = True
# Look down
goal = PointHeadGoal()
goal.target.header.stamp = rospy.Time.now()
goal.target.header.frame_id = '/base_link'
goal.target.point.x = 1.5
goal.target.point.y = 0.0
goal.target.point.z = -0.2
goal.min_duration = rospy.Duration(0.5)
if 0: logger.info('Point head to %s...', goal);
self.head_point_client.send_goal(goal)
if 0: logger.info('Point head sent')
goal = GripperCommandGoal()
goal.command.max_effort = 60
goal.command.position = 0.1
self.gripper_client.send_goal(goal)
joints = [
'shoulder_pan_joint',
'shoulder_lift_joint',
'upperarm_roll_joint',
'elbow_flex_joint',
'forearm_roll_joint',
'wrist_flex_joint',
'wrist_roll_joint',
]
pose = [0.0, +0.8, 0.0, -0.8, 0.0, 0.0, 0.0]
result = self.arm_move_group.moveToJointPosition(joints, pose, plan_only=True)
if result.error_code.val == MoveItErrorCodes.SUCCESS:
if 0: logger.info('Got trajectory %s', result.planned_trajectory)
follow_goal = FollowJointTrajectoryGoal()
follow_goal.trajectory = result.planned_trajectory.joint_trajectory
logger.info('sending trajectory to arm...')
self.arm_trajectory_client.send_goal(follow_goal)
result = self.arm_trajectory_client.wait_for_result()
logger.info('arm followed trajectory %s', result)
else:
logger.warn('moveToJointPosition returned %s', result)
return
result = self.head_point_client.wait_for_result()
logger.info('head followed trajectory %s', result)
logger.info('sending empty arm goal')
empty_goal = FollowJointTrajectoryGoal()
self.arm_trajectory_client.send_goal(empty_goal)
logger.info('Point head done')
self.moving_mode = False
def set_arm_action(self, action):
arm_joints = [
('shoulder_pan_joint', 1.57, 33.82),
('shoulder_lift_joint', 1.57, 131.76),
('upperarm_roll_joint', 1.57, 76.94),
('elbow_flex_joint', 1.57, 66.18),
('forearm_roll_joint', 1.57, 29.35),
('wrist_flex_joint', 2.26, 25.70),
('wrist_roll_joint', 2.26, 7.36),
]
arm_efforts = [min(1.0, max(-1.0, action[self.joint_name_map.get(name)])) * torque_scale * 0.75 for name, vel_scale, torque_scale in arm_joints]
arm_joint_names = [name for name, vel_scale, torque_scale in arm_joints]
if 1:
arm_joint_names.append('gravity_compensation')
arm_efforts.append(1.0)
arm_msg = JointTrajectory(joint_names=arm_joint_names, points=[JointTrajectoryPoint(effort = arm_efforts)])
self.arm_effort_pub.publish(arm_msg)
with self.timeseq_mutex:
if self.timeseq:
state = dict([(jn, {
'__type': 'WeightlessTorqueAction',
'action': action[self.joint_name_map.get(jn)],
'effort': arm_efforts[i],
}) for i, jn in enumerate(arm_joint_names)])
state['__type'] = 'FetchArmWeightlessTorqueAction'
self.timeseq.add(time.time(), 'arm_action', state)
def set_gripper_action(self, action):
grip = action[self.joint_name_map.get('l_gripper_finger_joint')]
goal = GripperCommandGoal()
if grip > 0:
goal.command.max_effort = 60*min(1.0, grip)
goal.command.position = 0.0
else:
goal.command.max_effort = 60
goal.command.position = 0.1
self.gripper_client.send_goal(goal)
with self.timeseq_mutex:
if self.timeseq:
state = {
'__type': 'FetchGripperAction',
'action': grip,
'effort': goal.command.max_effort,
'pos': goal.command.position,
}
self.timeseq.add(time.time(), 'gripper_action', state)
def set_waldo_action(self, joy):
twist = Twist()
twist.linear.x = joy.axes[0]
twist.linear.y = joy.axes[1]
twist.linear.z = joy.axes[2]
twist.angular.x = +3.0*joy.axes[3]
twist.angular.y = +3.0*joy.axes[4]
twist.angular.z = +2.0*joy.axes[5]
self.arm_cartesian_twist_pub.publish(twist)
if joy.buttons[1] and not self.prev_joy_buttons[1]:
goal = GripperCommandGoal()
goal.command.max_effort = 60
goal.command.position = 0.0
self.gripper_client.send_goal(goal)
elif not joy.buttons[1] and self.prev_joy_buttons[1]:
goal = GripperCommandGoal()
goal.command.max_effort = 60
goal.command.position = 0.1
self.gripper_client.send_goal(goal)
def spacenav_joy_cb(self, joy):
if 0: logger.info('joy %s', str(joy))
if joy.buttons[0] and not self.prev_joy_buttons[0]:
if self.waldo_mode:
self.stop_waldo_mode()
elif not self.moving_mode:
self.start_waldo_mode()
if self.waldo_mode and not self.moving_mode:
self.set_waldo_action(joy)
self.prev_joy_buttons = joy.buttons
def start_waldo_mode(self):
logger.info('Start waldo mode');
self.waldo_mode = True
self.start_timeseq('fetchwaldo')
self.start_axis_video()
def stop_waldo_mode(self):
logger.info('Stop waldo mode');
self.waldo_mode = False
timeseq_url = None
if self.timeseq:
timeseq_url = self.timeseq.get_url()
logger.info('save_logs url=%s', timeseq_url)
self.stop_axis_video()
self.close_timeseq()
mts_thread = threading.Thread(target=self.move_to_start)
mts_thread.daemon = True
mts_thread.start()
class Reaching:
def __init__(self):
rospy.init_node('reaching', anonymous=True) #ノードの初期化 reaching ていう名前
self.robot = moveit_commander.RobotCommander() # インスタンスの作成
self.model_pose = [0.0, 0.0, 0.0] #モデルの姿勢
self.obstacle_pose = [0.0, 0.0, 0.0] #障害物の位置
self.subsc_pose = [0.0, 0.0, 0.0] #購読した座標
self.subsc_orientation = [0.0, 0.0, 0.0, 1.0] #購読した四元数
self.set_forbidden() #禁止エリアの初期化
self.set_init_pose() #位置姿勢の初期化
self.before_pose = [0, 0, 0] #前回の場所
self.pcarray = 0 #pointcloud を np array にしたものを入れる
self.executeFlag = False #unity コントローラ B ボタンと対応
self.before_executeFlag = False #前フレームの executeFlag
self.waypoints = []
self.way_flag = False #unity コントローラの 中指のボタンに対応
self.before_wayflag = False #前フレームのway_flag
self.infirst_frame = True #最初のフレーム
self.calc_way_flag = False
self.frame = 0
# def set_ZEDbase(self):
# listener = tf.TransformListener()
# listener.waitForTransform('world','ee_link',rospy.Time(0),rospy.Duration(2.0))
# (self.trans,rot)=listener.lookupTransform('/world','/ee_link',rospy.Time(0)) # trans(x,y,z) rot (x,y,z,w)
# print('trans = '+str(self.trans))
# br = tf.TransformBroadcaster()
# br.sendTransform((self.trans[0],self.trans[1],self.trans[2]),(0.0,0.0,0.0,1.0),rospy.Time.now(),'base_forZED','world')
def set_forbidden(self):
#set forbidden erea
self.planning_scene = PlanningSceneInterface(
"base_link") #PlanningSceneInterface のインスタンスの作成
self.planning_scene.removeCollisionObject("my_ground")
self.planning_scene.addCube("my_ground", 2, 0, 0, -1.0)
self.planning_scene.addBox("obstacle", 0.1, 0.5, 1, -0.3, 0, 0.5)
self.planning_scene.addBox("obstacle2", 0.5, 0.1, 1, 0, -0.3, 0.5)
self.planning_scene.addCube("demo_cube", 0.06, 0, 0.3, 0)
print dir(self.planning_scene) #インスタンス内のあらゆるオブジェクトの属性とメソッドのリストを表示
import inspect
print "addBox's variable is ", inspect.getargspec(
self.planning_scene.addBox)
print "attachBox's variable is ", inspect.getargspec(
self.planning_scene.attachBox)
print "addCube's variable is ", inspect.getargspec(
self.planning_scene.addCube)
print "setColor's variable is ", inspect.getargspec(
self.planning_scene.setColor) #python関数のパラメータの名前とデフォルト値を取得
def set_init_pose(self):
#set init pose
move_group = MoveGroupInterface(
"manipulator", "base_link") #MoveGroupInterface のインスタンスの作成
planning_scene = PlanningSceneInterface(
"base_link") #PlanningSceneInterface のインスタンスの作成
joint_names = [
"shoulder_pan_joint",
"shoulder_lift_joint", #ジョイントの名前を定義
"elbow_joint",
"wrist_1_joint",
"wrist_2_joint",
"wrist_3_joint"
]
# pose = [-1.26 , -0.64 , -2.44 , -0.66 , 1.56 , 0.007]
# move_group.moveToJointPosition(joint_names, pose, wait=False)#joint_names を pose に動かす
# move_group.get_move_action().wait_for_result() #wait result
# result = move_group.get_move_action().get_result() #result を代入
move_group.get_move_action().cancel_all_goals() #すべてのゴールをキャンセル
self.state = {
'default': 0,
'hold': 1,
'way_standby': 2,
'plan_standby': 3
}
# default : 何もしていない target のセットか hold button を待つ
# hold : waypoint のセットを受け付ける hold button が離れたら終了
#way_standby : execute button が押されるのを待つ 押されたら default へ waypoint の設置をした場合
#plan_standby : execute button が押されるのを待つ 押されたら default へ waypoint を使わなかった場合
self.now_state = self.state['default'] #現在フレームの状態
# self.set_ZEDbase()
def callback(self, data): #トピックにデータが追加されるたびに呼ばれる
x = data.pose[1].position.x
y = data.pose[1].position.y
z = data.pose[1].position.z
self.model_pose = [x, y, z] #modelの姿勢を代入
x = data.pose[2].position.x
y = data.pose[2].position.y
z = data.pose[2].position.z
self.obstacle_pose = [x, y, z] #障害物の姿勢を代入
def callbacksub(self, data):
x = data.x
y = data.y
z = data.z
self.subsc_pose = [x, y, z] #購読した座標を代入
def callbackunity(self, data):
x = data.pose.position.x
y = data.pose.position.y
z = data.pose.position.z
self.subsc_pose = [y, -x, z] #unity からの座標 座標変換も行っている
x = data.pose.orientation.x
y = data.pose.orientation.y
z = data.pose.orientation.z
w = data.pose.orientation.w
self.subsc_orientation = [y, -x, z, w] #unity からの四元数
def rtabcallback(self, data):
# dtype_list = [(f.name, np.float32) for f in data.fields]
# cloud_arr = np.fromstring(data.data, dtype_list)
# self.pcarray = np.reshape(cloud_arr, (data.height, data.width))
# print(self.pcarray)
print('rtab subscribed ')
def excallback(self, data):
self.executeFlag = data.data
#print(data.data)
def waycallback(self, data):
self.way_flag = data.data
self.wayflagcalc()
def wayflagcalc(self):
if self.before_wayflag != self.way_flag:
if self.calc_way_flag:
self.calc_way_flag = False
else:
self.calc_way_flag = True
self.before_wayflag = self.way_flag
def start_subscriber(self): #購読をスタート
rospy.Subscriber("/gazebo/model_states", ModelStates, self.callback)
# /gazebo/model_states から gazebo_msgs/ModelStates 型のトピックを購読 トピックが更新されるたびに self.callback を呼ぶ
#rospy.Subscriber("/UR/reaching/pose",Vector3,self.callbacksub)
rospy.Subscriber("/unity/target", PoseStamped,
self.callbackunity) #unityから配信しているトピックを購読
#rospy.Subscriber('/rtabmap/cloud_map', PointCloud2, self.rtabcallback) #zed で slam したpointcloud を購読
rospy.Subscriber('/unity/execute', Bool, self.excallback)
rospy.Subscriber('/unity/wayflag', Bool, self.waycallback)
rospy.Subscriber('/pcl/near_points', PointCloud2, self.create_object)
def change_state(self, target):
#self.state = {'default':0,'hold':1,'way_standby':2,'plan_standby':3}
# default : 何もしていない target のセットか hold button を待つ
# hold : waypoint のセットを受け付ける hold button が離れたら終了
#way_standby : execute button が押されるのを待つ 押されたら default へ waypoint の設置をした場合 target が指定されたら plan_standby に以降
#plan_standby : execute button が押されるのを待つ 押されたら default へ waypoint を使わなかった場合
set_flag = (self.before_pose != target.position
) # target の位置が変化している = トリガーが押された
#self.calc_way_flag # hold button が押されている
exe_flag = (self.before_executeFlag != self.executeFlag
) # executeFlag が前フレームと違う = execute button が押された
print('set ' + str(set_flag))
print('hold ' + str(self.calc_way_flag))
print('exe ' + str(exe_flag))
print('state ' + str(self.now_state))
if self.now_state == self.state['default']:
if set_flag:
self.now_state = self.state['plan_standby']
elif self.calc_way_flag:
self.now_state = self.state['hold']
elif self.now_state == self.state['hold']:
if not self.calc_way_flag:
self.now_state = self.state['way_standby']
elif self.now_state == self.state['way_standby']:
if set_flag:
self.now_state = self.state['plan_standby']
elif exe_flag:
self.now_state = self.state['default']
elif self.now_state == self.state['plan_standby']:
if exe_flag:
self.now_state = self.state['default']
elif self.calc_way_flag:
self.now_state = self.state['hold']
def create_object(self, data):
self.Points = [data[0:3] for data in pc2.read_points(data)]
def target(self):
rate = rospy.Rate(1) #Rateクラスのインスタンス rateを１で
rospy.sleep(1) #1秒スリープ
useway = False
while not rospy.is_shutdown(): #シャットダウンフラグが立っていなければ、
print self.model_pose #model_poseを表示
group = moveit_commander.MoveGroupCommander(
"manipulator") #MoveGroupCommander クラスのインスタンス
#print group.get_current_pose().pose
pose_target = geometry_msgs.msg.Pose()
pose = group.get_current_pose().pose #エンドエフェクタの位置姿勢
pose_target = geometry_msgs.msg.Pose(
) #geometry_msgs.pose のインスタンス ここにターゲット設定
pose_target.orientation.x = self.subsc_orientation[0]
pose_target.orientation.y = self.subsc_orientation[1]
pose_target.orientation.z = self.subsc_orientation[2]
pose_target.orientation.w = self.subsc_orientation[
3] #トピックから 四元数を代入
pose_target.position.x = self.subsc_pose[0] #
pose_target.position.y = self.subsc_pose[1] #
pose_target.position.z = self.subsc_pose[2] #トピックから 座標を代入
# br = tf.TransformBroadcaster()
# br.sendTransform((self.trans[0],self.trans[1],self.trans[2]),(0.0,0.0,0.8509035,0.525322),rospy.Time.now(),'base_forZED','world')
# #逐一ブロードキャストする
if self.infirst_frame:
self.before_pose = pose_target.position
self.infirst_frame = False
#self.planning_scene.removeCollisionObject("targetOBJ")
#self.planning_scene.addCube("targetOBJ", 0.005,self.subsc_pose[0] ,self.subsc_pose[1] ,self.subsc_pose[2] )
self.way_flag = False
#self.state={'default':0,'hold':1,'way_standby':2,'plan_standby':3}
# default : 何もしていない target のセットか hold button を待つ
# hold : waypoint のセットを受け付ける hold button が離れたら終了
#way_standby : execute button が押されるのを待つ 押されたら default へ waypoint の設置をした場合
#plan_standby : execute button が押されるのを待つ 押されたら default へ waypoint を使わなかった場合
exe_flag = (self.before_executeFlag != self.executeFlag
) # executeFlag が前フレームと違う = execute button が押された
if self.now_state == self.state['default']:
if self.before_pose != pose_target.position:
group.set_pose_target(pose_target)
print(' Set Target !!!\n')
print(pose_target)
if self.now_state == self.state['hold']:
if self.before_pose != pose_target.position:
self.waypoints.append(copy.deepcopy(pose_target))
print(' Append Target !!!')
if self.now_state == self.state['way_standby']:
plan, fraction = group.compute_cartesian_path(
self.waypoints, 0.01, 0.0)
if self.before_pose != pose_target.position:
group.set_pose_target(pose_target)
print(' Set Target !!!\n')
print(pose_target)
elif exe_flag:
group.execute(plan)
print(' Planning Execute !!!')
print(self.waypoints)
self.waypoints = []
if self.now_state == self.state['plan_standby']:
group.set_pose_target(pose_target)
group.plan()
if self.before_pose != pose_target.position:
group.set_pose_target(pose_target)
print(' Set Target !!!\n')
print(pose_target)
elif exe_flag:
group.go()
print(' Planning Go !!!')
self.change_state(pose_target)
self.before_executeFlag = self.executeFlag
self.before_pose = pose_target.position
self.frame += 1
rospy.sleep(2) #2秒スリープ
if (rospy.Time.now() - t).to_sec() > 0.2:
rospy.sleep(0.1)
continue
(item_translation, item_orientation) = tf_listener.lookupTransform(
'/base_link', item_frame, t)
except (tf.Exception, tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
continue
gripper_goal.command.position = 0.15
gripper.send_goal(gripper_goal)
gripper.wait_for_result(rospy.Duration(1.0))
print "item: " + str(item_translation)
scene.addCube("item", 0.05, item_translation[0], item_translation[1],
item_translation[2])
p.position.x = item_translation[0] - 0.01 - 0.06
p.position.y = item_translation[1]
p.position.z = item_translation[2] + 0.04 + 0.14
p.orientation = Quaternion(*quaternion_from_euler(0, 1.2, 0))
arm.set_pose_target(p)
arm.go(True)
#os.system("rosservice call clear_octomap")
gripper_goal.command.position = 0
gripper.send_goal(gripper_goal)
gripper.wait_for_result(rospy.Duration(2.0))
scene.removeAttachedObject("item")
def picknplace():
# Define initial parameters
p = PlanningSceneInterface("base")
g = MoveGroupInterface("both_arms", "base")
gr = MoveGroupInterface("right_arm", "base")
gl = MoveGroupInterface("left_arm", "base")
leftgripper = baxter_interface.Gripper('left')
leftgripper.calibrate()
leftgripper.open()
jts_both = ['left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1', 'left_w2', 'right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2']
jts_right = ['right_e0', 'right_e1', 'right_s0', 'right_s1', 'right_w0', 'right_w1', 'right_w2']
jts_left = ['left_e0', 'left_e1', 'left_s0', 'left_s1', 'left_w0', 'left_w1', 'left_w2']
pos1 = [-1.441426162661994, 0.8389151064712133, 0.14240920034028015, -0.14501001475655606, -1.7630090377446503, -1.5706376573674472, 0.09225918246029519,1.7238109084167481, 1.7169079948791506, 0.36930587426147465, -0.33249033539428713, -1.2160632682067871, 1.668587600115967, -1.810097327636719]
pos2 = [-0.949534106616211, 1.4994662184448244, -0.6036214393432617, -0.7869321432861328, -2.4735440176391603, -1.212228316241455, -0.8690001153442384, 1.8342575250183106, 1.8668546167236328, -0.45674277907104494, -0.21667478604125978, -1.2712865765075685, 1.7472041154052735, -2.4582042097778323]
lpos1 = [-1.441426162661994, 0.8389151064712133, 0.14240920034028015, -0.14501001475655606, -1.7630090377446503, -1.5706376573674472, 0.09225918246029519]
lpos2 = [-0.949534106616211, 1.4994662184448244, -0.6036214393432617, -0.7869321432861328, -2.4735440176391603, -1.212228316241455, -0.8690001153442384]
rpos1 = [1.7238109084167481, 1.7169079948791506, 0.36930587426147465, -0.33249033539428713, -1.2160632682067871, 1.668587600115967, -1.810097327636719]
rpos2 = [1.8342575250183106, 1.8668546167236328, -0.45674277907104494, -0.21667478604125978, -1.2712865765075685, 1.7472041154052735, -2.4582042097778323]
# Clear planning scene
p.clear()
# Add table as attached object
p.attachBox('table', 0.7, 1.27, 0.54, 0.65, -0.2, -0.38, 'base', touch_links=['pedestal'])
# Move both arms to start state
g.moveToJointPosition(jts_both, pos1, plan_only=False)
# Get obj locations
temp = rospy.wait_for_message("Dpos", PoseArray)
locs = temp.poses
locs_x = []
locs_y = []
orien = []
size = []
for i in range(len(locs)):
locs_x.append(0.57 + locs[i].position.x)
locs_y.append(-0.011 + locs[i].position.y)
orien.append(locs[i].position.z*pi/180)
size.append(locs[i].orientation.x)
# Filter objects list to remove multiple detected lcoations for same objects --> required, as adding objects to collision scene
ind_rmv = []
for i in range(0,len(locs)):
if (locs_y[i] > 0.42):
ind_rmv.append(i)
continue
for j in range(i,len(locs)):
if not (i == j):
if sqrt((locs_x[i] - locs_x[j])**2 + (locs_y[i] - locs_y[j])**2)<0.018:
ind_rmv.append(i)
locs_x = del_meth(locs_x, ind_rmv)
locs_y = del_meth(locs_y, ind_rmv)
orien = del_meth(orien, ind_rmv)
size = del_meth(size, ind_rmv)
# Sort objects based on size (largest first to smallest last) This was done to enable stacking large cubes
if locs_x:
ig0 = itemgetter(0)
sorted_lists = zip(*sorted(zip(size,locs_x,locs_y,orien), reverse=True, key=ig0))
locs_x = list(sorted_lists[1])
locs_y = list(sorted_lists[2])
orien = list(sorted_lists[3])
size = list(sorted_lists[0])
# Loop to continue pick and place until all objects are cleared from table
j=0
k=0
while locs_x:
p.clear() # CLear planning scene of all collision objects
# Attach table as attached collision object to base of baxter
p.attachBox('table', 0.7, 1.27, 0.54, 0.65, -0.2, -0.38, 'base', touch_links=['pedestal'])
xn = locs_x[0]
yn = locs_y[0]
zn = -0.06
thn = orien[0]
sz = size[0]
if thn > pi/4:
thn = -1*(thn%(pi/4))
# Add collision objects into planning scene
objlist = ['obj01', 'obj02', 'obj03', 'obj04', 'obj05', 'obj06', 'obj07', 'obj08', 'obj09', 'obj10', 'obj11']
for i in range(1,len(locs_x)):
p.addCube(objlist[i], 0.05, locs_x[i], locs_y[i], -0.05)
p.waitForSync()
# Move both arms to pos2 (Right arm away and left arm on table)
g.moveToJointPosition(jts_both, pos2, plan_only=False)
# Move left arm to pick object and pick object
pickgoal = PoseStamped()
pickgoal.header.frame_id = "base"
pickgoal.header.stamp = rospy.Time.now()
pickgoal.pose.position.x = xn
pickgoal.pose.position.y = yn
pickgoal.pose.position.z = zn+0.1
pickgoal.pose.orientation.x = 1.0
pickgoal.pose.orientation.y = 0.0
pickgoal.pose.orientation.z = 0.0
pickgoal.pose.orientation.w = 0.0
gl.moveToPose(pickgoal, "left_gripper", plan_only=False)
# Orientate the gripper --> uses function from geometry.py (by Mike Ferguson) to 'rotate a pose' given rpy angles
pickgoal.pose = rotate_pose_msg_by_euler_angles(pickgoal.pose, 0.0, 0.0, thn)
gl.moveToPose(pickgoal, "left_gripper", plan_only=False)
pickgoal.pose.position.z = zn
gl.moveToPose(pickgoal, "left_gripper", max_velocity_scaling_factor = 0.14, plan_only=False)
leftgripper.close()
pickgoal.pose.position.z = zn+0.1
gl.moveToPose(pickgoal, "left_gripper", plan_only=False)
# Move both arms to pos1
g.moveToJointPosition(jts_both, pos1, plan_only=False)
# Get obj locations
temp = rospy.wait_for_message("Dpos", PoseArray)
locs = temp.poses
locs_x = []
locs_y = []
orien = []
size = []
for i in range(len(locs)):
locs_x.append(0.57 + locs[i].position.x)
locs_y.append(-0.011 + locs[i].position.y)
orien.append(locs[i].position.z*pi/180)
size.append(locs[i].orientation.x)
# Filter objects list to remove multiple detected lcoations for same objects --> required, as adding objects to collision scene
ind_rmv = []
for i in range(0,len(locs)):
if (locs_y[i] > 0.42):
ind_rmv.append(i)
continue
for j in range(i,len(locs)):
if not (i == j):
if sqrt((locs_x[i] - locs_x[j])**2 + (locs_y[i] - locs_y[j])**2)<0.018:
ind_rmv.append(i)
locs_x = del_meth(locs_x, ind_rmv)
locs_y = del_meth(locs_y, ind_rmv)
orien = del_meth(orien, ind_rmv)
size = del_meth(size, ind_rmv)
# Sort objects based on size (largest first to smallest last) This was done to enable stacking large cubes
if locs_x:
ig0 = itemgetter(0)
sorted_lists = zip(*sorted(zip(size,locs_x,locs_y,orien), reverse=True, key=ig0))
locs_x = list(sorted_lists[1])
locs_y = list(sorted_lists[2])
orien = list(sorted_lists[3])
size = list(sorted_lists[0])
# Place object
stleft = PoseStamped()
stleft.header.frame_id = "base"
stleft.header.stamp = rospy.Time.now()
stleft.pose.position.x = 0.65
stleft.pose.position.y = 0.55
stleft.pose.position.z = 0.1
stleft.pose.orientation.x = 1.0
stleft.pose.orientation.y = 0.0
stleft.pose.orientation.z = 0.0
stleft.pose.orientation.w = 0.0
# Stack objects (large cubes) if size if greater than some threshold
if sz > 16.:
stleft.pose.position.x = 0.65
stleft.pose.position.y = 0.7
stleft.pose.position.z = -0.04+(k*0.05)
k += 1
gl.moveToPose(stleft, "left_gripper", plan_only=False)
leftgripper.open()
stleft.pose.position.z = 0.3
gl.moveToPose(stleft, "left_gripper", plan_only=False)
if __name__ == '__main__':
rospy.init_node("hi")
# Create move group interface for a fetch robot
move_group = MoveGroupInterface("arm_with_torso", "base_link")
# Define ground plane
# This creates objects in the planning scene that mimic the ground
# If these were not in place gripper could hit the ground
planning_scene = PlanningSceneInterface("base_link")
planning_scene.removeCollisionObject("my_front_ground1")
planning_scene.removeCollisionObject("my_front_ground2")
planning_scene.removeCollisionObject("my_back_ground")
planning_scene.removeCollisionObject("my_right_ground")
planning_scene.removeCollisionObject("my_left_ground")
planning_scene.addCube("my_front_ground1", 0.5, 0.8, 0.25, 0.25)
planning_scene.addCube("my_front_ground2", 0.5, 0.8, 0.25, 1.20)
# This is the wrist link not the gripper itself
gripper_frame = 'wrist_roll_link'
# Position and rotation of two "wave end poses"
gripper_poses = [
Pose(Point(0.042, 0.584, 1.426),
Quaternion(0.173, -0.693, -0.242, 0.657)),
Pose(Point(0.547, 0.30, 0.72), Quaternion(0.0, 0.0, 0.0, 1.0))
]
# Construct a "pose_stamped" message as required by moveToPose
gripper_pose_stamped = PoseStamped()
gripper_pose_stamped.header.frame_id = 'base_link'
