33 lines
606 B
Python
33 lines
606 B
Python
import rospy
|
|
|
|
from controllers import *
|
|
from model import *
|
|
from robot_sim import *
|
|
from utils import *
|
|
|
|
|
|
# parameters
|
|
gui = True
|
|
dt = 1.0 / 60.0
|
|
|
|
rospy.init_node("demo")
|
|
|
|
env = SimPandaEnv(gui)
|
|
model = Model("panda_link0", "panda_ee")
|
|
|
|
q, dq = env.arm.get_state()
|
|
x0 = model.pose(q)
|
|
|
|
controller = CartesianPoseController(model, x0)
|
|
|
|
marker = InteractiveMarkerWrapper("target", "panda_link0", x0)
|
|
|
|
# run the control loop
|
|
while True:
|
|
controller.set_target(marker.pose)
|
|
q, dq = env.arm.get_state()
|
|
cmd = controller.update(q, dq)
|
|
env.arm.set_desired_joint_velocities(cmd)
|
|
|
|
env.forward(dt)
|