def drakePoseToQPInput(pose, atlasVersion=5, useValkyrie=True, useConstrainedDofs=False, fixedBase=False, forceControl=False): numFloatingBaseJoints = 6 # only publish the fixed base joints if fixedBase: numFloatingBaseJoints = 0 pose = pose[6:] numPositions = np.size(pose) msg = lcmt_qp_controller_input() msg.timestamp = getUtime() msg.num_support_data = 0 msg.num_tracked_bodies = 0 msg.num_external_wrenches = 0 msg.num_joint_pd_overrides = 0 if useValkyrie: if forceControl: msg.param_set_name = 'base' else: msg.param_set_name = 'streaming' else: msg.param_set_name = 'position_control' whole_body_data = lcmt_whole_body_data() whole_body_data.timestamp = getUtime() whole_body_data.num_positions = numPositions whole_body_data.q_des = pose # what are these? Is it still correct for valkyrie # if useValkyrie: # whole_body_data.num_constrained_dofs = len(valConstrainedJointsIdx) # whole_body_data.constrained_dofs = valConstrainedJointsIdx # else: # whole_body_data.num_constrained_dofs = numPositions - 6 # whole_body_data.constrained_dofs = range(7, numPositions+1) # be careful with off by one indexing of constrained_dofs for lcm if useConstrainedDofs: # if we aren't using the fixed base then remove the fixed base from this whole_body_data.num_constrained_dofs = numPositions - numFloatingBaseJoints whole_body_data.constrained_dofs = list( range(numFloatingBaseJoints + 1, numPositions + 1)) msg.whole_body_data = whole_body_data return msg
def drakePoseToQPInput(pose, atlasVersion=5, useValkyrie=True, useConstrainedDofs=False, fixedBase=False, forceControl=False): numFloatingBaseJoints = 6 # only publish the fixed base joints if fixedBase: numFloatingBaseJoints = 0 pose = pose[6:] numPositions = np.size(pose) msg = lcmt_qp_controller_input() msg.timestamp = getUtime() msg.num_support_data = 0 msg.num_tracked_bodies = 0 msg.num_external_wrenches = 0 msg.num_joint_pd_overrides = 0 if useValkyrie: if forceControl: msg.param_set_name = 'base' else: msg.param_set_name = 'streaming' else: msg.param_set_name = 'position_control' whole_body_data = lcmt_whole_body_data() whole_body_data.timestamp = getUtime() whole_body_data.num_positions = numPositions whole_body_data.q_des = pose # what are these? Is it still correct for valkyrie # if useValkyrie: # whole_body_data.num_constrained_dofs = len(valConstrainedJointsIdx) # whole_body_data.constrained_dofs = valConstrainedJointsIdx # else: # whole_body_data.num_constrained_dofs = numPositions - 6 # whole_body_data.constrained_dofs = range(7, numPositions+1) # be careful with off by one indexing of constrained_dofs for lcm if useConstrainedDofs: # if we aren't using the fixed base then remove the fixed base from this whole_body_data.num_constrained_dofs = numPositions - numFloatingBaseJoints whole_body_data.constrained_dofs = range(numFloatingBaseJoints+1, numPositions+1) msg.whole_body_data = whole_body_data return msg
def drakePoseToQPInput(pose, atlasVersion=5): if atlasVersion == 4: numPositions = 34 else: numPositions = 36 msg = lcmt_qp_controller_input() msg.timestamp = getUtime() msg.num_support_data = 0 msg.num_tracked_bodies = 0 msg.num_external_wrenches = 0 msg.num_joint_pd_overrides = 0 msg.param_set_name = 'position_control' whole_body_data = lcmt_whole_body_data() whole_body_data.timestamp = getUtime() whole_body_data.num_positions = numPositions whole_body_data.q_des = pose whole_body_data.num_constrained_dofs = numPositions - 6 whole_body_data.constrained_dofs = range(7, numPositions + 1) msg.whole_body_data = whole_body_data return msg
def drakePoseToQPInput(pose, atlasVersion=5): if atlasVersion == 4: numPositions = 34 else: numPositions = 36 msg = lcmt_qp_controller_input() msg.timestamp = getUtime() msg.num_support_data = 0 msg.num_tracked_bodies = 0 msg.num_external_wrenches = 0 msg.num_joint_pd_overrides = 0 msg.param_set_name = 'position_control' whole_body_data = lcmt_whole_body_data() whole_body_data.timestamp = getUtime() whole_body_data.num_positions = numPositions whole_body_data.q_des = pose whole_body_data.num_constrained_dofs = numPositions - 6 whole_body_data.constrained_dofs = range(7, numPositions+1) msg.whole_body_data = whole_body_data return msg