def move_arm_pick():
world = "odom_combined"
profile = Profile()
profile.vel = 0.2
profile.accl = 0.1
profile.profile_type = Profile.SINOID
ml = MoveLin(world, profile)
ml.move('Moving to object position...', [-1, -1.5, 1.2], hold_duration=0.5)
def move_around_torus():
world = "odom_combined"
profile = Profile()
profile.vel = 0.2
profile.accl = 0.1
profile.profile_type = Profile.SINOID
ml = MoveLin(world, profile)
ml.move('Move out of singularity', [-0.094, -0.987, 0.93])
ml.move('Move to center of torus', [-0.09, -0.607, 0.93])
ml.move('Move through torus', [0.558, -0.614, 0.93])
ml.move('Move to torso', [0.56, -0.25, 0.93], hold_duration=0.3)
ml.move('Move away from torso', [0.56, -0.69, 0.93], hold_duration=0.3)
ml.move('1st) Move to center of torus (front)', [0.45, -0.66, 0.94],
hold_duration=0.3)
ml.move('2nd time for safety. Move to center of torus (front)',
[0.45, -0.66, 0.94],
hold_duration=0.3)
ml.move('Move through torus (behind)', [-0.09, -0.607, 0.93],
hold_duration=0.3)
ml.move('Move through torus again (front)', [0.558, -0.607, 0.93],
hold_duration=0.3)
ml.move('Move up', [0.558, -0.607, 1.4], hold_duration=0.3)
ml.move('Move down to center of torus', [0.558, -0.607, 0.93],
hold_duration=0.3)
ml.move('1st) Move down', [0.558, -0.607, 0.5],
hold_duration=0.4) # leave some time for self-reconfig
ml.move('2nd) Move down', [0.558, -0.607, 0.5],
hold_duration=0.4) # leave some time for self-reconfig
ml.move('3rd) Move down', [0.558, -0.607, 0.5],
hold_duration=0.4) # leave some time for self-reconfig
ml.move('Move up to center of torus', [0.558, -0.607, 0.93],
hold_duration=0.3)
ml.move('Move through torus again (behind)', [-0.09, -0.607, 0.93],
hold_duration=0.3)
#! /usr/bin/env python
import math
import rospy
from geometry_msgs.msg import Pose
from cob_cartesian_controller.msg import Profile
import simple_cartesian_interface as sci
if __name__ == '__main__':
rospy.init_node('test_move_circ_interface')
pose = sci.gen_pose(pos=[0.0, 0.7, 1.0], rpy=[0.0, 0.0, 0.0])
start_angle = 0.0 * math.pi / 180.0
end_angle = 90.0 * math.pi / 180.0
profile = Profile()
profile.vel = 0.2
profile.accl = 0.1
#profile.profile_type = Profile.SINOID
profile.profile_type = Profile.RAMP
success, message = sci.move_circ(pose, "world", start_angle, end_angle,
0.3, profile)
if success:
rospy.loginfo(message)
else:
rospy.logerr(message)
from math import radians
import rospy
from geometry_msgs.msg import Pose
from cob_cartesian_controller.msg import Profile
import simple_cartesian_interface as sci
if __name__ == '__main__':
rospy.init_node('test_move_santa')
# Step 1
pose = sci.gen_pose(pos=[-1.0, 0.0, 0.0],
rpy=[radians(45.0), 0.0,
radians(-25.0)])
profile = Profile()
profile.vel = 0.1
profile.accl = 0.2
profile.profile_type = Profile.SINOID
success, message = sci.move_lin(pose, "world", profile)
rospy.sleep(4.0)
# Step 2
pose = sci.gen_pose(pos=[-0.5, 0.5, 0.0],
rpy=[radians(-45.0), 0.0,
radians(-30.0)])
profile = Profile()
profile.vel = 0.1
profile.accl = 0.2
profile.profile_type = Profile.SINOID
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
import math
import rospy
from geometry_msgs.msg import Pose
from cob_cartesian_controller.msg import Profile
import simple_cartesian_interface as sci
if __name__ == '__main__':
rospy.init_node('test_move_lin_interface')
pose = sci.gen_pose(pos=[-0.9, 0.0, 0.0], rpy=[0.0, 0.0, 0.0])
profile = Profile()
profile.vel = 0.2
profile.accl = 0.1
#profile.profile_type = Profile.SINOID
profile.profile_type = Profile.RAMP
success, message = sci.move_lin(pose, "world", profile)
if success:
rospy.loginfo(message)
else:
rospy.logerr(message)