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'"
"""
