#!/usr/bin/python

# Copyright 2014 Shadow Robot Company Ltd.
#
# This program is free software: you can redistribute it and/or modify it
# under the terms of the GNU General Public License as published by the Free
# Software Foundation, either version 2 of the License, or (at your option)
# any later version.
#
# This program is distributed in the hope that it will be useful, but WITHOUT
# ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
# FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
# more details.
#
# You should have received a copy of the GNU General Public License along
# with this program.  If not, see <http://www.gnu.org/licenses/>.

import rospy
from sr_hand.shadowhand_commander import Commander

rospy.init_node('receiver_example')
c = Commander()

# Add a 1 s delay before sending any command, to allow the undelying command publisher to be initialized.
rospy.sleep(1.0)

#
# The hand is moved by setting the angles (degrees) for the joints that we want to move.
# You do not need to supply all the angles here, you only need to give the
# joints you wish to move.
#
# Note that the move_hand call will return before the motion has finished (and
# sending another while the hand is still moving will cause the new move to
# take over). So you will need to add sleeps to control the motion.
#
# If you use the interpolation_time parameter in move_hand, it determines how long will 
# it take for the hand to reach the commanded position from the current one (this is
# achieved by sending interpolated targets to the hand).

# Use a dictionary to store a named hand position
hand_start = {
    "THJ1": 0, "THJ2": 0, "THJ3": 0, "THJ4": 0, "THJ5": 0,
    "FFJ0": 0, "FFJ3": 0, "FFJ4": 0,
    "MFJ0": 0, "MFJ3": 0, "MFJ4": 0,
    "RFJ0": 0, "RFJ3": 0, "RFJ4": 0,
    "LFJ0": 0, "LFJ3": 0, "LFJ4": 0, "LFJ5": 0,
    "WRJ1": 0, "WRJ2": 0 }

c.move_hand(hand_start)
rospy.loginfo("Move to hand_start position")
rospy.sleep(2.0)

# Grasp with the hand
c.move_hand({
    "THJ5": 14.4140625, "THJ4": 59.515625, "THJ3": 3.265625, "THJ2": -2.67578125, "THJ1": -0.72265625,
    "FFJ4": -2.19140625, "FFJ3": 57.328125, "FFJ0": 66.484375,
    "MFJ4": -0.3125, "MFJ3": 75.625, "MFJ0": 39.7265625,
    "RFJ4": 0.35546875, "RFJ3": 88.1875, "RFJ0": 65.6796875,
    "LFJ5": 0.328125, "LFJ4": 0.5234375, "LFJ3": 89.25, "LFJ0": 58.4453125,
    "interpolation_time": 4.0
})
rospy.loginfo("Move to grasping position")
rospy.sleep(4.5)

# Back to home
tmp = hand_start.copy()
tmp.update({'interpolation_time': 8.0})
c.move_hand(tmp)
rospy.loginfo("Move back to hand_start position")
rospy.sleep(2.0)

# An example of how a new move_hand command can override an ongoing movement for a certain set of joints
# (the previous command c.move_hand(tmp) is still ongoing)
c.move_hand({
    "FFJ4": 0.0, "FFJ3": 90.0, "FFJ0": 180.0,
    "interpolation_time": 2.0
})
# Here we send yet another command that also overrides the previous c.move_hand(tmp) 
c.move_hand({
    "LFJ0": 180.0,
    "interpolation_time": 1.0
})
rospy.sleep(6.5)

# Move one finger
c.move_hand({
    "MFJ4": -0.3125, "MFJ3": 75.625, "MFJ0": 39.7265625,
    "interpolation_time": 5.0
})
rospy.loginfo("Move middle finger")
rospy.sleep(6.0)

c.move_hand(hand_start)
rospy.loginfo("Move back to hand_start position")
rospy.sleep(2.0)

# We can also read the current hand position (position of every joint in degrees)
hand_pos = c.get_hand_position()
rospy.loginfo("Current position:")
print hand_pos
# Move FFJ0 150 degrees from it's current position
hand_pos['FFJ0'] = hand_pos['FFJ0'] + 150
c.move_hand(hand_pos)

rospy.sleep(0.1)

# We can also read the current hand joint velocities (in degrees/s)
hand_vel = c.get_hand_velocity()
rospy.loginfo("Current velocity:")
print hand_vel

# We can also read the current joint efforts
hand_effort = c.get_hand_effort()
rospy.loginfo("Current effort:")
print hand_effort

# The simulated hand does not provide the tactile state
# We can also read the tactile sensor type
tactile_type = c.get_tactile_type()
if tactile_type != None:
    rospy.loginfo("tactile type:")
    print tactile_type

# We can also read the current tactile sensor values
tactile_state = c.get_tactile_state()
if tactile_state != None:
    rospy.loginfo("tactile state:")
    print tactile_state

rospy.sleep(2.0)
