def goto_frame(self):
'''
Plays one single frame
'''
self.set_all_joints_stiff()
pos_msg = JointCommand()
pos_msg.joint_names = []
pos_msg.velocities = [1.0] * 20
pos_msg.positions = []
pos_msg.accelerations = [-1.0] * 20
pos_msg.max_currents = [-1.0] * 20
for k,v in self._workingValues.items():
pos_msg.joint_names.append(k)
pos_msg.positions.append(v)
torque_msg = JointTorque()
torque_msg.joint_names = []
torque_msg.on = []
for k, v in self._checkBoxesPower[self._widget.frameList.currentItem().text()].items():
torque_msg.joint_names.append(k)
torque_msg.on.append(v)
self.effort_pub.publish(torque_msg)
self._joint_pub.publish(pos_msg)
def cb(msg):
msg2 = JointCommand()
msg2.joint_names = msg.position.joint_names
msg2.positions = msg.position.points[0].positions
msg2.velocities = [-1] * len(msg2.joint_names)
msg2.accelerations = [-1] * len(msg2.joint_names)
msg2.max_currents = [-1] * len(msg2.joint_names)
pub.publish(msg2)
def animation_callback(self, msg):
""" The animation server is sending us goal positions for the next keyframe"""
self.blackboard.last_animation_goal_time = msg.header.stamp.to_sec()
if msg.request:
rospy.loginfo(
"Got Animation request. HCM will try to get controllable now.")
# animation has to wait
# state machine should try to become controllable
self.blackboard.animation_requested = True
return
if msg.first:
if msg.hcm:
# comming from ourselves
# we don't have to do anything, since we must be in the right state
pass
else:
# comming from outside
# check if we can run an animation now
if self.blackboard.current_state != STATE_CONTROLABLE:
rospy.logwarn(
"HCM is not controllable, animation refused.")
return
else:
# we're already controllable, go to animation running
self.blackboard.external_animation_running = True
if msg.last:
if msg.hcm:
# This was an animation from the DSD
self.blackboard.hcm_animation_finished = True
pass
else:
# this is the last frame, we want to tell the DSD, that we're finished with the animations
self.blackboard.external_animation_running = False
if msg.position is None:
# probably this was just to tell us we're finished
# we don't need to set another position to the motors
return
# forward positions to motors, if some where transmitted
if len(msg.position.points) > 0:
out_msg = JointCommand()
out_msg.positions = msg.position.points[0].positions
out_msg.joint_names = msg.position.joint_names
out_msg.accelerations = [-1] * len(out_msg.joint_names)
out_msg.velocities = [-1] * len(out_msg.joint_names)
out_msg.max_currents = [-1] * len(out_msg.joint_names)
if self.blackboard.shut_down_request:
# there are sometimes transmittions errors during shutdown due to race conditions
# there is nothing we can do so just ignore the errors in this case
try:
self.joint_goal_publisher.publish(out_msg)
except:
pass
else:
self.joint_goal_publisher.publish(out_msg)
def box_ticked(self):
'''
Controls whether a checkbox has been clicked, and reacts.
'''
for k, v in self._treeItems.items():
self._motorSwitched[k] = (v.checkState(0) == 2)
for k, v in self._motorSwitched.items():
self._textFields[k].setEnabled(v)
self._sliders[k].setEnabled(v)
self.set_sliders_and_text_fields(manual=False)
## Wolfgang part
torque_msg = JointTorque()
torque_msg.joint_names = []
torque_msg.on = []
if self._widget.treeModeSelector.currentIndex() == 0:
for k, v in sorted(self._treeItems.items()):
torque_msg.joint_names.append(k)
torque_msg.on.append(v.checkState(0) == 2)
pos_msg = JointCommand()
pos_msg.joint_names = []
pos_msg.velocities = [1.0] * 20
pos_msg.positions = []
pos_msg.accelerations = [-1.0] * 20
pos_msg.max_currents = [-1.0] * 20
for k, v in self._workingValues.items():
pos_msg.joint_names.append(k)
pos_msg.positions.append(v)
self._joint_pub.publish(pos_msg)
self.effort_pub.publish(torque_msg)
if not self._widget.frameList.currentItem() == None:
if not self._widget.frameList.currentItem().text(
) == "#CURRENT_FRAME":
self.treeModeChanged(
self._widget.treeModeSelector.currentIndex())
self.record(keep=True)
def goto_init(self):
'''
Plays init frame, sets all motor values to 0
'''
self.set_all_joints_stiff()
if self._widget.frameList.currentItem().text() == "#CURRENT_FRAME":
for k, v in self._workingValues.iteritems():
self._workingValues[k] = 0.0
for k, v in self._currentGoals.iteritems():
self._currentGoals[k] = 0.0
self.set_sliders_and_text_fields(manual=False)
pos_msg = JointCommand()
pos_msg.joint_names = self._initial_joints.name
pos_msg.velocities = [1.0] * len(self._initial_joints.name)
pos_msg.positions = [0.0] * len(self._initial_joints.name)
pos_msg.accelerations = [-1.0] * len(self._initial_joints.name)
pos_msg.max_currents = [-1.0] * len(self._initial_joints.name)
self._joint_pub.publish(pos_msg)
tf_buffer = tf2.Buffer(rospy.Duration(5))
tf_listener = tf2.TransformListener(tf_buffer)
request = IKRequest()
request.group_name = "Head"
request.timeout.secs = 1
request.approximate = True
request.look_at_goals.append(LookAtGoal())
request.look_at_goals[0].link_name = "camera"
request.look_at_goals[0].weight = 1
request.look_at_goals[0].axis.x = 1
pos_msg = JointCommand()
pos_msg.joint_names = ["HeadPan", "HeadTilt"]
pos_msg.positions = [0, 0]
pos_msg.velocities = [1.5, 1.5]
pos_msg.accelerations = [-1, -1]
pos_msg.max_currents = [-1, -1]
rospy.wait_for_service("/bio_ik/get_bio_ik")
bio_ik = rospy.ServiceProxy('/bio_ik/get_bio_ik', GetIK)
publish_motor_goals = rospy.Publisher('/head_motor_goals',
JointCommand,
queue_size=10)
while not rospy.is_shutdown():
x = float(input('x: '))
y = float(input('y: '))
point = PointStamped()
point.header.stamp = rospy.Time.now()
th = 0
status = 0
# Head Part
rospy.Subscriber("joint_states", JointState, joint_state_cb, queue_size=1)
if rospy.get_param("sim_active", default=False):
head_pub = rospy.Publisher("head_motor_goals",
JointCommand,
queue_size=1)
else:
head_pub = rospy.Publisher("DynamixelController/command",
JointCommand,
queue_size=1)
head_msg = JointCommand()
head_msg.max_currents = [-1] * 2
head_msg.velocities = [5] * 2
head_msg.accelerations = [40] * 2
head_msg.joint_names = ['HeadPan', 'HeadTilt']
head_msg.positions = [0] * 2
head_pan_step = 0.05
head_tilt_step = 0.05
try:
print(msg)
while (1):
key = getKey()
if key in moveBindings.keys():
x += moveBindings[key][0] * x_speed_step
x = round(x, 2)
"LHipRoll": 9,
"LHipPitch": 10,
"LKnee": 11,
"LAnklePitch": 12,
"LAnkleRoll": 13,
"RHipYaw": 14,
"RHipRoll": 15,
"RHipPitch": 16,
"RKnee": 17,
"RAnklePitch": 18,
"RAnkleRoll": 19
}
rospy.init_node("send_joint_command")
pos_msg = JointCommand()
pos_msg.joint_names = ids.keys()
#pos_msg.joint_names = ["RShoulderRoll"]
pos_msg.velocities = [-1.0] * 20
pos_msg.positions = [0.0] * 20
pos_msg.accelerations = [-1.0] * 20
pos_msg.max_currents = [-1.0] * 20
pub = rospy.Publisher("/DynamixelController/command",
JointCommand,
queue_size=1)
print(pos_msg)
while not rospy.is_shutdown():
pos_msg.header.stamp = rospy.Time.now()
pub.publish(pos_msg)
rospy.sleep(0.1)