def update_gripper_position(self, data):
u"""Открывает/закрывает гриппер."""
self.gripper_position.positions = []
# Open gripper
if data.buttons[6]:
tmp_gripper_position_r = JointValue()
tmp_gripper_position_r.joint_uri = 'gripper_finger_joint_r'
tmp_gripper_position_r.value = 0.011499
tmp_gripper_position_r.unit = 'm'
tmp_gripper_position_r.timeStamp = rospy.Time.now()
self.gripper_position.positions.append(tmp_gripper_position_r)
tmp_gripper_position_l = JointValue()
tmp_gripper_position_l.joint_uri = 'gripper_finger_joint_l'
tmp_gripper_position_l.value = 0.011499
tmp_gripper_position_l.unit = 'm'
tmp_gripper_position_l.timeStamp = rospy.Time.now()
self.gripper_position.positions.append(tmp_gripper_position_l)
# Close gripper
if data.buttons[7]:
tmp_gripper_position_r = JointValue()
tmp_gripper_position_r.joint_uri = 'gripper_finger_joint_r'
tmp_gripper_position_r.value = 0
tmp_gripper_position_r.unit = 'm'
tmp_gripper_position_r.timeStamp = rospy.Time.now()
self.gripper_position.positions.append(tmp_gripper_position_r)
tmp_gripper_position_l = JointValue()
tmp_gripper_position_l.joint_uri = 'gripper_finger_joint_l'
tmp_gripper_position_l.value = 0
tmp_gripper_position_l.unit = 'm'
tmp_gripper_position_l.timeStamp = rospy.Time.now()
self.gripper_position.positions.append(tmp_gripper_position_l)
def update_gripper_position(self, data):
u"""Открывает/закрывает гриппер."""
self.gripper_position.positions = []
# Open gripper
if data.buttons[6]:
tmp_gripper_position_r = JointValue()
tmp_gripper_position_r.joint_uri = 'gripper_finger_joint_r'
tmp_gripper_position_r.value = 0.011499
tmp_gripper_position_r.unit = 'm'
tmp_gripper_position_r.timeStamp = rospy.Time.now()
self.gripper_position.positions.append(tmp_gripper_position_r)
tmp_gripper_position_l = JointValue()
tmp_gripper_position_l.joint_uri = 'gripper_finger_joint_l'
tmp_gripper_position_l.value = 0.011499
tmp_gripper_position_l.unit = 'm'
tmp_gripper_position_l.timeStamp = rospy.Time.now()
self.gripper_position.positions.append(tmp_gripper_position_l)
# Close gripper
if data.buttons[7]:
tmp_gripper_position_r = JointValue()
tmp_gripper_position_r.joint_uri = 'gripper_finger_joint_r'
tmp_gripper_position_r.value = 0
tmp_gripper_position_r.unit = 'm'
tmp_gripper_position_r.timeStamp = rospy.Time.now()
self.gripper_position.positions.append(tmp_gripper_position_r)
tmp_gripper_position_l = JointValue()
tmp_gripper_position_l.joint_uri = 'gripper_finger_joint_l'
tmp_gripper_position_l.value = 0
tmp_gripper_position_l.unit = 'm'
tmp_gripper_position_l.timeStamp = rospy.Time.now()
self.gripper_position.positions.append(tmp_gripper_position_l)
def create_null_velocity(unit):
msg = JointVelocities()
time = rospy.Time.now()
for joint_name in joint_names:
j = JointValue()
j.timeStamp = time
j.joint_uri = joint_name
j.unit = unit
msg.velocities.append(j)
return msg
def moveGripper(self, gPublisher, floatVal):
jp = JointPositions()
myPoison = Poison()
myPoison.originator = 'yb_grip'
myPoison.description = 'whoknows'
myPoison.qos = 0.0
jp.poisonStamp = myPoison
nowTime = rospy.Time.now()
jvl = JointValue()
jvl.timeStamp = nowTime
jvl.joint_uri = 'gripper_finger_joint_l'
jvl.unit = 'm'
jvl.value = floatVal
jp.positions.append(jvl)
jvr = JointValue()
jvr.timeStamp = nowTime
jvr.joint_uri = 'gripper_finger_joint_r'
jvr.unit = 'm'
jvr.value = floatVal
jp.positions.append(jvr)
gPublisher.publish(jp)
def set_gripper_state(self, open_gripper=True):
"""Open/close gripper.
Arguments:
open_gripper (bool): if True - opens gripper, if False - otherwise
"""
self.gripper_position.positions = []
if open_gripper:
# Open gripper
tmp_gripper_position_r = JointValue()
tmp_gripper_position_r.joint_uri = 'gripper_finger_joint_r'
tmp_gripper_position_r.value = 0.011499
tmp_gripper_position_r.unit = 'm'
tmp_gripper_position_r.timeStamp = rospy.Time.now()
self.gripper_position.positions.append(tmp_gripper_position_r)
tmp_gripper_position_l = JointValue()
tmp_gripper_position_l.joint_uri = 'gripper_finger_joint_l'
tmp_gripper_position_l.value = 0.011499
tmp_gripper_position_l.unit = 'm'
tmp_gripper_position_l.timeStamp = rospy.Time.now()
self.gripper_position.positions.append(tmp_gripper_position_l)
else:
# Close gripper
tmp_gripper_position_r = JointValue()
tmp_gripper_position_r.joint_uri = 'gripper_finger_joint_r'
tmp_gripper_position_r.value = 0
tmp_gripper_position_r.unit = 'm'
tmp_gripper_position_r.timeStamp = rospy.Time.now()
self.gripper_position.positions.append(tmp_gripper_position_r)
tmp_gripper_position_l = JointValue()
tmp_gripper_position_l.joint_uri = 'gripper_finger_joint_l'
tmp_gripper_position_l.value = 0
tmp_gripper_position_l.unit = 'm'
tmp_gripper_position_l.timeStamp = rospy.Time.now()
self.gripper_position.positions.append(tmp_gripper_position_l)
self.gripper_position_publisher.publish(self.gripper_position)
def set_joints_velocities(self, *args): # TODO: remove *args
u"""Set velocity for each joint.
Arguments:
*args -- velocity for each joint (j1, j2, j3, j4, j5)
Устанавливает скорость каждой степени подвижности в радианах/с.
Аргументы:
*args -- скорости соотвествующих степеней (j1, j2, j3, j4, j5)
"""
assert len(args) == 5
self.joints_velocities.velocities = []
for index, value in enumerate(args):
tmp = JointValue()
tmp.timeStamp = rospy.Time.now()
tmp.joint_uri = 'arm_joint_{}'.format(index + 1)
tmp.unit = 's^-1 rad'
tmp.value = value
self.joints_velocities.velocities.append(tmp)
self.joints_velocities_publisher.publish(self.joints_velocities)
def set_joints_angles(self, *args): # TODO: remove *args
u"""Set arm joints to defined angles in radians.
Arguments:
*args -- joints angles (j1, j2, j3, j4, j5)
Устанавливает углы поворота степеней подвижности в радианах.
Аргументы:
*args -- уголы соотвествующих степеней (j1, j2, j3, j4, j5)
"""
assert len(args) <= 5
self.joints_positions.positions = []
for i in range(5):
tmp = JointValue()
tmp.timeStamp = rospy.Time.now()
tmp.joint_uri = 'arm_joint_{}'.format(i + 1)
tmp.unit = 'rad'
tmp.value = args[i]
self.joints_positions.positions.append(tmp)
self.joints_positions_publisher.publish(self.joints_positions)
from brics_actuator.msg import JointPositions, JointValue
from cob_srvs.srv import Trigger
if __name__ == "__main__":
rospy.init_node('gripper_open_node')
gripper_pos_pub = rospy.Publisher('/PG70_controller/command_pos', JointPositions)
gripper_recover_srv = rospy.ServiceProxy('/PG70_controller/recover', Trigger)
# recover
gripper_recover_srv()
rospy.sleep(4.0)
# open the gripper
joint_pos_msg = JointPositions()
joint_val = JointValue()
joint_val.timeStamp = rospy.Time.now()
joint_val.joint_uri = "left_arm_top_finger_joint"
joint_val.unit = "mm"
joint_val.value = 16
joint_pos_msg.positions.append(joint_val)
gripper_pos_pub.publish(joint_pos_msg)
rospy.sleep(4.0)
joint_pos_msg.positions[0].value = 4
gripper_pos_pub.publish(joint_pos_msg)
from cob_srvs.srv import Trigger
from brics_actuator.msg import JointVelocities
from brics_actuator.msg import JointValue
rospy.init_node("ipa_canopen_test")
rospy.wait_for_service('/tray_controller/init')
print "found init"
initService = rospy.ServiceProxy('/tray_controller/init', Trigger)
resp = initService()
velPublisher = rospy.Publisher('/tray_controller/command_vel', JointVelocities)
rospy.sleep(2.0)
v = JointVelocities()
vv1 = JointValue()
vv1.timeStamp = rospy.Time.now()
vv1.joint_uri = "tray_1_joint"
vv2 = copy.deepcopy(vv1)
vv2.joint_uri = "tray_2_joint"
vv3 = copy.deepcopy(vv1)
vv3.joint_uri = "tray_3_joint"
v.velocities = [vv1,vv2,vv3]
v.velocities[0].value = 0.0
while not rospy.is_shutdown():
v.velocities[0].value = 0.05
v.velocities[1].value = 0.05
velPublisher.publish(v)
rospy.sleep(1.0)
from brics_actuator.msg import JointVelocities
from brics_actuator.msg import JointValue
rospy.init_node("ipa_canopen_test")
rospy.wait_for_service('/mockarm_controller/init')
initService = rospy.ServiceProxy('/mockarm_controller/init', Trigger)
resp = initService()
print resp
velPublisher = rospy.Publisher('/mockarm_controller/command_vel', JointVelocities)
rospy.sleep(2.0)
v = JointVelocities()
vv = JointValue()
vv.timeStamp = rospy.Time.now()
vv.joint_uri = "mockarm_1_joint"
v.velocities = [vv]
while not rospy.is_shutdown():
v.velocities[0].value = 0.2
velPublisher.publish(v)
rospy.sleep(1.0)
v.velocities[0].value = 0
velPublisher.publish(v)
rospy.sleep(1.0)
v.velocities[0].value = - 0.2
if __name__=='__main__':
rospy.init_node('test_gripper_vel_control')
gripper_pos_pub = rospy.Publisher('/PG70_controller/command_pos', JointPositions)
gripper_vel_pub = rospy.Publisher('/PG70_controller/command_vel', JointVelocities)
rospy.sleep(1.0)
# set joint position
gripper_pos_msg = JointPositions()
gripper_pos = JointValue()
gripper_pos.timeStamp = rospy.Time.now()
gripper_pos.joint_uri = 'left_arm_top_finger_joint'
gripper_pos.unit = 'm'
gripper_pos.value = 0.025
gripper_pos_msg.positions = [gripper_pos]
gripper_pos_pub.publish(gripper_pos_msg)
rospy.sleep(5.0)
# send joint velocities
gripper_vel_msg = JointVelocities()
gripper_vel = JointValue()
gripper_vel.timeStamp = rospy.Time.now()