def init_simulation():
# m_friction
controls = {}
robotPath = "phantomx_description/urdf/phantomx.urdf"
sim = Simulation(robotPath, gui=True, panels=True, useUrdfInertia=False)
#sim.setFloorFrictions(lateral=1, spinning=0.1, rolling=0.1)
pos = sim.getRobotPose()
sim.setRobotPose([0, 0, 0.5], [0, 0, 0, 1])
leg_center_pos = [0.1248, -0.06164, 0.001116 + 0.5]
leg_angle = -math.pi / 4
sim_start_time = time.time()
return controls, sim, pos, leg_center_pos, leg_angle, sim_start_time
def init_simulation():
global sim
# m_friction
controls = {}
robotPath = "phantomx_description/urdf/phantomx.urdf"
sim = Simulation(robotPath, gui=True, panels=True, useUrdfInertia=False)
#sim.setFloorFrictions(lateral=1, spinning=0.1, rolling=0.1)
pos = sim.getRobotPose()
sim.setRobotPose([0, 0, 0.5], [0, 0, 0, 1])
leg_center_pos = [0.1248, -0.06164, 0.001116 + 0.5]
leg_angle = -math.pi / 4
targets = {}
for name in sim.getJoints():
if "c1" in name or "thigh" in name or "tibia" in name:
targets[name] = 0
return controls, sim, pos, leg_center_pos, leg_angle, targets
# Updates the values of the dictionnary targets to set 3 angles to a given leg
def set_leg_angles(alphas, leg_id, targets, params):
leg = params.legs[leg_id]
i = -1
for name in leg:
i += 1
targets[name] = alphas[i]
# m_friction
parser = argparse.ArgumentParser()
parser.add_argument("--mode", "-m", type=str, default="direct", help="test")
args = parser.parse_args()
controls = {}
robotPath = "phantomx_description/urdf/phantomx.urdf"
sim = Simulation(robotPath, gui=True, panels=True, useUrdfInertia=False)
# sim.setFloorFrictions(lateral=0, spinning=0, rolling=0)
pos, rpy = sim.getRobotPose()
sim.setRobotPose([0, 0, 0.5], [0, 0, 0, 1])
leg_center_pos = [0.1248, -0.06164, 0.001116 + 0.5]
leg_angle = -math.pi / 4
params = Parameters()
if args.mode == "frozen-direct":
crosses = []
for i in range(4*6):
crosses.append(p.loadURDF("target2/robot.urdf"))
for name in sim.getJoints():
print(name)
def move_cross_attached(cross, leg, pt, robotPos):
crossPos = absolute_position(leg, pt, robotPos)
p.resetBasePositionAndOrientation(
cross, crossPos,
to_pybullet_quaternion(robotPos[1][0], robotPos[1][1], robotPos[1][2]))
# m_friction
parser = argparse.ArgumentParser()
parser.add_argument("--mode", "-m", type=str, default="direct", help="test")
args = parser.parse_args()
controls = {}
robotPath = "phantomx_description/urdf/phantomx.urdf"
sim = Simulation(robotPath, gui=True, panels=True, useUrdfInertia=False)
# sim.setFloorFrictions(lateral=0, spinning=0, rolling=0)
pos, rpy = sim.getRobotPose()
sim.setRobotPose([0, 0, 0.5], [0, 0, 0, 1])
params = Parameters()
if args.mode == "frozen-direct":
crosses = []
for i in range(4):
crosses.append(p.loadURDF("target2/robot.urdf"))
for name in sim.getJoints():
# print(name)
if ("c1" in name or "thigh" in name
or "tibia" in name) and "rf" in name:
controls[name] = p.addUserDebugParameter(name, -math.pi, math.pi,
action="store_true")
parser.add_argument(
"-rotate",
help=
"MODE rotate : Rotating without moving the center of the robot (the 6 legs staying on the floor) ",
action="store_true")
parser.add_argument(
"-robot-ik",
help=
"MODE robot-ik : Moving the center of the robot to an arbitrary (x, y, z) position (the 6 legs staying on the floor)",
action="store_true")
args = parser.parse_args()
controls = {}
robotPath = "phantomx_description/urdf/phantomx.urdf"
sim = Simulation(robotPath, gui=True, panels=True, useUrdfInertia=False)
pos, rpy = sim.getRobotPose()
sim.setRobotPose([0, 0, 0.5], to_pybullet_quaternion(0, 0, 0))
params = Parameters()
# m_friction
# sim.setFloorFrictions(lateral=0, spinning=0, rolling=0)
""" Some custom variables """
raw = 0
pitch = 0
yaw = 0
posx = 0
posy = 0
leg_center_pos = [0.1248, -0.06164, 0.001116 + 0.5]