added cost =  torque+jointlimit and considers four conditions. didn't consider actuation torque. need to fit a model of actuation torque