def process_neck_command(self, binding, ch):
msg = NeckTrajectoryRosMessage()
msg.unique_id = -1
self._update_joint_values('neck', binding, ch)
self._append_trajectory_point_1d(
msg, 1.0, self.joint_values['neck'])
self.neck_publisher.publish(msg)
def sendTrajectory(self,joint_waypoints):
msg = NeckTrajectoryRosMessage()
msg.unique_id = -1
# for each set of joint states
for y in joint_waypoints:
# first value is time duration
time = float(y[0])
# subsequent values are desired joint positions
commandPosition = array([ float(x) for x in y[1].split() ])
msg = appendTrajectoryPoint(msg, time, commandPosition)
rospy.loginfo('publishing neck trajectory')
neckTrajectoryPublisher.publish(msg)
def process_key(self, ch, data):
ret = None
"""Process key event."""
if ch.lower() in self.WALKING_BINDINGS:
self.process_walking_command(self.WALKING_BINDINGS[ch.lower()], ch)
return ret
if ch.lower(
) == 'rst': #all walker cmds = rst-reset,wlk,sup-stepup,tlt-torso,nck-[3],abt
firstFoot = self.createFoot(Foot.RIGHT)
print("firstFoot", firstFoot)
standingFoot = self.createFoot(Foot.LEFT)
print("standFoot", standingFoot)
self.list = Steplist(firstFoot, standingFoot)
if data.R > 2.0:
foot = self.list.resetfeet()
ret = self.process_foot(foot)
return ret
if ch.lower(
) == 'wlk': #all walker cmds = rst-reset,wlk,sup-stepup,tlt-torso,nck-[3],abt
return self.process_something(data)
if ch.lower() == 'sup': #TODO sup-stepup,tlt-torso,nck-[3],abt
return ret
if ch.lower() == 'tlt': #TODO tlt-torso'
chestMsg = self.createChestCmd([0.0, data.R, 0.0])
self.chestTrajectoryPublisher.publish(chestMsg)
return ret
if ch.lower() == 'nck': #TODO nck-[3]
print("nck: ", data.R)
msg = NeckTrajectoryRosMessage()
msg.unique_id = -1
self._append_trajectory_point_1d(msg, 1.0, data.R)
self.neck_publisher.publish(msg)
return ret
if ch.lower() == 'abt':
msg = AbortWalkingRosMessage()
msg.unique_id = -1
self.abort_walking_publisher.publish(msg)
return 'walking aborted'
def sendNeckTrajectory():
global uid
msg = NeckTrajectoryRosMessage()
msg = appendTrajectoryPoint(msg, 1.0, NECK1)
time.sleep(2)
#msg = appendTrajectoryPoint(msg, 0.5, ARM_UP2)
#time.sleep(2)
#msg = appendTrajectoryPoint(msg, 0.5, ARM_UP2)
#time.sleep(2)
#msg = appendTrajectoryPoint(msg, 1.0, PUNCH)
msg.unique_id = time.time()
rospy.loginfo('publishing neck trajectory')
neckTrajectoryPublisher.publish(msg)
def neck_control(self, positions, wait=True):
""" Control all 3 points of the head:
lowerNeckPitch - 0 to 0.5 - looks down
neckYaw - -1.0 to 1.0 - looks left and right
upperNeckPitch - -0.5 to 0.0 - looks up
"""
trajectories = []
for p in positions:
trajectories.append(self.zarj.make_joint(self.MOVE_TIME, p))
msg = NeckTrajectoryRosMessage()
msg.joint_trajectory_messages = trajectories
msg.unique_id = self.zarj.uid
self.neck_publisher.publish(msg)
if wait:
rospy.sleep(self.MOVE_TIME + 0.1)
def sendNeck(self):
msg = NeckTrajectoryRosMessage()
msg.unique_id = -1
msg.joint_trajectory_messages = []
point0 = TrajectoryPoint1DRosMessage()
point0.time = 1.0
point0.position = -0.3
point0.velocity = 0
point1 = TrajectoryPoint1DRosMessage()
point1.time = 1.0
point1.position = -0.3
point1.velocity = 0
point2 = TrajectoryPoint1DRosMessage()
point2.time = 1.0
point2.position = -0.3
point2.velocity = 0
msg.joint_trajectory_messages.trajectory_points.append([point0,point1,point2])
self.neck_publisher.publish(msg)
def adjustNeck(position, time):
testmsg = NeckTrajectoryRosMessage()
testmsg.unique_id = -1
testmsg = appendTrajectoryPoint(testmsg, time, position)
neck_publisher.publish(testmsg)