def send_gripper_cmd(self, left, right):
rospy.loginfo("Send Gripper Command")
lgmsg = GripperCommandActionGoal()
lgmsg.header.seq = 0
lgmsg.goal.command.position = left
rgmsg = GripperCommandActionGoal()
rgmsg.header.seq = 0
rgmsg.goal.command.position = right
self.leftGripper.publish(lgmsg)
self.rightGripper.publish(rgmsg)
def __create_gripper_command(self, position, max_effort):
"""Creating GripperCommand message.
@position : [m]
@max_effort : [N]
"""
if self.__isSim:
msg = GripperCommandActionGoal()
msg.goal.command.position = position
msg.goal.command.max_effort = max_effort
else:
msg = GripperCommand()
msg.position = position
msg.max_effort = max_effort
return msg
def set_pos(self, pos):
"""
Set the gripper position. Function internally maps
values from API position range to URScript position
range. After sending position command, update internal
position variable by sending urscript program to
controller.
Args:
pos (float): Desired gripper position.
"""
pos = clamp(pos, self.cfgs.EETOOL.OPEN_ANGLE,
self.cfgs.EETOOL.CLOSE_ANGLE)
if not self._gazebo_sim:
urscript = self._get_new_urscript()
pos = int(pos * self.cfgs.EETOOL.POSITION_SCALING)
urscript.set_gripper_position(pos)
urscript.sleep(0.1)
gripper_cmd = urscript()
else:
gripper_cmd = GripperCommandActionGoal()
gripper_cmd.goal.command.position = pos
self._pub_command.publish(gripper_cmd)
time.sleep(1.0)
if not self._gazebo_sim:
self._get_current_pos_urscript()
def grasp2(self, width, force):
''' width, epsilon_inner, epsilon_outer, speed, force '''
grasp_goal = GripperCommandGoal()
grasp_goal.command.position = float(width)
grasp_goal.command.max_effort = float(force)
grasp_msg = GripperCommandActionGoal(goal=grasp_goal)
self.generic_grasp_client.publish(grasp_msg)
def gripper_closing(gripper_cmd_pub):
# gripper_cmd_pub = rospy.Publisher(
# rospy.resolve_name('gripper_controller/gripper_cmd/goal'),
# GripperCommandActionGoal, queue_size=10)
gripper_cmd = GripperCommandActionGoal()
gripper_cmd.goal.command.position = gripper_close_value # sapien # 0.6 # Gaezebo
gripper_cmd.goal.command.max_effort = 90.0
gripper_cmd_pub.publish(gripper_cmd)
rospy.loginfo("Pub gripper_cmd for closing")
gripper_block_until_near(gripper_close_value)
def __init__(self):
# get launch params
self.button_img = rospy.get_param('~panel_img')
self.pressed_button_img = rospy.get_param('~pressed_button_img')
self.press_offset_x = float(rospy.get_param('~press_offset_x'))
self.press_offset_y = float(rospy.get_param('~press_offset_y'))
self.acc_certainty = float(rospy.get_param('~acc_certainty'))
self.button_size = float(rospy.get_param('~button_size'))
self.show_image = rospy.get_param('~show_image')
self.show_status = rospy.get_param('~show_status')
self.gripper_pub = rospy.Publisher("/gripper_controller/gripper_cmd/goal", GripperCommandActionGoal,
queue_size=10)
# for handling movement towards and pushing buttons
self.pb = push_button()
self.push_ready = 0
self.counter = 0
# dispatching for push_button
self.move_control = [self.move_control0, self.move_control1, self.move_control2,
self.move_control3, self.move_control4, self.move_control5, self.move_control6,
self.move_control7, self.move_control8, self.move_control9, self.move_control10]
self.status_str = ["searching button", "searching button", "align robot to button",
"move towards button", "align robot to button", "adjust height to button",
"pushing button", "lower robot", "check doors", "failed, trying again",
"going into elevator"]
# move arm to 'button' position
move_arm.target_move(0, 0, 0, "button")
rospy.sleep(5)
# close gripper
gc = GripperCommandActionGoal()
gc.goal.command.max_effort = 0.3
self.gripper_pub.publish(gc)
rospy.sleep(2)
self.bridge = CvBridge()
rospy.Subscriber("/kinect2/qhd/image_color_rect/compressed", CompressedImage, self.kinect_callback)
rospy.Subscriber("/intel_cam/color/image_raw/compressed", CompressedImage, self.intel_callback)
# args updated by Button_finder
self.scale = 1
self.detected = 0
self.threshold = -1
self.button_location = (-1, -1)
self.button_height = -1
self.button_width = -1
self.max_scale = 1
self.min_scale = 0.2
self.final_align = 1
self.align_offset = 50
self.right_door = 0
self.left_door = 0
def gripper():
# create an empty control_msgs/GripperCommandActionGoal message
msg = GripperCommandActionGoal()
# position range is 0-1 (discreet, not sure why the data type is float)
# 0 commands the gripper to close
# 1 commands the gripper to open
msg.goal.command.position = gripper_closing
# effort indicates the maximum force which should be
# exerted to open or close the gripper
msg.goal.command.max_effort = gripper_effort
# publish the message
pub.publish(msg)
def __init__(self):
# self.interface = rospy.Publisher('gripper/gripper_cmd/goal', GripperCommandActionGoal, queue_size=10)
# self.interface = rospy.Publisher('ur10/gripper/grasping', Bool, queue_size=1)
self.rate1Hz = rospy.Rate(1)
self.gripperCommandGoal = GripperCommandActionGoal()
self.gripperCmd = GripperCommand()
self.goal = GripperCommandGoal()
self.goal.command = self.gripperCmd
self.gripperCommandGoal.goal = self.goal
self.msg = Bool()
self.msg.data = True
self.isOpen = True
def execute_callback(self, goal):
rospy.loginfo("Get clean task.")
print(goal)
##### User code example starts #####
# In the following, an example is provided on how to use the predefined software APIs
# to get the target configuration, to control the robot, to set the actionlib result.
# Example: load model.
rospy.loginfo("Load models")
for object_name in goal.object_list:
object_model_dir = self.models_dir + '/' + object_name
rospy.loginfo("Object model dir: " + object_model_dir)
rospy.sleep(1.0)
# Example: control ur5e by topic
arm_cmd = JointTrajectory()
arm_cmd.joint_names = ['shoulder_pan_joint', 'shoulder_lift_joint', \
'elbow_joint', 'wrist_1_joint', \
'wrist_2_joint', 'wrist_3_joint']
waypoint = JointTrajectoryPoint()
waypoint.positions = [-0.68, -0.63, 0.69, -0.88, -0.53, -0.19]
waypoint.time_from_start.secs = 2.0
arm_cmd.points.append(waypoint)
self.arm_cmd_pub.publish(arm_cmd)
rospy.loginfo("Pub arm_cmd")
rospy.sleep(1.0)
# Example: control 2f85 by topic
gripper_cmd = GripperCommandActionGoal()
gripper_cmd.goal.command.position = 0.5
gripper_cmd.goal.command.max_effort = 0.0
self.gripper_cmd_pub.publish(gripper_cmd)
rospy.loginfo("Pub gripper_cmd")
rospy.sleep(1.0)
# Example: set status "Aborted" and quit.
if self.action_server.is_preempt_requested():
self.result.status = "Aborted"
self.action_server.set_aborted(self.result)
return
# Example: send feedback.
self.feedback.text = "write_feedback_text_here"
self.action_server.publish_feedback(self.feedback)
rospy.loginfo("Pub feedback")
rospy.sleep(1.0)
# Example: set status "Finished" and quit.
self.result.status = "Finished"
rospy.loginfo("Done.")
self.action_server.set_succeeded(self.result)
def on_enter(self, userdata):
msg = GripperCommandActionGoal()
msg.goal.command.position = self.width
msg.goal.command.max_effort = self.effort
self._pub.publish(self._topic, msg)
if not self._connected:
(msg_path, msg_topic, fn) = rostopic.get_topic_type(self._topic_f)
if msg_topic == self._topic_f:
msg_type = self._get_msg_from_path(msg_path)
self._sub = ProxySubscriberCached({self._topic_f: msg_type})
self._connected = True
Logger.loginfo('Successfully subscribed to previously unavailable topic %s' % self._topic_f)
else:
Logger.logwarn('Topic %s still not available, giving up.' % self._topic_f)
self._sub.remove_last_msg(self._topic_f)
def gripper_closing_and_store_pose(gripper_cmd_pub, model_name,
global_grasp_pose, grasp_id):
close_value = gripper_close_value # 0 -> 0.044
# gripper_cmd_pub = rospy.Publisher(
# rospy.resolve_name('gripper_controller/gripper_cmd/goal'),
# GripperCommandActionGoal, queue_size=10)
# publish this so that gazebo can move accordingly
# Example: control ur5e by topic
gripper_cmd = GripperCommandActionGoal()
gripper_cmd.goal.command.position = close_value # sapien # 0.6 # Gaezebo
gripper_cmd.goal.command.max_effort = 0.0
gripper_cmd_pub.publish(gripper_cmd)
rospy.loginfo("Pub gripper_cmd for closing")
rospy.sleep(1.0)
# publish
store_pose_based_on_gripper(global_grasp_pose, grasp_id)
def gripper_openning(gripper_cmd_pub):
#gripper_cmd_pub = rospy.Publisher(
# rospy.resolve_name('gripper_controller/gripper_cmd/goal'),
# GripperCommandActionGoal, queue_size=10)
# real stage moveit does not know the current gripper status
# # check if the current gripper is open, if not, then open it
# state = robot.get_current_state().joint_state
# # go over each name to find the gripper driver
# for i in range(len(state.name)):
# if state.name[i] == gripper_driver_name:
# if np.abs(state.position[i] - gripper_open_value) <= 0.001:
# return # consider as open, no need to open
# # otherwise open
gripper_cmd = GripperCommandActionGoal()
gripper_cmd.goal.command.position = gripper_open_value
gripper_cmd.goal.command.max_effort = 50.0
gripper_cmd_pub.publish(gripper_cmd)
rospy.loginfo("Pub gripper_cmd for openning")
gripper_block_until_near(gripper_open_value)
def execute(self, userdata):
#open the arm slightly
command = GripperCommandActionGoal()
command.goal.command.position = 0.05
self.publisher.publish(command)
joint_values = arm_command.get_current_joint_values()
joint_values[self.joint_number] = joint_values[
self.joint_number] + self.wiggle_offset
try:
arm_command.set_joint_value_target(joint_values)
except Exception as e:
rospy.logerr('unable to set target position: %s' % (str(e)))
return 'failed'
error_code = arm_command.go(wait=self.blocking)
if error_code == moveit_msgs.msg.MoveItErrorCodes.SUCCESS:
return 'succeeded'
else:
rospy.logerr("Arm movement failed with error code: %d", error_code)
return 'failed'
#!/usr/bin/env python
from __future__ import print_function
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
from control_msgs.msg import GripperCommandActionGoal
import tf
import rospy
rospy.init_node('motion_planning_test',
anonymous=True)
gripper_cmd_pub = rospy.Publisher(
rospy.resolve_name('gripper_controller/gripper_cmd/goal'),
GripperCommandActionGoal, queue_size=10)
gripper_cmd = GripperCommandActionGoal()
gripper_cmd.goal.command.position = -0.5
gripper_cmd.goal.command.max_effort = 0.0
gripper_cmd_pub.publish(gripper_cmd)
rospy.loginfo("Pub gripper_cmd for openning")
def send_gripper_cmd(self, pos):
#rospy.loginfo("Send Gripper Command")
lgmsg = GripperCommandActionGoal()
lgmsg.header.seq = 0
lgmsg.goal.command.position = 1 - pos / 10.
self.pubg.publish(lgmsg)
print(kdl_kin.chain.getNrOfJoints()) q_targ = solver.DoubleVector(6) # ik.solvePoseIk(initialPose, q_targ, p_targ, False) posisions = [0 for i in range(len(joints))] zeros = [0 for i in range(len(joints))] posisions[0] = -0.5 posisions[1] = -1 posisions[2] = 1 posisions[3] = -1 gms = GripperCommandActionGoal() goal = GripperCommand() g = GripperCommandGoal() goal.position=0.8 g.command = goal gms.goal=g # gp.publish(gms) msg = JointTrajectory() subMsg = JointTrajectoryPoint() # print(msg) # print(subMsg) # print(type(zeros))