diff --git a/common/Motion/motion_physical.cpp b/common/Motion/motion_physical.cpp index ba7c0b6..f442981 100644 --- a/common/Motion/motion_physical.cpp +++ b/common/Motion/motion_physical.cpp @@ -10,6 +10,7 @@ #include #include #include +#include #include #include #include @@ -116,6 +117,7 @@ class MyControllerUpdateData : public ControllerUpdateData AutoMsg headState; AutoMsg leftGripperState,rightGripperState; AutoMsg leftReflexGripperState,rightReflexGripperState; + AutoMsg leftCuffState,rightCuffState; AutoMsg baseState; Config lastBaseDifferencePos; double lastBaseDifferenceTime; @@ -161,13 +163,17 @@ bool MyControllerUpdateData::MyStartup() { nh = new ros::NodeHandle(); larm_pub = nh->advertise(string("robot/limb/left/joint_command"),10,false); rarm_pub = nh->advertise(string("robot/limb/right/joint_command"),10,false); - head_pan_pub = nh->advertise(string("robot/head/command_head_pan"),100,false); + head_pan_pub = nh->advertise(string("robot/head/command_head_pan"),10,false); head_nod_pub = nh->advertise("robot/head/command_head_nod",10,false); base_twist_pub = nh->advertise("hstar_amp1/base/auto/cmd/ts",10,false); assemblyState.subscribe(*nh,"robot/state",10); jointState.subscribe(*nh,"robot/joint_states",10); headState.subscribe(*nh,"robot/head/head_state",10); + leftCuffState.state = 0; + rightCuffState.state = 0; + leftCuffState.subscribe(*nh,"robot/digital_io/left_lower_cuff/state",10); + rightCuffState.subscribe(*nh,"robot/digital_io/right_lower_cuff/state",10); leftGripperState.subscribe(*nh,"robot/end_effector/left_gripper/state",10); rightGripperState.subscribe(*nh,"robot/end_effector/right_gripper/state",10); @@ -441,12 +447,18 @@ bool MyControllerUpdateData::MyProcessSensors() robotState.rightLimb.sensedEffort[k] = jointState->effort[i]; } } - if(robotState.leftLimb.commandedConfig.empty()) { + if(robotState.leftLimb.commandedConfig.empty() || leftCuffState.state==1 /*startup or cuff pressed, reset commanded*/) { + SetLimbGovernor(LEFT,LimbState::Normal); + robotState.leftLimb.controlMode = LimbState::POSITION; + robotState.leftLimb.sendCommand = false; robotState.leftLimb.commandedConfig = robotState.leftLimb.sensedConfig; robotState.leftLimb.commandedVelocity = robotState.leftLimb.sensedVelocity; robotState.leftLimb.commandedEffort = robotState.leftLimb.sensedEffort; } - if(robotState.rightLimb.commandedConfig.empty()) { + if(robotState.rightLimb.commandedConfig.empty() || rightCuffState.state==1 /*startup or cuff pressed, reset commanded*/) { + SetLimbGovernor(RIGHT,LimbState::Normal); + robotState.rightLimb.controlMode = LimbState::POSITION; + robotState.rightLimb.sendCommand = false; robotState.rightLimb.commandedConfig = robotState.rightLimb.sensedConfig; robotState.rightLimb.commandedVelocity = robotState.rightLimb.sensedVelocity; robotState.rightLimb.commandedEffort = robotState.rightLimb.sensedEffort;