Skip to content

Instantly share code, notes, and snippets.

Embed
What would you like to do?
#!/usr/bin/env python
import time
import rospy
from hrpsys_ros_bridge.srv import *
import math
from uarm.msg import Joints
from std_msgs.msg import Bool
class MoveNextage:
def __init__(self):
rospy.loginfo('start wait')
rospy.wait_for_service('/SequencePlayerServiceROSBridge/setTargetPose')
self._set_target_pose = rospy.ServiceProxy(
'/SequencePlayerServiceROSBridge/setTargetPose',
OpenHRP_SequencePlayerService_setTargetPose)
rospy.wait_for_service(
'/SequencePlayerServiceROSBridge/waitInterpolation')
self._wait_interpolation = rospy.ServiceProxy(
'/SequencePlayerServiceROSBridge/waitInterpolation',
OpenHRP_SequencePlayerService_waitInterpolation)
rospy.loginfo('end wait')
def move_to(self, arm, position, time):
result = self._set_target_pose(
arm, position, [3.0, -math.pi / 2, -3.0], time)
print result
self._wait_interpolation()
def pick(self):
self.move_to('rarm', [0.4, -0.3, 1.3], 3.0)
self.move_to('rarm', [0.4, -0.3, 1.0], 3.0)
self.hold(True)
time.sleep(3.0)
self.move_to('rarm', [0.4, -0.3, 1.3], 1.5)
self.move_to('rarm', [0.4, -0.1, 1.3], 1.5)
self.move_to('rarm', [0.4, -0.1, 1.0], 1.5)
self.hold(False)
time.sleep(3.0)
self.move_to('rarm', [0.4, -0.1, 1.3], 1.5)
def hold(self, is_hold):
# TODO
pass
class MoveUarm(object):
def __init__(self):
self._pub = rospy.Publisher('/uarm/joint_commands', Joints)
self._gripper_pub = rospy.Publisher('/uarm/gripper', Bool)
def move(self, a, b, c, d):
msg = Joints(a, b, c, d)
self._pub.publish(msg)
time.sleep(1.0)
def pick(self):
self.move(30.0, 30.0, -45.0, 0.0)
self.move(180, 30, -45, 0)
self.hold(True)
self.move(180, -50, -45, 0)
self.move(180, 50, -45, 0)
self.move(180, 50, 45, 0)
self.hold(False)
self.move(30, 30, 0, 0)
def hold(self, is_hold):
msg = Bool(is_hold)
self._gripper_pub.publish(msg)
if __name__ == '__main__':
rospy.init_node('move_nextage_otl')
uarm = MoveUarm()
uarm.pick()
nextage = MoveNextage()
nextage.pick()
rospy.spin()
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
You can’t perform that action at this time.