def closeGripper(self): if self.isGripping: return self.currentModel2 = self.findClosestBlock() self.gripperMsg = AttachRequest() self.gripperMsg.link_name_1 = self.link1 self.gripperMsg.link_name_2 = self.link2 self.gripperMsg.model_name_1 = self.currentModel1 self.gripperMsg.model_name_2 = self.currentModel2 self.attServ.call(self.gripperMsg) self.isGripping = True rospy.sleep(1)
def openGripper(self): if not self.isGripping: return self.currentModel2 = self.findClosestBlock() self.gripperMsg = AttachRequest() self.gripperMsg.link_name_1 = self.link1 self.gripperMsg.link_name_2 = self.link2 self.gripperMsg.model_name_1 = self.currentModel1 self.gripperMsg.model_name_2 = self.currentModel2 self.detServ.call(self.gripperMsg) rospy.sleep(1) self.isGripping = False
def attach_link(model1, link1, model2, link2): req = AttachRequest() req.model_name_1 = model1 req.link_name_1 = link1 req.model_name_2 = model2 req.link_name_2 = link2 client_attach.call(req)
def vacuum(self, model_name_1, model_name_2, action): if model_name_1 == "UR1::urgripper": self.actions[0] = action elif model_name_1 == "UR2::urgripper": self.actions[1] = action elif model_name_1 == "UR3::urgripper": self.actions[2] = action elif model_name_1 == "UR4::urgripper": self.actions[3] = action elif model_name_1 == "ABB1::abbgripper": self.actions[4] = action elif model_name_1 == "ABB2::abbgripper": self.actions[5] = action elif model_name_1 == "ABB3::abbgripper": self.actions[6] = action elif model_name_1 == "ABB4::abbgripper": self.actions[7] = action req = AttachRequest() req.model_name_1 = model_name_1 req.link_name_1 = "body" req.model_name_2 = model_name_2 req.link_name_2 = "link" if action: attach_srv.call(req) else: detatch_srv.call(req)
def activate_gripper(self): rospy.loginfo("Attach request received") req = AttachRequest() req.model_name_1 = 'edrone' req.link_name_1 = 'base_frame' req.model_name_2 = 'parcel_box' req.link_name_2 = 'link' self._attach_srv_a.call(req)
def deactivate_gripper(self, model_name_2): rospy.loginfo("Detach request received") req = AttachRequest() req.model_name_1 = 'edrone' req.link_name_1 = 'base_frame' req.model_name_2 = model_name_2 req.link_name_2 = 'link' self._attach_srv_d.call(req)
def detach_link(model1, link1, model2, link2): detach_srv = rospy.ServiceProxy('/link_attacher_node/detach',Attach) # detach_srv.wait_for_service() req = AttachRequest() req.model_name_1 = model1 req.link_name_1 = link1 req.model_name_2 = model2 req.link_name_2 = link2 detach_srv.call(req)
def deactivate_vacuum_gripper(self): rospy.loginfo("Detach request received") req = AttachRequest() req.model_name_1 = self._vacuum_gripper_model_name req.link_name_1 = self._vacuum_gripper_link_name req.model_name_2 = self._object_model_name req.link_name_2 = self._object_link_name self._attach_srv_d.call(req)
def release(self, link_name): parent = self.attach_link.rsplit('::') child = link_name.rsplit('::') req = AttachRequest() req.model_name_1 = parent[0] req.link_name_1 = parent[1] req.model_name_2 = child[0] req.link_name_2 = child[1] res = self.detach_srv.call(req) return res.ok
def detach(self): attach_srv = rospy.ServiceProxy('/link_attacher_node/detach',Attach) attach_srv.wait_for_service() req = AttachRequest() req.model_name_1 = "iris" req.link_name_1 = "base_link" req.model_name_2 = "apriltag" req.link_name_2 = "link" attach_srv.call(req)
def vacuum(self, robot_name, obj_name, action): req = AttachRequest() req.model_name_1 = robot_name + '::' + 'gripper' req.link_name_1 = "body" req.model_name_2 = obj_name req.link_name_2 = "link" if action: attach_srv.call(req) else: detatch_srv.call(req)
def detach(self): attach_srv = rospy.ServiceProxy('/link_attacher_node/detach', Attach) attach_srv.wait_for_service() print("detaching") req = AttachRequest() req.model_name_1 = "iris_1" req.link_name_1 = "base_link" req.model_name_2 = "sample_probe" req.link_name_2 = "base_link" attach_srv.call(req)
def attach(self): print('Running attach_srv') attach_srv = rospy.ServiceProxy('/link_attacher_node/attach', Attach) attach_srv.wait_for_service() print('Trying to attach') req = AttachRequest() req.model_name_1 = "iris" req.link_name_1 = "base_link" req.model_name_2 = "dummy_probe" req.link_name_2 = "link" attach_srv.call(req)
def detach(self): attach_srv = rospy.ServiceProxy('/link_attacher_node/detach', Attach) attach_srv.wait_for_service() print('Trying to detach') req = AttachRequest() req.model_name_1 = "iris" req.link_name_1 = "base_link" req.model_name_2 = "apriltag" # Insert the model name you want to attach to req.link_name_2 = "link" # Insert link name you want to attach to attach_srv.call(req) print('Detached')
def place(self): req = AttachRequest() req.model_name_1 = self.gripper req.link_name_1 = self.gripper_link req.model_name_2 = self.holding_object req.link_name_2 = self.common_link self.detach_srv.call(req) time.sleep(2) self.holding_object = "" self.status = False self.update_gripper_info(False) print("Released Object") return self.status
def detach_handler(): try: s = rospy.ServiceProxy('/link_attacher_node/detach', Attach) s.wait_for_service() req = AttachRequest() req.model_name_1 = "nut" req.link_name_1 = "link_2" req.model_name_2 = "youbot" req.link_name_2 = "base_footprint" s.call(req) except rospy.ServiceException as e: rospy.loginfo("Service failed")
def deactivate_vacuum_gripper(self): rospy.set_param("vacuum_gripper_plugin_in_usage", True) rospy.loginfo("Detach request received") req = AttachRequest() req.model_name_1 = self._vacuum_gripper_model_name req.link_name_1 = self._vacuum_gripper_link_name req.model_name_2 = self._object_model_name req.link_name_2 = self._object_link_name self._attach_srv_d.call(req) rospy.set_param("vacuum_gripper_plugin_in_usage", False)
def attach(self): print('Running attach_srv') attach_srv = rospy.ServiceProxy('/link_attacher_node/attach', Attach) attach_srv.wait_for_service() print('Trying to attach') req = AttachRequest() req.model_name_1 = "iris" req.link_name_1 = "base_link" req.model_name_2 = "apriltag" req.link_name_2 = "link" attach_srv.call(req) print('Attached') self.mode="HOVER" self.hover1(self.hover_loc1)
def release(self, link_name): parent = self.attach_link.rsplit('::') if '::' in link_name: divider = '::' elif '.' in link_name: divider = '.' else: return False child = link_name.rsplit(divider) req = AttachRequest() req.model_name_1 = parent[0] req.link_name_1 = parent[1] req.model_name_2 = child[0] req.link_name_2 = child[1] res = self.detach_srv.call(req) return res.ok
def ativa_garra(letra): """! Cria o link entre o drone e o pacote Parametros: letra: letra do pacote """ rospy.wait_for_service('/link_attacher_node/attach') garra = rospy.ServiceProxy('/link_attacher_node/attach',Attach) req = AttachRequest() req.model_name_1 = "uav1" req.link_name_1 = "base_link" req.model_name_2 = "equipment"+letra req.link_name_2 = "link_"+letra garra(req)
def vacuum(self,robot_name,obj_name,action): if "ur" in robot_name: self.actions['ur']=action elif "sawyer" in robot_name: self.actions['sawyer']=action elif "abb" in robot_name: self.actions['abb']=action elif "staubli" in robot_name: self.actions['staubli']=action req = AttachRequest() req.model_name_1=robot_name+'::'+robot_name+'gripper' req.link_name_1 = "body" req.model_name_2 = obj_name req.link_name_2 = "link" if action: attach_srv.call(req) else: detatch_srv.call(req)
def pick(self, object): self.gazebo_callback(object) if self.grasping_condition(object): # Pause physic environment self.pause.call() self.move_object(object) time.sleep(1) # Attach req = AttachRequest() req.model_name_1 = self.gripper req.link_name_1 = self.gripper_link req.model_name_2 = object req.link_name_2 = self.common_link self.attach_srv.call(req) time.sleep(1) self.holding_object = object self.status = True # Release physic environment self.unpause.call() time.sleep(1) self.update_gripper_info(False) print("Picked Object") return self.status
import rospy from gazebo_ros_link_attacher.srv import Attach, AttachRequest, AttachResponse if __name__ == '__main__': rospy.init_node('demo_attach_links') rospy.loginfo("Creating ServiceProxy to /link_attacher_node/attach") attach_srv = rospy.ServiceProxy('/link_attacher_node/attach', Attach) attach_srv.wait_for_service() rospy.loginfo("Created ServiceProxy to /link_attacher_node/attach") # Link them rospy.loginfo("Attaching cube1 and cube2") req = AttachRequest() req.model_name_1 = "j2n6s300" req.link_name_1 = "j2n6s300_link_6" req.model_name_2 = "to_pickup_0" req.link_name_2 = "link" attach_srv.call(req) # From the shell: """ rosservice call /link_attacher_node/attach "model_name_1: 'cube1' link_name_1: 'link' model_name_2: 'cube2' link_name_2: 'link'" """ # rospy.loginfo("Attaching cube2 and cube3")
rospy.loginfo(goal.command.position) rospy.sleep(1) state = client.send_goal_and_wait(goal) cnt = cnt + 1 if state == 0: rospy.logerr('Pick up goal failed!: %s' % client.get_goal_status_text()) rospy.sleep(0.05) rospy.loginfo("Creating ServiceProxy to /link_attacher_node/attach") attach_srv = rospy.ServiceProxy('/link_attacher_node/attach', Attach) attach_srv.wait_for_service() rospy.loginfo("Created ServiceProxy to /link_attacher_node/attach") rospy.loginfo("Attaching finger and cube") req = AttachRequest() req.model_name_1 = "robot" req.link_name_1 = "robotiq_85_right_finger_tip_link" req.model_name_2 = "coke_can_box_model" req.link_name_2 = "coke_can" attach_srv.call(req) scene.remove_world_object("part") rospy.sleep(0.1) place_pose = PoseStamped() place_pose.header.frame_id = robot.get_planning_frame() place_pose.pose.position.x = -0.224157 place_pose.pose.position.y = 0.704343 place_pose.pose.position.z = 0.175525 place_pose.pose.orientation.x = 0
import rospy from gazebo_ros_link_attacher.srv import Attach, AttachRequest, AttachResponse if __name__ == '__main__': rospy.init_node('demo_attach_links') rospy.loginfo("Creating ServiceProxy to /link_attacher_node/attach") attach_srv = rospy.ServiceProxy('/link_attacher_node/attach', Attach) attach_srv.wait_for_service() rospy.loginfo("Created ServiceProxy to /link_attacher_node/attach") # Link them rospy.loginfo("Attaching cube1 and cube2") req = AttachRequest() req.model_name_1 = "cube1" req.link_name_1 = "link" req.model_name_2 = "cube2" req.link_name_2 = "link" attach_srv.call(req) # From the shell: """ rosservice call /link_attacher_node/attach "model_name_1: 'cube1' link_name_1: 'link' model_name_2: 'cube2' link_name_2: 'link'" """ rospy.loginfo("Attaching cube2 and cube3")
def __init__(self, init_node = True, limb='right', tip_name="right_gripper_tip", target_location_x = -100, target_location_y = -100, target_location_z = -100, z_tf=-0.705): # z_tf changes z dist from world origin to robot origin for eef dip. super(PickAndPlace, self).__init__() # print("Class init happening bro!") joint_state_topic = ['joint_states:=/robot/joint_states'] # at HOME position, orientation of gripper frame w.r.t world x=0.7, y=0.7, z=0.0, w=0.0 or [ rollx: -3.1415927, pitchy: 0, yawz: -1.5707963 ] # (roll about an X-axis w.r.t home) / (subsequent pitch about the Y-axis) / (subsequent yaw about the Z-axis) rollx = -3.1415926535 pitchy = 0.0 yawz = 0.0 # -1.5707963 # use q with moveit because giving quaternion.x doesn't work. q = quaternion_from_euler(rollx, pitchy, yawz) overhead_orientation_moveit = Quaternion( x=q[0], y=q[1], z=q[2], w=q[3]) moveit_commander.roscpp_initialize(joint_state_topic) # moveit_commander.roscpp_initialize(sys.argv) robot = moveit_commander.RobotCommander() scene = moveit_commander.PlanningSceneInterface() group_name = "right_arm" group = moveit_commander.MoveGroupCommander(group_name, wait_for_servers=5.0) # See ompl_planning.yaml for a complete list group.set_planner_id("RRTConnectkConfigDefault") # group.set_planner_id("TRRTkConfigDefault") display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path', moveit_msgs.msg.DisplayTrajectory, queue_size=20) planning_frame = group.get_planning_frame() eef_link = group.get_end_effector_link() group_names = robot.get_group_names() req = AttachRequest() if init_node: rospy.init_node('pnp_node', anonymous=True, disable_signals=False) self._limb_name = limb # string self._limb = intera_interface.Limb(limb) self._tip_name = tip_name # print(robot.get_current_state()) # Misc variables self.robot = robot self.scene = scene self.group = group self.req = req self.display_trajectory_publisher = display_trajectory_publisher self.planning_frame = planning_frame self.eef_link = eef_link self.group_names = group_names self.q = q self.overhead_orientation_moveit = overhead_orientation_moveit self.target_location_x = target_location_x self.target_location_y = target_location_y self.target_location_z = target_location_z self.onion_index = 0 self.onion_color = None self.z_tf = z_tf # z_tf changes z dist from world origin to robot origin for eef dip.
pose_goal.position.x = posx pose_goal.position.y = posy pose_goal.position.z = 0.045 move_group.set_pose_target(pose_goal) plan = move_group.go(wait=True) move_group.stop() move_group.clear_pose_targets() #close the gripper using effort-controller print("close gripper") moveFingers("close", [-0.5, -0.5, -0.5], 'j2n6s300', 3) #attach the block to the end-effector req = AttachRequest() req.model_name_1 = "j2n6s300" req.link_name_1 = "j2n6s300_link_6" req.model_name_2 = item_name req.link_name_2 = "link" attach_srv.call(req) #RAISE ARM BACK UP IN SAME X-Y POSITION pose_goal.orientation.w = 0 pose_goal.orientation.x = 1 pose_goal.orientation.y = 0 pose_goal.orientation.z = 0 pose_goal.position.x = posx pose_goal.position.y = posy pose_goal.position.z = z_height_reset
#!/usr/bin/env python import rospy from gazebo_ros_link_attacher.srv import Attach, AttachRequest, AttachResponse if __name__ == '__main__': rospy.init_node('demo_attach_links') rospy.loginfo("Creating ServiceProxy to /link_attacher_node/attach") attach_srv = rospy.ServiceProxy('/link_attacher_node/attach', Attach) attach_srv.wait_for_service() rospy.loginfo("Created ServiceProxy to /link_attacher_node/attach") # Link them rospy.loginfo("Attaching gripper and red box.") req = AttachRequest() req.model_name_1 = "mogi_arm" req.link_name_1 = "left_finger" req.model_name_2 = "red_box" req.link_name_2 = "link" attach_srv.call(req) # From command line: """ rosservice call /link_attacher_node/attach "model_name_1: 'cube1' link_name_1: 'link' model_name_2: 'cube2' link_name_2: 'link'" """
#!/usr/bin/env python import rospy from gazebo_ros_link_attacher.srv import Attach, AttachRequest, AttachResponse if __name__ == '__main__': rospy.init_node('demo_attach_links') rospy.loginfo("Creating ServiceProxy to /link_attacher_node/attach") attach_srv = rospy.ServiceProxy('/link_attacher_node/attach', Attach) attach_srv.wait_for_service() rospy.loginfo("Created ServiceProxy to /link_attacher_node/attach") # Link them rospy.loginfo("Attaching cube1 and cube2") req = AttachRequest() req.model_name_1 = "cube1" req.link_name_1 = "link" req.model_name_2 = "cube2" req.link_name_2 = "link" attach_srv.call(req) # From the shell: """ rosservice call /link_attacher_node/attach "model_name_1: 'cube1' link_name_1: 'link' model_name_2: 'cube2' link_name_2: 'link'" """ rospy.loginfo("Attaching cube2 and cube3")
# Print below to see the magnitude and degrees # print("{} {}".format(mag, np.degrees(np.arccos(np.clip(vector_iris[2] / mag, -1.0, 1.0))))) # 15 degree threshold and magnitude is less than 0.6 meters, attach works if mag < 0.6 and degrees < 30: rospy.loginfo( "Creating ServiceProxy to /link_attacher_node/attach") attach_srv = rospy.ServiceProxy('/link_attacher_node/attach', Attach) attach_srv.wait_for_service() rospy.loginfo( "Created ServiceProxy to /link_attacher_node/attach") # Link them rospy.loginfo("Attach drone to sample probe") req = AttachRequest() req.model_name_1 = "iris" req.link_name_1 = "base_link" req.model_name_2 = "sample_probe" req.link_name_2 = "base_link" attach_srv.call(req) mode = None elif mode == "DETACH": rospy.loginfo( "Creating ServiceProxy to /link_attacher_node/detach") attach_srv = rospy.ServiceProxy('/link_attacher_node/detach', Attach) attach_srv.wait_for_service() rospy.loginfo("Created ServiceProxy to /link_attacher_node/detach") # Link them
#!/usr/bin/env python import rospy from gazebo_ros_link_attacher.srv import Attach, AttachRequest, AttachResponse if __name__ == '__main__': rospy.init_node('demo_detach_links') rospy.loginfo("Creating ServiceProxy to /link_attacher_node/detach") attach_srv = rospy.ServiceProxy('/link_attacher_node/detach', Attach) attach_srv.wait_for_service() rospy.loginfo("Created ServiceProxy to /link_attacher_node/detach") # Link them rospy.loginfo("Detaching cube1 and cube2") req = AttachRequest() req.model_name_1 = "robot" req.link_name_1 = "right_link_6" req.model_name_2 = "unit_box" req.link_name_2 = "link" attach_srv.call(req) # From the shell: """ rosservice call /link_attacher_node/detach "model_name_1: 'cube1' link_name_1: 'link' model_name_2: 'cube2' link_name_2: 'link'" """