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
