test.ops

import("cosima-robot-sim")
# import("rtt-bullet-embedded")
# loadComponent("bullet","cosima::RTTBulletEmbedded")
# setActivity("bullet",0.1,10,ORO_SCHED_OTHER)
# bullet.connect()
# bullet.configure()
# bullet.spawnModel("kuka1", "/home/dwigand/code/cogimon/CoSimA/pyBullet/pyCompliantInteractionPlanning/gym_flexassembly/data/kuka-iiwa-7/model.urdf")
import("cosima-controller")

import("eigen_typekit_d")
# import("kdl_typekit")

loadComponent("bullet_robot","cosima::RTTRobotManipulatorSim")
# Faster than 1Kz, because reality is always fast :D
setActivity("bullet_robot",0.001,10,ORO_SCHED_OTHER)
bullet_robot.connect()
bullet_robot.spawnRobotAtPos("kuka1", "/home/dwigand/code/cogimon/CoSimA/pyBullet/pyCompliantInteractionPlanning/gym_flexassembly/data/kuka-iiwa-7/model.urdf", 0, -0.2, 0.5)
# bullet_robot.connectToExternallySpawnedRobot("kuka1", 1)
bullet_robot.spawnRobotAtPos("kuka2", "/home/dwigand/code/cogimon/CoSimA/pyBullet/pyCompliantInteractionPlanning/gym_flexassembly/data/kuka-iiwa-7/model.urdf", -1, -1, 0)
bullet_robot.configure()
bullet_robot.step=true

bullet_robot.setControlMode("kuka1", "JointTorqueCtrl")



loadComponent("KinDyn", "cosima::RTTKinDynMultiArm")
setActivity("KinDyn",0.001,10,ORO_SCHED_OTHER)

KinDyn.addChain("KDL","/home/dwigand/code/cogimon/CoSimA/pyBullet/pyCompliantInteractionPlanning/gym_flexassembly/data/kuka-iiwa-7/model.urdf","iiwa7_link_0","iiwa7_link_ee")

KinDyn.configure()

var ConnPolicy cp;
connect("bullet_robot.out_kuka1_jointstate_fdb","KinDyn.in_robotstatus_port",cp)

# bullet_robot.start()

# import pybullet as p
# client = p.connect(p.GUI_SERVER,1234)


# catkin build -DCMAKE_BUILD_TYPE=DEBUG -DOROCOS-RTT_DIR=/home/dwigand/citk/systems/cogimon-minimal-nightly/lib/cmake/orocos-rtt -DBullet7_DIR=/home/dwigand/citk/systems/cogimon-minimal-nightly/lib/cmake/bullet cosima-controller -Dorocos_kdl_DIR=/home/dwigand/citk/systems/cogimon-minimal-nightly/share/orocos_kdl -Dkdl_parser_DIR=/home/dwigand/code/cogimon/CoSimA/ros-cosima-ws/devel/share/kdl_parser