def __init__(self, pose, time = 0.50, reverse=False): Task.__init__(self) self.pose = pose self.time = time self.reverse = reverse walk_request.noWalk() kick_request.setNoKick()
def __init__(self): Task.__init__(self) kick_request.setNoKick() walk_request.noWalk() kick_request.kick_running_ = False behavior_mem.keeperDiving = core.Dive.NONE self.state = state_machine.SimpleStateMachine('stop', 'checkarms', 'movearms', 'sit', 'relaxknee', 'relaxbody', 'finish') self.skippedState = False self.lower_time = 0
def run(self): finished = walk_response.finished_standing_ received = walk_response.received_ kick_request.setNoKick() walk_request.standStraight() if received and self.state.inState('requested'): self.state.transition('running') elif self.state.inState('running') and finished: self.finish() if self.getTime() > 2: self.finish()
def run(self): for i in range(2, core.NUM_JOINTS): val = util.getPoseJoint(i, self.pose, self.reverse) if val != None: joint_commands.setJointCommand(i, val * core.DEG_T_RAD) joint_commands.send_body_angles_ = True joint_commands.body_angle_time = self.time * 1000.0 walk_request.noWalk() kick_request.setNoKick() if self.getTime() > self.time: self.finish()
def run(self): if self.first: for i in range(2, core.NUM_JOINTS): val = util.getPoseJoint(i, self.pose, self.reverse) if val != None: joint_commands.setJointCommand(i, val * core.DEG_T_RAD) joint_commands.send_body_angles_ = True joint_commands.body_angle_time_ = self.time * self.interpolate #1000.0 walk_request.noWalk() kick_request.setNoKick() self.first = False if self.getTime() > self.time: self.finish()
def run(self): if self.getTime() < 2.0: walk_request.noWalk() kick_request.setNoKick() commands.setStiffness(cfgstiff.One, 0.3) return st = self.state if st.inState(st.stop): st.transition(st.checkarms) if st.inState(st.checkarms): shoulderCutoff = core.DEG_T_RAD * -90 lpitch = core.joint_values[core.LShoulderPitch] rpitch = core.joint_values[core.RShoulderPitch] if lpitch > shoulderCutoff and rpitch > shoulderCutoff: st.transition(st.sit) else: st.transition(st.movearms) elif st.inState(st.movearms): pose = util.deepcopy(cfgpose.sittingPoseV3) for joint, val in cfgpose.armSidePose.items(): pose[joint] = val st.transition(st.sit) return ToPoseMoveHead(tilt=0.0, pose=pose) elif st.inState(st.sit): self.skippedState = False st.transition(st.relaxknee) return ToPoseMoveHead(pose=cfgpose.sittingPoseV3, time=1.0) elif st.inState(st.relaxknee): self.lower_time = self.getTime() commands.setStiffness(cfgstiff.ZeroKneeAnklePitch, 0.3) st.transition(st.relaxbody) elif st.inState(st.relaxbody) and st.timeSinceTransition() > 0.7: commands.setStiffness(cfgstiff.Zero, 0.3) st.transition(st.finish) elif st.inState(st.finish): self.finish()
def run(self): if self.getTime() < 2.0: walk_request.noWalk() kick_request.setNoKick() commands.setStiffness(cfgstiff.One, 0.3) return st = self.state if st.inState(st.stop): st.transition(st.checkarms) if st.inState(st.checkarms): shoulderCutoff = core.DEG_T_RAD * -90 lpitch = core.joint_values[core.LShoulderPitch] rpitch = core.joint_values[core.RShoulderPitch] if lpitch > shoulderCutoff and rpitch > shoulderCutoff: st.transition(st.sit) else: st.transition(st.movearms) elif st.inState(st.movearms): pose = util.deepcopy(cfgpose.sittingPoseV3) for joint, val in cfgpose.armSidePose.items(): pose[joint] = val st.transition(st.sit) return ToPoseMoveHead(tilt = 0.0, pose = pose) elif st.inState(st.sit): self.skippedState = False st.transition(st.relaxknee) return ToPoseMoveHead(pose = cfgpose.sittingPoseV3) elif st.inState(st.relaxknee): self.lower_time = self.getTime() commands.setStiffness(cfgstiff.ZeroKneeAnklePitch, 0.3) st.transition(st.relaxbody) elif st.inState(st.relaxbody) and st.timeSinceTransition() > 0.7: commands.setStiffness(cfgstiff.Zero, 0.3) st.transition(st.finish) elif st.inState(st.finish): self.finish()
def run(self): global start_frame, droll_off, dpitch_off, dlarmroll_off, dlarmpitch_off, drarmroll_off, drarmpitch_off if(start_frame == -1): start_frame = vision_frame_info.frame_id traj_idx = vision_frame_info.frame_id - start_frame if(traj_idx >= len(trajectory)): start_frame = -1 self.finish() print "done!" return # traj_idx = len(trajectory) - 1 # print "Kick is " + str(float(traj_idx)/float(len(trajectory)-1.)*100.) + " percent complete" inverted_joints = [1, -1, 1, -1, 1, 1, 1, -1, 1, 1, 1, 1, 1, 1, -1, 1, 1, 1, -1, -1, -1, -1] # inverted_joints[core.LShoulderPitch] = -1. # inverted_joints[core.RShoulderPitch] = -1. # inverted_joints[core.RShoulderRoll] = -1. # inverted_joints[core.RElbowRoll] = -1. pose = trajectory[traj_idx] for i in range(2, core.NUM_JOINTS): joint_commands.setJointCommand(i, pose[i]*inverted_joints[i]) # tweak left ankle commands to handle cop dynamics fl = sensors.getValue(core.fsrLFL) fr = sensors.getValue(core.fsrLFR) rl = sensors.getValue(core.fsrLRL) rr = sensors.getValue(core.fsrLRR) dx = (fl + fr)/2. - (rl + rr)/2. + 0. dy = (fl + rl)/2. - (fr + rr)/2. - 0.05 # print "dx: " + str(dx) # print "dy: " + str(dy) clamp = 0.2 # droll_off += -dy * 0.001 # dpitch_off += -dx * 0.0 dlarmroll_off = numpy.clip(dlarmroll_off - dy * 0.0025, -clamp, clamp) # dlarmpitch_off += dx * 0.0 drarmroll_off = numpy.clip(drarmroll_off - dy * 0.0025, -clamp, clamp) # drarmpitch_off += dx * 0.0 droll = dy * 0.01 + droll_off dpitch = -dx * 0.0 + dpitch_off dlarmroll = -dy * 0.6 + dlarmroll_off dlarmpitch = dx * 0.0 + dlarmpitch_off dlhiproll = -dy * 0.025 drarmroll = -dy * 0.6 + drarmroll_off drarmpitch = dx * 0.0 + drarmpitch_off # print "droll: " + str(droll) # print "dpitch: " + str(dpitch) # print "dlarmroll: " + str(dlarmroll) # print "dlarmpitch: " + str(dlarmpitch) # print "drarmroll: " + str(drarmroll) # print "drarmpitch: " + str(drarmpitch) # joint_commands.setJointCommand(core.LAnklePitch, (pose[core.LAnklePitch] + dpitch)*inverted_joints[core.LAnklePitch]) # joint_commands.setJointCommand(core.LAnkleRoll, (pose[core.LAnkleRoll] + droll)*inverted_joints[core.LAnkleRoll]) # joint_commands.setJointCommand(core.LShoulderPitch, (pose[core.LShoulderPitch] + dlarmpitch)*inverted_joints[core.LShoulderPitch]) # joint_commands.setJointCommand(core.LShoulderRoll, numpy.clip(pose[core.LShoulderRoll] + dlarmroll, 0.3, 10)*inverted_joints[core.LShoulderRoll]) # joint_commands.setJointCommand(core.LHipRoll, (pose[core.LHipRoll] + dlhiproll)*inverted_joints[core.LHipRoll]) # joint_commands.setJointCommand(core.RShoulderPitch, (pose[core.RShoulderPitch] + drarmpitch)*inverted_joints[core.RShoulderPitch]) # joint_commands.setJointCommand(core.RShoulderRoll, numpy.clip(pose[core.RShoulderRoll] + drarmroll, -0.3, -10)*inverted_joints[core.RShoulderRoll]) #send commands and prevent other controllers from operating joint_commands.send_body_angles_ = True joint_commands.body_angle_time = 1# 1.0/30.0 * 1000.0#todo: figure out what units this is walk_request.noWalk() kick_request.setNoKick()
def run(self): global start_frame, droll_off, dpitch_off, dlarmroll_off, dlarmpitch_off, drarmroll_off, drarmpitch_off if (start_frame == -1): start_frame = vision_frame_info.frame_id traj_idx = vision_frame_info.frame_id - start_frame if (traj_idx >= len(trajectory)): start_frame = -1 self.finish() print "done!" return # traj_idx = len(trajectory) - 1 # print "Kick is " + str(float(traj_idx)/float(len(trajectory)-1.)*100.) + " percent complete" inverted_joints = [ 1, -1, 1, -1, 1, 1, 1, -1, 1, 1, 1, 1, 1, 1, -1, 1, 1, 1, -1, -1, -1, -1 ] # inverted_joints[core.LShoulderPitch] = -1. # inverted_joints[core.RShoulderPitch] = -1. # inverted_joints[core.RShoulderRoll] = -1. # inverted_joints[core.RElbowRoll] = -1. pose = trajectory[traj_idx] for i in range(2, core.NUM_JOINTS): joint_commands.setJointCommand(i, pose[i] * inverted_joints[i]) # tweak left ankle commands to handle cop dynamics fl = sensors.getValue(core.fsrLFL) fr = sensors.getValue(core.fsrLFR) rl = sensors.getValue(core.fsrLRL) rr = sensors.getValue(core.fsrLRR) dx = (fl + fr) / 2. - (rl + rr) / 2. + 0. dy = (fl + rl) / 2. - (fr + rr) / 2. - 0.05 # print "dx: " + str(dx) # print "dy: " + str(dy) clamp = 0.2 # droll_off += -dy * 0.001 # dpitch_off += -dx * 0.0 dlarmroll_off = numpy.clip(dlarmroll_off - dy * 0.0025, -clamp, clamp) # dlarmpitch_off += dx * 0.0 drarmroll_off = numpy.clip(drarmroll_off - dy * 0.0025, -clamp, clamp) # drarmpitch_off += dx * 0.0 droll = dy * 0.01 + droll_off dpitch = -dx * 0.0 + dpitch_off dlarmroll = -dy * 0.6 + dlarmroll_off dlarmpitch = dx * 0.0 + dlarmpitch_off dlhiproll = -dy * 0.025 drarmroll = -dy * 0.6 + drarmroll_off drarmpitch = dx * 0.0 + drarmpitch_off # print "droll: " + str(droll) # print "dpitch: " + str(dpitch) # print "dlarmroll: " + str(dlarmroll) # print "dlarmpitch: " + str(dlarmpitch) # print "drarmroll: " + str(drarmroll) # print "drarmpitch: " + str(drarmpitch) # joint_commands.setJointCommand(core.LAnklePitch, (pose[core.LAnklePitch] + dpitch)*inverted_joints[core.LAnklePitch]) # joint_commands.setJointCommand(core.LAnkleRoll, (pose[core.LAnkleRoll] + droll)*inverted_joints[core.LAnkleRoll]) # joint_commands.setJointCommand(core.LShoulderPitch, (pose[core.LShoulderPitch] + dlarmpitch)*inverted_joints[core.LShoulderPitch]) # joint_commands.setJointCommand(core.LShoulderRoll, numpy.clip(pose[core.LShoulderRoll] + dlarmroll, 0.3, 10)*inverted_joints[core.LShoulderRoll]) # joint_commands.setJointCommand(core.LHipRoll, (pose[core.LHipRoll] + dlhiproll)*inverted_joints[core.LHipRoll]) # joint_commands.setJointCommand(core.RShoulderPitch, (pose[core.RShoulderPitch] + drarmpitch)*inverted_joints[core.RShoulderPitch]) # joint_commands.setJointCommand(core.RShoulderRoll, numpy.clip(pose[core.RShoulderRoll] + drarmroll, -0.3, -10)*inverted_joints[core.RShoulderRoll]) #send commands and prevent other controllers from operating joint_commands.send_body_angles_ = True joint_commands.body_angle_time = 1 # 1.0/30.0 * 1000.0#todo: figure out what units this is walk_request.noWalk() kick_request.setNoKick()