rbprmstate.py 5.86 KB
Newer Older
t steve's avatar
t steve committed
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
#!/usr/bin/env python
# Copyright (c) 2014 CNRS
# Author: Steve Tonneau
#
# This file is part of hpp-rbprm-corba.
# hpp-rbprm-corba is free software: you can redistribute it
# and/or modify it under the terms of the GNU Lesser General Public
# License as published by the Free Software Foundation, either version
# 3 of the License, or (at your option) any later version.
#
# hpp-manipulation-corba 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 Lesser Public License for more details.  You should have
# received a copy of the GNU Lesser General Public License along with
# hpp-manipulation-corba.  If not, see
# <http://www.gnu.org/licenses/>.

from hpp.corbaserver.rbprm import Client as RbprmClient
from hpp.corbaserver import Client as BasicClient
from numpy import array


## Creates a state given an Id pointing to an existing c++ state
#
#  A RbprmDevice robot is a set of two robots. One for the 
#  trunk of the robot, one for the range of motion
class State (object):
    ## Constructor
    def __init__ (self, fullBody, sId, isIntermediate = False):
        self.cl = fullBody.client.rbprm.rbprm
        self.sId = sId
        self.isIntermediate = isIntermediate    
        self.fullBody = fullBody
    
    ## assert for case where functions can't be used with intermediate state
    def _cni(self):
        assert not self.isIntermediate, "method can't be called with intermediate state"
            
    ## Get the state configuration
    def q(self):
        self._cni()
        return self.cl.getConfigAtState(self.sId)
        
    ## Set the state configuration
    # \param q configuration of the robot
    # \return whether or not the configuration was successfully set
    def setQ(self, q):
        self._cni()
        return self.cl.setConfigAtState(self.sId, q)    > 0
        
    # \param state1 current state considered
    # \param limb name of the limb for which the request aplies
    # \return whether the limb is in contact at this state
    def isLimbInContact(self, limbname):
        if(self.isIntermediate):
            return self.cl.isLimbInContactIntermediary(limbname, self.sId) >0
        else:            
            return self.cl.isLimbInContact(limbname, self.sId) >0
                
    # 
    # \param state1 current state considered
    # \param limb name of the limb for which the request aplies
    # \return all limbs in contact at this state
    def getLimbsInContact(self):
        return [limbName for limbName in self.fullBody.limbNames if self.isLimbInContact(limbName)]
        
    ## Get the end effector position for a given configuration, assuming z normal
    # \param limbName name of the limb from which to retrieve a sample
    # \param configuration configuration of the robot
    # \return world position of the limb end effector given the current robot configuration.
    # array of size 4, where each entry is the position of a corner of the contact surface
    def getEffectorPosition(self, limbName):
        self._cni()
        return self.cl.getEffectorPosition(limbName,self.q())
     
    ## Get the end effector position for a given configuration, assuming z normal
    # \param limbName name of the limb from which to retrieve a sample
    # \param configuration configuration of the robot
    # \return world position of the limb end effector given the current robot configuration.
    # array of size 4, where each entry is the position of a corner of the contact surface
    def getContactPosAndNormals(self):
        if(self.isIntermediate):  
            rawdata = self.cl.computeContactPointsAtState(self.sId, 1)
        else:            
            rawdata = self.cl.computeContactPointsAtState(self.sId, 0) 
        return [[b[i:i+3] for i in range(0, len(b), 6)] for b in rawdata], [[b[i+3:i+6] for i in range(0, len(b), 6)] for b in rawdata]
     
    ## Get the end effector position for a given configuration, assuming z normal
    # \param limbName name of the limb from which to retrieve a sample
    # \param configuration configuration of the robot
    # \return world position of the limb end effector given the current robot configuration.
    # array of size 4, where each entry is the position of a corner of the contact surface
    def getContactPosAndNormalsForLimb(self, limbName):
95
        assert self.isLimbInContact(limbName), "in getContactPosAndNormals: limb " + limbName +  "is not in contact at  state" + str(stateId) 
t steve's avatar
t steve committed
96
        if(self.isIntermediate):  
97
            rawdata = self.cl.computeContactPointsAtStateForLimb(self.sId,1, limbName)
t steve's avatar
t steve committed
98
        else:            
99
            rawdata = self.cl.computeContactPointsAtStateForLimb(self.sId,0, limbName) 
t steve's avatar
t steve committed
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
        return [[b[i:i+3] for i in range(0, len(b), 6)] for b in rawdata], [[b[i+3:i+6] for i in range(0, len(b), 6)] for b in rawdata]
        
    
    ## Get position of center of mass
    def getCenterOfMass (self):
        q0 = fullBody.client.basic.robot.getCurrentConfig()
        fullBody.client.basic.robot.setCurrentConfig(self.q())
        c = fullBody.client.basic.robot.getComPosition()
        fullBody.client.basic.robot.setCurrentConfig(q0)
        return c
    
    
    ## Get the end effector position for a given configuration, assuming z normal
    # \param limbName name of the limb from which to retrieve a sample
    # \param configuration configuration of the robot
    # \return world position of the limb end effector given the current robot configuration.
    # array of size 4, where each entry is the position of a corner of the contact surface
    def getContactCone(self, friction):
        if(self.isIntermediate):  
            rawdata = self.cl.getContactIntermediateCone(self.sId,friction)
        else:            
            rawdata = self.cl.getContactCone(self.sId,friction) 
        H_h =  array(rawdata)
        return H_h[:,:-1], H_h[:, -1]