def reset_arm_rnd(self, arm, mode, data):
"""
Issues a position command to an arm.
Args:
arm: Either TRIAL_ARM or AUXILIARY_ARM.
mode: An integer code (defined in gps_pb2).
data: An array of floats.
"""
reset_command = PositionCommand()
reset_command.mode = mode
# Reset to uniform random state
# Joints 1, 4, 5 and 6 have a range of -10,000 to +10,000 degrees. Joint 2 has a range of +42 to +318 degrees. Joint 3 has a range of +17 to +343 degrees. (see http://wiki.ros.org/jaco)
reset_command.data = [
(random() * 2 - 1) * pi,
pi + (random() * 2 - 1) * pi / 2,
# Limit elbow joints to 180 +/-90 degrees to prevent getting stuck in the ground
pi + (random() * 2 - 1) * pi / 2,
(random() * 2 - 1) * pi,
(random() * 2 - 1) * pi,
(random() * 2 - 1) * pi
]
reset_command.pd_gains = self._hyperparams['pid_params']
reset_command.arm = arm
timeout = self._hyperparams['trial_timeout']
reset_command.id = self._get_next_seq_id()
try:
self._reset_service.publish_and_wait(reset_command,
timeout=timeout)
except TimeoutException:
self.relax_arm(arm)
wait = raw_input(
'The robot arm seems to be stuck. Unstuck it and press <ENTER> to continue.'
)
self.reset_arm(arm, mode, data)
def main():
rospy.init_node('issue_com')
pub = rospy.Publisher(POS_COM_TOPIC, PositionCommand, queue_size=10)
test_pub = rospy.Publisher(TEST_TOPIC, Empty, queue_size=10)
sub = rospy.Subscriber(POS_COM_TOPIC, PositionCommand, listen)
#sub = rospy.Subscriber('/joint_states', JointState, listen)
pc = PositionCommand()
pc.mode = JOINT_SPACE
#pc.arm = PositionCommand.LEFT_ARM
pc.arm = 1#PositionCommand.RIGHT_ARM
pc.data = np.zeros(7)
r = rospy.Rate(1)
#while not rospy.is_shutdown():
# pub.publish(pc)
# r.sleep()
# print 'published!'
r.sleep()
test_pub.publish(Empty())
pub.publish(pc)
def reset_arm(self, arm, mode, data):
"""
Issues a position command to an arm.
Args:
arm: Either TRIAL_ARM or AUXILIARY_ARM.
mode: An integer code (defined in gps_pb2).
data: An array of floats.
"""
reset_command = PositionCommand()
reset_command.mode = mode
reset_command.data = data
reset_command.pd_gains = self._hyperparams['pid_params']
reset_command.arm = arm
timeout = self._hyperparams['trial_timeout']
reset_command.id = self._get_next_seq_id()
self._reset_service.publish_and_wait(reset_command, timeout=timeout)
def reset_arm(self, arm, mode, data):
"""
Issues a position command to an arm.
Args:
arm: Either TRIAL_ARM or AUXILIARY_ARM.
mode: An integer code (defined in gps_pb2).
data: An array of floats.
"""
reset_command = PositionCommand()
reset_command.mode = mode
reset_command.data = data
reset_command.pd_gains = self._hyperparams['pid_params']
reset_command.arm = arm
timeout = self._hyperparams['trial_timeout']
reset_command.id = self._get_next_seq_id()
self._reset_service.publish_and_wait(reset_command, timeout=timeout)
def reset_arm(self, arm, mode, data):
"""
Issues a position command to an arm.
Args:
arm: Either TRIAL_ARM or AUXILIARY_ARM.
mode: An integer code (defined in gps_pb2).
data: An array of floats.
"""
reset_command = PositionCommand()
reset_command.mode = mode
reset_command.data = data
reset_command.pd_gains = self._hyperparams['pid_params']
reset_command.arm = arm
timeout = self._hyperparams['trial_timeout']
reset_command.id = self._get_next_seq_id()
try:
self._reset_service.publish_and_wait(reset_command,
timeout=timeout)
except TimeoutException:
self.relax_arm(arm)
wait = raw_input(
'The robot arm seems to be stuck. Unstuck it and press <ENTER> to continue.'
)
self.reset_arm(arm, mode, data)
#!/usr/bin/env python
import rospy
import roslib
roslib.load_manifest('gps_agent_pkg')
from gps_agent_pkg.msg import PositionCommand
from gps_agent_pkg.msg import RelaxCommand
pos_pub = rospy.Publisher('/gps_controller_position_command', PositionCommand)
relax_pub = rospy.Publisher('/gps_controller_relax_command', RelaxCommand)
rospy.init_node('controller_pub_node')
r = rospy.Rate(10) # 10hz
count = 0
while not rospy.is_shutdown():
if (count % 1000 < 250):
new_msg = PositionCommand(mode=1,
arm=1,
data=[-0.75, -0.5, -0.5, 0.5, 0.5, 0.5, 0.5])
pos_pub.publish(new_msg)
elif (count % 1000 < 500):
new_msg = PositionCommand(mode=1,
arm=1,
data=[0.75, -0.5, -0.5, 0.5, 0.5, 0.5, 0.5])
pos_pub.publish(new_msg)
else:
new_msg_relax = RelaxCommand(arm=0)
relax_pub.publish(new_msg_relax)
new_msg_relax = RelaxCommand(arm=1)
relax_pub.publish(new_msg_relax)
count += 1
r.sleep()