def activateValve(self, movegroup): print "\n\n\n\n\n\Valve: " # move valve, select the correct pipe # Activate suction for correct move group # the valve is connected to the left gripper outlet if movegroup.data == 'left_arm_cartesian' or movegroup.data == 'left_arm' or movegroup.data == 'right_arm' or movegroup.data == 'right_arm_cartesian': print "straight go" msg = EndEffectorCommand() msg.command = 'stop' msg.id = 65537 msg.sender = 'user' msg.args = "{\"grip_attempt_seconds\": 1000}" self.valvePublisher.publish(msg) rospy.loginfo('Left') if movegroup.data == 'left_arm_90_cartesian' or movegroup.data == 'left_arm_90' or movegroup.data == 'right_arm_90_cartesian' or movegroup.data == 'right_arm_90': # NOTE must trigger line low before high to guarantee success msg = EndEffectorCommand() msg.command = 'stop' msg.id = 65537 msg.sender = 'user' self.valvePublisher.publish(msg) print "90 go" msg = EndEffectorCommand() msg.command = 'go' msg.id = 65537 msg.sender = 'user' msg.args = "{\"grip_attempt_seconds\": 1000}" self.valvePublisher.publish(msg)
def deactivatePump(self): print "pump stop" msg = EndEffectorCommand() msg.command = 'stop' msg.id = 65537 msg.sender = 'user' msg.args = "{\"grip_attempt_seconds\": 1000}" self.pumpPublisher.publish(msg)
def activatePump(self): # NOTE Need to trigger line low before high self.deactivatePump() msg = EndEffectorCommand() msg.command = 'go' msg.id = 65537 msg.sender = 'user' msg.args = "{\"grip_attempt_seconds\": 1000}" self.pumpPublisher.publish(msg)
def command(self, cmd, block=False, test=lambda: True, timeout=0.0, args=None): """ Raw command call to directly control gripper. @type cmd: str @param cmd: string of known gripper commands @type block: bool @param block: command is blocking or non-blocking [False] @type test: func @param test: test function for command validation @type timeout: float @param timeout: timeout in seconds for command evaluation @type args: dict({str:float}) @param args: dictionary of parameter:value """ ee_cmd = EndEffectorCommand() ee_cmd.id = self.hardware_id() ee_cmd.command = cmd ee_cmd.sender = self._cmd_sender % (cmd, ) ee_cmd.sequence = self._inc_cmd_sequence() ee_cmd.args = '' if args != None: ee_cmd.args = JSONEncoder().encode(args) seq_test = lambda: (self._state.command_sender == ee_cmd.sender and (self._state.command_sequence == ee_cmd.sequence or self._state.command_sequence == 0)) self._cmd_pub.publish(ee_cmd) if block: finish_time = rospy.get_time() + timeout cmd_seq = baxter_dataflow.wait_for( test=seq_test, timeout=timeout, raise_on_error=False, body=lambda: self._cmd_pub.publish(ee_cmd)) if not cmd_seq: seq_msg = (("Timed out on gripper command acknowledgement for" " %s:%s") % (self.name, ee_cmd.command)) rospy.logdebug(seq_msg) time_remain = max(0.5, finish_time - rospy.get_time()) return baxter_dataflow.wait_for( test=test, timeout=time_remain, raise_on_error=False, body=lambda: self._cmd_pub.publish(ee_cmd)) else: return True
def command(self, cmd, block=False, test=lambda: True, timeout=0.0, args=None): """ Raw command call to directly control gripper. @type cmd: str @param cmd: string of known gripper commands @type block: bool @param block: command is blocking or non-blocking [False] @type test: func @param test: test function for command validation @type timeout: float @param timeout: timeout in seconds for command evaluation @type args: dict({str:float}) @param args: dictionary of parameter:value """ ee_cmd = EndEffectorCommand() ee_cmd.id = self.hardware_id() ee_cmd.command = cmd ee_cmd.sender = self._cmd_sender % (cmd,) ee_cmd.sequence = self._inc_cmd_sequence() ee_cmd.args = '' if args != None: ee_cmd.args = JSONEncoder().encode(args) seq_test = lambda: (self._state.command_sender == ee_cmd.sender and (self._state.command_sequence == ee_cmd.sequence or self._state.command_sequence == 0)) self._cmd_pub.publish(ee_cmd) if block: finish_time = rospy.get_time() + timeout cmd_seq = baxter_dataflow.wait_for( test=seq_test, timeout=timeout, raise_on_error=False, body=lambda: self._cmd_pub.publish(ee_cmd) ) if not cmd_seq: seq_msg = (("Timed out on gripper command acknowledgement for" " %s:%s") % (self.name, ee_cmd.command)) rospy.logdebug(seq_msg) time_remain = max(0.5, finish_time - rospy.get_time()) return baxter_dataflow.wait_for( test=test, timeout=time_remain, raise_on_error=False, body=lambda: self._cmd_pub.publish(ee_cmd) ) else: return True
def setObjectWeight(self, kg=0.0): """ NOT used yet as the gripper does not now when it gripped something """ cmd = EndEffectorCommand() cmd.id = self.id cmd.command = "configure" cmd.args = ''.join([ '{ "object": { "name": "' + self.side + '_gripper",', '"inertial": { "mass": { "value": ' + str(kg) + '}, "origin":{ "xyz": [0.0, 0.0, 0.0] },', ' "inertia": { "ixx": 0.0, "ixy": 0.0, "ixz": 0.0, "iyy": 0.0, "iyz": 0.0, "izz": 0.0 } } } }' ]) self.object_weight = kg self.weight_pub.publish(cmd)
def setWeight(self, kg=0.8): cmd = EndEffectorCommand() cmd.id = self.id cmd.command = "configure" cmd.args = ''.join([ '{"urdf":{ "name": "' + self.side + '_gripper","link":[{ "name": "' + self.side + '_hand"', '}', ',{ "name": "' + self.side + '_gripper_base","inertial":{ "mass":{ "value": ' + str(kg) + '}', ',"origin":{"xyz":[0.0, 0.0, 0.08]} }', # ',"visual":{"origin":{"xyz":[0.0, 0.0, 0.0098]},"geometry":{"mesh":{"filename":"package://baxter_tasker/data/universalGripper.DAE","scale":[1,1,1]} } }', '}]}}' ]) # ',"collision":{"origin":{"xyz":[0.0, 0.0, 0.0098],"rpy":[0.0, 0.0, 0.0]},"geometry":{"cylinder":{"radius": 0.5,"length":0.6} } }', # print json.dumps(args) self.gripper_weight = kg self.weight_pub.publish(cmd)