async def main():
parser = argparse.ArgumentParser(description=__doc__)
parser.add_argument('-l', '--leg', type=int, help='leg (1-4) to zero')
args = parser.parse_args()
servo_ids = {
1: [1, 2, 3],
2: [4, 5, 6],
3: [7, 8, 9],
4: [10, 11, 12],
}[args.leg]
transport = moteus_pi3hat.Pi3HatRouter(servo_bus_map={
1: [1, 2, 3],
2: [4, 5, 6],
3: [7, 8, 9],
4: [10, 11, 12],
}, )
servos = [Servo(transport, x) for x in servo_ids]
[await servo.flush_read() for servo in servos]
aio_stdin = moteus.aiostream.AioStream(sys.stdin.buffer.raw)
read_future = asyncio.create_task(async_readline(aio_stdin))
while True:
print(', '.join([
f"{servo.id: 2d}: {await servo.read_position():7.3f}"
for servo in servos
]) + ' ',
end='\r',
flush=True)
if read_future.done():
break
print()
print()
print("Zeroing servos")
[await servo.zero_offset() for servo in servos]
print("DONE")
def __init__(self, knee, hip_pitch):
self.knee = knee
self.hip_pitch = hip_pitch
# servo_bus_map arg describes which IDs are found on which bus
self.transport = moteus_pi3hat.Pi3HatRouter(servo_bus_map={
1: [self.knee],
2: [self.hip_pitch],
}, )
# explicit servo_id's
self.port_knee = 0
self.port_hip_pitch = 1
# create a moteus.Controller instance for each servo
self.servos = {
servo_id: moteus.Controller(id=servo_id, transport=self.transport)
for servo_id in [1, 2] # number of motors, need to change manually
}
# commands for the motors, default set to mid values
self.commands = [
self.servos[self.knee].make_position(
position=math.nan,
velocity=0.5,
maximum_torque=2.0,
stop_position=(MAX_POS_KN - MIN_POS_KN) / 2,
feedforward_torque=-0.01,
watchdog_timeout=math.nan,
query=True),
self.servos[self.hip_pitch].make_position(
position=math.nan,
velocity=0.5,
maximum_torque=2.0,
stop_position=(MAX_POS_HP - MIN_POS_HP) / 2,
feedforward_torque=-0.01,
watchdog_timeout=math.nan,
query=True),
]
async def main():
print("creating moteus pi3hat transport")
# our system has 2 servos, each attached to a separate pi3hat bus
# servo_bus_map argument describes which IDs are found on which bus
transport = moteus_pi3hat.Pi3HatRouter(
servo_bus_map={
1: [1], # KNEE
2: [2], # HIP
}, )
# We create one 'moteus.Controller' instance for each servo. It is not
# strictly required to pass a 'transport' since we do not intend to use
# any 'set_*' methods, but it doesn't hurt.
#
# syntax is a python "dictionary comprehension"
servos = {
servo_id: moteus.Controller(id=servo_id, transport=transport)
for servo_id in [1, 2]
}
# We will start by sending a 'stop' to all servos, in the event that any had a fault
await transport.cycle([x.make_stop() for x in servos.values()])
print("sent stop cmd to clear any motor faults")
while True:
# the 'cycle' method accepts a list of commands, each of which is created by
# calling one of the 'make_foo' methods on Controller. The most command thing
# will be the 'make_position' method
now = time.time()
# construct a pos command for each servo, each of which consists of a
# sinusoidal velocity command starting from wherever the servo was at
# to begin with
#
# 'make_position' accepts optional keyword arguments that correspond
# to each of the available position mode registers in the moteus
# reference manual
half_kn = (MAX_POS_KN - MIN_POS_KN) / 2
half_hp = (MAX_POS_HP - MIN_POS_HP) / 2
commands_sinusoidal = [
servos[1].make_position( # KNEE
position=math.nan,
velocity=0.5,
maximum_torque=2.0,
stop_position=MIN_POS_KN + half_kn + math.sin(now) * half_kn,
feedforward_torque=-0.01,
watchdog_timeout=math.nan,
query=True),
servos[2].make_position( # HIP
position=math.nan,
velocity=0.5,
maximum_torque=2.0,
stop_position=MIN_POS_HP + half_hp +
math.sin(now + 1) * half_hp,
feedforward_torque=-0.01,
watchdog_timeout=math.nan,
query=True),
]
# position commands
commands_pos = [
servos[1].make_position( # KNEE
position=math.nan,
velocity=2.0,
maximum_torque=2.0,
stop_position=MAX_POS_KN,
feedforward_torque=-0.01,
watchdog_timeout=math.nan,
query=True),
servos[2].make_position( # HIP
position=math.nan,
velocity=2.0,
maximum_torque=2.0,
stop_position=MAX_POS_HP,
feedforward_torque=-0.01,
watchdog_timeout=math.nan,
query=True),
]
# By sending all commands to the transport in one go, the pi3hat
# can send out commands and retrieve responses simultaneously
# from all ports. It can also pipeline commands and responses
# for multiple servos on the same bus
results = await transport.cycle(commands_sinusoidal)
#results = await transport.cycle(commands_pos)
# The result is a list of 'moteus.Result' types, each of which
# identifies the servo it came from, and has a 'values' field
# that allows access to individual register results.
#
# NOTE: it is possible to not receive responses from all servos
# for which a query was requested
#
# Here, we'll just print the ID, position, and velocity of each
# servo for which a reply was returned
print("\n".join(f"({result.id}) " +
f"{result.values[moteus.Register.POSITION]} " +
f"{result.values[moteus.Register.VELOCITY]} "
for result in results),
end='\r')
#print(now, end='\r')
# We will wait 20ms between cycles. By default, each servo has
# a watchdog timeout, where if no CAN command is received for
# 100mc the controller will enter a latched fault state
await asyncio.sleep(0.02)
async def main():
# We will be assuming a system where there are 4 servos, each
# attached to a separate pi3hat bus. The servo_bus_map argument
# describes which IDs are found on which bus.
transport = moteus_pi3hat.Pi3HatRouter(
servo_bus_map = {
1:[11],
2:[12],
3:[13],
4:[14],
},
)
# We create one 'moteus.Controller' instance for each servo. It
# is not strictly required to pass a 'transport' since we do not
# intend to use any 'set_*' methods, but it doesn't hurt.
#
# This syntax is a python "dictionary comprehension":
# https://docs.python.org/3/tutorial/datastructures.html#dictionaries
servos = {
servo_id : moteus.Controller(id=servo_id, transport=transport)
for servo_id in [11, 12, 13, 14]
}
# We will start by sending a 'stop' to all servos, in the event
# that any had a fault.
await transport.cycle([x.make_stop() for x in servos.values()])
while True:
# The 'cycle' method accepts a list of commands, each of which
# is created by calling one of the `make_foo` methods on
# Controller. The most common thing will be the
# `make_position` method.
now = time.time()
# For now, we will just construct a position command for each
# of the 4 servos, each of which consists of a sinusoidal
# velocity command starting from wherever the servo was at to
# begin with.
#
# 'make_position' accepts optional keyword arguments that
# correspond to each of the available position mode registers
# in the moteus reference manual.
commands = [
servos[11].make_position(
position=math.nan,
velocity=0.1*math.sin(now),
query=True),
servos[12].make_position(
position=math.nan,
velocity=0.1*math.sin(now + 1),
query=True),
servos[13].make_position(
position=math.nan,
velocity=0.1*math.sin(now + 2),
query=True),
servos[14].make_position(
position=math.nan,
velocity=0.1*math.sin(now + 3),
query=True),
]
# By sending all commands to the transport in one go, the
# pi3hat can send out commands and retrieve responses
# simultaneously from all ports. It can also pipeline
# commands and responses for multiple servos on the same bus.
results = await transport.cycle(commands)
# The result is a list of 'moteus.Result' types, each of which
# identifies the servo it came from, and has a 'values' field
# that allows access to individual register results.
#
# Note: It is possible to not receive responses from all
# servos for which a query was requested.
#
# Here, we'll just print the ID, position, and velocity of
# each servo for which a reply was returned.
print(", ".join(
f"({result.id} " +
f"{result.values[moteus.Register.POSITION]} " +
f"{result.values[moteus.Register.VELOCITY]})"
for result in results))
# We will wait 20ms between cycles. By default, each servo
# has a watchdog timeout, where if no CAN command is received
# for 100ms the controller will enter a latched fault state.
await asyncio.sleep(0.02)
async def main():
transport = moteus_pi3hat.Pi3HatRouter(servo_bus_map={
1: [13, 23, 24],
2: [14, 18, 17],
3: [16, 22, 21],
4: [15, 19, 20]
})
c13 = moteus.Controller(id=13, transport=transport)
c14 = moteus.Controller(id=14, transport=transport)
c15 = moteus.Controller(id=15, transport=transport)
c16 = moteus.Controller(id=16, transport=transport)
c17 = moteus.Controller(id=17, transport=transport)
c18 = moteus.Controller(id=18, transport=transport)
c19 = moteus.Controller(id=19, transport=transport)
c20 = moteus.Controller(id=20, transport=transport)
c21 = moteus.Controller(id=21, transport=transport)
c22 = moteus.Controller(id=22, transport=transport)
c23 = moteus.Controller(id=23, transport=transport)
c24 = moteus.Controller(id=24, transport=transport)
await c13.set_stop()
await c14.set_stop()
await c15.set_stop()
await c16.set_stop()
await c17.set_stop()
await c18.set_stop()
await c19.set_stop()
await c20.set_stop()
await c21.set_stop()
await c22.set_stop()
await c23.set_stop()
await c24.set_stop()
maxT = 0.1
while True:
await transport.cycle([
c13.make_position(position=0.3499, maximum_torque=maxT,
query=True),
c23.make_position(position=0.15617 - 1,
maximum_torque=maxT,
query=True),
c24.make_position(position=0.1710, maximum_torque=maxT,
query=True),
c14.make_position(position=0.74639,
maximum_torque=maxT,
query=True),
c17.make_position(position=0.32684,
maximum_torque=maxT,
query=True),
c18.make_position(position=-0.73236 + 1,
maximum_torque=maxT,
query=True),
c16.make_position(position=-0.38513,
maximum_torque=maxT,
query=True),
c21.make_position(position=0.50958,
maximum_torque=maxT,
query=True),
c22.make_position(position=0.328186,
maximum_torque=maxT,
query=True),
c15.make_position(position=-0.920715,
maximum_torque=maxT,
query=True),
c19.make_position(position=0.6815185 - 1,
maximum_torque=maxT,
query=True),
c20.make_position(position=1.155334 - 1,
maximum_torque=maxT,
query=True),
])
if maxT < 0.4:
maxT = maxT + 0.05
await asyncio.sleep(0.05)
import math
import moteus
import moteus_pi3hat
import time
import argparse
import sys
# TODO: check out and if needed tune/change the motor's internal PID
# in order to increase damping!
knee = 1
hip = 2
transport = moteus_pi3hat.Pi3HatRouter(
servo_bus_map={
1: [knee], # KNEE
2: [hip], # HIP
}, )
servos = {
servo_id: moteus.Controller(id=servo_id, transport=transport)
for servo_id in [1, 2]
}
# NOTE: sin controller: Kp, dt, l0, l1, animation)
ctrlr = sinIkHopCtrlr(25.0, 0.015, 0.1, 0.15, False)
ctrlr_x = ctrlr_y = 0
theta0 = 0 # hip angle
theta1 = 0 # knee angle
commands = []
ctrlr.q[0] = 0 # x ft pos in sim
async def main():
# The parameters of each CAN bus can be set at construction time.
# The available fields can be found in the C++ header at
# Pi3Hat::CanConfiguration
slow_can = moteus_pi3hat.CanConfiguration()
slow_can.slow_bitrate = 125000
slow_can.fdcan_frame = False
slow_can.bitrate_switch = False
# If buses are not listed, then they default to the parameters
# necessary to communicate with a moteus controller.
can_config = {
5: slow_can,
}
transport = moteus_pi3hat.Pi3HatRouter(can=can_config)
# Since we didn't specify a 'servo_bus_map' to Pi3HatRouter, this
# will be assumed to be on bus 1.
controller = moteus.Controller(id=1, transport=transport)
while True:
# To send a raw CAN, you must manually instantiate a
# 'moteus.Command' and fill in its fields, along with which
# bus to send it on.
raw_message = moteus.Command()
raw_message.raw = True
raw_message.arbitration_id = 0x0405
raw_message.bus = 5
raw_message.data = b'1234'
raw_message.reply_required = False
# A single 'transport.cycle' call's message list can contain a
# mix of "raw" frames and those generated from
# 'moteus.Controller'.
#
# If you want to listen on a CAN bus without having sent a
# command with 'reply_required' set, you can use the
# 'force_can_check' optional parameter. It is a 1-indexed
# bitfield listing which additional CAN buses should be
# listened to.
results = await transport.cycle([
raw_message,
controller.make_query(),
],
force_can_check=(1 << 5))
# If any raw CAN frames are present, the result list will be a
# mix of moteus.Result elements and can.Message elements.
# They each have the 'bus', 'arbitration_id', and 'data'
# fields.
#
# moteus.Result elements additionally have an 'id' field which
# is the moteus servo ID and a 'values' field which reports
# the decoded response.
for result in results:
if hasattr(result, 'id'):
# This is a moteus structure.
print(f"{time.time():.3f} MOTEUS {result}")
else:
# This is a raw structure.
print(
f"{time.time():.3f} BUS {result.bus} " +
f"ID {result.arbitration_id:x} DATA {result.data.hex()}")
await asyncio.sleep(1.0)