-
Notifications
You must be signed in to change notification settings - Fork 3
/
Copy pathtest_rtt_cart_pd_controller.ops
138 lines (108 loc) · 5.65 KB
/
test_rtt_cart_pd_controller.ops
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
# ============================================================
#
# This file is a part of CoSiMA (CogIMon) project
#
# Copyright (C) 2020 by Dennis Leroy Wigand <[email protected]>
#
# This file may be licensed under the terms of the
# GNU Lesser General Public License Version 3 (the ``LGPL''),
# or (at your option) any later version.
#
# Software distributed under the License is distributed
# on an ``AS IS'' basis, WITHOUT WARRANTY OF ANY KIND, either
# express or implied. See the LGPL for the specific language
# governing rights and limitations.
#
# You should have received a copy of the LGPL along with this
# program. If not, go to http://www.gnu.org/licenses/lgpl.html
# or write to the Free Software Foundation, Inc.,
# 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
#
# The development of this software was supported by:
# CoR-Lab, Research Institute for Cognition and Robotics
# Bielefeld University
#
# ============================================================
import("cosima-robot-sim")
import("cosima-controller")
import("eigen_typekit_d")
# import("rtt_roscomm") # TODO need to be installed properly with all the required typekits
loadComponent("robot","cosima::RTTRobotManipulatorSim")
# We need to have the period at 0 to prevent the instantiation of a PeriodicActivity
setActivityOnCPU("robot",0.001,99,ORO_SCHED_OTHER,0)
robot.connectBullet()
# # # Spawn and connect to the robot in bullet
robot.spawnRobot("kuka1", "/home/dwigand/code/cogimon/CoSimA/pyBullet/pyCompliantInteractionPlanning/gym_flexassembly/data/kuka-iiwa-7/model.urdf", "bullet")
robot.setBasePosition("kuka1", 0, -0.2, 0.5)
# # # Connect to the already spawned robot in bullet
# robot.connectToExternallySpawnedRobot("kuka1", 1, "bullet")
robot.defineKinematicChain("kuka1", "/home/dwigand/code/cogimon/CoSimA/pyBullet/pyCompliantInteractionPlanning/gym_flexassembly/data/kuka-iiwa-7/model.urdf", "world", "iiwa7_link_ee")
robot.configure()
# eigen_vector joint_config = new eigen_vector()
# joint_config[0] = 0.2
# ...
# robot.resetJointConfiguration(joint_config)
loadComponent("KinDyn", "cosima::RTTKinDynMultiArm")
setActivityOnCPU("KinDyn",0.001,10,ORO_SCHED_RT,1)
KinDyn.addChain("KDL","/home/dwigand/code/cogimon/CoSimA/pyBullet/pyCompliantInteractionPlanning/gym_flexassembly/data/kuka-iiwa-7/model.urdf","world","iiwa7_link_ee")
KinDyn.configure()
loadComponent("jspdctrl", "cosima::controller::RTTJointPDCtrl")
setActivityOnCPU("jspdctrl",0.001,9,ORO_SCHED_RT,2)
jspdctrl.addRobot(7)
jspdctrl.configure()
jspdctrl.setGains(200,80)
var eigen_vector jconfig = eigen_vector(7)
jconfig[0]=0.0
jconfig[1]=1.2
jconfig[2]=0.0
jconfig[3]=-1.0
jconfig[4]=0.0
jconfig[5]=0.3
jconfig[6]=0.0
jspdctrl.setPositionCmd(jconfig)
var ConnPolicy cp;
connect("robot.out_kuka1_jointstate_fdb","KinDyn.in_robotstatus_0_port",cp)
connect("robot.out_kuka1_gc_fdb","KinDyn.in_external_gravity_0_port",cp)
# connect("robot.out_kuka1_gc_fdb","jspdctrl.in_coriolisAndGravity_port",cp)
connect("robot.out_kuka1_inertia_fdb","KinDyn.in_inertia_0_port",cp)
# connect("robot.out_kuka1_inertia_fdb","jspdctrl.in_inertia_port",cp)
connect("KinDyn.out_robotstatus_port","jspdctrl.in_robotstatus_port",cp)
connect("KinDyn.out_coriolisAndGravity_port","jspdctrl.in_coriolisAndGravity_port",cp)
connect("KinDyn.out_inertia_port","jspdctrl.in_inertia_port",cp)
# connect("robot.out_kuka1_jointstate_fdb","jspdctrl.in_robotstatus_port",cp)
connect("jspdctrl.out_torques_port","robot.in_kuka1_JointTorqueCtrl_cmd",cp)
# CART CONTROLLER
loadComponent("cartctrl", "cosima::controller::RTTCartPIDController")
setActivityOnCPU("cartctrl",0.001,11,ORO_SCHED_RT,3)
cartctrl.addRobot(6, 7);
cartctrl.preparePorts();
cartctrl.configure();
cartctrl.setGains(400,100)
cartctrl.setGainsOrientation(30,2)
connect("KinDyn.out_cartPos_port", "cartctrl.in_currentTaskSpacePosition_port", cp)
connect("KinDyn.out_cartVel_port", "cartctrl.in_currentTaskSpaceVelocity_port", cp)
connect("cartctrl.out_torques_port","robot.in_kuka1_JointTorqueCtrl_cmd", cp)
connect("KinDyn.out_coriolisAndGravity_port", "cartctrl.in_coriolisAndGravity_port", cp)
connect("KinDyn.out_inertia_port","cartctrl.in_inertia_port", cp)
connect("KinDyn.out_robotstatus_port", "cartctrl.in_robotstatus_port", cp)
connect("KinDyn.out_jacobian_port","cartctrl.in_jacobian_port", cp)
connect("KinDyn.out_jacobianDot_port","cartctrl.in_jacobianDot_port", cp)
# connect("Task.out_motion_tracking_1_robotstatus_port", "cartctrl.in_robotstatus_port", cp)
# connect("Task.out_motion_tracking_1_Jacobian_port","cartctrl.in_jacobian_port", cp)
# connect("Task.out_motion_tracking_1_Jacobian_dot_port","cartctrl.in_jacobianDot_port", cp)
# connect("Task.out_motion_tracking_1_Inertia_c_port","cartctrl.in_inertia_port", cp)
# connect("Task.out_motion_tracking_1_GC_port", "cartctrl.in_h_port", cp)
# connect("Task.out_motion_tracking_1_P_port","cartctrl.in_projection_port", cp)
# connect("Task.out_motion_tracking_1_P_dot_port","cartctrl.in_projectionDot_port", cp)
# connect("fkin.out_cartPos_port", "cartctrl.in_currentTaskSpacePosition_port", cp)
# connect("fkin.out_cartVel_port", "cartctrl.in_currentTaskSpaceVelocity_port", cp)
# # connect("trajectorygenerator.out_desiredTaskSpacePosition_port", "cartctrl.in_desiredTaskSpacePosition_port", cp)
# # connect("trajectorygenerator.out_desiredTaskSpaceVelocity_port", "cartctrl.in_desiredTaskSpaceVelocity_port", cp)
# # connect("trajectorygenerator.out_desiredTaskSpaceAcceleration_port", "cartctrl.in_desiredTaskSpaceAcceleration_port", cp)
# connect("cartctrl.out_torques_port","Prio.in_motion_tracking_1_torques", cp)
robot.setControlMode("kuka1", "JointTorqueCtrl")
robot.start()
KinDyn.start()
jspdctrl.start()
# jspdctrl.stop();
# cartctrl.start();