23 from geometry_msgs.msg
import Pose
24 from cob_cartesian_controller.msg
import CartesianControllerAction, CartesianControllerGoal
25 from cob_cartesian_controller.msg
import Profile
27 from simple_script_server
import simple_script_server
29 import simple_cartesian_interface
as sci
30 import twist_controller_config
as tcc
35 MANIPULABILITY: 0.1 lambda_max, 0.005 w_threshold 39 JLA mit k_H_JLA = -1.0 41 START FROM HOME_POSITION 43 Torus with Radius 0.25 should be placed here (world frame / e.g. odom combined): 44 pose.position.x = 0.25 45 pose.position.y = -0.60 46 pose.position.z = 0.95 52 def __init__(self, world, profile, rpy=(1.571, -0.0004, -0.0114,)):
57 def move(self, title, pos=[0.0, 0.0, 0.0], rpy=None, hold_duration=None):
61 pose = sci.gen_pose(pos, self.
_rpy)
63 if hold_duration
is not None:
64 time.sleep(hold_duration)
68 world =
"odom_combined" 72 profile.profile_type = Profile.SINOID
76 ml.move(
'Move out of singularity', [-0.094, -0.987, 0.93])
77 ml.move(
'Move to center of torus', [-0.09, -0.607, 0.93])
78 ml.move(
'Move through torus', [0.558, -0.614, 0.93])
79 ml.move(
'Move to torso', [0.56, -0.25, 0.93], hold_duration=0.3)
80 ml.move(
'Move away from torso', [0.56, -0.69, 0.93], hold_duration=0.3)
81 ml.move(
'1st) Move to center of torus (front)', [0.45, -0.66, 0.94], hold_duration=0.3)
82 ml.move(
'2nd time for safety. Move to center of torus (front)', [0.45, -0.66, 0.94], hold_duration=0.3)
83 ml.move(
'Move through torus (behind)', [-0.09, -0.607, 0.93], hold_duration=0.3)
84 ml.move(
'Move through torus again (front)', [0.558, -0.607, 0.93], hold_duration=0.3)
85 ml.move(
'Move up', [0.558, -0.607, 1.4], hold_duration=0.3)
86 ml.move(
'Move down to center of torus', [0.558, -0.607, 0.93], hold_duration=0.3)
87 ml.move(
'1st) Move down', [0.558, -0.607, 0.5], hold_duration=0.4)
88 ml.move(
'2nd) Move down', [0.558, -0.607, 0.5], hold_duration=0.4)
89 ml.move(
'3rd) Move down', [0.558, -0.607, 0.5], hold_duration=0.4)
90 ml.move(
'Move up to center of torus', [0.558, -0.607, 0.93], hold_duration=0.3)
91 ml.move(
'Move through torus again (behind)', [-0.09, -0.607, 0.93], hold_duration=0.3)
95 cli = tcc.TwistControllerReconfigureClient()
98 cli.set_config_param(tcc.DAMP_METHOD, tcc.TwistController_MANIPULABILITY)
99 cli.set_config_param(tcc.LAMBDA_MAX, 0.1)
100 cli.set_config_param(tcc.W_THRESH, 0.05)
101 cli.set_config_param(tcc.SOLVER, tcc.TwistController_STACK_OF_TASKS)
102 cli.set_config_param(tcc.K_H, 1.0)
104 cli.set_config_param(tcc.CONSTR_CA, tcc.TwistController_CA)
105 cli.set_config_param(tcc.K_H_CA, -1.9)
106 cli.set_config_param(tcc.DAMP_CA, 0.000001)
107 cli.set_config_param(tcc.ACTIV_THRESH_CA, 0.1)
109 cli.set_config_param(tcc.CONSTR_JLA, tcc.TwistController_JLA)
110 cli.set_config_param(tcc.K_H_JLA, -1.0)
111 cli.set_config_param(tcc.DAMP_JLA, 0.00001)
112 cli.set_config_param(tcc.ACTIV_THRESH_JLA, 10.0)
114 cli.set_config_param(tcc.KIN_EXT, tcc.TwistController_NO_EXTENSION)
115 cli.set_config_param(tcc.KEEP_DIR,
True)
116 cli.set_config_param(tcc.ENF_VEL_LIM,
True)
117 cli.set_config_param(tcc.ENF_POS_LIM,
True)
123 cli.set_config_param(tcc.K_H_CA, -2.0)
129 if __name__ ==
'__main__':
130 rospy.init_node(
'test_move_around_torus')
132 action_name = rospy.get_namespace()+
'cartesian_trajectory_action' 134 rospy.logwarn(
"Waiting for ActionServer: %s", action_name)
135 client.wait_for_server()
136 rospy.logwarn(
"...done")
141 sss.move(
"arm_right",
"home")
def __init__(self, world, profile, rpy=(1.571,-0.0004,-0.0114,))
def move(self, title, pos=[0.0, rpy=None, hold_duration=None)